#include "UartHandler.hpp"
#include "OperationTimeout.hpp"
#include "StateMachine.hpp"

UartHandler::UartHandler() noexcept
{
}

UartHandler::~UartHandler()
{
    // In case close was not called, close now
    if (isOpen()) {
        close();
    }
}

UartHandler &UartHandler::getInstance()
{
    static UartHandler instance;
    return instance;
}

bool UartHandler::init(const uint32_t baudRate)
{
    // Wait for serial port to connect, consider failed after one second
    Serial.begin(baudRate);
    OperationTimeout ot(1000);
    while (!Serial) {
        if (ot.isTimeout()) {
            StateMachine::getInstance().raiseError("Unable to open serial port");
            return false;
        }
        yield();
    }

    return true;
}

void UartHandler::close()
{
    Serial.end();
}

bool UartHandler::isOpen() const noexcept
{
    return (bool)Serial;
}

void UartHandler::poll()
{
    size_t bytesAvail = Serial.available();
    if (bytesAvail) {
        // Consume at max one byte at a time to give consumers a chance to consume packets
        // ALso this simplifies recognizing packet ends
        size_t bytesRead = Serial.readBytes(inBuf + inBufPos, 1);
        if (!bytesRead) {
            StateMachine::getInstance().raiseError("Unable to read available byte!");
            return;
        }
        inBufPos++;

        // Is our buffer full? Push it.
        if (inBufPos == PACKET_SIZE) {
            pushPacket();
            inBufPos = 0;
        }
    }
}

void UartHandler::pushPacket()
{
    memset(packetRingBuf + packetRingBufWritePos * PACKET_SIZE, 0x00, PACKET_SIZE);
    memcpy(packetRingBuf + packetRingBufWritePos * PACKET_SIZE, inBuf, inBufPos);
    packetRingBufWritePos = (packetRingBufWritePos + 1) % PACKET_RINGBUF_SIZE;
    numPacketsAvailable++;

    if (numPacketsAvailable >= PACKET_RINGBUF_SIZE) {
        // Move machine into error state -> reports errors and failsafes
        StateMachine::getInstance().raiseError("UART packet dropped because it was not read!");
    }
}

bool UartHandler::getNextPacket(char* dest)
{
    if (!numPacketsAvailable) {
        return false;
    }

    memcpy(dest, packetRingBuf + packetRingBufReadPos * PACKET_SIZE, PACKET_SIZE);
    packetRingBufReadPos = (packetRingBufReadPos + 1) % PACKET_RINGBUF_SIZE;
    numPacketsAvailable--;
    return true;
}


uint8_t UartHandler::getNumAvailablePackets() const
{
    return numPacketsAvailable;
}