Fix some bugs and add tests