/**
 * @file    emm_v5.c
 * @brief   Emm_V5.0 步进闭环电机驱动器 - ESP-IDF 实现
 */

#include "emm_v5.h"
#include <string.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "freertos/event_groups.h"
#include "esp_timer.h"

static const char *TAG = "EMM_V5";

/* 运行时配置 (由 Emm_V5_Init 设置) */
static uart_port_t s_uart_port;

/* ============================================================
 * RX 中断/事件驱动状态
 *
 * 驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令
 * 运动到位会主动回传到位帧: 地址 + FD + 9F + 6B。
 * UART RX 由内部任务独占读取, 解析出到位帧后置位事件组位
 * (bit = 电机地址 - 1); 调用方用 Emm_V5_Wait_Arrival 事件等待,
 * 全部到位即返回 —— 不再轮询 S_TPOS/S_CPOS, 避免"没到位就下一步"。
 * ============================================================ */

#define EMM_V5_ARRIVAL_MAX_MOTORS   8

static EventGroupHandle_t s_arrival_evt    = NULL;
static QueueHandle_t      s_uart_evt_queue = NULL;
static TaskHandle_t       s_rx_task        = NULL;

/* 请求-响应 (网页手动读参数等) */
static SemaphoreHandle_t s_resp_mutex = NULL;
static struct {
    bool               active;
    uint8_t            addr;
    uint8_t            func;
    uint8_t           *out;
    uint8_t            max;
    uint8_t            len;
    SemaphoreHandle_t  done;
} s_resp;

/* RX 帧累积缓冲 (0x6B 为帧尾) */
static uint8_t s_rx_frame[64];
static uint8_t s_rx_frame_len;

static void emm_v5_rx_task(void *arg);   /* 前置声明 (Emm_V5_Init 中创建) */

/* ============================================================
 * 初始化
 * ============================================================ */

void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin)
{
    s_uart_port = port;

    uart_config_t uart_cfg = {
        .baud_rate  = EMM_V5_UART_BAUDRATE,
        .data_bits  = UART_DATA_8_BITS,
        .parity     = UART_PARITY_DISABLE,
        .stop_bits  = UART_STOP_BITS_1,
        .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };

    /* 事件队列: 中断驱动 RX (到位帧解析); 64 深度防突发丢事件 */
    s_uart_evt_queue = xQueueCreate(64, sizeof(uart_event_t));

    ESP_ERROR_CHECK(uart_driver_install(s_uart_port,
                                        EMM_V5_UART_BUF_SIZE * 2,
                                        EMM_V5_UART_BUF_SIZE * 2,
                                        16, &s_uart_evt_queue, 0));
    ESP_ERROR_CHECK(uart_param_config(s_uart_port, &uart_cfg));
    ESP_ERROR_CHECK(uart_set_pin(s_uart_port,
                                 tx_pin,
                                 rx_pin,
                                 UART_PIN_NO_CHANGE,
                                 UART_PIN_NO_CHANGE));

    /* 到位事件组 / 请求-响应信号量 / RX 任务 */
    if (s_arrival_evt == NULL) s_arrival_evt = xEventGroupCreate();
    if (s_resp_mutex == NULL)  s_resp_mutex  = xSemaphoreCreateMutex();
    if (s_resp.done == NULL)   s_resp.done   = xSemaphoreCreateBinary();
    if (s_rx_task == NULL) {
        xTaskCreate(emm_v5_rx_task, "emm_rx", 3072, NULL, 5, &s_rx_task);
    }

    ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d, 到位帧中断已启用)",
             s_uart_port, tx_pin, rx_pin);
}

/* ============================================================
 * 内部辅助
 * ============================================================ */

static inline void Emm_V5_Send(const uint8_t *cmd, uint8_t len)
{
    uart_write_bytes(s_uart_port, (const char *)cmd, len);
}

/* ============================================================
 * RX 中断/事件驱动
 *
 * 帧格式: 地址 + 功能码 + 数据... + 0x6B (校验字节)
 * 到位帧:  地址 + FD + 9F + 6B  (位置模式命令运动到位后主动回传)
 * ============================================================ */

/* 处理一帧完整数据 (以 0x6B 结尾) */
static void emm_v5_handle_frame(const uint8_t *f, uint8_t len)
{
    if (len < 4) return;

    /* 到位帧: 地址 + FD + 9F + 6B → 置位事件组 (bit = 地址 - 1) */
    if (f[1] == 0xFD && f[2] == 0x9F) {
        uint8_t addr = f[0];
        if (addr >= 1 && addr <= EMM_V5_ARRIVAL_MAX_MOTORS && s_arrival_evt) {
            xEventGroupSetBits(s_arrival_evt, (EventBits_t)(1u << (addr - 1)));
            ESP_LOGI(TAG, "到位帧: 电机%d", addr);
        }
    }

    /* 请求-响应匹配 (网页手动读参数等) */
    if (s_resp_mutex) {
        xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
        if (s_resp.active && f[0] == s_resp.addr && f[1] == s_resp.func) {
            uint8_t n = (len < s_resp.max) ? len : s_resp.max;
            if (s_resp.out && n) memcpy(s_resp.out, f, n);
            s_resp.len = n;
            s_resp.active = false;
            xSemaphoreGive(s_resp.done);
        }
        xSemaphoreGive(s_resp_mutex);
    }
}

