initial prio queue impl

diff --git a/src/priority_queue.c b/src/priority_queue.c
index c782d23..1a5a65b 100644 (file)
--- 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 (file)
--- 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);