Rewrite code somewhat, add tests.