//
// Created by epagris on 2023.01.13..
//

#include <memory.h>
#include <stdbool.h>
#include "gen_queue.h"
#include "dynmem.h"

Queue *q_create(uint32_t length, uint32_t elemSize) {
    Queue * q = (Queue *) dynmem_alloc(sizeof(Queue) + length * elemSize);
    q->length = length;
    q->elemSize = elemSize;
    q->readIdx = 0;
    q->writeIdx = 0;
    memset(q->elements, 0, length * elemSize);

    return q;
}

void q_clear(Queue * q) {
    q->readIdx = 0;
    q->writeIdx = 0;
    memset(q->elements, 0, q->length * q->elemSize);
}

uint32_t q_avail(const Queue * q) {
    return q->writeIdx - q->readIdx;
}

#define MQ_NEXT(size,current) (((current)+1)%(size))

bool q_push(Queue * q, const void * src) {
    if (MQ_NEXT(q->length, q->writeIdx) == q->readIdx) { // cannot push, queue is full
        return false;
    }

    // can push
    memcpy(q->elements + q->writeIdx * q->elemSize, src, q->elemSize);
    q->writeIdx++;

    // advance write pointer
    q->writeIdx = MQ_NEXT(q->length, q->writeIdx);

    return true;
}

void q_top(Queue * q, void * dest) {
    memcpy(dest, q->elements + q->readIdx, q->elemSize);
}

void q_pop(Queue * q) {
    if (q_avail(q) > 0) { // if there's something to pop
        q->readIdx = MQ_NEXT(q->length, q->readIdx);
    }
}