//
// Created by epagris on 2023.11.22..
//

#include <memory.h>
#include "blocking_fifo.h"
#include "dynmem.h"
#include "utils.h"

#define MIN(a,b) (((a) < (b)) ? (a) : (b))

BlockingFifo *bfifo_new(uint32_t size) {
    uint32_t allocSize = sizeof(BlockingFifo) + size; // calculate full allocation size
    BlockingFifo * fifo = (BlockingFifo *) dynmem_alloc(allocSize); // allocate data block at the end
    ASSERT_NULL(fifo);
    memset(fifo, 0, sizeof(BlockingFifo));
    fifo->size = size;
    ETHLIB_OS_SEM_CREATE(&fifo->nonEmpty, 1);
    ETHLIB_OS_SEM_WAIT(&fifo->nonEmpty); // FIFO is empty
    ETHLIB_OS_SEM_CREATE(&fifo->notFull, 1);
    return fifo;
}

void bfifo_destroy(BlockingFifo * bfifo) {
    ETHLIB_OS_SEM_DESTROY(&bfifo->nonEmpty);
    ETHLIB_OS_SEM_DESTROY(&bfifo->notFull);
    bfifo->size = 0;
    dynmem_free(bfifo);
}

uint32_t bfifo_get_used(const BlockingFifo *bfifo) {
    return bfifo->absWriteIdx - bfifo->absReadIdx;
}

uint32_t bfifo_get_free(const BlockingFifo * bfifo) {
    return bfifo->size - bfifo_get_used(bfifo);
}

uint32_t bfifo_push_try(BlockingFifo * bfifo, const uint8_t * data, uint32_t size) {
    if (bfifo_get_free(bfifo) == 0) {
        return 0;
    } else {
        return bfifo_push(bfifo, data, size);
    }
}

uint32_t bfifo_push(BlockingFifo * bfifo, const uint8_t * data, uint32_t size) {
    ETHLIB_OS_SEM_WAIT(&bfifo->notFull); // take not full semaphore

    // calculate copy size, limit if required
    uint32_t freeArea = bfifo_get_free(bfifo);
    uint32_t copySize = MIN(size, freeArea);

    // determine first block size
    uint32_t spaceToBufEnd = bfifo->size - bfifo->writeIdx; // space to buffer end
    uint32_t firstBlockSize = MIN(spaceToBufEnd, copySize); // first block size
    uint32_t secondBlockSize = (firstBlockSize < copySize) ? copySize - firstBlockSize : 0; // second block size
    memcpy(bfifo->data + bfifo->writeIdx, data, firstBlockSize); // copy first block
    if (secondBlockSize > 0) { // copy second block if needed
        memcpy(bfifo->data, data + firstBlockSize, secondBlockSize);
    }

    // adjust indices
    uint32_t newWriteIdx = bfifo->writeIdx + copySize; // advance write index
    if (newWriteIdx >= bfifo->size) {
        newWriteIdx -= bfifo->size;
    }
    bfifo->writeIdx = newWriteIdx; // replace...
    bfifo->absWriteIdx += copySize;

    freeArea = bfifo_get_free(bfifo);
    if (freeArea > 0) { // FIFO is not full
        ETHLIB_OS_SEM_POST(&bfifo->notFull);
    }
    if (freeArea != bfifo->size) { // FIFO is not empty
        ETHLIB_OS_SEM_POST(&bfifo->nonEmpty);
    }

    return copySize;
}

uint32_t bfifo_peek(BlockingFifo * bfifo, uint8_t * dst, uint32_t size) {
    // determine copy size
    uint32_t usedArea = bfifo_get_used(bfifo);
    uint32_t copySize = MIN(size, usedArea);

    uint32_t spaceToBufEnd = bfifo->size - bfifo->readIdx; // space to buffer end
    uint32_t firstBlockSize = MIN(spaceToBufEnd, copySize); // first block size
    uint32_t secondBlockSize = (firstBlockSize < copySize) ? copySize - firstBlockSize : 0; // second block size
    memcpy(dst, bfifo->data + bfifo->readIdx, firstBlockSize); // copy first block
    if (secondBlockSize > 0) { // copy second block if needed
        memcpy(dst + firstBlockSize, bfifo->data, secondBlockSize);
    }
    return copySize;
}

uint32_t bfifo_pop(BlockingFifo *bfifo, uint8_t *dst, uint32_t size) {
    ETHLIB_OS_SEM_WAIT(&bfifo->nonEmpty);

    // if destination is given, then also perform a copy
    if (dst != NULL) {
        bfifo_peek(bfifo, dst, size);
    }

    // adjust indices
    uint32_t usedArea = bfifo_get_used(bfifo);
    uint32_t popSize = MIN(size, usedArea); // advance next written
    uint32_t newReadIdx = bfifo->readIdx + popSize;
    if (newReadIdx >= bfifo->size) {
        newReadIdx -= bfifo->size;
    }
    bfifo->readIdx = newReadIdx; // replace it instead of performing an in-place change
    bfifo->absReadIdx += popSize;

    usedArea = bfifo_get_used(bfifo);
    if (usedArea > 0) { // FIFO is NOT empty
        ETHLIB_OS_SEM_POST(&bfifo->nonEmpty);
    }
    if (usedArea != bfifo->size) { // FIFO is NOT full
        ETHLIB_OS_SEM_POST(&bfifo->notFull);
    }

    return popSize;
}