#include "can_obd2.h"
#include "can_hal.h"
#include "dtc.h"
#include "debugger.h"
#include "globals.h" 
#include <stdarg.h>

// Trace-Sink aus webui.cpp (o.ä.)
extern void TRACE_OnObdFrame(uint32_t id, bool rx, const uint8_t *d, uint8_t dlc, const char *note);

// =======================
// Konfiguration (anpassbar)
// =======================

// Abfrageintervall für Speed (PID 0x0D)
#ifndef OBD2_QUERY_INTERVAL_MS
#define OBD2_QUERY_INTERVAL_MS 100 // 10 Hz
#endif

// Antwort-Timeout auf eine einzelne Anfrage
#ifndef OBD2_RESP_TIMEOUT_MS
#define OBD2_RESP_TIMEOUT_MS 120 // etwas großzügiger für reale ECUs
#endif

// Wenn so lange keine valide Antwort kam, gilt die Geschwindigkeit als stale -> v=0
#ifndef OBD2_STALE_MS
#define OBD2_STALE_MS 600 // 0,6 s
#endif

// Begrenzung, wie viele RX-Frames pro Aufruf maximal gezogen werden
#ifndef OBD2_MAX_READS_PER_CALL
#define OBD2_MAX_READS_PER_CALL 4
#endif

// Optionales Debug-Rate-Limit
#ifndef OBD2_DEBUG_INTERVAL_MS
#define OBD2_DEBUG_INTERVAL_MS 1000
#endif

// Max. Delta-Zeit fürs Weg-Integrationsglied (Ausreißer-Klemme)
#ifndef OBD2_MAX_DT_MS
#define OBD2_MAX_DT_MS 200
#endif

// Erlaube einmaligen Fallback von funktionaler (0x7DF) auf physische Adresse (0x7E0)
#ifndef OBD2_ALLOW_PHYSICAL_FALLBACK
#define OBD2_ALLOW_PHYSICAL_FALLBACK 1
#endif

// =======================
// OBD-II IDs (11-bit)
// =======================
static constexpr uint16_t OBD_REQ_ID_FUNCTIONAL = 0x7DF; // Broadcast-Request
static constexpr uint16_t OBD_REQ_ID_PHYSICAL   = 0x7E0; // Engine ECU (Antwort 0x7E8)
static constexpr uint16_t OBD_RESP_MIN = 0x7E8;          // ECUs antworten 0x7E8..0x7EF
static constexpr uint16_t OBD_RESP_MAX = 0x7EF;

// =======================
// Interner Status
// =======================
enum class ObdState : uint8_t
{
  Idle = 0,
  Waiting = 1
};

static ObdState s_state = ObdState::Idle;
static uint32_t s_lastQueryTime = 0;
static uint32_t s_requestDeadline = 0;

static uint32_t s_lastRespTime = 0;
static uint32_t s_lastIntegrateMs = 0;
static uint32_t s_lastSpeedMMps = 0;

static uint32_t s_lastDbgMs = 0;

// =======================
// Hilfsfunktionen
// =======================
static inline bool isResponseId(unsigned long id)
{
  return (id >= OBD_RESP_MIN) && (id <= OBD_RESP_MAX);
}

static inline uint32_t kmh_to_mmps(uint16_t kmh)
{
  return (uint32_t)kmh * 1000000UL / 3600UL;
}

static inline void maybeDebug(uint32_t now, const char *fmt, ...)
{
#if 1
  if (now - s_lastDbgMs < OBD2_DEBUG_INTERVAL_MS)
    return;
  s_lastDbgMs = now;
  va_list ap;
  va_start(ap, fmt);
  Debug_pushMessage(fmt, ap); // nimmt va_list
  va_end(ap);
#else
  (void)now;
  (void)fmt;
#endif
}

// =======================
// Öffentliche API
// =======================
bool Init_CAN_OBD2()
{
  // 1) HAL bereitstellen (Selftest inklusive). Nur initialisieren, wenn noch nicht ready.
  if (!CAN_HAL_IsReady())
  {
    CanHalConfig cfg;
    cfg.baud = CAN_500KBPS;
    cfg.clock = MCP_16MHZ;
    cfg.listenOnlyProbeMs = 50;

    if (!CAN_HAL_Init(cfg))
    {
      // Hardware/Selftest failed → OBD2-CAN nicht nutzbar
      MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
      Debug_pushMessage("CAN(OBD2): HAL init failed\n");
      return false;
    }
  }

  // 2) Filter/Masken für 0x7E8..0x7EF
  CAN_HAL_SetStdMask11(0, 0x7F0);
  CAN_HAL_SetStdMask11(1, 0x7F0);

  CanFilter flist[8] = {
      {0x7E8, false},
      {0x7E9, false},
      {0x7EA, false},
      {0x7EB, false},
      {0x7EC, false},
      {0x7ED, false},
      {0x7EE, false},
      {0x7EF, false},
  };
  CAN_HAL_SetFilters(flist, 8);

  CAN_HAL_SetMode(MCP_NORMAL);

  // 3) DTC-Startzustände
  MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false);
  MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true); // bis erste Antwort kommt

  // 4) Zeitbasen resetten
  const uint32_t now = millis();
  s_state = ObdState::Idle;
  s_lastQueryTime = now;
  s_requestDeadline = 0;
  s_lastRespTime = 0;
  s_lastIntegrateMs = now;
  s_lastSpeedMMps = 0;
  s_lastDbgMs = 0;

  Debug_pushMessage("CAN(OBD2): Filters set (7E8..7EF), NORMAL mode\n");
  return true;
}

