#include "rs00_mit.h"

#include "esp_check.h"
#include "esp_log.h"
#include "esp_twai.h"
#include "esp_twai_onchip.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"

static const char *TAG = "rs00_mit";

typedef struct {
    uint32_t identifier;
    uint8_t data_length;
    bool is_extended;
    uint8_t data[TWAI_FRAME_MAX_LEN];
} rs00_rx_frame_t;

static twai_node_handle_t g_twai_node = NULL;
static QueueHandle_t g_rx_queue = NULL;

static float clampf(float value, float min_value, float max_value)
{
    if (value < min_value) {
        return min_value;
    }
    if (value > max_value) {
        return max_value;
    }
    return value;
}

static uint32_t float_to_uint(float value, float min_value, float max_value, uint8_t bits)
{
    const float clipped = clampf(value, min_value, max_value);
    const float span = max_value - min_value;
    const uint32_t max_int = (1u << bits) - 1u;
    return (uint32_t)((clipped - min_value) * (float)max_int / span + 0.5f);
}

static float uint_to_float(uint32_t value, float min_value, float max_value, uint8_t bits)
{
    const float span = max_value - min_value;
    const uint32_t max_int = (1u << bits) - 1u;
    return ((float)value) * span / (float)max_int + min_value;
}

static esp_err_t transmit_standard(uint32_t identifier, const uint8_t data[8])
{
    twai_frame_t frame = {
        .header = {
            .id = identifier,
            .dlc = 8,
            .ide = 0,
            .rtr = 0,
            .fdf = 0,
        },
        .buffer = (uint8_t *)data,
        .buffer_len = 8,
    };
    esp_err_t err = twai_node_transmit(g_twai_node, &frame, 3);
    if (err != ESP_OK) {
        return err;
    }
    // 每帧等待发送完成，保证调用方栈上的 8 字节缓冲区在驱动使用期间有效。
    return twai_node_transmit_wait_all_done(g_twai_node, 3);
}

static bool IRAM_ATTR rs00_rx_done_callback(
    twai_node_handle_t handle,
    const twai_rx_done_event_data_t *event_data,
    void *user_context)
{
    (void)event_data;
    (void)user_context;
    rs00_rx_frame_t received = {0};
    twai_frame_t frame = {
        .buffer = received.data,
        .buffer_len = sizeof(received.data),
    };
    BaseType_t higher_priority_task_woken = pdFALSE;

    if (twai_node_receive_from_isr(handle, &frame) == ESP_OK) {
        received.identifier = frame.header.id;
        received.data_length = (uint8_t)frame.header.dlc;
        received.is_extended = frame.header.ide != 0;
        xQueueSendFromISR(g_rx_queue, &received, &higher_priority_task_woken);
    }
    return higher_priority_task_woken == pdTRUE;
}

static bool IRAM_ATTR rs00_state_change_callback(
    twai_node_handle_t handle,
    const twai_state_change_event_data_t *event_data,
    void *user_context)
{
    (void)handle;
    (void)user_context;
    if (event_data->new_sta == TWAI_ERROR_BUS_OFF) {
        ESP_EARLY_LOGE(TAG, "TWAI 进入 BUS-OFF，请检查 TJA1050T、终端电阻和 CANH/CANL");
    }
    return false;
}

esp_err_t rs00_mit_init(gpio_num_t tx_gpio, gpio_num_t rx_gpio)
{
    if (g_twai_node != NULL) {
        return ESP_ERR_INVALID_STATE;
    }

    g_rx_queue = xQueueCreate(32, sizeof(rs00_rx_frame_t));
    if (g_rx_queue == NULL) {
        return ESP_ERR_NO_MEM;
    }

    const twai_onchip_node_config_t node_config = {
        .io_cfg = {
            .tx = tx_gpio,
            .rx = rx_gpio,
            .quanta_clk_out = GPIO_NUM_NC,
            .bus_off_indicator = GPIO_NUM_NC,
        },
        .bit_timing = {
            .bitrate = 1000000,
        },
        // 限制单帧失败重试次数，断线时避免某一帧永久占住发送队列。
        .fail_retry_cnt = 3,
        .tx_queue_depth = 32,
    };
    ESP_RETURN_ON_ERROR(twai_new_node_onchip(&node_config, &g_twai_node), TAG, "创建 TWAI node 失败");

    const twai_mask_filter_config_t accept_standard_frames = {
        .id = 0,
        .mask = 0,
        .is_ext = false,
        .no_classic = false,
        .no_fd = true,
    };
    ESP_RETURN_ON_ERROR(
        twai_node_config_mask_filter(g_twai_node, 0, &accept_standard_frames),
        TAG,
        "配置 TWAI 接收过滤器失败");

    const twai_event_callbacks_t callbacks = {
        .on_rx_done = rs00_rx_done_callback,
        .on_state_change = rs00_state_change_callback,
    };
    ESP_RETURN_ON_ERROR(
        twai_node_register_event_callbacks(g_twai_node, &callbacks, NULL),
        TAG,
        "注册 TWAI 回调失败");
    ESP_RETURN_ON_ERROR(twai_node_enable(g_twai_node), TAG, "启用 TWAI node 失败");

    ESP_LOGI(TAG, "TWAI 初始化完成: TX=%d RX=%d bitrate=1Mbps", tx_gpio, rx_gpio);
    return ESP_OK;
}

