// OBD-II K-line protocol handler (ISO 9141 / ISO 14230)
// 5-baud slow init, fast init, request/response with echo clearing.

#include "KLineObd2.h"
#include "Obd2Pids.h"

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

// --- 5-baud slow init (ISO 9141-2) ---
// Bit-bang the target address at 5 baud on the TX pin.
// The UART peripheral must be detached from the TX pin during this sequence,
// then re-attached for normal 10400 baud communication.
//
// 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, 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) {
    int8_t txp = _transport.txPin();
    uint8_t unum = _transport.uartNum();

    // Detach UART from TX pin so we can bit-bang
    uart_set_pin((uart_port_t)unum, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE,
                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);

    // Configure TX pin as GPIO output, start HIGH (idle)
    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
    uart_set_pin((uart_port_t)unum, txp, UART_PIN_NO_CHANGE,
                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);

    // 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
    uint8_t invKw2 = ~_kw2;
    _transport.serial()->write(invKw2);
    _transport.serial()->flush();

    // Clear our own echo (half-duplex bus)
    clearEcho(1);

    // 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;
    return true;
}

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

bool KLineObd2::fastInit() {
    int8_t txp = _transport.txPin();
    uint8_t unum = _transport.uartNum();

    // Detach UART from TX pin for the wake-up pulse
    uart_set_pin((uart_port_t)unum, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE,
                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);

    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
    uart_set_pin((uart_port_t)unum, txp, UART_PIN_NO_CHANGE,
                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);

    // Send StartCommunication request (KWP2000 service 0x81)
    // Format: [fmt+len, target, source, service_id]
    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);

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

    // Clear echo (5 bytes: 4 data + 1 checksum)
    clearEcho(5);

    // Read StartComm positive response
    // Expected: [fmt+len, source, target, 0xC1, kw1, kw2, checksum]
    uint8_t resp[7];
    uint8_t respLen = readResponse(resp, sizeof(resp), 300);
    if (respLen < 4) return false;

    // Positive response service ID is request + 0x40
    // Find the service byte — it's after header bytes
    // For physical addressing response: [fmt, target, source, 0xC1, kw1, kw2, chk]
    bool foundResponse = false;
    for (uint8_t i = 0; i < respLen; i++) {
        if (resp[i] == 0xC1) {  // StartComm positive response
            if (i + 2 < respLen) {
                _kw1 = resp[i + 1];
                _kw2 = resp[i + 2];
                foundResponse = true;
            }
            break;
        }
    }

    if (!foundResponse) return false;

    _ecuAddr = obd2::FUNC_ADDR;
    _initialized = true;
    return true;
}

// --- Request/response ---

void KLineObd2::sendRequest(const uint8_t* data, uint8_t len) {
    uint8_t checksum = KLineTransport::checksumMod256(data, len);

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

    // Clear echo: data bytes + checksum byte
    clearEcho(len + 1);
}

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
    // Bits 6-7: address mode (00=no addr, 10=phys, 11=func)
    // Bits 0-5: data length (0 = length in separate byte)
    uint8_t fmt = (uint8_t)first;
    uint8_t addrBytes = (fmt & 0x80) ? 2 : 0;  // target + source if addressing present
    uint8_t dataLen = fmt & 0x3F;

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

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

    // Read data bytes + checksum
    uint8_t remaining = dataLen + 1;  // +1 for checksum
    for (uint8_t i = 0; i < remaining && count < maxLen; i++) {
        uint16_t elapsed = millis() - start;
        if (elapsed >= timeoutMs) return count;
        int b = readByteTimeout(timeoutMs - elapsed);
        if (b < 0) return count;
        buf[count++] = (uint8_t)b;
    }

    return count;
}

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

    // 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[16];
    uint8_t rawLen = readResponse(raw, sizeof(raw));
    if (rawLen < 5) return 0;  // minimum: fmt + target + source + response_mode + pid

    // Find response data — skip header, copy from response_mode onward
    // Response mode = request mode + 0x40
    uint8_t respMode = mode + 0x40;
    for (uint8_t i = 0; i < rawLen; i++) {
        if (raw[i] == respMode && (i + 1) < rawLen && raw[i + 1] == pid) {
            // Copy data bytes after mode+pid, excluding checksum
            uint8_t dataStart = i + 2;
            uint8_t dataEnd = rawLen - 1;  // exclude checksum
            uint8_t dataLen = 0;
            for (uint8_t j = dataStart; j < dataEnd && dataLen < maxLen; j++) {
                response[dataLen++] = raw[j];
            }
            return dataLen;
        }
    }

    return 0;
}

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

void KLineObd2::clearEcho(uint8_t count) {
    for (uint8_t i = 0; i < count; i++) {
        readByteTimeout(KLINE_ECHO_TIMEOUT_MS);
    }
}

int KLineObd2::readByteTimeout(uint16_t timeoutMs) {
    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;
}