refactor, add tests, update readme.

Should now produce expected results on all tests.
> just let me know what you prefer.

I would prefer the previous nested one. I kinda found it easier to read. Highly subjective ofc :)