From 7c06012383b1f1c38992f3765e7eb6da7cda4c8e Mon Sep 17 00:00:00 2001
From: Bill Currie <bill@taniwha.org>
Date: Sun, 30 Oct 2022 23:38:14 +0900
Subject: [PATCH] [util] Support custom swap function for heapsort

Sorting ECS component pools needs to swap multiple chunks of data for
each element being swapped, so the standard swap function isn't enough.
---
 include/QF/heapsort.h | 13 ++++++++
 libs/util/heapsort.c  | 71 +++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 81 insertions(+), 3 deletions(-)

diff --git a/include/QF/heapsort.h b/include/QF/heapsort.h
index 153c4faee..77ec0ef33 100644
--- a/include/QF/heapsort.h
+++ b/include/QF/heapsort.h
@@ -40,22 +40,35 @@ typedef int (*__compar_fn_t)(const void *, const void *);
 typedef int (*__compar_d_fn_t)(const void *, const void *, void *);
 #endif
 
+#ifndef __swap_d_fn_t_defined
+#define __swap_d_fn_t_defined
+typedef void (*__swap_d_fn_t)(void *, void *, void *);
+#endif
+
 void heap_sink (void *base, size_t ind, size_t nmemb, size_t size,
 				__compar_fn_t cmp);
 void heap_sink_r (void *base, size_t ind, size_t nmemb, size_t size,
 				  __compar_d_fn_t cmp, void *arg);
+void heap_sink_s (void *base, size_t ind, size_t nmemb, size_t size,
+				  __compar_d_fn_t cmp, __swap_d_fn_t swp, void *arg);
 
 void heap_swim (void *base, size_t ind, size_t nmemb, size_t size,
 				__compar_fn_t cmp);
 void heap_swim_r (void *base, size_t ind, size_t nmemb, size_t size,
 				  __compar_d_fn_t cmp, void *arg);
+void heap_swim_s (void *base, size_t ind, size_t nmemb, size_t size,
+				  __compar_d_fn_t cmp, __swap_d_fn_t swp, void *arg);
 
 void heap_build (void *base, size_t nmemb, size_t size, __compar_fn_t cmp);
 void heap_build_r (void *base, size_t nmemb, size_t size,
 				   __compar_d_fn_t cmp, void *arg);
+void heap_build_s (void *base, size_t nmemb, size_t size,
+				   __compar_d_fn_t cmp, __swap_d_fn_t swp, void *arg);
 
 void heapsort (void *base, size_t nmemb, size_t size, __compar_fn_t cmp);
 void heapsort_r (void *base, size_t nmemb, size_t size,
 				 __compar_d_fn_t cmp, void *arg);
+void heapsort_s (void *base, size_t nmemb, size_t size,
+				 __compar_d_fn_t cmp, __swap_d_fn_t swp, void *arg);
 
 #endif//__QF_heapsort_h
diff --git a/libs/util/heapsort.c b/libs/util/heapsort.c
index aca74e574..e7f840433 100644
--- a/libs/util/heapsort.c
+++ b/libs/util/heapsort.c
@@ -46,7 +46,7 @@ heap_sink (void *base, size_t ind, size_t nmemb, size_t size,
 
 VISIBLE void
 heap_sink_r (void *base, size_t ind, size_t nmemb, size_t size,
-		 __compar_d_fn_t cmp, void *arg)
+			 __compar_d_fn_t cmp, void *arg)
 {
 	size_t      left_ind = 2 * ind + 1;
 	size_t      right_ind = 2 * ind + 2;
@@ -71,6 +71,33 @@ heap_sink_r (void *base, size_t ind, size_t nmemb, size_t size,
 	}
 }
 
