Replaced deques for waypoint storage with a simple doubly linked list.
It works perfectly.