/**
 * @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 <math.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;

static bool s_initialized = false;

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

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

/* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半) */
static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t period_ms)
{
    uint32_t half = pdMS_TO_TICKS(period_ms / 2);
    int cycles = (int)(ms / period_ms);
    for (int i = 0; i < cycles; i++) {
        rgb_set(r, g, b);
        vTaskDelay(half);
        rgb_off();
        vTaskDelay(half);
    }
}

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

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;
    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);
}

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

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);           /* 多机同步标志 */
}

/* ================================================================
 * 内部: 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)
{
    /* 固定速度/加速度 */
    uint16_t vel_xy    = 1500;
    uint8_t  acc_xy    = 200;
    uint16_t vel_angle = 50;
    uint8_t  acc_angle = 100;

    /* source 坐标 → 脉冲数 */
    uint32_t src_pulses_x     = (uint32_t)(frag->source.x         * s_scale_x);
    uint32_t src_pulses_y     = (uint32_t)(frag->source.y         * s_scale_y);
    uint32_t src_pulses_angle = (uint32_t)(frag->source.angle_x10 * s_scale_angle);

    /* target 坐标 → 脉冲数 */
    uint32_t dst_pulses_x     = (uint32_t)(frag->target.x         * s_scale_x);
    uint32_t dst_pulses_y     = (uint32_t)(frag->target.y         * s_scale_y);
    uint32_t dst_pulses_angle = (uint32_t)(frag->target.angle_x10 * s_scale_angle);

    ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%lu) → DST(X=%lu Y=%lu A=%lu)",
             src_pulses_x, src_pulses_y, src_pulses_angle,
             dst_pulses_x, dst_pulses_y, dst_pulses_angle);

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

    /* ---- 2. 运动到 source 位置 ---- */
    /* Angle / Z 旋转轴 */
    _actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE,
                       src_pulses_angle, vel_angle, acc_angle, false);
    _frame_gap();

    /* X 轴 */
    _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
                       src_pulses_x, vel_xy, acc_xy, false);
    _frame_gap();

    /* Y 轴 (双电机同步) */
    _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy);

    /* ---- 3. 等待 2s 运动到位 (红灯快闪) ---- */
    _wait_flash(2000, 128, 0, 0, 200);

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

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

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

    /* ---- 7. 运动到 target 位置 ---- */
    _actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE,
                       dst_pulses_angle, vel_angle, acc_angle, false);
    _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. 等待 2s 运动到位 (红灯快闪) ---- */
    _wait_flash(2000, 128, 0, 0, 200);

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

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

    /* ---- 11. 舵机抬起 ---- */
    servo_set_up();
    _wait_flash(500, 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;
    }

    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;
}

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

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 {
        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 → 执行 → DONE
 * ================================================================ */

static edc_cycle_t      s_last_cycle;
static bool             s_last_cycle_valid;
static portMUX_TYPE     s_cycle_lock = portMUX_INITIALIZER_UNLOCKED;

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 edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
{
    static uint16_t s_last_exec_id = 0xFFFF;

    /* 保存快照 */
    portENTER_CRITICAL(&s_cycle_lock);
    memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t));
    s_last_cycle_valid = true;
    portEXIT_CRITICAL(&s_cycle_lock);

    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;

    /* 执行电机运动 (阻塞, 含拾放全流程) */
    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);
}