/*
 * can_manager.c
 *
 *  Created on: Sep 16, 2025
 *      Author: herli
 */
#include "id.h"
#include "can_manager.h"
#include "can_db_symbols.h"   // we’ll reference message defs here
#include "can_encode.h"
#include "timeouts.h"
#include "ford_immo.h"
#include <string.h>

static CanTxFn s_tx = NULL;

/* Handler registry keyed by message DEF, not by ID */
#ifndef CAN_MANAGER_MAX_HANDLERS
#define CAN_MANAGER_MAX_HANDLERS 8
#endif
typedef struct {
    const CanMessageDef *msg;
    CanRxHandler         handler;
} _HandlerEntry;

static _HandlerEntry s_handlers[CAN_MANAGER_MAX_HANDLERS];
static uint8_t s_handlers_cnt = 0;


/* for request */
volatile uint16_t EMPF2_WORDS[4] = {0,0,0,0};


bool can_manager_register_handler_msg(const CanMessageDef *msg, CanRxHandler handler)
{
    if (!msg || !handler) return false;
    for (uint8_t i = 0; i < s_handlers_cnt; ++i) {
        if (s_handlers[i].msg == msg) { s_handlers[i].handler = handler; return true; }
    }
    if (s_handlers_cnt >= CAN_MANAGER_MAX_HANDLERS) return false;
    s_handlers[s_handlers_cnt++] = (_HandlerEntry){ msg, handler };
    return true;
}

static CanRxHandler _find_override_by_msg(const CanMessageDef *msg)
{
    for (uint8_t i = 0; i < s_handlers_cnt; ++i)
        if (s_handlers[i].msg == msg) return s_handlers[i].handler;
    return NULL;
}

void can_manager_init(CanTxFn tx_fn) { s_tx = tx_fn; }


void can_manager_boot(void)
{
    if (!s_tx) return;
    size_t n = 0;
    const CanMessageDef* const* db = can_db_all_ptr(&n);
    for (size_t i = 0; i < n; ++i) {
        const CanMessageDef *m = db[i];
        if (m->dir == CAN_DIR_TX && m->send_on_boot) {
            uint8_t buf[8] = {0};
            can_encode_message(m, buf);
            s_tx(m->can_id, buf, m->dlc);
        }
    }


}

void can_manager_on_rx(uint16_t can_id, const uint8_t data[8], uint8_t dlc)
{
    (void)dlc;
    if (!s_tx) return;

    const CanMessageDef *msg = can_db_find(can_id, CAN_DIR_RX);
    if (!msg) return;

    if (msg->symbol_count && msg->symbols)
        can_decode_message(msg, data);

    if(msg->can_id == MSG_ID_SEND1.can_id){ //si es el command, reseteo
        Timeout_ResetByIndex(15, TIM16->CNT);
    }

    CanRxHandler h = msg->rx_handler ? msg->rx_handler : _find_override_by_msg(msg);
    if (h) h(msg, data, s_tx);
}

bool can_manager_send_msg(const CanMessageDef *msg)
{
    if (!s_tx || !msg || msg->dir != CAN_DIR_TX) return false;
    uint8_t buf[8] = {0};
    can_encode_message(msg, buf);
    return s_tx(msg->can_id, buf, msg->dlc);
}

bool can_manager_send(uint16_t can_id)
{
    const CanMessageDef *msg = can_db_find(can_id, CAN_DIR_TX);
    if (!msg) return false;
    return can_manager_send_msg(msg);
}

/* ---------- Startup and request trigger handlers (reply by DEF) ---------- */
static const CanMessageDef *s_startup_reply_msg = NULL;  // -> MSG_ID_EMPF3
static const CanMessageDef *s_request_reply_msg = NULL;  // -> MSG_ID_EMPF2

extern void    Timeout_ResetByIndex(uint8_t idx, uint16_t now);
extern TIM_HandleTypeDef* htim16;

void can_manager_set_startup_reply_msg(const CanMessageDef *reply_msg)
{
    s_startup_reply_msg = reply_msg;
}
uint8_t startup = 1;

/*
void can_manager_rx_startup_trigger(const CanMessageDef *msg, const uint8_t in[8], CanTxFn tx)
{
    (void)msg; (void)in;
    if (!tx || !s_startup_reply_msg) return;

    if (startup) {
        //Send by DEFINITION: use reply_msg’s ID & DLC and (if present) its tx_template
        uint8_t buf[8] = {0};
        can_encode_message(s_startup_reply_msg, buf);            // template + symbols
        tx(s_startup_reply_msg->can_id, buf, s_startup_reply_msg->dlc);

        startup = (startup < 3) ? (startup + 1) : 0;
        Timeout_ResetByIndex(11, (uint16_t)htim16->Instance->CNT);
    }else{
    	//register for answering next messages, on end should also reregister for the boot sequence right??
    	can_manager_register_handler_msg(&MSG_STARTUP_TRIGGER_RX, can_manager_rx_boot_reply_always);
    }
    Timeout_ResetByIndex(15, (uint16_t)htim16->Instance->CNT);
}
*/
// Always send the boot message (EMPF3) as a reply, regardless of 'startup'
uint8_t count = 0;

