// can_native.cpp – Mehrmodell-Setup (Integer-only), Triumph nutzt NUR Kanal B (W23)

#include "can_native.h"
#include "globals.h"   // enthält LubeConfig.CANSource
#include "dtc.h"
#include "debugger.h"

// ====================== Gemeinsame Konstanten / Helpers ======================

// KTM-Faktoren: raw/FACTOR -> km/h
static constexpr uint16_t FACTOR_RWP_KMH_890ADV = 18;
static constexpr uint16_t FACTOR_RWP_KMH_1290SD = 18;

// Triumph 0x208: Fit ≈ 0.0073 km/h/LSB -> exakt 73/10000 km/h/LSB
// mm/s = km/h * 1_000_000 / 3600 -> 73/36 mm/s pro LSB (bei EINEM 16-Bit-Wert)
static constexpr uint16_t TRI_MMPS_NUM        = 73;
static constexpr uint16_t TRI_MMPS_DEN_SINGLE = 36;  // EIN Kanal (W23)

// Gemeinsamer Integrations-/Alive-Status
static uint32_t s_lastIntegrateMs = 0;
static uint32_t s_lastRxMs        = 0;       // für DTC_NO_CAN_SIGNAL
static uint32_t s_lastSpeed_mmps  = 0;       // aktuelle Geschwindigkeit [mm/s]

// mm = (mm/s * ms) / 1000
static inline uint32_t integrate_mm(uint32_t v_mmps, uint32_t dt_ms)
{
    return (uint64_t)v_mmps * dt_ms / 1000ULL;
}

// ========================== Modell-Decoder (Integer) =========================

// --- KTM: 11-bit ID 0x12D, Speed in data[5..6] (BE), raw/FACTOR -> km/h -> mm/s
static uint32_t dec_ktm_rearwheel_mmps(uint8_t dlc, const uint8_t data[8], uint8_t bikeVariant /*0=890,1=1290*/)
{
    if (dlc < 7) return 0; // benötigt data[5], data[6]
    const uint16_t raw = (uint16_t(data[5]) << 8) | data[6];

    uint16_t factor = FACTOR_RWP_KMH_890ADV;
    if (bikeVariant == 1) factor = FACTOR_RWP_KMH_1290SD;

    // mm/s = (raw/factor) * 1_000_000 / 3600  -> reine Integer-Mathe:
    const uint32_t num = (uint32_t)raw * 1000000UL;
    const uint32_t kmh_times1e6 = num / factor;
    return kmh_times1e6 / 3600UL;
}

// --- Triumph: 11-bit ID 0x208, NUR Kanal B = W23 (B2..B3, Little-Endian)
static uint32_t dec_triumph_0x208_w23_mmps(uint8_t dlc, const uint8_t data[8], uint8_t /*unused*/)
{
    if (dlc < 4) return 0;

    // W23 = (B2) + 256*(B3), LE
    const uint16_t W23 = (uint16_t)data[2] | ((uint16_t)data[3] << 8);

    if (W23 == 0) return 0;

    // mm/s = (W23 * 73) / 36 — rundendes Integer-Divide
    return ( (uint32_t)W23 * TRI_MMPS_NUM + (TRI_MMPS_DEN_SINGLE/2) ) / TRI_MMPS_DEN_SINGLE;
}

// ============================ Modell-Registry ================================
struct ModelSpec
{
    // Erwartete 11-bit CAN-ID, min DLC, ob Extended (false=Standard)
    uint16_t can_id;
    uint8_t  min_dlc;
    bool     ext;

    // Decoder-Funktion → mm/s (Integer). bikeVariant: optionale Untervariante.
    uint32_t (*decode_mmps)(uint8_t dlc, const uint8_t data[8], uint8_t bikeVariant);

    // Optionaler Untervarianten-Index (z.B. 0=890ADV, 1=1290SD)
    uint8_t bikeVariant;
};

// Konkrete Modelle (einfach erweiterbar)
static constexpr uint16_t ID_KTM_REAR_WHEEL = 0x12D;
static constexpr uint16_t ID_TRIUMPH_SPEED  = 0x208;

static uint32_t trampoline_ktm_890(uint8_t dlc, const uint8_t data[8], uint8_t) {
    return dec_ktm_rearwheel_mmps(dlc, data, 0);
}
static uint32_t trampoline_ktm_1290(uint8_t dlc, const uint8_t data[8], uint8_t) {
    return dec_ktm_rearwheel_mmps(dlc, data, 1);
}
static uint32_t trampoline_triumph_w23(uint8_t dlc, const uint8_t data[8], uint8_t) {
    return dec_triumph_0x208_w23_mmps(dlc, data, 0);
}

