From 37d3d88305ff9a4da66a7a346b4b43337ef6a96d Mon Sep 17 00:00:00 2001
From: Jacob Gulotta <jgulotta@gmail.com>
Date: Thu, 20 Dec 2012 17:33:44 -0800
Subject: [PATCH] Fix ordering bug and cleanup HeapSet implementation

Also added a missed call to std::move
---
 examples/n-puzzle/input/goal  |  2 +-
 examples/n-puzzle/input/start |  2 +-
 include/astar/heapset.h       | 50 +++++++++++++++--------------------
 include/astar/solver.h        |  2 +-
 4 files changed, 25 insertions(+), 31 deletions(-)

diff --git a/examples/n-puzzle/input/goal b/examples/n-puzzle/input/goal
index 76e67f5..2176c5d 100644
--- a/examples/n-puzzle/input/goal
+++ b/examples/n-puzzle/input/goal
@@ -1,3 +1,3 @@
 1 2 3 0
-1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 0
 1 2 3 4 5 6 7 8 0
+1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 0
diff --git a/examples/n-puzzle/input/start b/examples/n-puzzle/input/start
index 875815f..89a7d2c 100644
--- a/examples/n-puzzle/input/start
+++ b/examples/n-puzzle/input/start
@@ -1,4 +1,4 @@
 0 1 3 2
+4 3 5 1 0 8 2 6 7
 1 2 3 4 5 6 7 8 9 10 11 12 13 14 0 15
 12 1 10 2 7 0 4 14 5 11 9 15 8 13 6 3
-4 3 5 1 0 8 2 6 7
diff --git a/include/astar/heapset.h b/include/astar/heapset.h
index 7080397..d126f55 100644
--- a/include/astar/heapset.h
+++ b/include/astar/heapset.h
@@ -17,8 +17,10 @@ class HeapSet {
     private:
         std::set<T,SC> states_;
         std::priority_queue<T,std::vector<T>,VC> heap_;
+        // returns true if second arg is higher priority than the first
         VC higher_priority_;
         T pop_heap();
+        void update_heap(const T& old, const T& updated);
 };
 
 template<class T, class SC, class VC>
@@ -46,20 +48,8 @@ void HeapSet<T,SC,VC>::push(const T& t) {
         heap_.push(t);
     } else {
         auto old = *it_inserted.first;
-        if (higher_priority_(t, old)) {
-            std::deque<T> queue;
-
-            do {
-                queue.push_back(pop_heap());
-            } while (queue.back() != old);
-
-            queue.back() = t;
-
-            while (!queue.empty()) {
-                heap_.push(queue.back());
-                queue.pop_back();
-            }
-
+        if (higher_priority_(old, t)) {
+            update_heap(old, t);
             auto it = states_.erase(it_inserted.first);
             states_.insert(it, t);
         }
@@ -74,20 +64,8 @@ void HeapSet<T,SC,VC>::push(T&& t) {
         heap_.push(std::forward<T>(t));
     } else {
         auto old = *it_inserted.first;
-        if (higher_priority_(t, old)) {
-            std::deque<T> queue;
-
-            do {
-                queue.push_back(pop_heap());
-            } while (queue.back() != old);
-
-            queue.back() = t;
-
-            while (!queue.empty()) {
-                heap_.push(queue.back());
-                queue.pop_back();
-            }
-
+        if (higher_priority_(old, t)) {
+            update_heap(old, t);
             auto it = states_.erase(it_inserted.first);
             states_.insert(it, std::forward<T>(t));
         }
@@ -100,3 +78,19 @@ T HeapSet<T,SC,VC>::pop_heap() {
     heap_.pop();
     return std::move(t);
 }
+
+template<class T, class SC, class VC>
+void HeapSet<T,SC,VC>::update_heap(const T& old, const T& updated) {
+    std::deque<T> queue;
+
+    do {
+        queue.push_back(pop_heap());
+    } while (queue.back() != old);
+
+    queue.back() = updated;
+
+    while (!queue.empty()) {
+        heap_.push(queue.back());
+        queue.pop_back();
+    }
+}
diff --git a/include/astar/solver.h b/include/astar/solver.h
index be30e48..81ea67e 100644
--- a/include/astar/solver.h
+++ b/include/astar/solver.h
@@ -128,7 +128,7 @@ bool AStarSolver<T,G,H>::solve()
         for (auto& n : generator_func_(snode->state_)) {
             auto new_node = make_snode(*this, n, snode);
             if (closed_set_.find(new_node) == end(closed_set_)) {
-                open_set_.push(new_node);
+                open_set_.push(std::move(new_node));
             }
         }
     }