Cleaned up code and added test file