I think I fixed the weird waypoint bug where wrong waypoints were inserted