refactor, add tests, update readme.

Should now produce expected results on all tests.
hmm, couldn't it be as simple as it was just without this line?