Add more formal state handling logic and start saving waypoints.