Clean up!

Do some refactoring, improving, and add real unit tests!