uint8_t startup_finished = 0;
#if defined(T06301)
uint8_t startup_count = 9;
#elif defined(T06209) || defined(T06216) || defined(T06211)
uint8_t startup_count = 0;
uint8_t startup_sent_count = 0;

#else
#endif
extern int watchdog_active;
void can_manager_rx_boot_reply_always(const CanMessageDef *msg, const uint8_t in[8], CanTxFn tx)
{
    (void)msg; (void)in;
    if (!tx || !s_startup_reply_msg) return;

#if defined(T06301)
    if(!watchdog_active){return;}

	if(count < 2){
		count++;
	}

    startup = 0;
    startup_count = 0;
    Timeout_ResetByIndex(18, TIM16->CNT);  // first callback in 40 ms
    Timeout_StopByIndex(21);  // first callback in 40 ms
    if(count > 1){
        Timeout_StartIfStopped(20, TIM16->CNT - 500);
        //no deberia hacer na esto pq bootea started.
    }
#elif defined(T06209) || defined(T06216) || defined(T06211)
    if (!startup_sent_count){return;}

    if(startup_finished){
    	startup_count++; //probar esto
    	startup_count %= 3;
    	if (!startup_count){
    		Timeout_ResetByIndex(18, TIM16->CNT);  // re-arm for another 40ms
    	}
    }else{
    	if(startup_count){
    		startup_count--;
    	}else{
    		startup_finished = 1;
    	}
    }
#else
#endif
}

void can_manager_set_request_reply_msg(const CanMessageDef *reply_msg)
{
    s_request_reply_msg = reply_msg;   // set to &MSG_ID_EMPF2
}

volatile uint8_t s_empf2_pending = 0;

bool can_manager_empf2_has_pending(void)   { return s_empf2_pending != 0; }
void can_manager_empf2_clear_pending(void) { s_empf2_pending = 0; }

void can_manager_rx_address_request(const CanMessageDef *msg, const uint8_t in[8], CanTxFn tx)
{
    (void)msg; (void)tx;

    // Parse 4 big-endian request words
    uint16_t req[4];
    for (int i = 0; i < 4; ++i)
        req[i] = (uint16_t)((in[2*i] << 8) | in[2*i+1]);

    for (int i = 0; i < 4; ++i) {
        uint16_t a = req[i];
        int32_t  raw = 0;          // signed accumulator before clamping
        int      found = 0;

        for (size_t j = 0; j < CAN_ANSWERS_COUNT; ++j) {
            const CanAddressEntry *E = &CAN_ANSWERS[j];
            if (E->addr != a) continue;
            found = 1;

            if (E->storage == CAN_STORE_FLOAT) {
                // physical float with optional scale
                const float phys = *(const float*)E->ptr;
                const float f = E->scale ? E->scale->factor : 1.0f;
                const float o = E->scale ? E->scale->offset : 0.0f;
                float scaled = (phys - o) / f;
                //if (!isfinite(scaled)) scaled = 0.0f;
                raw = (int32_t)llroundf(scaled);
            } else {
                // integer-backed: read as-is (no scaling)
                switch (E->storage) {
                    case CAN_STORE_U32: raw = (int32_t)*(const uint32_t*)E->ptr; break;
                    case CAN_STORE_S32: raw = (int32_t)*(const int32_t *)E->ptr; break;
                    case CAN_STORE_U16: raw = (int32_t)*(const uint16_t*)E->ptr; break;
                    case CAN_STORE_S16: raw = (int32_t)*(const int16_t *)E->ptr; break;
                    case CAN_STORE_U8:  raw = (int32_t)*(const uint8_t *)E->ptr; break;
                    case CAN_STORE_S8:  raw = (int32_t)*(const int8_t  *)E->ptr; break;
                    default:            raw = 0; break;
                }
            }

            // Clamp to 16-bit range according to raw_type
            if (E->raw_type == CAN_SYM_SX) {
                if (raw < -32768) raw = -32768;
                if (raw >  32767) raw =  32767;
            } else {
                if (raw < 0)      raw = 0;
                if (raw > 65535)  raw = 65535;
            }
            break;
        }

        uint16_t raw16 = (uint16_t)raw;  // two's complement for signed values
        // Pack little-endian into EMPF2_BYTES (shared TX buffer for MSG_ID_EMPF2)
        EMPF2_BYTES[2*i + 0] = (uint8_t)(raw16 & 0xFF);
        EMPF2_BYTES[2*i + 1] = (uint8_t)(raw16 >> 8);
    }

    // Mark pending and start 60ms window only if not already running
    s_empf2_pending = 1;
    Timeout_StartIfStopped(17, (uint16_t)TIM16->CNT);
}