Rearranged some code, fixed some bugs, added tests