Cleaned up code and added unit tests