Making a Waypoint and Tree class could make the code more clear. Not a necessary change as functionality would not change but could be done.