Fix bugs and add tests