Reworked waypoints_loop1.  TODO: waypoints_loop2