feat: priority queue, heap implementation

priority-queue/.gitignore

@@ -1,4 +1,5 @@
 .vscode
 
+*.out
 test/test
 

priority-queue/priority_queue_heap.c

@@ -0,0 +1,84 @@
+#include "priority_queue_heap.h"
+
+int _PQ_parent(int i) { return (i-1)/2; }
+int _PQ_left(int i)   { return 2*i+1;   }
+int _PQ_right(int i)  { return 2*i+2;   }
+
+bool PQ_GE(int a, int b) { return a >= b; }
+bool PQ_GT(int a, int b) { return a  > b; }
+bool PQ_LE(int a, int b) { return a <= b; }
+bool PQ_LT(int a, int b) { return a  < b; }
+
+void _PQ_swap(PQ *pq, int a, int b)
+{
+    int tmp = pq->priority[a];
+    pq->priority[a] = pq->priority[b];
+    pq->priority[b] = tmp;
+    memcpy(pq->queue+pq->max_elem*pq->elem_size, pq->queue+a*pq->elem_size, pq->elem_size);
+    memcpy(pq->queue+a*pq->elem_size, pq->queue+b*pq->elem_size, pq->elem_size);
+    memcpy(pq->queue+b*pq->elem_size, pq->queue+pq->max_elem*pq->elem_size, pq->elem_size);
+}
+
+void PQ_init(PQ *pq, int pq_size, int elem_size, uint8_t *queue, int *priority, bool (*compare)(int, int))
+{
+    pq->pq_size = pq_size;
+    pq->elem_size = elem_size;
+    pq->queue = queue;
+    pq->compare = compare;
+    pq->elem_cnt = pq->tail = 0;
+    pq->max_elem = pq->pq_size / pq->elem_size - 1;     //last element for swapping
+    pq->priority = priority;
+}
+
+bool PQ_is_full(PQ *pq)  { return pq->elem_cnt == pq->max_elem; }
+bool PQ_is_empty(PQ *pq) { return pq->elem_cnt == 0; }
+
+bool PQ_insert(PQ *pq, int priority, uint8_t *element)
+{
+    if(PQ_is_full(pq)) return 0;
+
+    memcpy(pq->priority+pq->tail, &priority, 4);
+    memcpy(pq->queue+pq->tail*pq->elem_size, element, pq->elem_size);
+    int i = pq->tail;
+    pq->tail++;
+    pq->elem_cnt++;
+    while(i > 0 && pq->compare(pq->priority[_PQ_parent(i)], pq->priority[i]))
+    {
+        _PQ_swap(pq, _PQ_parent(i), i);
+        i = _PQ_parent(i);
+    }
+
+    return 1;
+}
+void PQ_top(PQ *pq, int *priority, uint8_t *element)
+{
+    memcpy(element, pq->queue, pq->elem_size);
+    memcpy(priority, pq->priority, 4);
+}
+bool PQ_pop(PQ *pq, int *priority, uint8_t *element)
+{
+    if(PQ_is_empty(pq)) return 0;
+
+    PQ_top(pq, priority, element);
+    _PQ_swap(pq, 0, pq->tail-1);
+    pq->elem_cnt--;
+    pq->tail--;
+    _PQ_heap_restore(pq, 0);
+
+    return 1;
+}
+
+void _PQ_heap_restore(PQ *pq, int i)
+{
+    int l = _PQ_left(i), r = _PQ_right(i);
+    int best = i;
+    if(l < pq->elem_cnt && pq->compare(pq->priority[i], pq->priority[l]))
+        best = l;
+    if(r < pq->elem_cnt && pq->compare(pq->priority[best], pq->priority[r]))
+        best = r;
+    if(best != i)
+    {
+        _PQ_swap(pq, i, best);
+        _PQ_heap_restore(pq, best);
+    }
+}

priority-queue/priority_queue_heap.h

