On closer inspection, it appears that not all the waypoint are being added to the deque.  This should fix it