Cleaning up code, some bugs in the testing file