Added unit tests and fixes bug found on the way