/* UART RX 独占任务: 事件队列驱动, 解析帧并分派 */
static void emm_v5_rx_task(void *arg)
{
    uart_event_t evt;
    uint8_t buf[64];

    while (1) {
        if (xQueueReceive(s_uart_evt_queue, &evt, portMAX_DELAY) != pdTRUE)
            continue;

        if (evt.type == UART_DATA || evt.type == UART_FIFO_OVF ||
            evt.type == UART_BUFFER_FULL) {
            int len;
            while ((len = uart_read_bytes(s_uart_port, buf, sizeof(buf), 0)) > 0) {
                for (int i = 0; i < len; i++) {
                    s_rx_frame[s_rx_frame_len++] = buf[i];
                    if (s_rx_frame_len >= sizeof(s_rx_frame)) {
                        s_rx_frame_len = 0;   /* 缓冲溢出: 重新同步 */
                    }
                    if (buf[i] == 0x6B) {     /* 帧尾 */
                        emm_v5_handle_frame(s_rx_frame, s_rx_frame_len);
                        s_rx_frame_len = 0;
                    }
                }
            }
            if (evt.type == UART_FIFO_OVF || evt.type == UART_BUFFER_FULL) {
                /* 溢出只说明有部分字节被丢弃: 重置帧累积重新同步即可。
                   不能 uart_flush_input —— 那会把缓冲里仍有效的到位帧一并冲掉,
                   是偶发丢电机2/3/4 到位帧的根因之一 */
                s_rx_frame_len = 0;
            }
        }
    }
}

/* 内部: 发送命令并等待匹配的响应帧 (地址+功能码), 供读函数复用 */
static int emm_v5_request_response(const uint8_t *cmd, uint8_t cmd_len,
                                   uint8_t addr, uint8_t func,
                                   uint8_t *out, uint8_t max,
                                   uint32_t timeout_ms)
{
    if (!s_resp_mutex || !s_resp.done) return -1;

    xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
    if (s_resp.active) {
        xSemaphoreGive(s_resp_mutex);
        ESP_LOGW(TAG, "请求-响应忙, 放弃 (addr=%u func=0x%02X)", addr, func);
        return -1;
    }
    s_resp.active = true;
    s_resp.addr   = addr;
    s_resp.func   = func;
    s_resp.out    = out;
    s_resp.max    = max;
    s_resp.len    = 0;
    xSemaphoreGive(s_resp_mutex);

    xSemaphoreTake(s_resp.done, 0);   /* 清残留计数 */
    Emm_V5_Send(cmd, cmd_len);

    if (xSemaphoreTake(s_resp.done, pdMS_TO_TICKS(timeout_ms)) != pdTRUE) {
        xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
        s_resp.active = false;        /* 放弃等待 */
        xSemaphoreGive(s_resp_mutex);
        ESP_LOGW(TAG, "响应超时 (addr=%u func=0x%02X)", addr, func);
        return -1;
    }

    xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
    int n = s_resp.len;
    xSemaphoreGive(s_resp_mutex);
    return n;
}

/* ============================================================
 * 到位等待 (事件驱动)
 *
 * 注意: Emm_V5_Wait_Arrival 不消费到位位 (便于调用方分段等待时闪烁 LED);
 * 一段 motion 的等待结束后必须调用 Emm_V5_Arrival_Consume 清除到位位,
 * 防止残留位污染下一段 motion。
 * ============================================================ */

bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms)
{
    if (!s_arrival_evt) return false;

    EventBits_t expected = 0;
    for (uint8_t i = 0; i < count; i++) {
        uint8_t a = addrs[i];
        if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
            expected |= (EventBits_t)(1u << (a - 1));
    }
    if (expected == 0) return true;

    EventBits_t got = xEventGroupWaitBits(s_arrival_evt, expected,
                                          pdFALSE, pdTRUE,
                                          pdMS_TO_TICKS(timeout_ms));
    return ((got & expected) == expected);
}

void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count)
{
    if (!s_arrival_evt) return;

    EventBits_t expected = 0;
    for (uint8_t i = 0; i < count; i++) {
        uint8_t a = addrs[i];
        if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
            expected |= (EventBits_t)(1u << (a - 1));
    }
    if (expected)
        xEventGroupClearBits(s_arrival_evt, expected);
}

