/**
 * @file    actuator.c
 * @brief   执行器应用层封装实现
 */

#include "actuator.h"
#include "emm_v5.h"
#include "servo.h"
#include "magnet.h"
#include "rgb.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_timer.h"
#include <math.h>
#include <string.h>

static const char *TAG = "ACTUATOR";

/* ================================================================
 * 运行时状态
 * ================================================================ */

static float s_scale_x     = ACTUATOR_SCALE_X;
static float s_scale_y     = ACTUATOR_SCALE_Y;
static float s_scale_angle = ACTUATOR_SCALE_ANGLE;

/* Z 轴当前命令位置 (度, 绝对), 初始与驱动零位一致; 用于行程内选近路 */
static float s_cur_angle_deg = 0.0f;

/* Z 旋转轴心 → 工具末端 偏移补偿 (mm) */
static float s_offset_x_mm = ACTUATOR_OFFSET_X_MM;
static float s_offset_y_mm = ACTUATOR_OFFSET_Y_MM;

static bool s_initialized = false;

/* 最近一次执行的 Cycle 快照 */
static edc_cycle_t s_last_cycle;
static bool        s_last_cycle_valid = false;

/* ================================================================
 * 内部: 帧间间隔
 *
 * Emm_V5 驱动板无法解析 UART 上零间隔的背靠背连续帧 (会丢字节,
 * 导致本帧与后续帧全部失效), 凡连发多帧必须插入该延时。
 * 曾导致故障: Y 轴 3/4 号电机使能 (单帧) 正常, 速度/位置 (连发多帧) 无反应
 * ================================================================ */

static inline void _frame_gap(void)
{
    vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
}

/* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半; 精确到系统 tick) */
static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t period_ms)
{
    TickType_t total = pdMS_TO_TICKS(ms);
    TickType_t half  = pdMS_TO_TICKS(period_ms / 2);
    if (half == 0) half = 1;
    bool led_on = true;

    while (total > 0) {
        TickType_t chunk = (total > half) ? half : total;
        if (led_on) rgb_set(r, g, b);
        else        rgb_off();
        vTaskDelay(chunk);
        total -= chunk;
        led_on = !led_on;
    }
    rgb_off();
}

/* 到位等待 (闭环反馈)
 *
 * 实测发现这台 X42S 的 S_FLAG Prf_TF 到位标志只有 X 轴会置位 (0x03),
 * Z/Y1/Y2 始终为 0x01 (仅使能位), 轮询 Prf_TF 无法判定到位。
 * 因此改为直接比对电机实时位置 (S_CPOS) 与目标位置 (S_TPOS):
 * 偏差 < ACTUATOR_ARRIVAL_TOL_RAW 即认为到位 —— 不依赖 Prf_TF。
 * 每轮重新读 S_TPOS (电机处理命令后目标才更新, 首轮可能是旧目标)。
 * 需连续 2 次在容差内才确认: 确保电机已稳定停下 (减速途中/振荡时
 * 位置可能短暂进入容差, 单独一次会"没到位就进行下一步")。
 *
 * ⚠️ 防坑: 轮询第一帧前必须留帧间隔 —— 运动命令最后一条是 Y 同步广播
 *     触发 (00 FF 66 6B), 若紧跟发轮询帧会"零间隔背靠背", 驱动板丢字节
 *     吞掉触发帧, 电机不启动。
 *
 * @param base_wait_ms 纯运动时间估算 (兜底超时基准)
 * @param moved_mask   实际有行程的电机位掩码 (bit0=Z, bit1=X, bit2/3=Y1/Y2)
 * @return true=全部到位, false=超时
 */
bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask)
{
    const uint8_t addrs[4] = {
        ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
        ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2
    };
    bool arrived[4] = {false, false, false, false};
    uint8_t set_cnt[4] = {0, 0, 0, 0};   /* 连续读到在容差内的次数 (≥2 确认已停稳) */
    int32_t last_cpos[4] = {-1, -1, -1, -1}; /* 诊断 */
    int32_t last_tpos[4] = {-1, -1, -1, -1}; /* 诊断 */
    uint32_t polls = 0;                    /* 诊断: 轮询轮数 */
    uint32_t timeout_ms = base_wait_ms + ACTUATOR_SETTLE_MARGIN_MS;
    int64_t deadline = esp_timer_get_time() + (int64_t)timeout_ms * 1000;

    for (int i = 0; i < 4; i++) {
        if (!(moved_mask & (1u << i)))
            arrived[i] = true;   /* 无行程: 视为已到位 */
    }

    /* 与上一条运动命令 (尤其 Y 同步触发) 之间留帧间隔, 防丢字节 */
    vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));

    while (esp_timer_get_time() < deadline) {
        bool all = true;
        for (int i = 0; i < 4; i++) {
            if (arrived[i])
                continue;

            bool at_target = false;
            int32_t tpos = Emm_V5_Read_TPOS(addrs[i]);
            last_tpos[i] = tpos;
            if (tpos >= 0) {
                int32_t cpos = Emm_V5_Read_CPOS(addrs[i]);
                last_cpos[i] = cpos;
                if (cpos >= 0) {
                    int32_t diff = (int32_t)((uint16_t)cpos - (uint16_t)tpos);
                    if (diff < 0) diff = -diff;
                    if (diff > 32768) diff = 65536 - diff;   /* 环形距离 */
                    at_target = (diff <= (int32_t)ACTUATOR_ARRIVAL_TOL_RAW);
                }
            } else {
                /* 目标位置读失败: 退回 Prf_TF 到位标志 (仅 X 可靠) */
                uint8_t flags = Emm_V5_Read_Status_Flag(addrs[i], 50);
                at_target = (flags != 0xFF && (flags & 0x02));
            }

            if (at_target) {
                /* 连续 2 次在容差内才确认: 电机已稳定停下,
                   防止减速途中/振荡时误判"到位"提前进入下一步 */
                if (++set_cnt[i] >= 2) {
                    arrived[i] = true;
                } else {
                    all = false;
                }
            } else {
                set_cnt[i] = 0;
                all = false;
            }
            vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));  /* 帧间隔防粘包 */
        }
        polls++;
        if (all)
            return true;

        /* 诊断: 前 3 轮打印各电机位置 (调试用, 定位后删除) */
        if (polls <= 3) {
            ESP_LOGI(TAG, "到位轮询#%u: cpos Z=%ld X=%ld Y1=%ld Y2=%ld  "
                     "tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
                     polls,
                     (long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
                     (long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
        }

        /* 红灯快闪反馈 (与旧 _wait_flash 一致) */
        rgb_set(128, 0, 0);
        vTaskDelay(pdMS_TO_TICKS(10));
        rgb_off();
        vTaskDelay(pdMS_TO_TICKS(10));
    }

    ESP_LOGW(TAG, "到位超时 (%u ms, %u 轮): cpos Z=%ld X=%ld Y1=%ld Y2=%ld  "
             "tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
             timeout_ms, polls,
             (long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
             (long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
    return false;
}

/* ================================================================
 * 运动到位时间估算
 *
 * Emm_V5 曲线加减速 (官方文档):
 *   t2 - t1 = (256 - acc) * 50(us), Vt2 = Vt1 + 1(RPM)
 * 即每提升 1 RPM 耗时 (256-acc)*50us, 按梯形/三角形加减速估算。
 * ================================================================ */

uint32_t actuator_motion_time_ms(uint32_t pulses, uint16_t vel, uint8_t acc)
{
    if (pulses == 0 || vel == 0) return 0;

    float revs  = (float)pulses / (float)EMM_V5_PULSES_PER_REV;
    float v_rps = (float)vel / 60.0f;
    float t_ms;

    if (acc == 0) {
        /* acc=0: 无曲线加减速, 直接按设定速度恒速运行 */
        t_ms = revs / v_rps * 1000.0f;
    } else {
        /* 从 0 加速到目标转速的时间与加速段距离 (线性曲线近似) */
        float t_ramp_s   = (float)vel * (float)(256 - acc) * 50e-6f;
        float a_rps2     = v_rps / t_ramp_s;
        float d_ramp_rev = 0.5f * a_rps2 * t_ramp_s * t_ramp_s;

        if (revs <= 2.0f * d_ramp_rev) {
            /* 行程不足, 未到目标转速即减速 → 三角形 */
            t_ms = 2.0f * sqrtf(revs / a_rps2) * 1000.0f;
        } else {
            /* 加速 + 恒速 + 减速 */
            t_ms = (2.0f * t_ramp_s + (revs - 2.0f * d_ramp_rev) / v_rps) * 1000.0f;
        }
    }
    return (uint32_t)(t_ms + 0.999f);
}

uint32_t actuator_settle_ms(uint32_t pulses, uint16_t vel, uint8_t acc)
{
    return actuator_motion_time_ms(pulses, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
}

uint32_t actuator_mm_motion_time_ms(float distance_mm, uint16_t vel, uint8_t acc)
{
    if (distance_mm < 0.0f) distance_mm = -distance_mm;
    uint32_t pulses = (uint32_t)(distance_mm * s_scale_x + 0.5f);
    return actuator_motion_time_ms(pulses, vel, acc);
}

uint32_t actuator_angle_motion_time_ms(float angle_deg, uint16_t vel, uint8_t acc)
{
    if (angle_deg < 0.0f) angle_deg = -angle_deg;
    uint32_t pulses = (uint32_t)(angle_deg * 10.0f * s_scale_angle + 0.5f);
    return actuator_motion_time_ms(pulses, vel, acc);
}

uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc)
{
    return actuator_mm_motion_time_ms(distance_mm, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
}

uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc)
{
    return actuator_angle_motion_time_ms(angle_deg, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
}

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

void actuator_init(void)
{
    /* 初始化电机驱动 UART */
    Emm_V5_Init(ACTUATOR_UART_PORT, ACTUATOR_UART_TX, ACTUATOR_UART_RX);

    /* 初始化舵机 PWM */
    servo_init(ACTUATOR_SERVO_GPIO);

    /* 配置舵机限位: 抬起=80°, 放下=160° */
    servo_set_limits(0.0f, 95.0f);

    /* 上电默认归位到抬起状态 */
    servo_set_up();

    /* 配置 4 路电机: 闭环位置模式 + 使能 */
    const uint8_t addrs[] = {
        ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
        ACTUATOR_ADDR_Y1,    ACTUATOR_ADDR_Y2
    };
    for (int i = 0; i < 4; i++) {
        uint8_t addr = addrs[i];
        Emm_V5_Modify_Ctrl_Mode(addr, false, ACTUATOR_CTRL_MODE);
        vTaskDelay(pdMS_TO_TICKS(20));
        Emm_V5_En_Control(addr, true, false);
        vTaskDelay(pdMS_TO_TICKS(20));
    }

    s_initialized = true;
    /* Z 轴命令位置跟踪与驱动零位对齐 (重新初始化时复位) */
    s_cur_angle_deg = 0.0f;
    ESP_LOGI(TAG, "初始化完成 (Angle=addr%d X=addr%d Y=addr%d/%d, servo=GPIO%d)",
             ACTUATOR_ADDR_ANGLE,
             ACTUATOR_ADDR_X,
             ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2,
             ACTUATOR_SERVO_GPIO);
}

/* ================================================================
 * 脉冲换算
 * ================================================================ */

void actuator_set_scale(float x_scale, float y_scale, float angle_scale)
{
    s_scale_x     = x_scale;
    s_scale_y     = y_scale;
    s_scale_angle = angle_scale;
    ESP_LOGI(TAG, "脉冲比例: X=%.3f Y=%.3f Angle=%.3f",
             s_scale_x, s_scale_y, s_scale_angle);
}

/* ================================================================
 * Z 旋转轴心 ↔ 工具末端 偏移补偿
 *
 * 终端执行机构与 Z 旋转机构不在同一条纵轴上,
 * 旋转角度 θ 时, 工具末端 XY 位置 = 旋转中心 + R(θ)·(dx, dy)
 *
 * 要将工具末端放到 target 位置, 旋转中心需要去:
 *   center = target - R(θ)·(dx, dy)
 *
 * 即:
 *   X_motor = X_tgt - (dx·cosθ - dy·sinθ)
 *   Y_motor = Y_tgt - (dx·sinθ + dy·cosθ)
 * ================================================================ */

void actuator_set_offset(float offset_x_mm, float offset_y_mm)
{
    s_offset_x_mm = offset_x_mm;
    s_offset_y_mm = offset_y_mm;
    ESP_LOGI(TAG, "偏移补偿: dx=%.2fmm dy=%.2fmm", (double)offset_x_mm, (double)offset_y_mm);
}

void actuator_get_offset(float *offset_x_mm, float *offset_y_mm)
{
    if (offset_x_mm) *offset_x_mm = s_offset_x_mm;
    if (offset_y_mm) *offset_y_mm = s_offset_y_mm;
}

void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
                                        float angle_deg)
{
    if (!target_x || !target_y) return;

    /* 偏移量为零时跳过, 避免浮点运算 */
    if (s_offset_x_mm == 0.0f && s_offset_y_mm == 0.0f) return;

    float theta_rad = angle_deg * (float)(M_PI / 180.0);
    float cos_t     = cosf(theta_rad);
    float sin_t     = sinf(theta_rad);

    /* R(θ)·(dx, dy): 旋转后的偏移向量 */
    float rot_dx = s_offset_x_mm * cos_t - s_offset_y_mm * sin_t;
    float rot_dy = s_offset_x_mm * sin_t + s_offset_y_mm * cos_t;

    /* center = target - rotated_offset, 四舍五入 */
    float cx = (float)(*target_x) - rot_dx;
    float cy = (float)(*target_y) - rot_dy;

    /* 钳位到有效范围并写回 */
    if (cx < 0.0f) cx = 0.0f;
    if (cy < 0.0f) cy = 0.0f;
    *target_x = (uint16_t)(cx + 0.5f);
    *target_y = (uint16_t)(cy + 0.5f);

    ESP_LOGD(TAG, "补偿: (%.1f°) offset(%.2f,%.2f)→rot(%.2f,%.2f) "
             "tgt→ctr (%u,%u)",
             (double)angle_deg,
             (double)s_offset_x_mm, (double)s_offset_y_mm,
             (double)rot_dx, (double)rot_dy,
             *target_x, *target_y);
}

/* ================================================================
 * 内部: 单轴绝对位置运动
 * ================================================================ */

static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses,
                               uint16_t vel, uint8_t acc, bool snF)
{
    Emm_V5_Pos_Control(addr, dir, vel, acc,
                       pulses,
                       true,           /* raF=true: 绝对运动 */
                       snF);           /* 多机同步标志 */
}

/* ================================================================
 * 内部: Z 旋转轴角度运动 (绝对角, ±行程内选最近可达代表角)
 * ================================================================ */

/** 在 ±limit 行程内选择与当前位置最近的等价角 (mod 360) */
static float _closest_reachable_angle(float target_deg, float cur_deg, float limit)
{
    target_deg = fmodf(target_deg, 360.0f);
    if (target_deg < 0.0f)
        target_deg += 360.0f;

    float best = target_deg;
    float best_dist = 1e30f;
    for (int k = -1; k <= 1; k++) {
        float rep = target_deg + 360.0f * (float)k;
        if (rep < -limit || rep > limit)
            continue;
        float dist = fabsf(rep - cur_deg);
        if (dist < best_dist - 1e-4f ||
            (fabsf(dist - best_dist) <= 1e-4f && rep > best)) {
            best_dist = dist;
            best = rep;
        }
    }
    return best;
}

/** Z 轴绝对运动到目标朝向 (用于 source 归位); 负角自动翻转方向, 并更新当前位置跟踪 */
static void _actuator_move_angle_abs(float angle_deg, uint16_t vel, uint8_t acc)
{
    float target = _closest_reachable_angle(angle_deg, s_cur_angle_deg,
                                            ACTUATOR_ANGLE_LIMIT_DEG);
    uint8_t dir = ACTUATOR_DIR_ANGLE;
    float abs_deg = fabsf(target);
    uint32_t pulses = (uint32_t)(abs_deg * 10.0f * s_scale_angle + 0.5f);
    if (target < 0.0f)
        dir = (uint8_t)!dir;

    _actuator_move_abs(ACTUATOR_ADDR_ANGLE, dir, pulses, vel, acc, false);
    s_cur_angle_deg = target;
    ESP_LOGI(TAG, "Z 角度: 目标 %.1f° -> 最近可达 %.1f° (%lu pulses, %s)",
             (double)angle_deg, (double)target, (unsigned long)pulses,
             dir ? "CCW" : "CW");
}

/** Z 轴相对运动 delta (带符号度): 正负自动选方向, 语义明确, 用于片间旋转 */
static void _actuator_move_angle_rel(float delta_deg, uint16_t vel, uint8_t acc)
{
    uint8_t dir = ACTUATOR_DIR_ANGLE;
    float abs_deg = fabsf(delta_deg);
    uint32_t pulses = (uint32_t)(abs_deg * 10.0f * s_scale_angle + 0.5f);
    if (delta_deg < 0.0f)
        dir = (uint8_t)!dir;

    Emm_V5_Pos_Control(ACTUATOR_ADDR_ANGLE, dir, vel, acc, pulses,
                       false,           /* raF=false: 相对运动 */
                       false);
    s_cur_angle_deg += delta_deg;
    ESP_LOGI(TAG, "Z 角度: 相对 %.1f° (%lu pulses, %s)",
             (double)delta_deg, (unsigned long)pulses, dir ? "CCW" : "CW");
}

/* ================================================================
 * 内部: Y 轴双电机同步运动
 * ================================================================ */

static void _actuator_move_y_sync(uint32_t pulses_y, uint16_t vel, uint8_t acc)
{
    /* Y 轴正向 = CCW (dir=1), 双电机同步 */
    _actuator_move_abs(ACTUATOR_ADDR_Y1, ACTUATOR_DIR_Y, pulses_y, vel, acc, true);
    _frame_gap();
    _actuator_move_abs(ACTUATOR_ADDR_Y2, ACTUATOR_DIR_Y, pulses_y, vel, acc, true);
    _frame_gap();

    /* 广播地址触发一次, 两台电机同时开始运动 */
    Emm_V5_Synchronous_motion(0);

    ESP_LOGD(TAG, "Y 同步(CCW): addr%d+%d -> %lu pulses (%.2fmm) vel=%d acc=%d",
             ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2, pulses_y,
             (double)pulses_y / (double)ACTUATOR_SCALE_Y, vel, acc);
}

/* ================================================================
 * 内部: 执行单个 Fragment
 * ================================================================ */

static void _actuator_move_fragment(const edc_fragment_t *frag)
{
    /* XY 执行速度/加速度 (3000RPM + 250 档, 实测稳定);
       Z 旋转轴保持低速 (见 ACTUATOR_EXEC_Z_VEL/Z_ACC) */
    uint16_t vel_xy    = ACTUATOR_EXEC_VEL;
    uint8_t  acc_xy    = ACTUATOR_EXEC_ACC;
    uint16_t vel_angle = ACTUATOR_EXEC_Z_VEL;
    uint8_t  acc_angle = ACTUATOR_EXEC_Z_ACC;

    /* ---- 提取 source / target 坐标 (可被补偿修改) ---- */
    uint16_t src_x     = frag->source.x;
    uint16_t src_y     = frag->source.y;
    uint16_t src_angle = frag->source.angle_x10;
    uint16_t tgt_x     = frag->target.x;
    uint16_t tgt_y     = frag->target.y;
    uint16_t tgt_angle = frag->target.angle_x10;

    /* 协议角度: source=0 (保持当前), target=相对转角 [0,360);
       执行时自动选行程内与当前位置最近的等价角 */
    float src_angle_deg = (float)src_angle * 0.1f;
    float tgt_angle_deg = (float)tgt_angle * 0.1f;

    /* Z 旋转轴心偏移补偿 (仅对 target 做, 与旧协议行为保持一致) */
    actuator_apply_offset_compensation(&tgt_x, &tgt_y, tgt_angle_deg);

    /* source 坐标 → 脉冲数 */
    uint32_t src_pulses_x     = (uint32_t)(src_x     * s_scale_x);
    uint32_t src_pulses_y     = (uint32_t)(src_y     * s_scale_y);

    /* target 坐标 → 脉冲数 */
    uint32_t dst_pulses_x     = (uint32_t)(tgt_x     * s_scale_x);
    uint32_t dst_pulses_y     = (uint32_t)(tgt_y     * s_scale_y);

    ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%.1f) -> DST(X=%lu Y=%lu A=%.1f comp x=%u y=%u)",
             (unsigned long)src_pulses_x, (unsigned long)src_pulses_y,
             (double)src_angle_deg,
             (unsigned long)dst_pulses_x, (unsigned long)dst_pulses_y,
             (double)tgt_angle_deg,
             tgt_x, tgt_y);

    /* 到位轮询掩码: 只等待实际有行程的电机 (脉冲>0), 无行程直接视为已到位 */
    float src_target = _closest_reachable_angle(src_angle_deg, s_cur_angle_deg,
                                                 ACTUATOR_ANGLE_LIMIT_DEG);
    uint32_t src_pulses_z = (uint32_t)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f);
    uint32_t src_mask = 0;
    if (src_pulses_z > 0) src_mask |= (1u << 0);                 /* Z 旋转 */
    if (src_pulses_x > 0) src_mask |= (1u << 1);                 /* X */
    if (src_pulses_y > 0) src_mask |= (1u << 2) | (1u << 3);     /* Y 双电机 */

    /* 纯运动时间 (无裕量), 仅作到位等待的兜底超时基准 */
    uint32_t src_wait_ms = actuator_motion_time_ms(src_pulses_z, vel_angle, acc_angle);
    uint32_t t_wait = actuator_motion_time_ms(src_pulses_x, vel_xy, acc_xy);
    if (t_wait > src_wait_ms) src_wait_ms = t_wait;
    t_wait = actuator_motion_time_ms(src_pulses_y, vel_xy, acc_xy);
    if (t_wait > src_wait_ms) src_wait_ms = t_wait;

    /* ---- 1. 磁铁掉电 ---- */
    magnet_set(false);

    /* ---- 2. 运动到 source 位置 ---- */
    _actuator_move_angle_abs(src_angle_deg, vel_angle, acc_angle);
    _frame_gap();
    _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
                       src_pulses_x, vel_xy, acc_xy, false);
    _frame_gap();
    _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy);

    /* ---- 3. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
    actuator_wait_arrival(src_wait_ms, src_mask);

    /* ---- 4. 舵机放下 ---- */
    servo_set_down();
    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);

    /* ---- 5. 磁铁吸合 ---- */
    magnet_set(true);
    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);

    /* ---- 6. 舵机抬起 ---- */
    servo_set_up();
    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);

    /* ---- 7. 运动到 target 位置 ---- */
    /* 目标角度取行程内与当前位置最近的等价角, 再用相对运动执行:
       终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */
    float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg,
                                             ACTUATOR_ANGLE_LIMIT_DEG);
    uint32_t dst_pulses_z = (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
    uint32_t dst_mask = 0;
    if (dst_pulses_z > 0) dst_mask |= (1u << 0);
    if (dst_pulses_x > 0) dst_mask |= (1u << 1);
    if (dst_pulses_y > 0) dst_mask |= (1u << 2) | (1u << 3);

    uint32_t dst_wait_ms = actuator_motion_time_ms(dst_pulses_z, vel_angle, acc_angle);
    t_wait = actuator_motion_time_ms(dst_pulses_x, vel_xy, acc_xy);
    if (t_wait > dst_wait_ms) dst_wait_ms = t_wait;
    t_wait = actuator_motion_time_ms(dst_pulses_y, vel_xy, acc_xy);
    if (t_wait > dst_wait_ms) dst_wait_ms = t_wait;

    _actuator_move_angle_rel(dst_rep - s_cur_angle_deg, vel_angle, acc_angle);
    _frame_gap();
    _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
                       dst_pulses_x, vel_xy, acc_xy, false);
    _frame_gap();
    _actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy);

    /* ---- 8. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
    actuator_wait_arrival(dst_wait_ms, dst_mask);

    /* ---- 9. 舵机放下 ---- */
    servo_set_down();
    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);

    /* ---- 10. 磁铁掉电 ---- */
    magnet_set(false);
    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);

    /* ---- 11. 舵机抬起 ---- */
    servo_set_up();
    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
}

