Clean up code
Add test