Cleanup code, add new test