#include "can_hal.h"
#include "dtc.h"
#include <string.h> // memcpy, memcmp

// =====================
// Interner Zustand/Helper
// =====================
MCP_CAN CAN0(GPIO_CS_CAN);

static bool s_ready = false;
static uint8_t s_nextFiltSlot = 0;   // 0..5 (MCP2515 hat 6 Filter-Slots)
static uint16_t s_modeSettleMs = 10; // Default aus Config

// Trace-Hook
static CanTraceSink s_traceSink = nullptr;

// RAW-Sniffer-Steuerung (Filter offen + Restore der vorherigen Konfig)
static bool s_rawSnifferEnabled = false;

// Spiegel der "Normal"-Konfiguration (damit wir nach RAW wiederherstellen können)
static uint16_t s_savedStdMask[2] = {0x000, 0x000};
static struct
{
  uint32_t id;
  bool ext;
} s_savedFilt[6];
static uint8_t s_savedFiltCount = 0;

// 11-bit: Lib erwartet (value << 16)
static inline uint32_t _std_to_hw(uint16_t v11) { return ((uint32_t)v11) << 16; }

// „Bestätigter“ Mode-Wechsel mithilfe der Lib-Funktion setMode(newMode)
static bool _trySetMode(uint8_t mode, uint16_t settleMs)
{
  const uint32_t t0 = millis();
  do
  {
    if (CAN0.setMode(mode) == CAN_OK)
      return true;
    delay(1);
  } while ((millis() - t0) < settleMs);
  return false;
}

// LOOPBACK-Selftest (ohne Bus)
static bool _selftest_loopback(uint16_t windowMs)
{
  if (!_trySetMode(MCP_LOOPBACK, s_modeSettleMs))
    return false;

  const unsigned long tid = 0x123;
  uint8_t tx[8] = {0xA5, 0x5A, 0x11, 0x22, 0x33, 0x44, 0x77, 0x88};
  if (CAN0.sendMsgBuf(tid, 0, 8, tx) != CAN_OK)
  {
    (void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
    return false;
  }

  bool got = false;
  const uint32_t t0 = millis();
  while ((millis() - t0) < windowMs)
  {
    if (CAN0.checkReceive() == CAN_MSGAVAIL)
    {
      unsigned long rid;
      uint8_t len, rx[8];
      if (CAN0.readMsgBuf(&rid, &len, rx) == CAN_OK)
      {
        if (rid == tid && len == 8 && memcmp(tx, rx, 8) == 0)
        {
          got = true;
          break;
        }
      }
    }
    delay(1);
  }

  (void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
  return got;
}

// Optional: kurzer ListenOnly-Hörtest (nur Heuristik, keine DTC-Änderung)
static void _probe_listen_only(uint16_t ms)
{
  if (ms == 0)
    return;
  if (!_trySetMode(MCP_LISTENONLY, s_modeSettleMs))
    return;
  const uint32_t t0 = millis();
  while ((millis() - t0) < ms)
  {
    if (CAN0.checkReceive() == CAN_MSGAVAIL)
      break;
    delay(1);
  }
  (void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
}

// Offen konfigurieren (RAW-Sniffer)
static bool _apply_open_filters()
{
  if (!_trySetMode(MODE_CONFIG, s_modeSettleMs))
    return false;

  // Masken 0 -> alles durchlassen
  CAN0.init_Mask(0, 0, _std_to_hw(0x000));
  CAN0.init_Mask(1, 0, _std_to_hw(0x000));
  // Filter egal
  for (uint8_t i = 0; i < 6; ++i)
  {
    CAN0.init_Filt(i, 0, _std_to_hw(0x000));
  }
  s_nextFiltSlot = 0;

  return _trySetMode(MCP_NORMAL, s_modeSettleMs);
}

// Gespeicherte Normal-Konfiguration anwenden
static bool _apply_saved_filters()
{
  if (!_trySetMode(MODE_CONFIG, s_modeSettleMs))
    return false;

  CAN0.init_Mask(0, 0, _std_to_hw(s_savedStdMask[0]));
  CAN0.init_Mask(1, 0, _std_to_hw(s_savedStdMask[1]));

  // Erst alle Filter neutralisieren
  for (uint8_t i = 0; i < 6; ++i)
  {
    CAN0.init_Filt(i, 0, _std_to_hw(0x000));
  }

  // Dann gespeicherte Filter wieder setzen
  s_nextFiltSlot = 0;
  for (uint8_t i = 0; i < s_savedFiltCount && s_nextFiltSlot < 6; ++i)
  {
    const auto &F = s_savedFilt[i];
    const uint32_t hwId = F.ext ? F.id : _std_to_hw((uint16_t)F.id);
    CAN0.init_Filt(s_nextFiltSlot++, F.ext ? 1 : 0, hwId);
  }

  return _trySetMode(MCP_NORMAL, s_modeSettleMs);
}

// =====================
// Öffentliche API
// =====================

void CAN_HAL_SetTraceSink(CanTraceSink sink)
{
  s_traceSink = sink;
}

void CAN_HAL_EnableRawSniffer(bool enable)
{
  if (enable == s_rawSnifferEnabled)
    return;

  if (enable)
  {
    // Auf RAW öffnen
    if (_apply_open_filters())
    {
      s_rawSnifferEnabled = true;
    }
    else
    {
      // Falls es nicht klappt, lieber Defekt melden als im Zwischending zu bleiben
      MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    }
  }
  else
  {
    // Gespeicherte "Normal"-Konfiguration wieder aktivieren
    if (_apply_saved_filters())
    {
      s_rawSnifferEnabled = false;
    }
    else
    {
      MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    }
  }
}

bool CAN_HAL_IsRawSnifferEnabled()
{
  return s_rawSnifferEnabled;
}

bool CAN_HAL_Init(const CanHalConfig &cfg)
{
  s_ready = false;
  s_modeSettleMs = cfg.modeSettleMs ? cfg.modeSettleMs : 10;
  s_traceSink = nullptr;
  s_rawSnifferEnabled = false;

  // 1) SPI/MCP starten (STDEXT ist robust gegen Fehlpfade in Lib-Forks)
  if (CAN0.begin(MCP_STDEXT, cfg.baud, cfg.clock) != CAN_OK)
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }

  // 2) Loopback-Selftest (ohne Bus)
  if (!_selftest_loopback(20))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }

  // 3) Optional Listen-Only-Probe (nur Info)
  _probe_listen_only(cfg.listenOnlyProbeMs);

  // 4) Default: Filter/Masks offen, Mode NORMAL
  if (!_apply_open_filters())
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }

  // Initiale "Normal"-Spiegelung: alles offen (bis die App später echte Filter setzt)
  s_savedStdMask[0] = 0x000;
  s_savedStdMask[1] = 0x000;
  s_savedFiltCount = 0;
  for (uint8_t i = 0; i < 6; ++i)
  {
    s_savedFilt[i].id = 0;
    s_savedFilt[i].ext = false;
  }

  MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false);
  s_ready = true;
  return true;
}

