removed Waypoint class, made a struct GraphNode type in NavigatorGraph
new PriorityQueue class, prioritizes min value