Cleaned up the code. Added a few comments. Still needs to be tested.