Clean up and add unit tests