Add some tests and clean up code