Cleanup, revamp and add tests