working on waypoint state machine