#include "obd2_kline.h"

// === Konstante für Anfrageintervalle ===
#define OBD2_QUERY_INTERVAL 500 // alle 500 ms neue Anfrage

// === Private Variablen ===
static Stream *klineSerial = nullptr;
static uint32_t lastQueryTime = 0;
static uint32_t lastRecvTime = 0;
static uint32_t lastSpeedMMperSec = 0;

// === Slow Init nach ISO9141-2 ===
void OBD2_KLine_SlowInit()
{
    pinMode(1, OUTPUT); // TXD-Pin (z.B. D1)
    digitalWrite(1, HIGH);
    delay(3000);

    digitalWrite(1, LOW);
    delay(200); // 200ms
    digitalWrite(1, HIGH);
    delay(400); // 400ms
    digitalWrite(1, LOW);
    delay(400); // 400ms
    digitalWrite(1, HIGH);
    delay(200); // 200ms

    // zurück auf Serialbetrieb
    pinMode(1, INPUT);
    delay(300);
}

// === Initialisierung ===
void Init_OBD2_KLine(Stream &serial)
{
    klineSerial = &serial;
    OBD2_KLine_SlowInit();
    delay(100);

    // Sende 01 0D (Vehicle Speed)
    byte speedRequest[] = {0x68, 0x6A, 0xF1, 0x01, 0x0D}; // OBD2 PID-Request
    klineSerial->write(speedRequest, sizeof(speedRequest));
}

// === Geschwindigkeit abfragen ===
uint32_t Process_OBD2_KLine_Speed()
{
    if (!klineSerial || (millis() - lastQueryTime < OBD2_QUERY_INTERVAL)) return 0;

    byte req[] = {0x68, 0x6A, 0xF1, 0x01, 0x0D};
    klineSerial->write(req, sizeof(req));
    lastQueryTime = millis();

    uint8_t buf[16];
    size_t len = klineSerial->readBytes(buf, sizeof(buf));
    for (size_t i = 0; i < len - 2; ++i)
    {
        if (buf[i] == 0x48 && buf[i + 1] == 0x6B && buf[i + 2] == 0x10)
        {
            if (buf[i + 3] == 0x41 && buf[i + 4] == 0x0D)
            {
                uint8_t speed_kmh = buf[i + 5];
                uint32_t speed_mm_per_sec = (uint32_t)speed_kmh * 1000000 / 3600;
                uint32_t dt = millis() - lastRecvTime;
                lastRecvTime = millis();
                lastSpeedMMperSec = speed_mm_per_sec;

                return (speed_mm_per_sec * dt) / 1000;
            }
        }
    }

    return 0;
}