Cleanup and refactoring, writing tests