Adding unit tests, working through correctness