Fix bugs, add some tests