#include "can_native.h"
#include "globals.h" // für LubeConfig, etc.
#include "dtc.h"
#include "debugger.h"

// ===== Bike-spezifische Konstanten =====
// Faktor zur Umrechnung der Rohdaten -> km/h (aus deinem bisherigen Code)
static constexpr uint16_t FACTOR_RWP_KMH_890ADV = 18;
static constexpr uint16_t FACTOR_RWP_KMH_1290SD = 18;

// Erwartete CAN-ID(s) für die genutzten Bikes (11-bit)
static constexpr uint16_t ID_KTM_REAR_WHEEL = 0x12D; // aus deinem Filter-Setup

// ===== Interner 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; // mm pro Sekunde (Rear Wheel)

// Hilfsfunktion: aus km/h -> mm/s
static inline uint32_t kmh_to_mmps(uint16_t kmh)
{
    // 1 km/h = 1'000'000 mm / 3600 s
    return (uint32_t)kmh * 1000000UL / 3600UL;
}

// Hilfsfunktion: aus Rohdaten -> mm/s je nach Bike-Konfiguration
static uint32_t parse_speed_mmps_from_frame(uint8_t dlc, const uint8_t data[8])
{
    if (dlc < 7)
        return 0; // wir brauchen data[5] & data[6]
    uint16_t raw = (uint16_t)data[5] << 8 | data[6];

    switch (LubeConfig.CANSource)
    {
    case KTM_890_ADV_R_2021:
        // (raw / FACTOR) km/h -> mm/s
        // Deine Kommentare: raw * 500 -> cm/s — hier sauber über kmh_to_mmps
        return (((uint32_t)raw * 1000000UL) / FACTOR_RWP_KMH_890ADV) / 3600UL;

    case KTM_1290_SD_R_2023:
        return (((uint32_t)raw * 1000000UL) / FACTOR_RWP_KMH_1290SD) / 3600UL;

    default:
        return 0;
    }
}

bool Init_CAN_Native()
{
    // 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; // kurzer, unkritischer „Bus lebt?“-Blick

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

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

    CanFilter flist[1] = {{ID_KTM_REAR_WHEEL, false}};
    CAN_HAL_SetFilters(flist, 1);

    CAN_HAL_SetMode(MCP_NORMAL);

    // 3) Startzustand/Flags
    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false);
    // DTC_NO_CAN_SIGNAL wird in Process_* verwaltet

    // 4) Status resetten
    s_lastIntegrateMs = millis();
    s_lastRxMs = 0;
    s_lastSpeed_mmps = 0;

    Debug_pushMessage("CAN(Native): Filters set (ID=0x%03X), NORMAL mode\n", ID_KTM_REAR_WHEEL);
    return true;
}

uint32_t Process_CAN_Native_WheelSpeed()
{
    const uint32_t now = millis();
    uint32_t add_mm = 0;

    // 1) Frames non-blocking ziehen und relevante 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;

        // Wir erwarten 11-bit 0x12D (Filter sind gesetzt, aber doppelter Boden schadet nicht)
        if (id == ID_KTM_REAR_WHEEL)
        {
            s_lastSpeed_mmps = parse_speed_mmps_from_frame(dlc, buf);
            s_lastRxMs = now;
            // Kein "break": falls mehrere Frames in der Queue sind, nehmen wir das letzte als aktuellsten
        }
    }

    // 2) CAN-Heartbeat -> DTC_NO_CAN_SIGNAL (Warnung, wenn >10s nix mehr kam)
    if (s_lastRxMs != 0)
    {
        const bool stale = (now - s_lastRxMs) > 10000UL;
        MaintainDTC(DTC_NO_CAN_SIGNAL, stale);
    }
    else
    {
        // Seit Start noch kein Frame gesehen -> noch nicht entscheiden, DTC-Logik darf warten
        // Optional: nach 1s ohne Frames Warnung setzen
        static uint32_t t0 = now;
        if (now - t0 > 1000UL)
        {
            MaintainDTC(DTC_NO_CAN_SIGNAL, true);
        }
    }

    // 3) Integration der Distanz (mm) über dt
    if (s_lastIntegrateMs == 0)
        s_lastIntegrateMs = now;
    const uint32_t dt_ms = now - s_lastIntegrateMs;
    s_lastIntegrateMs = now;

    // Wenn seit 600 ms keine neue Geschwindigkeit kam, setze v -> 0 (Stale-Schutz)
    const bool speedStale = (s_lastRxMs == 0) || ((now - s_lastRxMs) > 600UL);
    const uint32_t v_mmps = speedStale ? 0 : s_lastSpeed_mmps;

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

    return add_mm;
}