From 4d63cd4e0cc405716c0b106892a52ec5a332363f Mon Sep 17 00:00:00 2001
From: Andreas Anderberg <eas@svep.se>
Date: Wed, 13 Jul 2016 16:31:26 +0200
Subject: [PATCH] Fixed problem with BinaryHeap::remove()

When last element was swapped with the removed element it was only propagated downwards.
Depending if the swapped value is greater or smaller than its parent it may instead
need to propagate upwards.

Fixes #108

Change-Id: I9583a3412ccb88166d4e3091ed31a4c8a3aa4486
---
 core-util/BinaryHeap.h | 21 +++++++++++++++++----
 1 file changed, 17 insertions(+), 4 deletions(-)

diff --git a/core-util/BinaryHeap.h b/core-util/BinaryHeap.h
index 38b3833..a0e05b9 100644
--- a/core-util/BinaryHeap.h
+++ b/core-util/BinaryHeap.h
@@ -193,8 +193,11 @@ class BinaryHeap {
                 return false;
             _swap(i, --_elements); // element i will be destroyed by 'pop_back()' below
             _array.pop_back();
-            if (_elements > 1)
-                _propagate_down(i);
+            if ((_elements > 1) && (_elements != i)) {
+                if (!_propagate_up(i)) {
+                    _propagate_down(i);
+                }
+            }
             return true;
         }
     }
@@ -233,20 +236,26 @@ class BinaryHeap {
         return (i - 1) / 2;
     }
 
-    void _propagate_up(size_t node) {
+    bool _propagate_up(size_t node) {
         // This is called when a node is added in the last position in the heap
         // We might need to move the node up towards the parent until the heap property
         // is satisfied
         size_t parent = _parent(node);
+        bool moved = false;
         // Move the node up until it satisfies the heap property
         while ((node > 0) && _comparator(_array[node], _array[parent])) {
             _swap(node, parent);
             node = parent;
             parent = _parent(node);
+            if (node != parent) {
+                moved = true;
+            }
         }
+        return moved;
     }
 
-    void _propagate_down(size_t node) {
+    bool _propagate_down(size_t node) {
+        bool moved = false;
         // This is called when an existing node is removed
         // When that happens, it is replaced with the node at the last position in the heap
         // Since that might make the heap inconsistent, we need to move the node down if its
@@ -268,8 +277,12 @@ class BinaryHeap {
                 break;
             }
             _swap(node, temp);
+            if (node != temp) {
+                moved = true;
+            }
             node = temp;
         }
+        return moved;
     }
 
     void _swap(size_t pos1, size_t pos2) {