uint32_t Process_CAN_OBD2_Speed()
{
  const uint32_t now = millis();

  // 1) Anfrage senden (nur wenn Idle und Intervall um)
  if (s_state == ObdState::Idle && (now - s_lastQueryTime) >= OBD2_QUERY_INTERVAL_MS)
  {
    uint8_t req[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00}; // Mode 01, PID 0x0D (Speed)

    // Trace: geplanter Request (functional)
    TRACE_OnObdFrame(OBD_REQ_ID_FUNCTIONAL, /*rx=*/false, req, 8, "req 01 0D (functional)");

    uint8_t st = CAN_HAL_Send(OBD_REQ_ID_FUNCTIONAL, /*ext=*/false, 8, req);
    s_lastQueryTime = now;

    if (st == CAN_OK)
    {
      s_state = ObdState::Waiting;
      s_requestDeadline = now + OBD2_RESP_TIMEOUT_MS;
    }
    else
    {
#if OBD2_ALLOW_PHYSICAL_FALLBACK
      // einmalig physisch versuchen (0x7E0 → Antwort 0x7E8)
      TRACE_OnObdFrame(OBD_REQ_ID_PHYSICAL, /*rx=*/false, req, 8, "req 01 0D (physical)");
      st = CAN_HAL_Send(OBD_REQ_ID_PHYSICAL, /*ext=*/false, 8, req);
      s_lastQueryTime = now;
      if (st == CAN_OK)
      {
        s_state = ObdState::Waiting;
        s_requestDeadline = now + OBD2_RESP_TIMEOUT_MS;
      }
      else
#endif
      {
        // Senden fehlgeschlagen -> harter Timeout-DTC
        MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
        maybeDebug(now, "OBD2-CAN send failed (%u)\n", st);
      }
    }
  }

  // 2) Non-blocking Receive: wenige Frames pro Tick ziehen
  for (uint8_t i = 0; i < OBD2_MAX_READS_PER_CALL; ++i)
  {
    unsigned long rxId;
    uint8_t len;
    uint8_t rx[8];
    if (!CAN_HAL_Read(rxId, len, rx))
      break;
    if (!isResponseId(rxId))
      continue;
    if (len < 3)
      continue;

    // Erwartete Formate:
    //  - Einfache Antwort:   0x41 0x0D <A> ...
    //  - Mit Längen-Byte:    0x03/0x04 0x41 0x0D <A> ...
    uint8_t modeResp = 0, pid = 0, speedKmh = 0;
    if ((rx[0] == 0x03 || rx[0] == 0x04) && len >= 4 && rx[1] == 0x41 && rx[2] == 0x0D)
    {
      modeResp = rx[1];
      pid = rx[2];
      speedKmh = rx[3];
    }
    else if (rx[0] == 0x41 && rx[1] == 0x0D && len >= 3)
    {
      modeResp = rx[0];
      pid = rx[1];
      speedKmh = rx[2];
    }
    else
    {
      // Nicht das gesuchte PID → optional trotzdem loggen
      TRACE_OnObdFrame(rxId, /*rx=*/true, rx, len, "other OBD resp");
      continue;
    }

    if (modeResp == 0x41 && pid == 0x0D)
    {
      // Valide Antwort
      s_lastSpeedMMps = kmh_to_mmps(speedKmh);
      s_lastRespTime = now;
      s_state = ObdState::Idle;

      MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false);
      MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false);

      char note[40];
      snprintf(note, sizeof(note), "speed=%ukmh", (unsigned)speedKmh);
      TRACE_OnObdFrame(rxId, /*rx=*/true, rx, len, note);

      maybeDebug(now, "OBD2 speed: %u km/h (%lu mm/s)\n",
                 (unsigned)speedKmh, (unsigned long)s_lastSpeedMMps);
      break; // eine valide Antwort pro Zyklus reicht
    }
    else
    {
      // ist zwar OBD-II Antwort, aber nicht unser PID – optional loggen
      TRACE_OnObdFrame(rxId, /*rx=*/true, rx, len, "other OBD resp");
    }
  }

  // 3) Offene Anfrage: Timeout prüfen
  if (s_state == ObdState::Waiting && (int32_t)(now - s_requestDeadline) >= 0)
  {
    // Keine passende Antwort erhalten
    MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
    s_state = ObdState::Idle;
    TRACE_OnObdFrame(0x000, /*rx=*/true, nullptr, 0, "timeout 01 0D");
  }

  // 4) Integration (mm) über dt
  uint32_t add_mm = 0;
  if (s_lastIntegrateMs == 0)
    s_lastIntegrateMs = now;

  uint32_t raw_dt = now - s_lastIntegrateMs;
  if (raw_dt > OBD2_MAX_DT_MS) raw_dt = OBD2_MAX_DT_MS; // Ausreißer klemmen
  const uint32_t dt_ms = raw_dt;
  s_lastIntegrateMs = now;

  // Stale-Schutz: wenn lange keine Antwort -> v=0
  const bool stale = (s_lastRespTime == 0) || ((now - s_lastRespTime) > OBD2_STALE_MS);
  const uint32_t v_mmps = stale ? 0 : s_lastSpeedMMps;

  // mm = (mm/s * ms) / 1000
  add_mm = (uint64_t)v_mmps * dt_ms / 1000ULL;

  return add_mm;
}