refactor, add tests, update readme.

Should now produce expected results on all tests.
i don't really see the benefit of extracting this into a method. it's only a few lines when inline.