internal/priorityqueue: Add

1 files changed, 36 insertions, 0 deletions

diff --git a/internal/priorityqueue/sift.go b/internal/priorityqueue/sift.go
new file mode 100644
index 00000000..e43e17a4
--- /dev/null
+++ b/internal/priorityqueue/sift.go
@@ -0,0 +1,36 @@
+package priorityqueue
+
+func (queue *Queue[T]) siftDown(idx int) {
+	for {
+		left := idx*2 + 1
+		if left >= len(queue.items) {
+			return
+		}
+
+		best := left
+
+		right := left + 1
+		if right < len(queue.items) && queue.less(queue.items[right], queue.items[left]) {
+			best = right
+		}
+
+		if !queue.less(queue.items[best], queue.items[idx]) {
+			return
+		}
+
+		queue.items[idx], queue.items[best] = queue.items[best], queue.items[idx]
+		idx = best
+	}
+}
+
+func (queue *Queue[T]) siftUp(idx int) {
+	for idx > 0 {
+		parent := (idx - 1) / 2
+		if !queue.less(queue.items[idx], queue.items[parent]) {
+			return
+		}
+
+		queue.items[idx], queue.items[parent] = queue.items[parent], queue.items[idx]
+		idx = parent
+	}
+}