added the option of using a better end of waypoint heuristic.