Better reverse handling.

Better reverse handling on the last reverse waypoint.