Mostly implemented waypoint parsing