Fix bugs, add tests