// OBD-II K-line Scanner — ESP32 + transistor/optocoupler interface
// Polls common PIDs (RPM, speed, coolant temp) and prints to serial.
//
// Hardware: Transistor circuit (Tucker k-line-board) or PC817 optocoupler.
// Set KLINE_TX_INVERT=1 in build flags for optocoupler, 0 for transistor.

#include <Arduino.h>
#include "config.h"
#include "KLineTransport.h"
#include "KLineObd2.h"
#include "Obd2Pids.h"

KLineTransport transport;
KLineObd2* scanner = nullptr;

static unsigned long lastPollMs = 0;
static const unsigned long POLL_INTERVAL_MS = 500;
static uint8_t consecutiveFailures = 0;
static const uint8_t MAX_FAILURES_BEFORE_REINIT = 5;

// Attempt ECU initialization (slow init, then fast init fallback)
static bool tryInit() {
    Serial.print("Slow init (0x");
    Serial.print(KLINE_INIT_ADDR, HEX);
    Serial.print(")... ");

    if (scanner->slowInit(KLINE_INIT_ADDR)) {
        Serial.println("OK");
        Serial.print("Keywords: ");
        Serial.print(scanner->keyword1(), HEX);
        Serial.print(" ");
        Serial.println(scanner->keyword2(), HEX);
        return true;
    }

    Serial.println("failed");
    Serial.print("Fast init... ");

    if (scanner->fastInit()) {
        Serial.println("OK");
        Serial.print("Keywords: ");
        Serial.print(scanner->keyword1(), HEX);
        Serial.print(" ");
        Serial.println(scanner->keyword2(), HEX);
        return true;
    }

    Serial.println("failed");
    return false;
}

void setup() {
    Serial.begin(115200);
    delay(500);

    Serial.println();
    Serial.println("=== OBD-II K-line Scanner ===");
    Serial.print("Bus UART: ");
    Serial.print(KLINE_BAUD);
    Serial.println(" 8N1");
    Serial.print("RX pin: GPIO ");
    Serial.print(KLINE_RX_PIN);
    Serial.print("  TX pin: GPIO ");
    Serial.println(KLINE_TX_PIN);
    Serial.println();

    KLineConfig config;
    config.baud         = KLINE_BAUD;
    config.framing      = KLINE_FRAMING;
    config.idleTimeoutUs = 0;  // master/slave — no idle detection needed
    config.idleCheckUs  = 0;
    config.packetGapMs  = 50;  // P3: inter-message timing
    config.txInvert     = KLINE_TX_INVERT;
    config.checksumType = CHECKSUM_MOD256;

    transport.begin(Serial1, KLINE_RX_PIN, KLINE_TX_PIN, KLINE_LED_PIN,
                    KLINE_UART_NUM, config);

    scanner = new KLineObd2(transport);
    if (!scanner) {
        Serial.println("ERROR: allocation failed");
        return;
    }

    if (!tryInit()) {
        Serial.println("No ECU response. Check wiring and key position.");
    }

    Serial.println();
    if (scanner->isInitialized()) {
        Serial.println("Polling PIDs...");
        Serial.println("RPM      | Speed   | Coolant | Throttle | Voltage");
        Serial.println("---------+---------+---------+----------+--------");
    }
}

void loop() {
    if (!scanner) {
        delay(1000);
        return;
    }

    // Re-init after consecutive failures (session lost)
    if (!scanner->isInitialized()) {
        delay(3000);
        Serial.println("Retrying init...");
        scanner->reset();
        if (tryInit()) {
            consecutiveFailures = 0;
            Serial.println("Polling PIDs...");
        }
        return;
    }

    if (millis() - lastPollMs < POLL_INTERVAL_MS) return;
    lastPollMs = millis();

    uint8_t data[4];
    uint8_t successes = 0;

    // RPM (2 data bytes)
    uint8_t len = scanner->requestPid(obd2::MODE_CURRENT, obd2::PID_RPM, data, sizeof(data));
    if (len >= 2) {
        float rpm = obd2::decodeRpm(data[0], data[1]);
        Serial.print(rpm, 0);
        Serial.print(" rpm");
        successes++;
    } else {
        Serial.print("---     ");
    }
    Serial.print(" | ");

    // Speed (1 data byte)
    len = scanner->requestPid(obd2::MODE_CURRENT, obd2::PID_SPEED, data, sizeof(data));
    if (len >= 1) {
        Serial.print(obd2::decodeSpeed(data[0]));
        Serial.print(" km/h");
        successes++;
    } else {
        Serial.print("---    ");
    }
    Serial.print(" | ");

    // Coolant temp (1 data byte)
    len = scanner->requestPid(obd2::MODE_CURRENT, obd2::PID_COOLANT_TEMP, data, sizeof(data));
    if (len >= 1) {
        Serial.print(obd2::decodeCoolantTemp(data[0]));
        Serial.print(" C");
        successes++;
    } else {
        Serial.print("---    ");
    }
    Serial.print(" | ");

    // Throttle (1 data byte)
    len = scanner->requestPid(obd2::MODE_CURRENT, obd2::PID_THROTTLE, data, sizeof(data));
    if (len >= 1) {
        Serial.print(obd2::decodeThrottle(data[0]), 1);
        Serial.print(" %");
        successes++;
    } else {
        Serial.print("---     ");
    }
    Serial.print(" | ");

    // Control module voltage (2 data bytes)
    len = scanner->requestPid(obd2::MODE_CURRENT, obd2::PID_CONTROL_VOLTAGE, data, sizeof(data));
    if (len >= 2) {
        Serial.print(obd2::decodeControlVoltage(data[0], data[1]), 1);
        Serial.print(" V");
        successes++;
    } else {
        Serial.print("---   ");
    }

    Serial.println();

    // Track consecutive total failures for session loss detection
    if (successes == 0) {
        consecutiveFailures++;
        if (consecutiveFailures >= MAX_FAILURES_BEFORE_REINIT) {
            Serial.println("Session lost — will re-initialize.");
            scanner->reset();
            consecutiveFailures = 0;
        }
    } else {
        consecutiveFailures = 0;
    }
}