Fixed bugs in previous commit, added unit test.