Cleaned up code and added a few test cases