/* 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 <inttypes.h>
#include <string.h>

static const char *TAG = "vesc_can";

vesc_state_t g_vesc[2] = {0};
volatile bool g_twai_bus_off = false;

/* 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;
    gcfg.tx_queue_len = 5;

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

    ESP_LOGI(TAG, "TWAI: installing driver tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO);
    esp_err_t err = twai_driver_install(&gcfg, &tcfg, &fcfg);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "TWAI install failed (0x%x) — CAN disabled", err);
        g_twai_bus_off = true;
        return;
    }

    err = twai_start();
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "TWAI start failed (0x%x) — CAN disabled", err);
        g_twai_bus_off = true;
        return;
    }
    vTaskDelay(pdMS_TO_TICKS(200));
    ESP_LOGI(TAG, "TWAI: started OK — bus active");
}

void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
{
    if (g_twai_bus_off) { return; }
    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);
    ESP_LOGD(TAG, "send_rpm vesc_id=%u erpm=%" PRId32 " ext_id=0x%08" PRIx32, vesc_id, erpm, ext_id);
    esp_err_t err = twai_transmit(&msg, pdMS_TO_TICKS(5));
    if (err != ESP_OK) {
        ESP_LOGW(TAG, "twai_transmit failed vesc_id=%u err=0x%x", vesc_id, err);
    }
}

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

    for (;;) {
        esp_err_t rx_err = twai_receive(&msg, pdMS_TO_TICKS(50));
        if (rx_err != ESP_OK) {
            if (rx_err != ESP_ERR_TIMEOUT) {
                ESP_LOGW(TAG, "twai_receive err=0x%x", rx_err);
            }
            twai_status_info_t si;
            if (twai_get_status_info(&si) == ESP_OK) {
                /* Mark bus-off for ANY non-running state so vesc_can_send_rpm
                 * won't flood with failed transmits during recovery. */
                g_twai_bus_off = (si.state != TWAI_STATE_RUNNING);

                if (si.state == TWAI_STATE_BUS_OFF) {
                    ESP_LOGE(TAG, "TWAI BUS OFF tx_err=%lu rx_err=%lu — recovering",
                             (unsigned long)si.tx_error_counter, (unsigned long)si.rx_error_counter);
                    twai_initiate_recovery();
                    vTaskDelay(pdMS_TO_TICKS(1000));
                } else if (si.state == TWAI_STATE_STOPPED) {
                    esp_err_t serr = twai_start();
                    if (serr == ESP_OK) {
                        g_twai_bus_off = false;
                        ESP_LOGI(TAG, "TWAI recovered — bus active");
                    } else {
                        ESP_LOGE(TAG, "TWAI restart failed 0x%x — backing off", serr);
                        vTaskDelay(pdMS_TO_TICKS(2000));
                    }
                }
                /* TWAI_STATE_RECOVERING: initiation already called, just wait. */
            }
            continue;
        }
        ESP_LOGI(TAG, "twai_receive OK id=0x%08" PRIx32 " dlc=%u", msg.identifier, msg.data_length_code);
        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;
        }
    }
}