#include "hardware_serial.hpp"

ProtoSerial::ProtoSerial()
    : serial(nullptr), initialized(false), currentState(WAITING_FOR_LENGTH),
      payloadLength(0), messageAvailable(false) {
    receivedMessage = INCOMING_MESSAGE_INIT;
}

ProtoSerial::~ProtoSerial() {
    if (serial) {
        delete serial;
    }
}

void ProtoSerial::begin(int rxPin, int txPin, long baud) {
    if (serial) {
        delete serial;
    }
#ifdef ESP32
    serial = new HardwareSerial(1); // Use UART 1
    serial->begin(baud, SERIAL_8N1, rxPin, txPin);
#elif defined(ESP8266)
    serial = new SoftwareSerial(rxPin, txPin);
    serial->begin(baud);
#endif
    initialized = true;
    currentState = WAITING_FOR_LENGTH;
    payloadLength = 0;
    messageAvailable = false;
}

bool ProtoSerial::sendMessage(const OutgoingMessage& message) {
    if (!initialized || !serial) {
        return false;
    }

    uint8_t buffer[OUTGOING_MESSAGE_SIZE];

    // Create a stream that writes to our buffer
    pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));

    // Encode the message
    if (!pb_encode(&stream, OUTGOING_MESSAGE_FIELDS, &message)) {
        // Encoding failed
        return false;
    }

    uint16_t messageLength = stream.bytes_written;

    // Send the length first (as two bytes, little-endian)
    serial->write((uint8_t*)&messageLength, sizeof(messageLength));

    // Send the actual payload
    serial->write(buffer, messageLength);

    // Ensure data is sent
    serial->flush();

    return true;
}

void ProtoSerial::update() {
    if (!initialized || !serial) {
        return;
    }

    switch (currentState) {
        case WAITING_FOR_LENGTH: {
            if (serial->available() >= sizeof(payloadLength)) {
                // Read the 2-byte length
                serial->readBytes((uint8_t*)&payloadLength, sizeof(payloadLength));

                // Protect against absurdly large lengths
                if (payloadLength > sizeof(payloadBuffer) || payloadLength == 0) {
                    // Invalid length, reset
                    payloadLength = 0;
                    currentState = WAITING_FOR_LENGTH;
                } else {
                    currentState = READING_PAYLOAD;
                }
            }
            break;
        }

        case READING_PAYLOAD: {
            if (serial->available() >= payloadLength) {
                // Read the payload
                serial->readBytes(payloadBuffer, payloadLength);

                // Process the message
                processReceivedMessage(payloadBuffer, payloadLength);

                // Reset for the next message
                currentState = WAITING_FOR_LENGTH;
                payloadLength = 0;
            }
            break;
        }
    }
}

bool ProtoSerial::hasMessage() const {
    return messageAvailable;
}

const IncomingMessage& ProtoSerial::getMessage() const {
    return receivedMessage;
}

void ProtoSerial::clearMessage() {
    messageAvailable = false;
    receivedMessage = INCOMING_MESSAGE_INIT;
}

void ProtoSerial::processReceivedMessage(uint8_t* buffer, uint16_t length) {
    // Reset the message
    receivedMessage = INCOMING_MESSAGE_INIT;

    // Create a stream from the buffer
    pb_istream_t stream = pb_istream_from_buffer(buffer, length);

    // Decode the message
    if (pb_decode(&stream, INCOMING_MESSAGE_FIELDS, &receivedMessage)) {
        messageAvailable = true;
    } else {
        // Decoding failed, do not set messageAvailable
    }
}