Add some tests, refactor and simplify a little.