Add JUnit tests and clean code