Clean up code and add tests