void Emm_V5_Arrival_Reset(void)
{
    if (!s_arrival_evt) return;
    xEventGroupClearBits(s_arrival_evt, 0xFF);   /* 清 addr 1~8 对应位 (bit=地址-1) */
}

/* 查询当前已置位的到位位 (调试用): 返回 addr1~8 对应位掩码 */
uint32_t Emm_V5_Arrival_Get_Bits(void)
{
    if (!s_arrival_evt) return 0;
    return xEventGroupGetBits(s_arrival_evt) & 0xFF;
}

/* ============================================================
 * 接收数据 (已废弃)
 * ============================================================ */

uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen)
{
    /* 已废弃: UART RX 由内部 RX 任务独占 (中断驱动), 此函数不再直接读 UART;
       网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp */
    (void)rxCmd;
    (void)maxLen;
    return 0;
}

/* ============================================================
 * 控制命令 (协议与 Arduino 版本完全一致)
 * ============================================================ */

void Emm_V5_Reset_CurPos_To_Zero(uint8_t addr)
{
    uint8_t cmd[4] = {addr, 0x0A, 0x6D, 0x6B};
    Emm_V5_Send(cmd, 4);
}

void Emm_V5_Reset_Clog_Pro(uint8_t addr)
{
    uint8_t cmd[4] = {addr, 0x0E, 0x52, 0x6B};
    Emm_V5_Send(cmd, 4);
}

/* 系统参数 → 功能码 (读取响应帧按 地址+功能码 匹配) */
static uint8_t emm_v5_sysparam_func(EmmSysParams_t s)
{
    switch (s) {
        case S_VER:   return 0x1F;
        case S_RL:    return 0x20;
        case S_PID:   return 0x21;
        case S_VBUS:  return 0x24;
        case S_CPHA:  return 0x27;
        case S_ENCL:  return 0x31;
        case S_TPOS:  return 0x33;
        case S_VEL:   return 0x35;
        case S_CPOS:  return 0x36;
        case S_PERR:  return 0x37;
        case S_FLAG:  return 0x3A;
        case S_ORG:   return 0x3B;
        case S_Conf:  return 0x42;
        case S_State: return 0x43;
        default:      return 0x00;
    }
}

void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s)
{
    uint8_t cmd[8] = {0};
    uint8_t i = 0;
    uint8_t func = emm_v5_sysparam_func(s);

    cmd[i++] = addr;
    if (func) cmd[i++] = func;
    if (s == S_Conf)  cmd[i++] = 0x6C;
    if (s == S_State) cmd[i++] = 0x7A;
    cmd[i++] = 0x6B;
    Emm_V5_Send(cmd, i);
}

uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s,
                                    uint8_t *rxOut, uint8_t maxLen,
                                    uint32_t timeout_ms)
{
    uint8_t cmd[8] = {0};
    uint8_t i = 0;
    uint8_t func = emm_v5_sysparam_func(s);

    cmd[i++] = addr;
    if (func) cmd[i++] = func;
    if (s == S_Conf)  cmd[i++] = 0x6C;
    if (s == S_State) cmd[i++] = 0x7A;
    cmd[i++] = 0x6B;

    int n = emm_v5_request_response(cmd, i, addr, func, rxOut, maxLen,
                                    (timeout_ms > 0) ? timeout_ms
                                                      : EMM_V5_RX_TIMEOUT_MS);
    return (n > 0) ? (uint8_t)n : 0;
}

/* ============================================================
 * 读取电机状态标志 (S_FLAG 0x3A) 并解析返回
 * ============================================================ */

uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms)
{
    uint8_t rx[8] = {0};
    uint8_t cmd[3] = {addr, 0x3A, 0x6B};
    uint8_t read_to = (timeout_ms > 0) ? timeout_ms : EMM_V5_RX_TIMEOUT_MS;

    int n = emm_v5_request_response(cmd, sizeof(cmd), addr, 0x3A,
                                    rx, sizeof(rx), read_to);
    if (n >= 4 && rx[0] == addr && rx[1] == 0x3A && rx[3] == 0x6B)
        return rx[2];
    return 0xFF;
}

/* ============================================================
 * 读取电机位置 (S_TPOS 0x33 / S_CPOS 0x36) 并解析返回
 *
 * 返回帧: 地址 + 功能码 + 符号 + 位置(4B) + 6B (共 8 字节)
 * Emm 固件: 0~65535 对应 0~360°
 * ============================================================ */

