Working on debugging the waypoints offset