#include "./CommandProcessor.h"
#include "./Commands.h"
#include "./Globals.h"

// Gets set to true once the command processor has connected to a host
bool CMDProcessor::connected = false;

namespace {
    // Keeps track of when the last received byte was (used for timeouts)
    unsigned long lastRX = 0;
    // Keeps track of the last received ping when connected, and the last sent ping when not
    unsigned long lastPing = 0;
    // The current phase of the processor
    // 0 = Awaiting CMD ID
    // 1 = Awaiting args
    // 2 = Awaiting checksum
    uint8_t phase = 0;
    // The incoming command ID. 0xFF is basically just null
    uint8_t cmdID = 0xFF;
    // Holds the received argument bytes which will be memcpy'd into the actual command args
    uint8_t argsBufferLength = 0;
    uint8_t argsBuffer[255];
    // Holds the calculated and received checksums
    uint8_t calcChecksum = 0;
    uint8_t rxChecksum = 0;

    Commands::TransmitArgs transmitArgs;

    void reset() {
        phase = 0;
        cmdID = 0xFF;
        argsBufferLength = 0;
        calcChecksum = 0;
        rxChecksum = 0;
    }

    void resetAfterNull() {
        cmdID = 0xFF;
        phase = 3;
    }

    void sendPacket(char* buffer, uint8_t packetSize) {
        uint8_t checksum = 0;

        for (uint8_t i = 0; i < packetSize; i++) {
            Serial.write(buffer[i]);
            checksum ^= buffer[i];
        }
        checksum ^= 0x55;
        Serial.write(checksum);
        Serial.write((char) 0x00);
    }
}

void CMDProcessor::process() {
    // Command read timeout
    if (phase != 0 && millis() - lastRX > CMD_PROCESSOR_RX_TIMEOUT_MS) {
        sendLog("Error: Command read timed out.");
        reset();
        return;
    }

    if (CMDProcessor::connected && millis() - lastPing > CMD_PROCESSOR_PING_TIMEOUT_MS) {
        sendLog("Error: Ping timed out.");
        CMDProcessor::connected = false;
        reset();
    } else if (!CMDProcessor::connected && millis() - lastPing > 250) {
        Serial.print("/WHSPAH");
        Serial.print((char) 0x00);
        lastPing = millis();
    }

    // Return if there's no new data
    if (Serial.available() == 0 && phase != 4) { return; }

    lastRX = millis();
    
    switch (phase) {
    case 0: // Command ID phase
        cmdID = Serial.read();
        phase++;
        break;
    case 1: // Args phase
        switch (cmdID) {
        case 0x01: // If no arguments, continue to next phase
        case 0x02:
            phase++;
            break;
        case 0x10: // Transmit
            argsBufferLength = sizeof(transmitArgs);
            if (Serial.available() >= argsBufferLength) {
                Serial.readBytes(argsBuffer, argsBufferLength);
                memcpy(&transmitArgs, &argsBuffer, argsBufferLength);
                phase++;
            }
            break;
        default: // If unknown, revert to first phase after null
            resetAfterNull();
            break;
        }
        break;
    case 2: // Checksum phase
        rxChecksum = Serial.read();
        calcChecksum = cmdID;
        for (int i = 0; i < argsBufferLength; i++) {
            calcChecksum ^= argsBuffer[i];
        }
        calcChecksum ^= 0x55;

        if (rxChecksum == calcChecksum) {
            phase++;
        } else {
            sendLog("Malformed packet: Checksum mismatch.");
            resetAfterNull();
        }
        break;
    case 3: // Null terminator
        if (Serial.read() == 0x00) {
            phase++;
        } else {
            sendLog("Malformed packet: No null terminator found.");
            // If a null terminator is not present in the expected place,
            // set the command ID to 0xFF (no op) and remain in phase 3.
            // This keeps the firmware waiting for a valid null terminator
            // before continuing normal opperation to prevent any unwanted
            // malformed commands from executing.
            cmdID = 0xFF;
        }
    case 4: // Execution phase
        // Connect
        if (cmdID == 0x01 && !CMDProcessor::connected) {
            CMDProcessor::connected = true;
            lastPing = millis();
            sendPacket("\x01", 1);
        // Ping
        } else if (cmdID == 0x02 && CMDProcessor::connected) {
            lastPing = millis();
            sendPacket("\x02", 1);
        // Transmit
        } else if (cmdID == 0x10 && CMDProcessor::connected) {
            Commands::transmit(transmitArgs);
        }

        reset();
        break;
    }
}
