Started writing unit tests. Code cleanup and adding javadocs.