Work in progress waypoints code, new system.

For Alex to have a look at in the mean-a-time.