+VISIBLE void
+heap_sink_s (void *base, size_t ind, size_t nmemb, size_t size,
+			 __compar_d_fn_t cmp, __swap_d_fn_t swp, void *arg)
+{
+	size_t      left_ind = 2 * ind + 1;
+	size_t      right_ind = 2 * ind + 2;
+	void       *node = (byte *) base + ind * size;
+	void       *left = (byte *) base + left_ind * size;
+	void       *right = (byte *) base + right_ind * size;
+
+	size_t      largest_ind = ind;
+	void       *largest = node;
+
+	if (left_ind < nmemb && cmp (left, node, arg) > 0) {
+		largest = left;
+		largest_ind = left_ind;
+	}
+	if (right_ind < nmemb && cmp (right, largest, arg) > 0) {
+		largest = right;
+		largest_ind = right_ind;
+	}
+	if (largest_ind != ind) {
+		swp (largest, node, arg);
+		heap_sink_s (base, largest_ind, nmemb, size, cmp, swp, arg);
+	}
+}
+
 VISIBLE void
 heap_swim (void *base, size_t ind, size_t nmemb, size_t size,
 		   __compar_fn_t cmp)
@@ -87,7 +114,7 @@ heap_swim (void *base, size_t ind, size_t nmemb, size_t size,
 
 VISIBLE void
 heap_swim_r (void *base, size_t ind, size_t nmemb, size_t size,
-		   __compar_d_fn_t cmp, void *arg)
+			 __compar_d_fn_t cmp, void *arg)
 {
 	size_t      parent_ind = (ind - 1) / 2;
 	void       *node = (byte *) base + ind * size;
@@ -99,6 +126,20 @@ heap_swim_r (void *base, size_t ind, size_t nmemb, size_t size,
 	}
 }
 
+VISIBLE void
+heap_swim_s (void *base, size_t ind, size_t nmemb, size_t size,
+			 __compar_d_fn_t cmp, __swap_d_fn_t swp, void *arg)
+{
+	size_t      parent_ind = (ind - 1) / 2;
+	void       *node = (byte *) base + ind * size;
+	void       *parent = (byte *) base + parent_ind * size;
+
+	if (ind > 0 && cmp (node, parent, arg) > 0) {
+		swp (node, parent, arg);
+		heap_swim_s (base, parent_ind, nmemb, size, cmp, swp, arg);
+	}
+}
+
 VISIBLE void
 heap_build (void *base, size_t nmemb, size_t size, __compar_fn_t cmp)
 {
@@ -112,7 +153,7 @@ heap_build (void *base, size_t nmemb, size_t size, __compar_fn_t cmp)
 
 VISIBLE void
 heap_build_r (void *base, size_t nmemb, size_t size,
-				 __compar_d_fn_t cmp, void *arg)
+			  __compar_d_fn_t cmp, void *arg)
 {
 	if (nmemb < 2) {
 		return;
@@ -122,6 +163,18 @@ heap_build_r (void *base, size_t nmemb, size_t size,
 	}
 }
 
+VISIBLE void
+heap_build_s (void *base, size_t nmemb, size_t size,
+			  __compar_d_fn_t cmp, __swap_d_fn_t swp, void *arg)
+{
+	if (nmemb < 2) {
+		return;
+	}
+	for (size_t i = nmemb / 2; i-- > 0; ) {
+		heap_sink_s (base, i, nmemb, size, cmp, swp, arg);
+	}
+}
+
 VISIBLE void
 heapsort (void *base, size_t nmemb, size_t size, __compar_fn_t cmp)
 {
@@ -144,3 +197,15 @@ heapsort_r (void *base, size_t nmemb, size_t size, __compar_d_fn_t cmp,
 		heap_sink_r (base, 0, i, size, cmp, arg);
 	}
 }
+
+VISIBLE void
+heapsort_s (void *base, size_t nmemb, size_t size, __compar_d_fn_t cmp,
+			__swap_d_fn_t swp, void *arg)
+{
+	heap_build_s (base, nmemb, size, cmp, swp, arg);
+	for (size_t i = nmemb; i-- > 1; ) {
+		void       *last = (byte *) base + i * size;
+		swp (base, last, arg);
+		heap_sink_s (base, 0, i, size, cmp, swp, arg);
+	}
+}