Cleanup code, fix failing unit tests