From 5953d807826bb6d8d9f2244f11541e570a1c6281 Mon Sep 17 00:00:00 2001
From: Matthew Owens <matthew@owens.tech>
Date: Sun, 26 Dec 2021 15:10:13 +0000
Subject: [PATCH] initial prio queue impl

---
 src/priority_queue.c | 142 +++++++++++++++++++++++++++++++++++++++++++
 src/priority_queue.h |  20 ++++++
 2 files changed, 162 insertions(+)

diff --git a/src/priority_queue.c b/src/priority_queue.c
index c782d23..1a5a65b 100644
--- a/src/priority_queue.c
+++ b/src/priority_queue.c
@@ -1 +1,143 @@
 #include "priority_queue.h"
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+
+void PrioQueueInit(PrioQueue *q, size_t memSize)
+{
+	assert(q != NULL);
+	assert(memSize > 0);
+
+	q->memSize = memSize;
+	q->tail = NULL;
+	q->head = NULL;
+}
+
+static void prioQueueInsert(PrioQueue *q, PrioQueueNode *n)
+{
+	PrioQueueNode *current = q->head;
+	PrioQueueNode *prev = NULL;
+
+	// starting with the head as the highest prio, eg:
+	// {foo, 3} -> {bar, 2} -> {baz, 1}
+
+	// do we only have one element?
+	if(q->head == q->tail) {
+		// do we need to insert before or after?
+		if(n->priority > current->priority) {
+			n->next = current;
+			q->head = n->next;
+			current->next = NULL;
+			return;
+		} else {
+			n->next = NULL;
+			current->next = n;
+			q->tail = n;
+			return;
+		}
+	} else {
+		do {
+			// ensuring we have prev populated
+			if(prev == NULL) {
+				prev = current;
+				continue;
+			}
+			if(prev->priority > n->priority > current->priority) {
+				n->next = current;
+				prev->next = n;
+				return;
+			}
+			prev = current;
+			current = current->next;
+		} while(current != NULL);
+	}
+
+	n->next = NULL;
+	q->tail->next = n;
+	q->tail = n;
+}
+
+void PrioQueuePush(PrioQueue *q, const void *data, int priority)
+{
+	assert(q != NULL);
+	PrioQueueNode *n = malloc(sizeof(PrioQueueNode));
+
+	assert(n != NULL);
+	n->data = malloc(q->memSize);
+	assert(n->data != NULL);
+	n->next = NULL;
+
+	memcpy(n->data, data, q->memSize);
+	n->priority = priority;
+
+	if(q->head == NULL) {
+		q->head = q->tail = n;
+	} else {
+		prioQueueInsert(q, n);
+	}
+}
+
+void PrioQueuePop(PrioQueue *q, void *data)
+{
+	assert(q != NULL);
+	assert(q->head != NULL);
+
+	PrioQueueNode *tmp = q->head;
+
+	if(data != NULL) {
+		memcpy(data, q->head->data, q->memSize);
+	}
+	q->head = q->head->next;
+
+	if(q->head == NULL) {
+		q->tail = NULL;
+	}
+
+	free(tmp->data);
+	free(tmp);
+}
+
+void PrioQueuePeek(PrioQueue *q, void *data)
+{
+	assert(q != NULL);
+	assert(q->head != NULL);
+	assert(q->head->data != NULL);
+	assert(data != NULL);
+
+	memcpy(data, q->head->data, q->memSize);
+}
+
+void PrioQueueClear(PrioQueue *q)
+{
+	assert(q != NULL);
+	if(q->head == NULL) { return; }
+
+	int count = 0;
+	PrioQueueNode *tmp = NULL;
+	do {
+		tmp = q->head;
+		count++;
+		q->head = tmp->next;
+		free(tmp->data);
+		free(tmp);
+	}
+	while(tmp != q->tail);
+
+	q->head = q->tail = NULL;
+}
+
+size_t PrioQueueGetSize(PrioQueue *q)
+{
+	size_t size = 0;
+	assert(q != NULL);
+	if(q->head == NULL) { return 0; }
+
+	PrioQueueNode *head = q->head;
+	do {
+		size++;
+		head = head->next;
+	} while(head != q->tail);
+
+
+	return size;
+}
diff --git a/src/priority_queue.h b/src/priority_queue.h
index 6f70f09..fce0557 100644
--- a/src/priority_queue.h
+++ b/src/priority_queue.h
@@ -1 +1,21 @@
 #pragma once
+#include <stddef.h>
+
+typedef struct PrioQueueNode {
+	void *data;
+	int priority;
+	struct PrioQueueNode *next;
+} PrioQueueNode;
+
+typedef struct PrioQueue {
+	size_t memSize;
+	PrioQueueNode *tail;
+	PrioQueueNode *head;
+} PrioQueue;
+
+void PrioQueueInit(PrioQueue *q, size_t memSize);
+void PrioQueuePush(PrioQueue *q, const void *data, int priority);
+void PrioQueuePop(PrioQueue *q, void *data);
+void PrioQueuePeek(PrioQueue *q, void *data);
+void PrioQueueClear(PrioQueue *q);
+size_t PrioQueueGetSize(PrioQueue *q);
-- 
2.20.1