Minor edits to waypoint algorithm