Completed the code

Cleaned the code, added comments, and added test code.