Better waypoint selection and  fixes

Now just the first waypoint is chosen randomly, the following are selected by distance.