#include "gen_queue.h"

#include <memory.h>
#include <stdbool.h>

Queue *q_create(uint32_t length, uint32_t elemSize, uint8_t *mem) {
    Queue *q = (Queue *)mem;
    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);

    // 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);
    }
}