/* ================================================================
 * 执行完整 Cycle
 * ================================================================ */

int actuator_execute_cycle(const edc_cycle_t *cycle)
{
    if (!s_initialized) {
        ESP_LOGE(TAG, "未初始化, 无法执行 Cycle");
        return -1;
    }
    if (!cycle) {
        ESP_LOGE(TAG, "Cycle 指针为空");
        return -1;
    }
    if (cycle->frag_count < EDC_FRAG_MIN || cycle->frag_count > EDC_FRAG_MAX) {
        ESP_LOGE(TAG, "Fragment 数量无效: %u (需 %d~%d)",
                 cycle->frag_count, EDC_FRAG_MIN, EDC_FRAG_MAX);
        return -1;
    }

    /* 保存快照 (执行前先存, 即使执行中也可查询) */
    memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t));
    s_last_cycle_valid = true;

    ESP_LOGI(TAG, "=== Cycle #%u 开始 (%u fragments) ===",
             cycle->cycle_id, cycle->frag_count);

    for (int i = 0; i < cycle->frag_count; i++) {
        ESP_LOGI(TAG, "-- Fragment %d/%d --", i + 1, cycle->frag_count);
        _actuator_move_fragment(&cycle->fragments[i]);
    }

    ESP_LOGI(TAG, "=== Cycle #%u 完成 ===", cycle->cycle_id);
    return 0;
}