esp_err_t rs00_mit_send_enable(uint8_t motor_id)
{
    const uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};
    return transmit_standard(motor_id, data);
}

esp_err_t rs00_mit_send_stop(uint8_t motor_id)
{
    const uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};
    return transmit_standard(motor_id, data);
}

esp_err_t rs00_mit_send_clear_fault(uint8_t motor_id)
{
    const uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB};
    return transmit_standard(motor_id, data);
}

esp_err_t rs00_mit_send_set_mode(uint8_t motor_id, uint8_t mode)
{
    uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, mode, 0xFC};
    return transmit_standard(motor_id, data);
}

esp_err_t rs00_mit_send_control(
    uint8_t motor_id,
    float position_rad,
    float velocity_rad_s,
    float kp,
    float kd,
    float torque_ff_nm)
{
    const uint32_t p = float_to_uint(position_rad, -12.57f, 12.57f, 16);
    const uint32_t v = float_to_uint(velocity_rad_s, -33.0f, 33.0f, 12);
    const uint32_t kp_u = float_to_uint(kp, 0.0f, 500.0f, 12);
    const uint32_t kd_u = float_to_uint(kd, 0.0f, 5.0f, 12);
    const uint32_t t = float_to_uint(torque_ff_nm, -14.0f, 14.0f, 12);

    uint8_t data[8] = {
        (uint8_t)(p >> 8),
        (uint8_t)(p & 0xFFu),
        (uint8_t)(v >> 4),
        (uint8_t)(((v & 0x0Fu) << 4) | (kp_u >> 8)),
        (uint8_t)(kp_u & 0xFFu),
        (uint8_t)(kd_u >> 4),
        (uint8_t)(((kd_u & 0x0Fu) << 4) | (t >> 8)),
        (uint8_t)(t & 0xFFu),
    };
    return transmit_standard(motor_id, data);
}

bool rs00_mit_poll_feedback(rs00_feedback_t *feedback, uint32_t timeout_ms)
{
    rs00_rx_frame_t frame = {0};
    if (xQueueReceive(g_rx_queue, &frame, pdMS_TO_TICKS(timeout_ms)) != pdTRUE) {
        return false;
    }
    if (frame.is_extended || frame.data_length != 8) {
        return false;
    }

    const uint8_t *data = frame.data;
    const uint32_t p = ((uint32_t)data[1] << 8) | data[2];
    const uint32_t v = ((uint32_t)data[3] << 4) | (data[4] >> 4);
    const uint32_t t = ((uint32_t)(data[4] & 0x0F) << 8) | data[5];
    const uint32_t temp_raw = ((uint32_t)(data[6] & 0x0F) << 8) | data[7];

    feedback->motor_id = data[0];
    feedback->position_rad = uint_to_float(p, -12.57f, 12.57f, 16);
    feedback->velocity_rad_s = uint_to_float(v, -33.0f, 33.0f, 12);
    feedback->torque_nm = uint_to_float(t, -14.0f, 14.0f, 12);
    feedback->mode_state = (data[6] >> 6) & 0x03u;
    feedback->fault = (data[6] & 0x20u) != 0;
    feedback->warning = (data[6] & 0x10u) != 0;
    feedback->temperature_c = (float)temp_raw * 0.1f;
    feedback->last_feedback_ms = (uint32_t)(esp_timer_get_time() / 1000ULL);
    return true;
}
