Implemented tests and refactored.
Some more cleanup