From 4f48405b5494684d534ddaabc8e7955ba39c2217 Mon Sep 17 00:00:00 2001
From: nihonium <nihonium@rape.lol>
Date: Thu, 28 Apr 2022 18:36:04 +0300
Subject: [PATCH] pqueue

---
 algo/first/pq_test.c | 26 ++++++++++++
 algo/first/pqueue.c  | 95 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 121 insertions(+)
 create mode 100644 algo/first/pq_test.c
 create mode 100644 algo/first/pqueue.c

diff --git a/algo/first/pq_test.c b/algo/first/pq_test.c
new file mode 100644
index 0000000..693f7df
--- /dev/null
+++ b/algo/first/pq_test.c
@@ -0,0 +1,26 @@
+#include <stdio.h>
+#include "pqueue.c"
+
+int main() {
+	int n;
+	scanf("%d", &n);
+	binary_heap *bh = binary_heap_new(n);
+	binary_heap_insert(bh, (bhnode) {10, 2});
+	binary_heap_insert(bh, (bhnode) {113, 34});
+	binary_heap_insert(bh, (bhnode) {1, 56});
+	binary_heap_insert(bh, (bhnode) {4, 6745});
+	binary_heap_insert(bh, (bhnode) {5, 321});
+	binary_heap_insert(bh, (bhnode) {6, 346});
+	print_heap(bh);
+
+	binary_heap_erase(bh);
+	print_heap(bh);
+
+	binary_heap_erase(bh);
+	binary_heap_erase(bh);
+
+	print_heap(bh);
+
+	binary_heap_destroy(bh);
+
+}
diff --git a/algo/first/pqueue.c b/algo/first/pqueue.c
new file mode 100644
index 0000000..9d4e718
--- /dev/null
+++ b/algo/first/pqueue.c
@@ -0,0 +1,95 @@
+#include <stdlib.h>
+#include <assert.h>
+
+typedef int Data;
+
+typedef struct bhnode_s {
+	/* Приоритет */
+	int priority;
+	/* Значение */
+	Data val;
+
+} bhnode;
+
+/* Структура бинарной кучи */
+typedef struct binary_heap_s {
+	bhnode *body;
+	/* Максимальны размер */
+	int bodysize;
+	/* Текущий размер */
+	int numnodes;
+} binary_heap;
+
+/* Создание бинарной кучи */
+binary_heap *binary_heap_new(int maxsize) {
+	binary_heap *bh = malloc(sizeof(binary_heap));
+	bh->body = calloc(sizeof(bhnode), maxsize + 1);
+	bh->bodysize  = maxsize;
+	bh->numnodes = 0;
+	return bh;
+}
+
+void print_heap(binary_heap *bh) {
+	printf("Numnodes: %d\n", bh->numnodes);
+	for (int i = 1; i < bh->numnodes + 1; ++i) {
+		printf("%d (priority: %d)\n", bh->body[i].val, bh->body[i].priority);
+	}
+}
+void binary_heap_destroy(binary_heap *bh) {
+	free(bh->body);
+	free(bh);
+}
+
+void binary_heap_swap(binary_heap *bh, int a, int b) {
+#ifdef DEBUG
+	printf("Swapping %d and %d\n", a, b);
+#endif	
+	bhnode tmp = bh->body[a];
+	bh->body[a] = bh->body[b];
+	bh->body[b] = tmp;
+}
+
+bhnode binary_heap_fetch(binary_heap *bh) {
+	assert(bh->numnodes > 0);
+	return bh->body[1];
+}
+
+int binary_heap_insert(binary_heap *bh, bhnode node) {
+	if (bh->numnodes > bh->bodysize) {
+		return -1;
+	}
+	bh->body[++bh->numnodes] = node;
+	for (size_t i = bh->numnodes;
+		i> 1 && bh->body[i].priority > bh->body[i/2].priority;
+		i /= 2) {
+		binary_heap_swap(bh, i, i/2);
+	}
+	return 0;
+}
+
+void binary_heap_erase(binary_heap *bh) {
+	assert(bh->numnodes > 0);
+	bh->body[1] = bh->body[bh->numnodes--];
+#ifdef DEBUG
+	printf("Now root is %d (priority %d)\n", bh->body[1].val, bh->body[1].priority);
+#endif
+	size_t index = 1;
+	for (;;) {
+		size_t left = 2 * index;
+		size_t right = left + 1;	
+		size_t largest  = index;
+		if (left <= bh->numnodes && bh->body[left].priority > bh->body[index].priority)
+			largest = left;
+		if (right <= bh->numnodes && bh->body[right].priority > bh->body[largest].priority)
+			largest = right;
+		if (largest == index) break;
+		binary_heap_swap(bh, index, largest);
+		index = largest;
+#ifdef DEBUG
+		printf("###\n");
+		print_heap(bh);
+#endif
+	}
+}
+
+