Cleaned up code and wrote doxygen comments.