/* ================================================================
 * 协议回调: 收到 Cycle → 执行 → DONE
 * ================================================================ */

void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
{
    static uint16_t s_last_exec_id = 0xFFFF;

    ESP_LOGI(TAG, "Cycle #%u via %s, %u frags",
             cycle->cycle_id, ch == EDC_CH_I2C ? "I2C" : "UART",
             cycle->frag_count);

    /* I2C / UART 冗余下发, 同一 cycle_id 只执行一次 */
    if (cycle->cycle_id == s_last_exec_id) {
        ESP_LOGD(TAG, "Cycle #%u 重复, 跳过执行", cycle->cycle_id);
        return;
    }
    s_last_exec_id = cycle->cycle_id;

    /* 新一轮执行开始: 复位 RGB 状态 (执行期间 _wait_flash 显示红灯) */
    rgb_set_state(RGB_STATE_IDLE);

    /* 执行电机运动 (阻塞, 含拾放全流程) */
    ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id);
    actuator_execute_cycle(cycle);

    /* 全部 Fragment 执行完成后回复 DONE */
    edc_proto_send_done(cycle->cycle_id);
    ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id);

    /* 设置 RGB 状态为 DONE, 主循环将持续绿灯慢闪 */
    rgb_set_state(RGB_STATE_DONE);
}

