Break out waypoint handling logic
Add tests for the different input variations.