refactor, add tests, update readme.

Should now produce expected results on all tests.
lol, I've had that too. I already included one in my last commit.