/* ================================================================
 * 急停
 * ================================================================ */

void actuator_stop_all(void)
{
    const uint8_t addrs[] = {
        ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
        ACTUATOR_ADDR_Y1,    ACTUATOR_ADDR_Y2
    };
    _frame_gap();  /* 防止与上一帧 UART 冲突 */
    for (int i = 0; i < 4; i++) {
        Emm_V5_Stop_Now(addrs[i], false);
        _frame_gap();
    }
    ESP_LOGW(TAG, "全部电机急停");
}

/* ================================================================
 * 舵机控制
 * ================================================================ */

void actuator_servo_up(void)
{
    servo_set_up();
    ESP_LOGD(TAG, "舵机: 抬起");
}

void actuator_servo_down(void)
{
    servo_set_down();
    ESP_LOGD(TAG, "舵机: 放下");
}

void actuator_servo_set_angle(float angle)
{
    servo_set_angle(angle);
    ESP_LOGD(TAG, "舵机: %.1f°", (double)angle);
}

/* ================================================================
 * 单电机手动控制 (Y 轴自动同步)
 * ================================================================ */

static inline bool _is_y_motor(uint8_t addr)
{
    return (addr == ACTUATOR_ADDR_Y1 || addr == ACTUATOR_ADDR_Y2);
}

