#include "hardware_serial.hpp"
#include <Arduino.h>
#include <logger.hpp>

//#define DEBUG_PROTO
#ifdef DEBUG_PROTO
#define LOG(msg) LOG_DEBUG(msg)
#else
#define LOG(msg)
#endif

ProtoSerial::ProtoSerial()
    : serial(nullptr), initialized(false), lastError{0}, callback(nullptr),
      currentState(WAITING_FOR_SYNC_START), payloadLength(0), messageAvailable(false),
      lastUpdate(0), lastByteTime(0) {
    receivedMessage = INCOMING_MESSAGE_INIT;
}

ProtoSerial::~ProtoSerial() {
    // No delete needed; serial is caller-managed
}

void ProtoSerial::begin(SerialType& serialPort) {
    serial = &serialPort;
    initialized = true;
    currentState = WAITING_FOR_SYNC_START;
    payloadLength = 0;
    messageAvailable = false;
    lastError[0] = '\0';
    lastUpdate = 0;
    lastByteTime = 0;
    snprintf(lastError, sizeof(lastError), "Initialized ProtoSerial");
    LOG(lastError);
}

bool ProtoSerial::sendMessage(const OutgoingMessage& message) {
    if (!initialized || !serial || !serial->availableForWrite()) {
        snprintf(lastError, sizeof(lastError), "Serial not initialized or not ready");
        LOG(lastError);
        return false;
    }

    uint8_t buffer[OUTGOING_MESSAGE_SIZE];
    pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));

    // Encode the message
    if (!pb_encode(&stream, OUTGOING_MESSAGE_FIELDS, &message)) {
        snprintf(lastError, sizeof(lastError), "Failed to encode message");
        LOG(lastError);
        return false;
    }

    uint16_t messageLength = stream.bytes_written;
    if (messageLength > sizeof(buffer)) {
        snprintf(lastError, sizeof(lastError), "Encoded message too large: %u", messageLength);
        LOG(lastError);
        return false;
    }

    // Send framing: sync start, length, payload, CRC, sync end
    serial->write(SYNC_START);
    serial->write((uint8_t*)&messageLength, sizeof(messageLength));
    serial->write(buffer, messageLength);
    uint8_t crc = calculate_crc8(buffer, messageLength);
    serial->write(crc);
    serial->write(SYNC_END);
    serial->flush();

    #ifdef DEBUG_PROTO
    dumpHex(buffer, messageLength, "Sent");
    snprintf(lastError, sizeof(lastError), "Sent message, length: %u, CRC: 0x%02X", messageLength, crc);
    LOG(lastError);
    #endif
    return true;
}

void ProtoSerial::update() {
    if (!initialized || !serial) {
        snprintf(lastError, sizeof(lastError), "Serial not initialized");
        LOG(lastError);
        return;
    }

    // Throttle updates to ~100Hz
    unsigned long now = millis();
    if (now - lastUpdate < 10) return;
    lastUpdate = now;

    // Timeout if no data received for 1 second
    if (currentState != WAITING_FOR_SYNC_START && now - lastByteTime > 1000) {
        snprintf(lastError, sizeof(lastError), "Receive timeout");
        LOG(lastError);
        currentState = WAITING_FOR_SYNC_START;
        payloadLength = 0;
    }

    while (serial->available()) {
        lastByteTime = now;
        switch (currentState) {
            case WAITING_FOR_SYNC_START: {
                uint8_t byte = serial->read();
                if (byte == SYNC_START) {
                    currentState = WAITING_FOR_LENGTH;
                    LOG("Received SYNC_START");
                }
                break;
            }
            case WAITING_FOR_LENGTH: {
                if (serial->available() >= sizeof(payloadLength)) {
                    serial->readBytes((uint8_t*)&payloadLength, sizeof(payloadLength));
                    if (payloadLength > sizeof(payloadBuffer) || payloadLength == 0) {
                        snprintf(lastError, sizeof(lastError), "Invalid payload length: %u", payloadLength);
                        LOG(lastError);
                        currentState = WAITING_FOR_SYNC_START;
                        payloadLength = 0;
                    } else {
                        currentState = READING_PAYLOAD;
                        snprintf(lastError, sizeof(lastError), "Received length: %u", payloadLength);
                        LOG(lastError);
                    }
                }
                break;
            }
            case READING_PAYLOAD: {
                if (serial->available() >= payloadLength) {
                    serial->readBytes(payloadBuffer, payloadLength);
                    currentState = READING_CRC;
                }
                break;
            }
            case READING_CRC: {
                if (serial->available() >= 1) {
                    uint8_t received_crc = serial->read();
                    uint8_t expected_crc = calculate_crc8(payloadBuffer, payloadLength);
                    if (received_crc != expected_crc) {
                        snprintf(lastError, sizeof(lastError), "CRC mismatch: received 0x%02X, expected 0x%02X", received_crc, expected_crc);
                        LOG(lastError);
                    } else if (serial->available() >= 1 && serial->read() != SYNC_END) {
                        snprintf(lastError, sizeof(lastError), "Missing SYNC_END");
                        LOG(lastError);
                    } else {
                        dumpHex(payloadBuffer, payloadLength, "Received");
                        processReceivedMessage(payloadBuffer, payloadLength);
                        snprintf(lastError, sizeof(lastError), "Received valid message, length: %u", payloadLength);
                        LOG(lastError);
                    }
                    currentState = WAITING_FOR_SYNC_START;
                    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;
    snprintf(lastError, sizeof(lastError), "Message cleared");
    LOG(lastError);
}

void ProtoSerial::setCallback(Callback cb) {
    callback = cb;
    snprintf(lastError, sizeof(lastError), "Callback set");
    LOG(lastError);
}

void ProtoSerial::processReceivedMessage(uint8_t* buffer, uint16_t length) {
    receivedMessage = INCOMING_MESSAGE_INIT;
    pb_istream_t stream = pb_istream_from_buffer(buffer, length);
    if (pb_decode(&stream, INCOMING_MESSAGE_FIELDS, &receivedMessage)) {
        if (receivedMessage.which_payload == 0) {
            snprintf(lastError, sizeof(lastError), "Invalid message: which_payload not set");
            LOG(lastError);
            return;
        }
        messageAvailable = true;
        if (callback) {
            callback(receivedMessage);
        }
        snprintf(lastError, sizeof(lastError), "Message decoded successfully");
        LOG(lastError);
    } else {
        snprintf(lastError, sizeof(lastError), "Failed to decode message");
        LOG(lastError);
    }
}

uint8_t ProtoSerial::calculate_crc8(const uint8_t* data, uint16_t len) {
    uint8_t crc = 0x00;
    for (uint16_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x80) crc = (crc << 1) ^ 0x31; // CRC-8 polynomial
            else crc <<= 1;
        }
    }
    return crc;
}

void ProtoSerial::dumpHex(const uint8_t* data, uint16_t len, const char* label) {
#ifdef DEBUG_PROTO
    Serial.print(label);
    Serial.print(": ");
    for (uint16_t i = 0; i < len; i++) {
        if (i % 16 == 0) Serial.println();
        Serial.printf("%02X ", data[i]);
    }
    Serial.println();
#endif
}

ProtoSerial pserial;