#include "obd2_can.h"
#include <mcp_can.h>
#include <SPI.h>
#include "common.h"
#include "globals.h"
#include "dtc.h"
#include "debugger.h"

// === Setup: MCP2515 CS-Pin definieren ===
#define OBD2_CAN_CS_PIN 10
#define OBD2_OBD_REQUEST_ID 0x7DF
#define OBD2_OBD_RESPONSE_ID 0x7E8

MCP_CAN OBD_CAN(OBD2_CAN_CS_PIN);

static uint32_t lastQueryTime = 0;
static uint32_t lastRecvTime = 0;
static uint32_t lastSpeedMMperSec = 0;

#define OBD2_QUERY_INTERVAL 500 // alle 500ms

void Init_OBD2_CAN()
{
    if (OBD_CAN.begin(MCP_STD, CAN_500KBPS, MCP_16MHZ) != CAN_OK)
    {
        Serial.println("OBD2 CAN Init FAILED!");
        return;
    }

    OBD_CAN.setMode(MCP_NORMAL);
    delay(100);
    Serial.println("OBD2 CAN Init OK");
}

uint32_t Process_OBD2_CAN_Speed()
{
    if (millis() - lastQueryTime < OBD2_QUERY_INTERVAL)
        return 0;

    lastQueryTime = millis();

    // Anfrage: 01 0D → Geschwindigkeit
    byte obdRequest[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00};
    byte sendStat = OBD_CAN.sendMsgBuf(OBD2_OBD_REQUEST_ID, 0, 8, obdRequest);

    if (sendStat != CAN_OK)
    {
        MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
        Debug_pushMessage("OBD2_CAN: send failed (%d)\n", sendStat);
        return 0;
    }

    unsigned long rxId;
    byte len = 0;
    byte rxBuf[8];
    uint32_t timeout = millis() + 100;

    while (millis() < timeout)
    {
        if (OBD_CAN.checkReceive() == CAN_MSGAVAIL)
        {
            OBD_CAN.readMsgBuf(&rxId, &len, rxBuf);
            if ((rxId & 0xFFF8) == OBD2_OBD_RESPONSE_ID && rxBuf[1] == 0x0D)
            {
                MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false); // alles ok

                uint8_t speed_kmh = rxBuf[3];
                uint32_t speed_mm_per_sec = (uint32_t)speed_kmh * 1000000 / 3600;
                uint32_t dt = millis() - lastRecvTime;
                lastRecvTime = millis();
                lastSpeedMMperSec = speed_mm_per_sec;

                Debug_pushMessage("OBD2_CAN: %d km/h (%lu mm/s)\n", speed_kmh, speed_mm_per_sec);
                return (speed_mm_per_sec * dt) / 1000;
            }
        }
    }

    // Keine Antwort erhalten
    MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
    Debug_pushMessage("OBD2_CAN: no response within timeout\n");
    return 0;
}