#include "CircularBuffer.h"

using namespace bell;

CircularBuffer::CircularBuffer(size_t dataCapacity)
{
    this->dataCapacity = dataCapacity;
    buffer = std::vector<uint8_t>(dataCapacity);
    this->dataSemaphore = std::make_unique<bell::WrappedSemaphore>(5);
};

size_t CircularBuffer::write(const uint8_t *data, size_t bytes)
{
    if (bytes == 0)
        return 0;

    std::lock_guard<std::mutex> guard(bufferMutex);
    size_t bytesToWrite = std::min(bytes, dataCapacity - dataSize);
    // Write in a single step
    if (bytesToWrite <= dataCapacity - endIndex)
    {
        memcpy(buffer.data() + endIndex, data, bytesToWrite);
        endIndex += bytesToWrite;
        if (endIndex == dataCapacity)
            endIndex = 0;
    }

    // Write in two steps
    else {
        size_t firstChunkSize = dataCapacity - endIndex;
        memcpy(buffer.data() + endIndex, data, firstChunkSize);
        size_t secondChunkSize = bytesToWrite - firstChunkSize;
        memcpy(buffer.data(), data + firstChunkSize, secondChunkSize);
        endIndex = secondChunkSize;
    }

    dataSize += bytesToWrite;

    // this->dataSemaphore->give();
    return bytesToWrite;
}

void CircularBuffer::emptyBuffer() {
    std::lock_guard<std::mutex> guard(bufferMutex);
    begIndex = 0;
    dataSize = 0;
    endIndex = 0;
}

void CircularBuffer::emptyExcept(size_t sizeToSet) {
    std::lock_guard<std::mutex> guard(bufferMutex);
    if (sizeToSet > dataSize)
        sizeToSet = dataSize;
    dataSize = sizeToSet;
    endIndex = begIndex + sizeToSet;
    if (endIndex > dataCapacity) {
        endIndex -= dataCapacity;
    }
}

size_t CircularBuffer::read(uint8_t *data, size_t bytes)
{
    if (bytes == 0)
        return 0;

    std::lock_guard<std::mutex> guard(bufferMutex);
    size_t bytesToRead = std::min(bytes, dataSize);

    // Read in a single step
    if (bytesToRead <= dataCapacity - begIndex)
    {
        memcpy(data, buffer.data() + begIndex, bytesToRead);
        begIndex += bytesToRead;
        if (begIndex == dataCapacity)
            begIndex = 0;
    }
    // Read in two steps
    else
    {
        size_t firstChunkSize = dataCapacity - begIndex;
        memcpy(data, buffer.data() + begIndex, firstChunkSize);
        size_t secondChunkSize = bytesToRead - firstChunkSize;
        memcpy(data + firstChunkSize, buffer.data(), secondChunkSize);
        begIndex = secondChunkSize;
    }

    dataSize -= bytesToRead;
    return bytesToRead;
}