Changed a little, hope it is done! Add JUnit test.