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

constexpr uint8_t PACKET_MAX_BYTES_READ_AT_ONCE = 4;

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()
{
    int bytesAvail = Serial.available();
    if (bytesAvail) {
        // Available bytes fit within remaining buffer
        if (bytesAvail <= PACKET_SIZE - inBufPos) {
            // Consume at max one byte at a time to give consumers a chance to consume packets
            size_t bytesRead = Serial.readBytes(inBuf + inBufPos, min(PACKET_MAX_BYTES_READ_AT_ONCE, bytesAvail));
            inBufPos += bytesRead;
        }
        // Else, these bytes would overflow the buffer.
        // Read as much as we can and go on. It may just be multiple packets queued.
        else {
            size_t bytesRead = Serial.readBytes(inBuf + inBufPos, min(PACKET_MAX_BYTES_READ_AT_ONCE, PACKET_SIZE - inBufPos));
            inBufPos += bytesRead;
        }
        
        // Is the last byte read 0x0A? Then push the packet
        if (inBufPos > 0 && inBuf[inBufPos - 1] == 0x0A) {
            pushPacket();
            inBufPos = 0;
        }

        // Is the last byte NOT 0x0A and we are on the last possibly byte?
        // Then something is wrong and we are declaring error.
        if (inBufPos == PACKET_SIZE && inBuf[inBufPos - 1] != 0x0A) {
            StateMachine::getInstance().raiseError("UART packet did not contain the 0x0A endbyte! May it be too long?");
        }
    }
}

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