Changes made

include/util/query_heap.hpp

@@ -2,6 +2,7 @@
 #define OSRM_UTIL_QUERY_HEAP_HPP
 
 #include <boost/assert.hpp>
+#include <boost/optional.hpp>
 #include <boost/heap/d_ary_heap.hpp>
 
 #include <algorithm>
@@ -194,10 +195,27 @@ template <typename NodeID,
           typename IndexStorage = ArrayStorage<NodeID, NodeID>>
 class QueryHeap
 {
+  private:
+
+    using HeapData = std::pair<Weight, Key>;
+    using HeapContainer = boost::heap::d_ary_heap<HeapData,
+        boost::heap::arity<4>,
+        boost::heap::mutable_<true>,
+        boost::heap::compare<std::greater<HeapData>>>;
+    using HeapHandle = typename HeapContainer::handle_type;
+
   public:
     using WeightType = Weight;
     using DataType = Data;
 
+    struct HeapNode
+    {
+        HeapHandle handle;
+        NodeID node;
+        Weight weight;
+        Data data;
+    };
+
     template <typename... StorageArgs> explicit QueryHeap(StorageArgs... args) : node_index(args...)
     {
         Clear();
@@ -230,6 +248,13 @@ class QueryHeap
         return inserted_nodes[index].data;
     }
 
+    HeapNode& getHeapNode(NodeID node)
+    {
+        const auto index = node_index.peek_index(node);
+        BOOST_ASSERT((int)index >= 0 && (int)index < (int)inserted_nodes.size());
+        return inserted_nodes[index];
+    }
+
     Data const &GetData(NodeID node) const
     {
         const auto index = node_index.peek_index(node);
@@ -269,6 +294,28 @@ class QueryHeap
         return inserted_nodes[index].node == node;
     }
 
+    boost::optional<HeapNode&> WasInsertedGetHeapNode(const NodeID node)
+    {
+        const auto index = node_index.peek_index(node);
+        if (index >= static_cast<decltype(index)>(inserted_nodes.size()))
+        {
+            return {};
+        }
+        return inserted_nodes[index];
+    }
+
+    const boost::optional<const HeapNode&> WasInsertedGetHeapNode(const NodeID node) const
+    {
+        const auto index = node_index.peek_index(node);
+        if (index >= static_cast<decltype(index)>(inserted_nodes.size()))
+        {
+            return {};
+        }
+        return inserted_nodes[index];
+    }
+
+
+
     NodeID Min() const
     {
         BOOST_ASSERT(!heap.empty());
@@ -290,6 +337,15 @@ class QueryHeap
         return inserted_nodes[removedIndex].node;
     }
 
+    HeapNode& DeleteMinGetHeapNode()
+    {
+        BOOST_ASSERT(!heap.empty());
+        const Key removedIndex = heap.top().second;
+        heap.pop();
+        inserted_nodes[removedIndex].handle = heap.s_handle_from_iterator(heap.end());
+        return inserted_nodes[removedIndex];
+    }
+
     void DeleteAll()
     {
         auto const none_handle = heap.s_handle_from_iterator(heap.end());
@@ -308,22 +364,13 @@ class QueryHeap
         heap.increase(reference.handle, std::make_pair(weight, index));
     }
 
-  private:
-    using HeapData = std::pair<Weight, Key>;
-    using HeapContainer = boost::heap::d_ary_heap<HeapData,
-                                                  boost::heap::arity<4>,
-                                                  boost::heap::mutable_<true>,
-                                                  boost::heap::compare<std::greater<HeapData>>>;
-    using HeapHandle = typename HeapContainer::handle_type;
-
-    struct HeapNode
+    void DecreaseKey(const HeapNode& heapNode)
     {
-        HeapHandle handle;
-        NodeID node;
-        Weight weight;
-        Data data;
-    };
+        BOOST_ASSERT(!WasRemoved(heapNode.node));
+        heap.increase(heapNode.handle, std::make_pair(heapNode.weight, (*heapNode.handle).second));
+    }
 
+  private:
     std::vector<HeapNode> inserted_nodes;
     HeapContainer heap;
     IndexStorage node_index;