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

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

#include <stdio.h>

MsgQueue *mq_create(uint32_t size) {
    MsgQueue * mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt));
    mq->size = size;
    mq->readIdx = 0;
    mq->writeIdx = 0;
    memset(mq->pckts, 0, mq->size * sizeof(RawPckt));

    return mq;
}

void mq_clear(MsgQueue * mq) {
    mq->readIdx = 0;
    mq->writeIdx = 0;
    memset(mq->pckts, 0, mq->size * sizeof(RawPckt));
}

uint32_t mq_avail(const MsgQueue * mq) {
    return mq->writeIdx - mq->readIdx;
}

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

bool mq_push(MsgQueue * mq, const RawPckt * raw) {
    if (MQ_NEXT(mq->size, mq->writeIdx) == mq->readIdx) { // cannot push, queue is full
        return false;
    }

    // can push
    mq->pckts[mq->writeIdx] = *raw;

    // advance write pointer
    mq->writeIdx = MQ_NEXT(mq->size, mq->writeIdx);

    return true;
}

RawPckt mq_top(MsgQueue * mq) {
    return mq->pckts[mq->readIdx];
}

void mq_pop(MsgQueue * mq) {
    if (mq_avail(mq) > 0) { // if there's anything to pop
        mq->readIdx = MQ_NEXT(mq->size, mq->readIdx);
    }
}