bool CAN_HAL_IsReady() { return s_ready; }

bool CAN_HAL_SetMode(uint8_t mode)
{
  const bool ok = _trySetMode(mode, s_modeSettleMs);
  if (!ok)
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
  return ok;
}

bool CAN_HAL_SetMask(uint8_t bank, bool ext, uint32_t rawMask)
{
  if (bank > 1)
    return false;

  if (!CAN_HAL_SetMode(MODE_CONFIG))
    return false;

  const bool ok = (CAN0.init_Mask(bank, ext ? 1 : 0, rawMask) == CAN_OK);

  // Spiegeln (nur STD-11 Spiegel führen wir – ext-Masken selten; bei ext ignorieren)
  if (!ext)
  {
    const uint16_t m11 = (uint16_t)(rawMask >> 16);
    s_savedStdMask[bank] = m11;
  }

  if (!CAN_HAL_SetMode(MCP_NORMAL))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }
  return ok;
}

bool CAN_HAL_SetStdMask11(uint8_t bank, uint16_t mask11)
{
  return CAN_HAL_SetMask(bank, false, _std_to_hw(mask11));
}

void CAN_HAL_ClearFilters()
{
  if (!CAN_HAL_SetMode(MODE_CONFIG))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return;
  }

  CAN0.init_Mask(0, 0, _std_to_hw(0x000));
  CAN0.init_Mask(1, 0, _std_to_hw(0x000));
  for (uint8_t i = 0; i < 6; ++i)
  {
    CAN0.init_Filt(i, 0, _std_to_hw(0x000));
  }
  s_nextFiltSlot = 0;

  // Spiegel auch zurücksetzen
  s_savedStdMask[0] = 0x000;
  s_savedStdMask[1] = 0x000;
  s_savedFiltCount = 0;
  for (uint8_t i = 0; i < 6; ++i)
  {
    s_savedFilt[i].id = 0;
    s_savedFilt[i].ext = false;
  }

  if (!CAN_HAL_SetMode(MCP_NORMAL))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
  }
}

