Proposing new waypoints code

Hopefully a more flexible approach.  Will discuss on Tuesday or
Wednesday.  TODO: at the moment I'm not printing things to the screen.
Will do that soon.