Fix some bugs, add tests