From 79b6b1b24d7cdb48dac8cc9524df642ea23f00c1 Mon Sep 17 00:00:00 2001
From: dmatetelki <denes.matetelki@emerson.com>
Date: Thu, 27 Aug 2015 12:04:58 +0200
Subject: [PATCH] Dijkstra uses priority queue

---
 lib/graph/graph_algorithms.hpp | 40 +++++++++++++++-------------------
 lib/graph/priority_queue.hpp   | 10 ++++-----
 2 files changed, 23 insertions(+), 27 deletions(-)

diff --git a/lib/graph/graph_algorithms.hpp b/lib/graph/graph_algorithms.hpp
index ad750bc..7768c97 100644
--- a/lib/graph/graph_algorithms.hpp
+++ b/lib/graph/graph_algorithms.hpp
@@ -2,10 +2,11 @@
 #define GRAPH_ALGORITHMS_HPP
 
 #include "graph.hpp"
+#include "priority_queue.hpp"
 
 #include <vector>
 #include <unordered_map>
-#include <unordered_set>
+
 #include <queue>
 #include <set>
 
@@ -17,19 +18,6 @@
 
 namespace {
 
-template <typename V, typename W>
-inline V closestNode(const std::unordered_set<V>& q, const std::unordered_map<V, std::pair<W, V> >& dist_prev)
-{
-  const typename std::unordered_set<V>::const_iterator smallest_it =
-    std::min_element(q.begin(), q.end(),
-      [&dist_prev](const V& v1, const V& v2)
-      { const auto v1_it = dist_prev.find(v1);
-        const auto v2_it = dist_prev.find(v2);
-        return !(  v2_it != dist_prev.end() && ((v1_it == dist_prev.end()) || (v1_it->second.first > v2_it->second.first))  ); } );
-
-  return *smallest_it;
-}
-
 template <typename V, typename W>
 std::vector<V> pathFromPrevList(const V& dest, const std::unordered_map<V, std::pair<W, V> >& dist_prev)
 {
@@ -60,15 +48,18 @@ dijkstra_shortest_path_to(const Graph<V>& graph,
   std::unordered_map<V, std::pair<W, V> > dist_prev;
 
   dist_prev.emplace(source, std::pair<W, V>(W(), V()));
-  std::unordered_set<V> q;
+//   std::unordered_set<V> q;
+  PriorityQueue<W, V> q;
   for (const auto& v : graph.neighboursOf(source)) {
-    q.insert(v);
-    dist_prev.emplace(v, std::pair<W, V>(distanceCompute(source, v), source));
+    const W d = distanceCompute(source, v);
+    q.push(d, v);
+    dist_prev.emplace(v, std::pair<W, V>(d, source));
   }
 
   while (!q.empty()) {
-    const V& u = closestNode<V, W>(q, dist_prev);
-    q.erase(u);
+    const V u = q.top().second;
+    q.pop();
+
     if (u == dest)
       break;
 
@@ -78,9 +69,14 @@ dijkstra_shortest_path_to(const Graph<V>& graph,
 
       if (dist_prev.find(v) == dist_prev.end()) { // new node
         dist_prev.emplace(v, std::pair<W, V>(alt, u));
-        q.insert(v);
-      } else if (alt < dist_prev.at(v).first) { // better route
-        dist_prev[v] = std::pair<W, V>(alt, u);
+//         q.insert(v);
+        q.push(alt, v);
+      } else {
+        const W prev_d = dist_prev.at(v).first;
+        if (alt < prev_d) { // better route
+          dist_prev[v] = std::pair<W, V>(alt, u);
+          q.modifyKey(prev_d, v, alt);
+        }
       }
     }
   }
diff --git a/lib/graph/priority_queue.hpp b/lib/graph/priority_queue.hpp
index bf864f3..996f9ff 100644
--- a/lib/graph/priority_queue.hpp
+++ b/lib/graph/priority_queue.hpp
@@ -37,11 +37,11 @@ public:
   // modifiers
   void pop() { m_map.erase(m_map.begin()); }
   void push(const Key& key, const T& value) { m_map.emplace(key, value); }
-  void modifyKey(const T& value, const Key& diff) {
-    auto it = std::find_if(m_map.begin(), m_map.end(), [&value](const std::pair<const Key, T> & p) { return p.second == value; } );
-    Key key = it->first; // take a copy
+  void modifyKey(const Key& key, const T& value, const Key& new_key) {
+    auto it = std::find_if(m_map.begin(), m_map.end(), [&key, &value](const std::pair<const Key, T> & p) { return p.first == key && p.second == value; } );
+    T v = it->second; // take a copy
     m_map.erase(it);
-    m_map.emplace(key + diff, value);
+    m_map.emplace(new_key, v);
   }
 
   bool contains(const T& value) const {
@@ -50,7 +50,7 @@ public:
   }
 
 private:
-  std::map<Key, T, Compare, Allocator> m_map;
+  std::multimap<Key, T, Compare, Allocator> m_map;
 };
 
 #endif // PRIORITY_QUEUE_HPP