Remade the navigation state code, and readied it for the new waypoint lists