updated the logic and added new param to enable waypoint pruning - right now it will pick the first one outside the threshold