void actuator_motor_enable(uint8_t addr, bool state)
{
    _frame_gap();  /* 防止与上一帧 UART 冲突 */
    Emm_V5_En_Control(addr, state, false);
    ESP_LOGI(TAG, "电机 %d %s", addr, state ? "使能" : "关闭");
}

void actuator_motor_vel(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc)
{
    _frame_gap();  /* 防止与上一帧 UART 冲突 */
    Emm_V5_Vel_Control(addr, dir, vel, acc, false);
    ESP_LOGI(TAG, "速度: 电机%d %s %dRPM acc=%d",
             addr, dir ? "CCW" : "CW", vel, acc);

    /* Y 轴同步: 对端电机同样命令 (帧间留间隔) */
    if (_is_y_motor(addr)) {
        uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
                       ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
        _frame_gap();
        Emm_V5_Vel_Control(peer, dir, vel, acc, false);
        ESP_LOGD(TAG, "Y 同步: 电机%d 同速", peer);
    }
}

void actuator_motor_pos(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
                        uint32_t clk, bool raF)
{
    _frame_gap();  /* 防止与上一帧 UART 冲突 */
    if (_is_y_motor(addr)) {
        /* Y 轴双电机同步位置 (帧间留间隔, 广播地址触发一次) */
        uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
                       ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
        Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, true);
        _frame_gap();
        Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
        _frame_gap();
        Emm_V5_Synchronous_motion(0);
        ESP_LOGI(TAG, "位置(Y同步): 电机%d+%d %s %dRPM %lu脉冲 %s",
                 addr, peer, dir ? "CCW" : "CW", vel, clk,
                 raF ? "绝对" : "相对");
    } else {
        Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, false);
        ESP_LOGI(TAG, "位置: 电机%d %s %dRPM %lu脉冲 %s",
                 addr, dir ? "CCW" : "CW", vel, clk,
                 raF ? "绝对" : "相对");
    }
}