bool CAN_HAL_AddFilter(const CanFilter &f)
{
  if (s_nextFiltSlot >= 6)
    return false;
  if (!CAN_HAL_SetMode(MODE_CONFIG))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }

  const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id);
  const uint8_t slot = s_nextFiltSlot++;
  const bool ok = (CAN0.init_Filt(slot, f.ext ? 1 : 0, hwId) == CAN_OK);

  // Spiegeln
  if (ok)
  {
    if (s_savedFiltCount < 6)
    {
      s_savedFilt[s_savedFiltCount].id = f.ext ? f.id : (uint32_t)((uint16_t)f.id);
      s_savedFilt[s_savedFiltCount].ext = f.ext;
      ++s_savedFiltCount;
    }
  }

  if (!CAN_HAL_SetMode(MCP_NORMAL))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }
  return ok;
}

bool CAN_HAL_SetFilters(const CanFilter *list, size_t count)
{
  if (!CAN_HAL_SetMode(MODE_CONFIG))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }

  // Slots zurücksetzen
  s_nextFiltSlot = 0;
  for (uint8_t i = 0; i < 6; ++i)
  {
    CAN0.init_Filt(i, 0, _std_to_hw(0x000));
  }

  // Setzen
  for (size_t i = 0; i < count && s_nextFiltSlot < 6; ++i)
  {
    const auto &f = list[i];
    const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id);
    CAN0.init_Filt(s_nextFiltSlot++, f.ext ? 1 : 0, hwId);
  }

  // Spiegel aktualisieren
  s_savedFiltCount = 0;
  for (size_t i = 0; i < count && i < 6; ++i)
  {
    s_savedFilt[i].id = list[i].ext ? list[i].id : (uint32_t)((uint16_t)list[i].id);
    s_savedFilt[i].ext = list[i].ext;
    ++s_savedFiltCount;
  }

  if (!CAN_HAL_SetMode(MCP_NORMAL))
  {
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }
  return true;
}

bool CAN_HAL_Read(unsigned long &id, uint8_t &len, uint8_t data[8])
{
  if (CAN0.checkReceive() != CAN_MSGAVAIL)
    return false;

  if (CAN0.readMsgBuf(&id, &len, data) != CAN_OK)
  {
    // Echte Lese-Fehler -> vermutlich SPI/Controller-Problem
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
    return false;
  }

  // MCP_CAN schreibt Flags in das ID-Wort:
  //  bit31 = EXT, bit30 = RTR, Rest = rohe ID (11 oder 29 Bit)
  const bool ext = (id & 0x80000000UL) != 0;
  const bool rtr = (id & 0x40000000UL) != 0; // aktuell nur informativ

  // "Saubere" ID für Aufrufer herstellen
  const uint32_t clean_id = ext ? (id & 0x1FFFFFFFUL) : (id & 0x7FFUL);
  id = clean_id;

  // Trace-Hook (RX)
  if (s_traceSink)
  {
    CanLogFrame f{};
    f.ts_ms = millis();
    f.id = clean_id;
    f.ext = ext;
    f.rx = true;
    f.dlc = len;
    if (len)
      memcpy(f.data, data, len);
    s_traceSink(f);
  }

  // Optional: Wenn du RTR-Frames speziell behandeln willst, könntest du hier
  // (rtr==true) markieren/loggen oder len=0 erzwingen. Für jetzt: einfach durchreichen.
  (void)rtr;

  return true;
}

uint8_t CAN_HAL_Send(unsigned long id, bool ext, uint8_t len, const uint8_t *data)
{
  // Senden
  uint8_t st = CAN0.sendMsgBuf(id, ext ? 1 : 0, len, const_cast<uint8_t *>(data));

  // Trace-Hook (TX) nur bei Erfolg loggen – optional: immer loggen
  if (st == CAN_OK && s_traceSink)
  {
    CanLogFrame f{};
    f.ts_ms = millis();
    f.id = id;
    f.ext = ext;
    f.rx = false;
    f.dlc = len;
    if (len)
      memcpy(f.data, data, len);
    s_traceSink(f);
  }
  return st;
}

// ==== Diagnose/Utilities ====

uint8_t CAN_HAL_GetErrorFlags()
{
  // getError() liefert MCP_EFLG Snapshot (Lib-abhängig)
  return CAN0.getError();
}

void CAN_HAL_GetErrorCounters(uint8_t &tec, uint8_t &rec)
{
  tec = CAN0.errorCountTX();
  rec = CAN0.errorCountRX();
}