Cleaned up code and added tests