/* orin_can.c — Orin↔FC CAN protocol driver (Issue #674)
 *
 * Receives high-level drive/mode/estop commands from Orin over CAN.
 * Broadcasts FC status and VESC telemetry back to Orin at ORIN_TLM_HZ.
 *
 * Registered as the standard-ID callback with can_driver via
 * can_driver_set_std_cb(orin_can_on_frame).
 *
 * Balance independence: if Orin link drops (orin_can_is_alive() == false),
 * main loop stops injecting Orin commands and the balance PID holds
 * upright in-place.  No action required here — the safety is in main.c.
 */

#include "orin_can.h"
#include "can_driver.h"
#include "stm32f7xx_hal.h"
#include <string.h>

volatile OrinCanState      orin_can_state;
volatile orin_can_led_cmd_t orin_can_led_override;
volatile uint8_t            orin_can_led_updated;

static uint32_t s_tlm_tick;
static uint32_t s_imu_tick;
static uint32_t s_baro_tick;

void orin_can_init(void)
{
    memset((void *)&orin_can_state,      0, sizeof(orin_can_state));
    memset((void *)&orin_can_led_override, 0, sizeof(orin_can_led_override));
    orin_can_led_updated = 0u;
    /* Pre-wind so first broadcasts fire on the first eligible tick */
    s_tlm_tick  = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ));
    s_imu_tick  = (uint32_t)(-(uint32_t)(1000u / ORIN_IMU_TLM_HZ));
    s_baro_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_BARO_TLM_HZ));
    can_driver_set_std_cb(orin_can_on_frame);
}

void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len)
{
    /* Any frame from Orin refreshes the heartbeat */
    orin_can_state.last_rx_ms = HAL_GetTick();

    switch (std_id) {
    case ORIN_CAN_ID_HEARTBEAT:
        /* Heartbeat payload (sequence counter) ignored — timestamp is enough */
        break;

    case ORIN_CAN_ID_DRIVE:
        /* int16 speed (BE), int16 steer (BE) */
        if (len < 4u) { break; }
        orin_can_state.speed = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
        orin_can_state.steer = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
        orin_can_state.drive_updated = 1u;
        break;

    case ORIN_CAN_ID_MODE:
        /* uint8 mode */
        if (len < 1u) { break; }
        orin_can_state.mode = data[0];
        orin_can_state.mode_updated = 1u;
        break;

    case ORIN_CAN_ID_ESTOP:
        /* uint8: 1 = assert estop, 0 = clear estop */
        if (len < 1u) { break; }
        if (data[0] != 0u) {
            orin_can_state.estop_req = 1u;
        } else {
            orin_can_state.estop_clear_req = 1u;
        }
        break;

    case ORIN_CAN_ID_LED_CMD:
        /* pattern(u8), brightness(u8), duration_ms(u16 LE) — Issue #685 */
        if (len < 4u) { break; }
        orin_can_led_override.pattern     = data[0];
        orin_can_led_override.brightness  = data[1];
        orin_can_led_override.duration_ms = (uint16_t)((uint16_t)data[2] |
                                             ((uint16_t)data[3] << 8u));
        orin_can_led_updated = 1u;
        break;

    case ORIN_CAN_ID_PID_SET:
        /* kp_x100(u16 BE), ki_x100(u16 BE), kd_x100(u16 BE) -- Issue #693 */
        if (len < 6u) { break; }
        orin_can_state.pid_kp_x100 = (uint16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
        orin_can_state.pid_ki_x100 = (uint16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
        orin_can_state.pid_kd_x100 = (uint16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
        orin_can_state.pid_updated = 1u;
        break;

    default:
        break;
    }
}

void orin_can_send_pid_ack(float kp, float ki, float kd)
{
    orin_can_fc_pid_ack_t ack;
    if (kp < 0.0f) kp = 0.0f;
    if (ki < 0.0f) ki = 0.0f;
    if (kd < 0.0f) kd = 0.0f;
    ack.kp_x100 = (uint16_t)(kp * 100.0f + 0.5f);
    ack.ki_x100 = (uint16_t)(ki * 100.0f + 0.5f);
    ack.kd_x100 = (uint16_t)(kd * 100.0f + 0.5f);
    uint8_t buf[6];
    memcpy(buf, &ack, sizeof(ack));
    can_driver_send_std(ORIN_CAN_ID_FC_PID_ACK, buf, (uint8_t)sizeof(ack));
}

bool orin_can_is_alive(uint32_t now_ms)
{
    if (orin_can_state.last_rx_ms == 0u) {
        return false;
    }
    return (now_ms - orin_can_state.last_rx_ms) < ORIN_HB_TIMEOUT_MS;
}

void orin_can_broadcast(uint32_t now_ms,
                        const orin_can_fc_status_t *status,
                        const orin_can_fc_vesc_t   *vesc)
{
    if ((now_ms - s_tlm_tick) < (1000u / ORIN_TLM_HZ)) {
        return;
    }
    s_tlm_tick = now_ms;

    uint8_t buf[8];

    /* FC_STATUS (0x400): 8 bytes */
    memcpy(buf, status, sizeof(orin_can_fc_status_t));
    can_driver_send_std(ORIN_CAN_ID_FC_STATUS, buf, (uint8_t)sizeof(orin_can_fc_status_t));

    /* FC_VESC (0x401): 8 bytes */
    memcpy(buf, vesc, sizeof(orin_can_fc_vesc_t));
    can_driver_send_std(ORIN_CAN_ID_FC_VESC, buf, (uint8_t)sizeof(orin_can_fc_vesc_t));
}

void orin_can_broadcast_imu(uint32_t now_ms,
                            const orin_can_fc_imu_t *imu_tlm)
{
    if ((now_ms - s_imu_tick) < (1000u / ORIN_IMU_TLM_HZ)) {
        return;
    }
    s_imu_tick = now_ms;

    uint8_t buf[8];
    memcpy(buf, imu_tlm, sizeof(orin_can_fc_imu_t));
    can_driver_send_std(ORIN_CAN_ID_FC_IMU, buf, (uint8_t)sizeof(orin_can_fc_imu_t));
}

void orin_can_broadcast_baro(uint32_t now_ms,
                             const orin_can_fc_baro_t *baro_tlm)
{
    if (baro_tlm == NULL) return;
    if ((now_ms - s_baro_tick) < (1000u / ORIN_BARO_TLM_HZ)) {
        return;
    }
    s_baro_tick = now_ms;

    uint8_t buf[8];
    memcpy(buf, baro_tlm, sizeof(orin_can_fc_baro_t));
    can_driver_send_std(ORIN_CAN_ID_FC_BARO, buf, (uint8_t)sizeof(orin_can_fc_baro_t));
}

void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state)
{
    orin_can_fc_btn_t btn;
    btn.event_id      = event_id;
    btn.balance_state = balance_state;
    uint8_t buf[2];
    memcpy(buf, &btn, sizeof(orin_can_fc_btn_t));
    can_driver_send_std(ORIN_CAN_ID_FC_BTN, buf, (uint8_t)sizeof(orin_can_fc_btn_t));
}