@@ -0,0 +1,108 @@
+#ifndef PRIORITY_QUEUE_HEAP_H
+#define PRIORITY_QUEUE_HEAP_H
+
+#include "inttypes.h"
+#include "stdbool.h"
+#include "string.h"
+
+typedef struct{
+    int pq_size;
+    int elem_size;
+    int tail;
+    int elem_cnt;
+    int max_elem;
+    bool (*compare)(int, int);
+    uint8_t *queue;
+    int *priority;
+} PQ;
+
+/*
+    DEFAULT COMPARE FUNCTIONS
+*/
+
+/* greater or equal */
+bool PQ_GE(int a, int b); 
+/* greater than */
+bool PQ_GT(int a, int b);
+/* less or equal */
+bool PQ_LE(int a, int b); 
+/* less then */
+bool PQ_LT(int a, int b); 
+
+/**
+ *
+ * @param pq pointer to the queue to initialize
+ * @param pq_size size of the memory allocated in bytes(*queue)
+ * @param elem_size size of the single element
+ * @param queue pointer to the allocated memory
+ * @param priority pointer to the allocated memory for the priority
+ * @param compare pointer to the compare function, you can use the default compare functions (PQ_GE, PQ_GT,)
+ * 
+ * @warning the last member is used for swapping, if you need n elements remember to allocate n+1 elements
+ */
+void PQ_init(PQ *pq, int pq_size, int elem_size, uint8_t *queue, int *priority, bool (*compare)(int, int));
+
+/**
+ * 
+ * @brief get the first element of the queue, O(1)
+ * 
+ * @param pq pointer to the queue
+ * @param priority destination pointer for priority
+ * @param element destination pointer for the element
+*/
+void PQ_top(PQ *pq, int *priority, uint8_t *element);
+
+/**
+ * 
+ * @brief insert a new element on the queue, O(log(n))
+ * 
+ * @param pq pointer to the queue
+ * @param priority the priority of the element to insert
+ * @param element pointer to the element to insert (it will be copied)
+ * 
+ * @returns true if inserted successfully, false otherwise
+*/
+bool PQ_insert(PQ *pq, int priority, uint8_t *element);
+
+
+/**
+ * 
+ * @brief get the first element of the queue and delete it, O(log(n))
+ * 
+ * @param pq pointer to the queue
+ * @param priority destination pointer for priority
+ * @param element destination pointer for the element
+ * 
+ * @returns true if popped successfully, false otherwise
+*/
+bool PQ_pop(PQ *pq, int *priority, uint8_t *element);
+
+/**
+ * 
+ * @param pq pointer to the queue
+ * 
+ * @returns true if the queue is full, false otherwise
+*/
+bool PQ_is_full(PQ *pq);
+
+/**
+ * 
+ * @param pq pointer to the queue
+ * 
+ * @returns true if the queue is full, false otherwise
+*/
+bool PQ_is_empty(PQ *pq);
+
+
+/*
+    PRIVATE METHODS
+*/
+
+void _PQ_heap_restore(PQ *pq, int i);
+int _PQ_parent(int i);
+int _PQ_left(int i);
+int _PQ_right(int i);
+void _PQ_swap(PQ *pq, int a, int b);
+
+
+#endif

priority-queue/test_heap/main.cpp

@@ -0,0 +1,93 @@
+#include <queue>
+#include <iostream>
+#include <vector>
+#include <assert.h>
+#include <algorithm>
+#include "../priority_queue_heap.h"
+
+using namespace std;
+
+const int n = 100;
+struct prova{
+    int id;
+    char name[50];
+};
+prova *tmp = new prova[n];
+vector<int> priority(n);
+
+PQ *pq2;
+
+class Compare {
+public:
+    bool operator()(prova a, prova b)
+    {
+        return priority[a.id] >= priority[b.id];
+    }
+};
+
+bool compare(int a, int b)
+{
+    return a >= b;
+}
+priority_queue<prova, vector<prova>, Compare> pq1;
+
+void push()
+{
+    static int last_push = 0;
+    for(;last_push<n;last_push++)
+    {
+        if(PQ_insert(pq2, priority[last_push], (uint8_t*)&tmp[last_push]) == 0) 
+            break;
+        pq1.push(tmp[last_push]);
+    }
+}
+void pop()
+{
+    int cnt = 0;
+    while(!PQ_is_empty(pq2))
+    {
+        if(cnt++ > n/3) break;
+        prova tmp1, tmp2;
+        int p;
+        PQ_pop(pq2, &p, (uint8_t*)&tmp1);
+        tmp2 = pq1.top();
+        pq1.pop();
+        cout<<tmp1.name<<" | "<<tmp2.name<<endl;
+        assert(priority[tmp2.id] == p);
+        assert(strcmp(tmp1.name, tmp2.name) == 0);
+    }
+}
+
+int main()
+{
+    pq2 = (PQ*)malloc(sizeof(PQ));
+    for(int i=0;i<n;i++)
+    {
+        priority[i] = i;
+        tmp[i].id = i;
+        string str = "test"+to_string(i);
+        cout<<str<<" "<<priority[i]<<endl;
+        memcpy(tmp[i].name, str.c_str(), str.size());
+    }
+    int size = 30*(sizeof(prova));
+    auto mem = (uint8_t*)malloc(size);
+    int *pmem = (int*)malloc(4*30);
+    PQ_init(pq2, size, sizeof(prova), mem, pmem, PQ_GE);
+    random_shuffle(priority.begin(), priority.end());
+    push();
+    cout<<"OK push\n";
+    pop();
+    cout<<"OK pop\n";
+    push();
+    pop();
+
+    cout<<"Test passed"<<endl;
+
+    delete[] tmp;
+    free(pq2);
+    free(mem);
+    free(pmem);
+
+    return 0;
+}
+