Optimal Waypoint Function Finished