/* vesc_can.c — VESC CAN protocol driver (Issue #674)
 *
 * Registers vesc_can_on_frame as the extended-frame callback with can_driver.
 * VESC uses 29-bit arb IDs: (pkt_type << 8) | vesc_node_id.
 * All wire values are big-endian (VESC FW 6.x).
 */

#include "vesc_can.h"
#include "can_driver.h"
#include "jlink.h"
#include "stm32f7xx_hal.h"
#include <string.h>
#include <stddef.h>

static uint8_t      s_id_left;
static uint8_t      s_id_right;
static vesc_state_t s_state[2];   /* [0] = left, [1] = right */
static uint32_t     s_tlm_tick;

static vesc_state_t *state_for_id(uint8_t vesc_id)
{
    if (vesc_id == s_id_left)  return &s_state[0];
    if (vesc_id == s_id_right) return &s_state[1];
    return NULL;
}

void vesc_can_init(uint8_t id_left, uint8_t id_right)
{
    s_id_left  = id_left;
    s_id_right = id_right;
    memset(s_state, 0, sizeof(s_state));
    /* Pre-wind so first send_tlm call fires immediately */
    s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / VESC_TLM_HZ));
    can_driver_set_ext_cb(vesc_can_on_frame);
}

void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm)
{
    /* arb_id = (VESC_PKT_SET_RPM << 8) | vesc_id */
    uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | (uint32_t)vesc_id;

    /* Payload: int32 RPM, big-endian */
    uint32_t urpm = (uint32_t)rpm;
    uint8_t  data[4];
    data[0] = (uint8_t)(urpm >> 24u);
    data[1] = (uint8_t)(urpm >> 16u);
    data[2] = (uint8_t)(urpm >>  8u);
    data[3] = (uint8_t)(urpm);

    can_driver_send_ext(ext_id, data, 4u);
}

void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len)
{
    uint8_t       pkt_type = (uint8_t)(ext_id >> 8u);
    uint8_t       vesc_id  = (uint8_t)(ext_id & 0xFFu);
    vesc_state_t *s        = state_for_id(vesc_id);

    if (s == NULL) {
        return;
    }

    switch (pkt_type) {
    case VESC_PKT_STATUS:   /* 9: int32 RPM, int16 I×10, int16 duty×1000 (8 bytes) */
        if (len < 8u) { break; }
        s->rpm = (int32_t)(((uint32_t)data[0] << 24u) |
                           ((uint32_t)data[1] << 16u) |
                           ((uint32_t)data[2] <<  8u) |
                            (uint32_t)data[3]);
        s->current_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
        s->duty_x1000  = (int16_t)(((uint16_t)data[6] << 8u) | (uint16_t)data[7]);
        s->last_rx_ms  = HAL_GetTick();
        break;

    case VESC_PKT_STATUS_4: /* 16: int16 T_fet×10, T_mot×10, I_in×10 (6 bytes) */
        if (len < 6u) { break; }
        s->temp_fet_x10   = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
        s->temp_motor_x10 = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
        s->current_in_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
        break;

    case VESC_PKT_STATUS_5: /* 27: int32 tacho (ignored), int16 V_in×10 (6 bytes) */
        if (len < 6u) { break; }
        /* bytes [0..3] = odometer tachometer — not stored */
        s->voltage_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
        break;

    default:
        break;
    }
}

bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out)
{
    if (out == NULL) {
        return false;
    }
    vesc_state_t *s = state_for_id(vesc_id);
    if (s == NULL || s->last_rx_ms == 0u) {
        return false;
    }
    memcpy(out, s, sizeof(vesc_state_t));
    return true;
}

bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms)
{
    vesc_state_t *s = state_for_id(vesc_id);
    if (s == NULL || s->last_rx_ms == 0u) {
        return false;
    }
    return (now_ms - s->last_rx_ms) < VESC_ALIVE_TIMEOUT_MS;
}

void vesc_can_send_tlm(uint32_t now_ms)
{
    if ((now_ms - s_tlm_tick) < (1000u / VESC_TLM_HZ)) {
        return;
    }
    s_tlm_tick = now_ms;

    jlink_tlm_vesc_state_t tlm;
    memset(&tlm, 0, sizeof(tlm));

    tlm.left_rpm           = s_state[0].rpm;
    tlm.right_rpm          = s_state[1].rpm;
    tlm.left_current_x10   = s_state[0].current_x10;
    tlm.right_current_x10  = s_state[1].current_x10;
    tlm.left_temp_x10      = s_state[0].temp_fet_x10;
    tlm.right_temp_x10     = s_state[1].temp_fet_x10;
    /* Use left voltage; fall back to right if left not yet received */
    tlm.voltage_x10        = s_state[0].voltage_x10
                                 ? s_state[0].voltage_x10
                                 : s_state[1].voltage_x10;
    tlm.left_fault         = s_state[0].fault_code;
    tlm.right_fault        = s_state[1].fault_code;
    tlm.left_alive         = vesc_can_is_alive(s_id_left,  now_ms) ? 1u : 0u;
    tlm.right_alive        = vesc_can_is_alive(s_id_right, now_ms) ? 1u : 0u;

    jlink_send_vesc_state_tlm(&tlm);
}