Clean up code and fix #4