// OBD-II K-line protocol handler (ISO 9141 / ISO 14230)
// 5-baud slow init, fast init, request/response with echo clearing.
//
// Post-review hardening: proper pin detach (C1), timeout guards (C2),
// checksum validation (C3), echo verification (C4), RX flush (I1),
// structural frame parsing (I2/I3/I5), negative response handling.

#include "KLineObd2.h"
#include "Obd2Pids.h"
#include <string.h>

KLineObd2::KLineObd2(KLineTransport& transport)
    : _transport(transport),
      _initialized(false),
      _kw1(0),
      _kw2(0),
      _testerAddr(obd2::TESTER_ADDR),
      _ecuAddr(0),
      _lastRequestMs(0) {}

// --- UART pin management for init sequences ---

void KLineObd2::detachUartTx() {
    // Disconnect UART TX peripheral signal from the GPIO pin.
    // pinMatrixOutDetach() routes the plain GPIO output register
    // to the pin instead of the UART TX signal, so digitalWrite()
    // has full control. Without this, UART and GPIO fight over the pin.
    pinMatrixOutDetach(_transport.txPin(), false, false);
}

void KLineObd2::reattachUartTx() {
    // Re-connect UART TX signal to the GPIO pin for normal serial I/O
    uart_set_pin((uart_port_t)_transport.uartNum(),
                 _transport.txPin(), UART_PIN_NO_CHANGE,
                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
}

// --- Timeout helper ---

uint16_t KLineObd2::remainingMs(unsigned long startMs, uint16_t timeoutMs) {
    unsigned long elapsed = millis() - startMs;
    return (elapsed < timeoutMs) ? (uint16_t)(timeoutMs - elapsed) : 0;
}

// --- 5-baud slow init (ISO 9141-2) ---
// Bit-bang the target address at 5 baud on the TX pin.
//
// Sequence:
//   1. TX pin LOW for 200ms (start bit at 5 baud)
//   2. 8 data bits of targetAddr, LSB first, 200ms each
//   3. TX pin HIGH for 200ms (stop bit)
//   4. Re-attach UART, flush RX, wait for ECU response
//   5. ECU sends: 0x55 (sync), keyword1, keyword2
//   6. Tester sends: inverted keyword2
//   7. ECU sends: inverted tester address (0xCC for 0x33)

bool KLineObd2::slowInit(uint8_t targetAddr) {
    // Detach UART TX so we can bit-bang
    detachUartTx();

    // Configure TX pin as GPIO output, start HIGH (idle)
    int8_t txp = _transport.txPin();
    pinMode(txp, OUTPUT);
    digitalWrite(txp, HIGH);
    delay(300);  // W0: idle before init (>300ms per ISO 9141)

    // Bit-bang target address at 5 baud
    sendByte5Baud(targetAddr);

    // Re-attach UART to TX pin
    reattachUartTx();

    // Flush stale bytes — UART RX was receiving framing errors
    // during the 2-second bit-bang period
    _transport.flushRx();

    // Wait for ECU sync byte (0x55) — W1: 20-300ms per ISO 9141
    int sync = readByteTimeout(300);
    if (sync != obd2::SYNC_BYTE) {
        return false;
    }

    // Read keyword bytes — W2: 5-20ms between bytes
    int kw1 = readByteTimeout(50);
    if (kw1 < 0) return false;
    _kw1 = (uint8_t)kw1;

    int kw2 = readByteTimeout(50);
    if (kw2 < 0) return false;
    _kw2 = (uint8_t)kw2;

    // W3: 25-50ms before tester responds
    delay(30);

    // Send inverted keyword2 back to ECU and verify echo
    uint8_t invKw2 = ~_kw2;
    _transport.serial()->write(invKw2);
    _transport.serial()->flush();
    if (!clearEcho(&invKw2, 1)) {
        return false;  // bus contention during handshake
    }

    // W4: ECU confirms by sending inverted target address
    int ack = readByteTimeout(50);
    if (ack < 0) return false;

    // The ack should be the complement of the target address
    if ((uint8_t)ack != (uint8_t)~targetAddr) {
        return false;
    }

    _ecuAddr = targetAddr;
    _initialized = true;
    _lastRequestMs = millis();
    return true;
}

// --- Fast init (ISO 14230-2, KWP2000) ---
// 25ms LOW + 25ms HIGH "TiniPulse" on TX, then StartComm request.

bool KLineObd2::fastInit() {
    // Detach UART TX for the wake-up pulse
    detachUartTx();

    int8_t txp = _transport.txPin();
    pinMode(txp, OUTPUT);
    digitalWrite(txp, HIGH);
    delay(300);  // idle before pulse

    // TiniPulse: 25ms LOW + 25ms HIGH
    digitalWrite(txp, LOW);
    delay(25);
    digitalWrite(txp, HIGH);
    delay(25);

    // Re-attach UART and flush stale RX data
    reattachUartTx();
    _transport.flushRx();

    // Build StartCommunication request (KWP2000 service 0x81)
    uint8_t startComm[] = {
        0xC1,                    // format: functional addressing, 1 data byte
        obd2::FUNC_ADDR,        // target: functional address
        obd2::TESTER_ADDR,      // source: tester
        0x81                     // StartCommunication service
    };
    uint8_t checksum = KLineTransport::checksumMod256(startComm, 4);

    // Full message with checksum for echo verification
    uint8_t fullMsg[5];
    memcpy(fullMsg, startComm, 4);
    fullMsg[4] = checksum;

    for (uint8_t i = 0; i < 5; i++) {
        _transport.serial()->write(fullMsg[i]);
    }
    _transport.serial()->flush();

    // Verify echo (5 bytes: 4 data + 1 checksum)
    if (!clearEcho(fullMsg, 5)) {
        return false;  // bus contention during fast init
    }

    // Read StartComm positive response
    uint8_t resp[12];
    uint8_t respLen = readResponse(resp, sizeof(resp), 300);
    if (respLen < 4) return false;

    // Parse response structurally using ISO 14230 format byte
    uint8_t dLen = 0;
    int8_t dataStart = parseFrameData(resp, respLen, &dLen);
    if (dataStart < 0) return false;
    if (dLen < 3) return false;  // need service_id + kw1 + kw2

    // Verify StartComm positive response (0x81 + 0x40 = 0xC1)
    if (resp[dataStart] != 0xC1) return false;

    _kw1 = resp[dataStart + 1];
    _kw2 = resp[dataStart + 2];

    _ecuAddr = obd2::FUNC_ADDR;
    _initialized = true;
    _lastRequestMs = millis();
    return true;
}

// --- Request/response ---

void KLineObd2::sendRequest(const uint8_t* data, uint8_t len) {
    if (len > KLINE_MAX_MSG) return;

    // Build complete message with checksum for echo verification
    uint8_t buf[KLINE_MAX_MSG + 1];
    memcpy(buf, data, len);
    buf[len] = KLineTransport::checksumMod256(data, len);
    uint8_t totalLen = len + 1;

    for (uint8_t i = 0; i < totalLen; i++) {
        _transport.serial()->write(buf[i]);
    }
    _transport.serial()->flush();

    // Verify echo (data bytes + checksum byte)
    clearEcho(buf, totalLen);

    _lastRequestMs = millis();
}

uint8_t KLineObd2::readResponse(uint8_t* buf, uint8_t maxLen, uint16_t timeoutMs) {
    uint8_t count = 0;
    unsigned long start = millis();

    // Read first byte (format/header) to determine message length
    int first = readByteTimeout(timeoutMs);
    if (first < 0) return 0;
    buf[count++] = (uint8_t)first;

    // Determine expected length from format byte
    uint8_t fmt = (uint8_t)first;
    uint8_t addrMode = (fmt >> 6) & 0x03;
    uint8_t dataLen = fmt & 0x3F;

    // Determine address byte count from address mode
    uint8_t addrBytes;
    switch (addrMode) {
        case 0:  // no address information
            addrBytes = 0;
            break;
        case 1:  // CARB mode — unsupported, treat as 0 extra
            addrBytes = 0;
            break;
        case 2:  // physical addressing: target + source
        case 3:  // functional addressing: target + source
        default:
            addrBytes = 2;
            break;
    }

    // Read address bytes if present
    for (uint8_t i = 0; i < addrBytes && count < maxLen; i++) {
        uint16_t rem = remainingMs(start, timeoutMs);
        if (rem == 0) return count;
        int b = readByteTimeout(rem);
        if (b < 0) return count;
        buf[count++] = (uint8_t)b;
    }

    // If dataLen == 0, next byte is the actual length
    if (dataLen == 0 && count < maxLen) {
        uint16_t rem = remainingMs(start, timeoutMs);
        if (rem == 0) return count;
        int lenByte = readByteTimeout(rem);
        if (lenByte < 0) return count;
        buf[count++] = (uint8_t)lenByte;
        dataLen = (uint8_t)lenByte;
    }

    // Read data bytes + checksum
    uint8_t expected = dataLen + 1;  // +1 for checksum
    for (uint8_t i = 0; i < expected; i++) {
        if (count >= maxLen) {
#ifdef KLINE_DEBUG
            Serial.println("KLINE: response buffer full");
#endif
            return count;  // truncated — can't validate checksum
        }
        uint16_t rem = remainingMs(start, timeoutMs);
        if (rem == 0) return count;
        int b = readByteTimeout(rem);
        if (b < 0) return count;
        buf[count++] = (uint8_t)b;
    }

    // Validate checksum (mod256 sum of all bytes except last)
    if (count >= 2) {
        uint8_t computed = KLineTransport::checksumMod256(buf, count - 1);
        if (computed != buf[count - 1]) {
#ifdef KLINE_DEBUG
            Serial.println("KLINE: checksum fail");
#endif
            return 0;
        }
    }

    return count;
}

uint8_t KLineObd2::requestPid(uint8_t mode, uint8_t pid,
                               uint8_t* response, uint8_t maxLen) {
    if (!_initialized) return 0;

    // Send TesterPresent keepalive if idle too long (P3 timeout prevention)
    if (_lastRequestMs > 0 && millis() - _lastRequestMs > P3_KEEPALIVE_MS) {
        sendTesterPresent();
    }

    // ISO 14230 format: physical addressing, 2 data bytes
    uint8_t req[] = {
        (uint8_t)(0x82),         // fmt: physical addressing, 2 data bytes
        _ecuAddr,                // target
        _testerAddr,             // source
        mode,                    // service/mode
        pid                      // PID
    };

    sendRequest(req, sizeof(req));

    uint8_t raw[32];
    uint8_t rawLen = readResponse(raw, sizeof(raw));
    if (rawLen < 5) return 0;  // minimum: fmt + target + source + response_mode + pid

    // Parse response structurally
    uint8_t dLen = 0;
    int8_t dataStart = parseFrameData(raw, rawLen, &dLen);
    if (dataStart < 0 || dLen < 2) return 0;

    // Check for negative response (service 0x7F)
    if (raw[dataStart] == 0x7F) {
#ifdef KLINE_DEBUG
        if (dLen >= 3) {
            Serial.print("KLINE: NRC 0x");
            Serial.println(raw[dataStart + 2], HEX);
        }
#endif
        return 0;
    }

    // Verify positive response: service ID = request mode + 0x40
    uint8_t respMode = mode + 0x40;
    if (raw[dataStart] != respMode) return 0;
    if (raw[dataStart + 1] != pid) return 0;

    // Copy data bytes after service_id + pid
    uint8_t copyLen = 0;
    for (uint8_t i = 2; i < dLen && copyLen < maxLen; i++) {
        response[copyLen++] = raw[dataStart + i];
    }
    return copyLen;
}

// --- TesterPresent keepalive ---

void KLineObd2::sendTesterPresent() {
    if (!_initialized) return;

    // ISO 14230: TesterPresent service (0x3E), no sub-function
    uint8_t req[] = {
        (uint8_t)(0x81),         // fmt: physical addressing, 1 data byte
        _ecuAddr,
        _testerAddr,
        0x3E                     // TesterPresent service
    };

    sendRequest(req, sizeof(req));

    // Read and discard the positive response
    uint8_t resp[8];
    readResponse(resp, sizeof(resp), 500);
}

// --- Private helpers ---

void KLineObd2::sendByte5Baud(uint8_t data) {
    int8_t txp = _transport.txPin();

    // Start bit (LOW)
    digitalWrite(txp, LOW);
    delay(200);

    // 8 data bits, LSB first
    for (uint8_t i = 0; i < 8; i++) {
        digitalWrite(txp, (data >> i) & 0x01 ? HIGH : LOW);
        delay(200);
    }

    // Stop bit (HIGH)
    digitalWrite(txp, HIGH);
    delay(200);
}

bool KLineObd2::clearEcho(const uint8_t* expected, uint8_t count) {
    bool match = true;
    for (uint8_t i = 0; i < count; i++) {
        int b = readByteTimeout(KLINE_ECHO_TIMEOUT_MS);
        if (b < 0) {
#ifdef KLINE_DEBUG
            Serial.print("KLINE: echo timeout byte ");
            Serial.println(i);
#endif
            match = false;
            continue;
        }
        if ((uint8_t)b != expected[i]) {
#ifdef KLINE_DEBUG
            Serial.print("KLINE: echo mismatch byte ");
            Serial.print(i);
            Serial.print(" exp=0x");
            Serial.print(expected[i], HEX);
            Serial.print(" got=0x");
            Serial.println((uint8_t)b, HEX);
#endif
            match = false;
        }
    }
    return match;
}

int KLineObd2::readByteTimeout(uint16_t timeoutMs) {
    if (timeoutMs == 0) return -1;
    unsigned long start = millis();
    while (millis() - start < timeoutMs) {
        _transport.drainUart();
        if (_transport.rxAvailable() > 0) {
            return _transport.rxRead();
        }
        delayMicroseconds(100);  // prevent busy-wait burning CPU
    }
    return -1;
}

int8_t KLineObd2::parseFrameData(const uint8_t* buf, uint8_t bufLen, uint8_t* dataLen) {
    if (bufLen < 1) return -1;

    uint8_t fmt = buf[0];
    uint8_t addrMode = (fmt >> 6) & 0x03;
    uint8_t inlineLen = fmt & 0x3F;

    uint8_t headerSize = 1;  // format byte

    switch (addrMode) {
        case 0: // no address bytes
            break;
        case 1: // CARB exception — not supported
#ifdef KLINE_DEBUG
            Serial.println("KLINE: CARB mode unsupported");
#endif
            return -1;
        case 2: // physical addressing: target + source
        case 3: // functional addressing: target + source
            headerSize += 2;
            break;
    }

    uint8_t len;
    if (inlineLen > 0) {
        len = inlineLen;
    } else {
        // Length in separate byte after address bytes
        if (bufLen < headerSize + 1) return -1;
        len = buf[headerSize];
        headerSize += 1;
    }

    if (dataLen) *dataLen = len;

    if (headerSize > bufLen) return -1;
    return (int8_t)headerSize;
}