void actuator_motor_angle(uint8_t addr, float angle_deg, uint16_t vel,
                          uint8_t acc, bool raF)
{
    _frame_gap();  /* 防止与上一帧 UART 冲突 */
    /* 方向由 Emm_V5_Pos_Control_Angle 根据角度正负自动翻转 */
    uint8_t dir = ACTUATOR_DIR_ANGLE;

    if (_is_y_motor(addr)) {
        uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
                       ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
        Emm_V5_Pos_Control_Angle(addr, dir, vel, acc, angle_deg, raF, true);
        _frame_gap();
        Emm_V5_Pos_Control_Angle(peer, dir, vel, acc, angle_deg, raF, true);
        _frame_gap();
        Emm_V5_Synchronous_motion(0);
        ESP_LOGI(TAG, "角度(Y同步): 电机%d+%d %.1f° %s vel=%dRPM",
                 addr, peer, (double)angle_deg,
                 raF ? "绝对" : "相对", vel);
    } else {
        /* 跟踪 Z 轴命令位置 (供 fragment 行程内近路选择) */
        if (addr == ACTUATOR_ADDR_ANGLE)
            s_cur_angle_deg = raF ? angle_deg : (s_cur_angle_deg + angle_deg);
        Emm_V5_Pos_Control_Angle(addr, dir, vel, acc, angle_deg, raF, false);
        ESP_LOGI(TAG, "角度: 电机%d %.1f° %s vel=%dRPM",
                 addr, (double)angle_deg,
                 raF ? "绝对" : "相对", vel);
    }
}

