Cleanup waypoints code