// getSpec(): mappt LubeConfig.CANSource → ModelSpec
static bool getSpec(ModelSpec &out)
{
    switch (LubeConfig.CANSource)
    {
    case KTM_890_ADV_R_2021:
        out = { ID_KTM_REAR_WHEEL, 7, false, trampoline_ktm_890, 0 };
        return true;

    case KTM_1290_SD_R_2023:
        out = { ID_KTM_REAR_WHEEL, 7, false, trampoline_ktm_1290, 1 };
        return true;

    case TRIUMPH_SPEED_TWIN_1200_RS_2025:
        // Triumph nutzt NUR W23 (Hinterrad-Kanal B)
        out = { ID_TRIUMPH_SPEED, 4, false, trampoline_triumph_w23, 0 };
        return true;

    default:
        return false; // unbekannt → optional generisch behandeln
    }
}

// ============================== Initialisierung ==============================
bool Init_CAN_Native()
{
    // HAL bereitstellen
    if (!CAN_HAL_IsReady())
    {
        CanHalConfig cfg;
        cfg.baud = CAN_500KBPS;
        cfg.clock = MCP_16MHZ;
        cfg.listenOnlyProbeMs = 50;

        if (!CAN_HAL_Init(cfg))
        {
            MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
            Debug_pushMessage("CAN(Native): HAL init failed\n");
            return false;
        }
    }

    // Spec laden
    ModelSpec spec;
    const bool haveSpec = getSpec(spec);

    // Masken/Filter
    CAN_HAL_SetStdMask11(0, 0x7FF);
    CAN_HAL_SetStdMask11(1, 0x7FF);

    if (haveSpec)
    {
        CanFilter flist[1] = { { spec.can_id, spec.ext } };
        CAN_HAL_SetFilters(flist, 1);
        Debug_pushMessage("CAN(Native): Filter set (ID=0x%03X, minDLC=%u)\n", spec.can_id, spec.min_dlc);
    }
    else
    {
        // Fallback: beide IDs aktivieren (KTM+Triumph), falls Quelle unbekannt
        CanFilter flist[2] = { { ID_KTM_REAR_WHEEL, false }, { ID_TRIUMPH_SPEED, false } };
        CAN_HAL_SetFilters(flist, 2);
        Debug_pushMessage("CAN(Native): Fallback filters (KTM=0x%03X, TRI=0x%03X)\n", ID_KTM_REAR_WHEEL, ID_TRIUMPH_SPEED);
    }

    CAN_HAL_SetMode(MCP_NORMAL);

    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false);

    s_lastIntegrateMs = millis();
    s_lastRxMs        = 0;
    s_lastSpeed_mmps  = 0;

    return true;
}

// ============================== Verarbeitung ================================
uint32_t Process_CAN_Native_WheelSpeed()
{
    const uint32_t now = millis();
    ModelSpec spec;
    const bool haveSpec = getSpec(spec);

    // Frames non-blocking verarbeiten
    for (uint8_t i = 0; i < 6; ++i) // kleine Obergrenze gegen Busy-Loops
    {
        unsigned long id;
        uint8_t dlc;
        uint8_t buf[8];
        if (!CAN_HAL_Read(id, dlc, buf))
            break;

        if (haveSpec)
        {
            if (id == spec.can_id && dlc >= spec.min_dlc)
            {
                s_lastSpeed_mmps = spec.decode_mmps(dlc, buf, spec.bikeVariant);
                s_lastRxMs = now;
            }
        }
        else
        {
            // Fallback: KTM prüfen
            if (id == ID_KTM_REAR_WHEEL && dlc >= 7)
            {
                s_lastSpeed_mmps = dec_ktm_rearwheel_mmps(dlc, buf, 0);
                s_lastRxMs = now;
            }
            // Fallback: Triumph prüfen (nur W23)
            else if (id == ID_TRIUMPH_SPEED && dlc >= 4)
            {
                s_lastSpeed_mmps = dec_triumph_0x208_w23_mmps(dlc, buf, 0);
                s_lastRxMs = now;
            }
        }
    }

    // CAN-Heartbeat / DTC
    if (s_lastRxMs != 0)
    {
        const bool stale = (now - s_lastRxMs) > 10000UL;
        MaintainDTC(DTC_NO_CAN_SIGNAL, stale);
    }
    else
    {
        static uint32_t t0 = now;
        if (now - t0 > 1000UL)
            MaintainDTC(DTC_NO_CAN_SIGNAL, true);
    }

    // Integration Strecke (mm)
    if (s_lastIntegrateMs == 0) s_lastIntegrateMs = now;
    const uint32_t dt_ms = now - s_lastIntegrateMs;
    s_lastIntegrateMs = now;

    const bool speedStale = (s_lastRxMs == 0) || ((now - s_lastRxMs) > 600UL);
    const uint32_t v_mmps = speedStale ? 0u : s_lastSpeed_mmps;

    return integrate_mm(v_mmps, dt_ms);
}