void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel,
                       uint8_t acc, bool raF)
{
    /* 软限位钳位 */
    if (_is_y_motor(addr)) {
        if (distance_mm < ACTUATOR_LIMIT_Y_MIN) distance_mm = ACTUATOR_LIMIT_Y_MIN;
        if (distance_mm > ACTUATOR_LIMIT_Y_MAX) distance_mm = ACTUATOR_LIMIT_Y_MAX;
    } else {
        if (distance_mm < ACTUATOR_LIMIT_X_MIN) distance_mm = ACTUATOR_LIMIT_X_MIN;
        if (distance_mm > ACTUATOR_LIMIT_X_MAX) distance_mm = ACTUATOR_LIMIT_X_MAX;
    }

    _frame_gap();  /* 防止与上一帧 UART 冲突 */
    uint32_t clk = ACTUATOR_MM_TO_PULSES(distance_mm);
    /* Y 轴正向 = CCW, X 轴正向 = CW */
    uint8_t dir = _is_y_motor(addr) ? ACTUATOR_DIR_Y : ACTUATOR_DIR_X;

    if (_is_y_motor(addr)) {
        uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
                       ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
        Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, true);
        _frame_gap();
        Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
        _frame_gap();
        Emm_V5_Synchronous_motion(0);
        ESP_LOGI(TAG, "直线(Y同步 %s): 电机%d+%d %.2fmm (%lu脉冲) %s vel=%dRPM",
                 dir ? "CCW" : "CW", addr, peer, (double)distance_mm,
                 (unsigned long)clk, raF ? "绝对" : "相对", vel);
    } else {
        Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, false);
        ESP_LOGI(TAG, "直线(%s): 电机%d %.2fmm (%lu脉冲) %s vel=%dRPM",
                 dir ? "CCW" : "CW", addr, (double)distance_mm,
                 (unsigned long)clk, raF ? "绝对" : "相对", vel);
    }
}

void actuator_motor_stop(uint8_t addr)
{
    _frame_gap();  /* 防止与上一帧 UART 冲突 */
    Emm_V5_Stop_Now(addr, false);
    ESP_LOGI(TAG, "停止: 电机%d", addr);

    /* Y 轴同步停止 (帧间留间隔) */
    if (_is_y_motor(addr)) {
        uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
                       ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
        _frame_gap();
        Emm_V5_Stop_Now(peer, false);
        ESP_LOGD(TAG, "Y 同步: 电机%d 同停", peer);
    }
}

/* ================================================================
 * 最近 Cycle 快照查询
 * ================================================================ */

const edc_cycle_t *actuator_get_last_cycle(void)
{
    return s_last_cycle_valid ? &s_last_cycle : NULL;
}

bool actuator_is_last_cycle_valid(void)
{
    return s_last_cycle_valid;
}

void actuator_clear_last_cycle(void)
{
    s_last_cycle_valid = false;
}