/* vesc_can.c — VESC CAN TWAI driver (bd-66hx)
 *
 * Receives VESC STATUS/4/5 frames via TWAI, proxies to Orin over serial.
 * Transmits SET_RPM commands from Orin drive requests.
 */

#include "vesc_can.h"
#include "orin_serial.h"
#include "config.h"
#include "driver/twai.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <string.h>

static const char *TAG = "vesc_can";

vesc_state_t g_vesc[2] = {0};

/* Index for a given VESC node ID: 0=VESC_ID_A, 1=VESC_ID_B */
static int vesc_idx(uint8_t id)
{
    if (id == VESC_ID_A) return 0;
    if (id == VESC_ID_B) return 1;
    return -1;
}

void vesc_can_init(void)
{
    twai_general_config_t gcfg = TWAI_GENERAL_CONFIG_DEFAULT(
        (gpio_num_t)VESC_CAN_TX_GPIO,
        (gpio_num_t)VESC_CAN_RX_GPIO,
        TWAI_MODE_NORMAL);
    gcfg.rx_queue_len = VESC_CAN_RX_QUEUE;

    twai_timing_config_t  tcfg = TWAI_TIMING_CONFIG_500KBITS();
    twai_filter_config_t  fcfg = TWAI_FILTER_CONFIG_ACCEPT_ALL();

    ESP_ERROR_CHECK(twai_driver_install(&gcfg, &tcfg, &fcfg));
    ESP_ERROR_CHECK(twai_start());
    ESP_LOGI(TAG, "TWAI init OK: tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO);
}

void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
{
    uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | vesc_id;
    twai_message_t msg = {
        .extd            = 1,
        .identifier      = ext_id,
        .data_length_code = 4,
    };
    uint32_t u = (uint32_t)erpm;
    msg.data[0] = (uint8_t)(u >> 24u);
    msg.data[1] = (uint8_t)(u >> 16u);
    msg.data[2] = (uint8_t)(u >>  8u);
    msg.data[3] = (uint8_t)(u);
    twai_transmit(&msg, pdMS_TO_TICKS(5));
}

void vesc_can_rx_task(void *arg)
{
    QueueHandle_t tx_q = (QueueHandle_t)arg;
    twai_message_t msg;

    for (;;) {
        if (twai_receive(&msg, pdMS_TO_TICKS(50)) != ESP_OK) {
            continue;
        }
        if (!msg.extd) {
            continue;  /* ignore standard frames */
        }

        uint8_t pkt_type = (uint8_t)(msg.identifier >> 8u);
        uint8_t vesc_id  = (uint8_t)(msg.identifier & 0xFFu);
        int idx = vesc_idx(vesc_id);
        if (idx < 0) {
            continue;  /* not our VESC */
        }

        uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
        vesc_state_t *s = &g_vesc[idx];

        switch (pkt_type) {
        case VESC_PKT_STATUS:
            if (msg.data_length_code < 8u) { break; }
            s->erpm = (int32_t)(
                ((uint32_t)msg.data[0] << 24u) | ((uint32_t)msg.data[1] << 16u) |
                ((uint32_t)msg.data[2] <<  8u) |  (uint32_t)msg.data[3]);
            s->current_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
            s->last_rx_ms  = now_ms;
            /* Proxy to Orin: voltage from STATUS_5 (may be zero until received) */
            {
                uint8_t ttype = (vesc_id == VESC_ID_A) ? TELEM_VESC_LEFT : TELEM_VESC_RIGHT;
                /* voltage_mv: V×10 → mV (/10 * 1000 = *100); current_ma: A×10 → mA (*100) */
                uint16_t vmv = (uint16_t)((int32_t)s->voltage_x10 * 100);
                int16_t  ima = (int16_t)((int32_t)s->current_x10 * 100);
                orin_send_vesc(tx_q, ttype, s->erpm, vmv, ima,
                               (uint16_t)s->temp_mot_x10);
            }
            break;

        case VESC_PKT_STATUS_4:
            if (msg.data_length_code < 6u) { break; }
            /* T_fet×10, T_mot×10, I_in×10 */
            s->temp_mot_x10 = (int16_t)(((uint16_t)msg.data[2] << 8u) | msg.data[3]);
            break;

        case VESC_PKT_STATUS_5:
            if (msg.data_length_code < 6u) { break; }
            /* int32 tacho (ignored), int16 V_in×10 */
            s->voltage_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
            break;

        default:
            break;
        }
    }
}