Start of plotting waypoints - almost there.