From dfc82124a67c1721299a1db521b457b99191b384 Mon Sep 17 00:00:00 2001
From: Scott Worley <scottworley@scottworley.com>
Date: Sat, 9 Jan 2016 22:35:20 -0800
Subject: [PATCH] Switch from priority_queue to multimap

---
 sliding_tile_lib.cc | 28 +++++++++++++---------------
 1 file changed, 13 insertions(+), 15 deletions(-)

diff --git a/sliding_tile_lib.cc b/sliding_tile_lib.cc
index 9ac0217..551bc1f 100644
--- a/sliding_tile_lib.cc
+++ b/sliding_tile_lib.cc
@@ -2,6 +2,7 @@
 
 #include <cstdlib>
 #include <istream>
+#include <map>
 #include <memory>
 #include <ostream>
 #include <queue>
@@ -204,34 +205,31 @@ std::shared_ptr<Step> find_path(const std::string& start, const std::string& goa
 
 std::shared_ptr<Step> find_path(const Board& start, const Board& goal) {
   InvertedBoard invgoal = goal.invert();
-  auto heap_greater = [invgoal](const std::shared_ptr<Step>& a, const std::shared_ptr<Step>& b) {
-    return a->cost(invgoal) > b->cost(invgoal);
-  };
-  std::priority_queue<std::shared_ptr<Step>,
-                      std::vector<std::shared_ptr<Step>>,
-                      decltype(heap_greater)> todo(heap_greater);
+  std::multimap<int, std::shared_ptr<Step>> todo;
   std::set<Board> seen;
 
-  seen.emplace(start);
-  todo.push(std::make_shared<Step>(start, nullptr));
+  seen.insert(start);
+  auto start_step = std::make_shared<Step>(start, nullptr);
+  todo.emplace(start_step->cost(invgoal), start_step);
   while (!todo.empty()) {
-    if (todo.top()->board == goal) {
-      return todo.top();
+    auto cur = todo.begin()->second;
+    todo.erase(todo.begin());
+    if (cur->board == goal) {
+      return cur;
     }
-    std::vector<std::shared_ptr<Step>> successors = todo.top()->successors(todo.top());
+    std::vector<std::shared_ptr<Step>> successors = cur->successors(cur);
     for (const std::shared_ptr<Step>& s : successors) {
       if (seen.find(s->board) == seen.end()) {
-        seen.emplace(s->board);
-        todo.push(s);
+        seen.insert(s->board);
+        todo.emplace(s->cost(invgoal), s);
         if (seen.size() % 10000 == 0) {
           std::cerr << "Examined " << seen.size() << " boards.  Tracking "
                     << Step::count << " steps.  " << todo.size()
                     << " waiting.  Considering paths of length "
-                    << todo.top()->cost(invgoal) << std::endl;
+                    << cur->cost(invgoal) << std::endl;
         }
       }
     }
-    todo.pop();
   }
   throw std::runtime_error("No path from start to goal");
 }
-- 
2.44.1