Clean up code, add tests, fix bugs found in tests