Adding unit tests and fixing broken code found by test