static int32_t Emm_V5_Read_Pos(uint8_t addr, uint8_t func)
{
    uint8_t rx[16] = {0};
    uint8_t cmd[3] = {addr, func, 0x6B};

    int n = emm_v5_request_response(cmd, sizeof(cmd), addr, func,
                                    rx, sizeof(rx), EMM_V5_RX_TIMEOUT_MS);
    if (n >= 8 && rx[0] == addr && rx[1] == func &&
        (rx[2] == 0 || rx[2] == 1) && rx[7] == 0x6B)
        return (int32_t)(((uint32_t)rx[3] << 24) |
                         ((uint32_t)rx[4] << 16) |
                         ((uint32_t)rx[5] << 8) |
                         (uint32_t)rx[6]);
    return -1;
}

int32_t Emm_V5_Read_TPOS(uint8_t addr)
{
    return Emm_V5_Read_Pos(addr, 0x33);
}

int32_t Emm_V5_Read_CPOS(uint8_t addr)
{
    return Emm_V5_Read_Pos(addr, 0x36);
}

void Emm_V5_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode)
{
    uint8_t cmd[6] = {addr, 0x46, 0x69, (uint8_t)svF, ctrl_mode, 0x6B};
    Emm_V5_Send(cmd, 6);
}

void Emm_V5_En_Control(uint8_t addr, bool state, bool snF)
{
    uint8_t cmd[6] = {addr, 0xF3, 0xAB, (uint8_t)state, (uint8_t)snF, 0x6B};
    Emm_V5_Send(cmd, 6);
}

void Emm_V5_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF)
{
    uint8_t cmd[8] = {
        addr, 0xF6, dir,
        (uint8_t)(vel >> 8), (uint8_t)(vel >> 0),
        acc, (uint8_t)snF, 0x6B
    };
    Emm_V5_Send(cmd, 8);
}

void Emm_V5_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
                        uint32_t clk, bool raF, bool snF)
{
    uint8_t cmd[13] = {
        addr, 0xFD, dir,
        (uint8_t)(vel >> 8),  (uint8_t)(vel >> 0), acc,
        (uint8_t)(clk >> 24), (uint8_t)(clk >> 16),
        (uint8_t)(clk >> 8),  (uint8_t)(clk >> 0),
        (uint8_t)raF, (uint8_t)snF, 0x6B
    };
    Emm_V5_Send(cmd, 13);
}

void Emm_V5_Pos_Control_Angle(uint8_t addr, uint8_t dir, uint16_t vel,
                              uint8_t acc, float angle_deg, bool raF, bool snF)
{
    /* 负角度 → 取绝对值 + 翻转方向 */
    float    abs_deg = fabsf(angle_deg);
    uint32_t clk     = EMM_V5_DEG_TO_CLK(abs_deg);
    if (angle_deg < 0.0f) dir = (uint8_t)(!dir);
    ESP_LOGD("EMM_V5", "角度→脉冲: %.1f° (%s) → %lu",
             (double)angle_deg, dir ? "CCW" : "CW", (unsigned long)clk);
    Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, snF);
}

void Emm_V5_Stop_Now(uint8_t addr, bool snF)
{
    uint8_t cmd[5] = {addr, 0xFE, 0x98, (uint8_t)snF, 0x6B};
    Emm_V5_Send(cmd, 5);
}

void Emm_V5_Synchronous_motion(uint8_t addr)
{
    uint8_t cmd[4] = {addr, 0xFF, 0x66, 0x6B};
    Emm_V5_Send(cmd, 4);
}

void Emm_V5_Origin_Set_O(uint8_t addr, bool svF)
{
    uint8_t cmd[5] = {addr, 0x93, 0x88, (uint8_t)svF, 0x6B};
    Emm_V5_Send(cmd, 5);
}

void Emm_V5_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode,
    uint8_t o_dir, uint16_t o_vel, uint32_t o_tm,
    uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF)
{
    uint8_t cmd[20] = {
        addr, 0x4C, 0xAE, (uint8_t)svF,
        o_mode, o_dir,
        (uint8_t)(o_vel >> 8),  (uint8_t)(o_vel >> 0),
        (uint8_t)(o_tm  >> 24), (uint8_t)(o_tm  >> 16),
        (uint8_t)(o_tm  >> 8),  (uint8_t)(o_tm  >> 0),
        (uint8_t)(sl_vel >> 8), (uint8_t)(sl_vel >> 0),
        (uint8_t)(sl_ma  >> 8), (uint8_t)(sl_ma  >> 0),
        (uint8_t)(sl_ms  >> 8), (uint8_t)(sl_ms  >> 0),
        (uint8_t)potF, 0x6B
    };
    Emm_V5_Send(cmd, 20);
}

void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF)
{
    uint8_t cmd[5] = {addr, 0x9A, o_mode, (uint8_t)snF, 0x6B};
    Emm_V5_Send(cmd, 5);
}

void Emm_V5_Origin_Interrupt(uint8_t addr)
{
    uint8_t cmd[4] = {addr, 0x9C, 0x48, 0x6B};
    Emm_V5_Send(cmd, 4);
}