From b5592cb1813f2af3d2c7c56ae874e67c023644a9 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 8 Dec 2023 23:12:14 +0100
Subject: workaround bug with push_heap _GLIBCXX_DEBUG

---
 compat/heap.hpp  | 87 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/dijkstra.cpp |  5 ++--
 2 files changed, 90 insertions(+), 2 deletions(-)
 create mode 100644 compat/heap.hpp

diff --git a/compat/heap.hpp b/compat/heap.hpp
new file mode 100644
index 00000000..e9cac8cf
--- /dev/null
+++ b/compat/heap.hpp
@@ -0,0 +1,87 @@
+#pragma once
+
+#if defined __GLIBCXX__ && defined _GLIBCXX_DEBUG
+#include <utility>
+#include <iterator>
+// regular STL implementation stripped of debug code
+namespace floormat::Heap {
+
+template<typename It, typename Compare>
+inline void _push_heap(It first,
+                       std::iter_difference_t<It> holeIndex,
+                       std::iter_difference_t<It> topIndex,
+                       std::iter_value_t<It> value,
+                       Compare& comp)
+{
+    std::iter_difference_t<It> parent = (holeIndex - 1) / 2;
+    while (holeIndex > topIndex && comp(*(first + parent), value))
+    {
+        *(first + holeIndex) = std::move(*(first + parent));
+        holeIndex = parent;
+        parent = (holeIndex - 1) / 2;
+    }
+    *(first + holeIndex) = std::move(value);
+}
+
+template<typename It, typename Compare>
+void push_heap(It first, It last, Compare comp)
+{
+    _push_heap(first,
+               std::iter_difference_t<It>((last - first) - 1),
+               std::iter_difference_t<It>(0),
+               std::move(*(last - 1)),
+               comp);
+}
+
+template<typename It, typename Compare>
+void _adjust_heap(It first,
+                  std::iter_difference_t<It> holeIndex,
+                  std::iter_difference_t<It> len,
+                  std::iter_value_t<It> value,
+                  Compare& comp)
+{
+    const auto topIndex = holeIndex;
+    auto secondChild = holeIndex;
+    while (secondChild < (len - 1) / 2)
+    {
+        secondChild = 2 * (secondChild + 1);
+        if (comp(*(first + secondChild), *(first + (secondChild - 1))))
+            secondChild--;
+        *(first + holeIndex) = std::move(*(first + secondChild));
+        holeIndex = secondChild;
+    }
+    if ((len & 1) == 0 && secondChild == (len - 2) / 2)
+    {
+        secondChild = 2 * (secondChild + 1);
+        *(first + holeIndex) = std::move(*(first + (secondChild - 1)));
+        holeIndex = secondChild - 1;
+    }
+    _push_heap(first, holeIndex, topIndex, std::move(value), comp);
+}
+
+template<typename It, typename Compare>
+void pop_heap(It first, It last, Compare comp)
+{
+    if (last - first > 1)
+    {
+        --last;
+        auto value = std::move(*last);
+        *last = std::move(*first);
+        _adjust_heap(first,
+                     std::iter_difference_t<It>(0),
+                     std::iter_difference_t<It>(last - first),
+                     std::move(value),
+                     comp);
+    }
+}
+
+} // namespace floormat::Heap
+#else
+#include <algorithm>
+namespace floormat::Heap {
+
+using std::push_heap;
+using std::pop_heap;
+
+} // namespace floormat::Heap
+#endif
diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp
index 8dd8d97a..d49934a1 100644
--- a/src/dijkstra.cpp
+++ b/src/dijkstra.cpp
@@ -1,6 +1,7 @@
 #include "path-search.hpp"
 #include "compat/format.hpp"
 #include "compat/debug.hpp"
+#include "compat/heap.hpp"
 #include "object.hpp"
 #include "point.hpp"
 #include <cstdio>
@@ -143,12 +144,12 @@ void astar::clear()
 void astar::add_to_heap(uint32_t id)
 {
     Q.push_back(id);
-    std::push_heap(Q.begin(), Q.end(), heap_comparator{nodes});
+    Heap::push_heap(Q.begin(), Q.end(), heap_comparator{nodes});
 }
 
 uint32_t astar::pop_from_heap()
 {
-    std::pop_heap(Q.begin(), Q.end(), heap_comparator{nodes});
+    Heap::pop_heap(Q.begin(), Q.end(), heap_comparator{nodes});
     const auto id = Q.back();
     Q.pop_back();
     return id;
-- 
cgit v1.2.3