/**
 * @file    actuator.h
 * @brief   执行器应用层封装 — 4 路步进电机 + 1 路舵机
 *
 * 轴映射:
 *   1 号电机 (addr=1)  ── Angle/Z 旋转轴
 *   2 号电机 (addr=2)  ── X 轴
 *   3 号电机 (addr=3)  ── Y 轴  ─┐ 完全同步
 *   4 号电机 (addr=4)  ── Y 轴  ─┘
 *   舵机 (PWM)         ── Z 轴抬升 (仅抬起/放下两态)
 *
 * 运动模式: 位置模式, 绝对定位 (raF=true), 暂不标定
 */

#ifndef ACTUATOR_H
#define ACTUATOR_H

#include <stdint.h>
#include <stdbool.h>
#include "driver/gpio.h"
#include "protocol.h"
#include "emm_v5.h"

#ifdef __cplusplus
extern "C" {
#endif

/* ================================================================
 * 可配置宏 (可在项目级或 -D 覆盖)
 * ================================================================ */

/** 丝杆参数 (X / Y 轴直线运动) */
#define ACTUATOR_SCREW_LEAD_MM   8.0f    /* 导程: 一圈 = 8mm 直线位移 */
#define ACTUATOR_SCREW_PITCH_MM  2.0f    /* 螺距 */

/** 电机 Emm_V5 地址 */
#ifndef ACTUATOR_ADDR_ANGLE
#define ACTUATOR_ADDR_ANGLE     1       /* Angle / Z 旋转轴 */
#endif
#ifndef ACTUATOR_ADDR_X
#define ACTUATOR_ADDR_X         2       /* X 轴 */
#endif
#ifndef ACTUATOR_ADDR_Y1
#define ACTUATOR_ADDR_Y1        3       /* Y 轴 #1 (同步) */
#endif
#ifndef ACTUATOR_ADDR_Y2
#define ACTUATOR_ADDR_Y2        4       /* Y 轴 #2 (同步) */
#endif

/** 舵机 PWM GPIO */
#ifndef ACTUATOR_SERVO_GPIO
#define ACTUATOR_SERVO_GPIO     GPIO_NUM_4
#endif

/** 电机 UART 引脚 */
#ifndef ACTUATOR_UART_PORT
#define ACTUATOR_UART_PORT      UART_NUM_1
#endif
#ifndef ACTUATOR_UART_TX
#define ACTUATOR_UART_TX        GPIO_NUM_6
#endif
#ifndef ACTUATOR_UART_RX
#define ACTUATOR_UART_RX        GPIO_NUM_7
#endif

/** 默认速度 (RPM, 0~5000) */
#ifndef ACTUATOR_DEFAULT_VEL
#define ACTUATOR_DEFAULT_VEL     500
#endif

/** 默认加速度 (0~255, 0=直接启动) */
#ifndef ACTUATOR_DEFAULT_ACC
#define ACTUATOR_DEFAULT_ACC     50
#endif

/** 脉冲换算系数 (坐标值 × scale = 脉冲数)
 *
 * X/Y 轴: 丝杆导程 8mm → 一圈 8mm = 3200 脉冲
 *          scale = 3200 / 8 = 400 脉冲/mm
 *
 * Z 旋转: angle_x10 单位为 0.1°
 *          一圈 360° = 3600 (0.1°) = 3200 脉冲
 *          scale = 3200 / 3600 ≈ 0.8889 脉冲/(0.1°)
 */
#ifndef ACTUATOR_SCALE_X
#define ACTUATOR_SCALE_X         400.0f              /* mm → 脉冲 */
#endif
#ifndef ACTUATOR_SCALE_Y
#define ACTUATOR_SCALE_Y         400.0f              /* mm → 脉冲 */
#endif
#ifndef ACTUATOR_SCALE_ANGLE
#define ACTUATOR_SCALE_ANGLE     ((float)EMM_V5_PULSES_PER_REV / 3600.0f)  /* 0.1° → 脉冲 */
#endif

/** 便捷宏: mm 直线距离 → 脉冲数 (四舍五入) */
#define ACTUATOR_MM_TO_PULSES(mm)  ((uint32_t)((float)(mm) * ACTUATOR_SCALE_X + 0.5f))

/** 软限位 (左上零点坐标系, 单位 mm) */
#define ACTUATOR_LIMIT_X_MIN       0.0f
#define ACTUATOR_LIMIT_X_MAX       210.0f
#define ACTUATOR_LIMIT_Y_MIN       0.0f
#define ACTUATOR_LIMIT_Y_MAX       335.0f

/** Z 旋转轴软限位 (±°, 机械行程) */
#ifndef ACTUATOR_ANGLE_LIMIT_DEG
#define ACTUATOR_ANGLE_LIMIT_DEG  270.0f
#endif

/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位 (实测 3000RPM + 250 档稳定) */
#ifndef ACTUATOR_EXEC_VEL
#define ACTUATOR_EXEC_VEL        3000
#endif
#ifndef ACTUATOR_EXEC_ACC
#define ACTUATOR_EXEC_ACC        250
#endif

/** Z 旋转轴执行速度与加速度 (结构刚性有限, 必须保持低速; 300RPM 已偏快) */
#ifndef ACTUATOR_EXEC_Z_VEL
#define ACTUATOR_EXEC_Z_VEL      50
#endif
#ifndef ACTUATOR_EXEC_Z_ACC
#define ACTUATOR_EXEC_Z_ACC      100
#endif

/** 运动到位等待裕量 (ms): 无到位反馈, 在计算运动时间上叠加的安全余量 */
#ifndef ACTUATOR_SETTLE_MARGIN_MS
#define ACTUATOR_SETTLE_MARGIN_MS  400
#endif

/** 舵机动作 / 磁铁吸合后的等待时间 (ms) */
#ifndef ACTUATOR_GRIP_SETTLE_MS
#define ACTUATOR_GRIP_SETTLE_MS    300
#endif

/**
 * 连续多帧之间的间隔 (ms)
 * Emm_V5 驱动板无法解析 UART 上零间隔的背靠背连续帧 (会丢字节),
 * 凡是一次动作需要连发多帧 (如 Y 轴双电机), 帧间必须插入该延时
 */
#ifndef ACTUATOR_FRAME_GAP_MS
#define ACTUATOR_FRAME_GAP_MS    10
#endif

/** 控制模式: 2 = 闭环位置模式 */
#define ACTUATOR_CTRL_MODE       2

/* ================================================================
 * Z 旋转轴心 ↔ 工具末端 偏移补偿
 *
 * 终端执行机构 (工具末端) 与 Z 旋转机构不在同一条纵轴上,
 * 旋转角度 θ 时, 工具末端的 XY 位置会绕旋转中心偏移。
 *
 * 补偿公式 (将工具末端定位到 target):
 *   X_motor = X_tgt - (dx * cosθ - dy * sinθ)
 *   Y_motor = Y_tgt - (dx * sinθ + dy * cosθ)
 *
 * dx, dy 为工具末端相对于 Z 旋转中心的 XY 偏移 (mm),
 * 待机械测量后填入。
 * ================================================================ */
#ifndef ACTUATOR_OFFSET_X_MM
#define ACTUATOR_OFFSET_X_MM     6.6f    /* angle=0° 时工具末端偏右 ~8mm (实测) */
#endif
#ifndef ACTUATOR_OFFSET_Y_MM
#define ACTUATOR_OFFSET_Y_MM     -35.75f  /* angle=0° 时工具末端偏上 ~30mm (实测) */
#endif

/**
 * XY 平面左上零点坐标系 运动方向:
 *   X 轴正向 (→): CW  (dir=0)  ── 电机2
 *   Y 轴正向 (↓): CCW (dir=1)  ── 电机3+4 (双电机同步)
 *   Z 旋转正向 (↻): CW (dir=0) ── 电机1
 */
#define ACTUATOR_DIR_X      0   /* X轴正向 = CW */
#define ACTUATOR_DIR_Y      1   /* Y轴正向 = CCW */
#define ACTUATOR_DIR_ANGLE  0   /* Z旋转正向 = CW */

/* ================================================================
 * API
 * ================================================================ */

/**
 * @brief  初始化所有执行器
 * @note   内部调用 Emm_V5_Init + servo_init + 4 路电机使能 + 闭环模式
 */
void actuator_init(void);

/**
 * @brief  设置脉冲换算系数
 * @param  x_scale     X 轴: 坐标值 × x_scale = 脉冲数
 * @param  y_scale     Y 轴: 坐标值 × y_scale = 脉冲数
 * @param  angle_scale Z 旋转: angle_x10 × angle_scale = 脉冲数
 */
void actuator_set_scale(float x_scale, float y_scale, float angle_scale);

/**
 * @brief  按 Emm_V5 曲线加减速公式估算纯运动时间 (ms)
 * @param  pulses  脉冲数 (行程)
 * @param  vel     速度 RPM
 * @param  acc     加速度档位 0~255 (0=直接启动, 无曲线)
 * @note   曲线公式: 每提升 1 RPM 耗时 (256-acc)*50us, 按梯形/三角形加减速估算
 */
uint32_t actuator_motion_time_ms(uint32_t pulses, uint16_t vel, uint8_t acc);

/**
 * @brief  运动到位等待 = 估算运动时间 + 安全裕量
 * @param  pulses  脉冲数 (行程)
 * @param  vel     速度 RPM
 * @param  acc     加速度档位 0~255
 */
uint32_t actuator_settle_ms(uint32_t pulses, uint16_t vel, uint8_t acc);

/**
 * @brief  直线轴运动到位等待 (按实际 mm 距离换算脉冲)
 * @param  distance_mm  直线距离 (mm, 取绝对值)
 * @param  vel          速度 RPM
 * @param  acc          加速度档位 0~255
 */
uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc);

/**
 * @brief  Z 旋转轴运动到位等待 (按实际角度换算脉冲)
 * @param  angle_deg  旋转角度 (°, 取绝对值)
 * @param  vel        速度 RPM
 * @param  acc        加速度档位 0~255
 */
uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc);

/**
 * @brief  设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm)
 * @param  offset_x_mm  工具末端相对于旋转中心的 X 偏移 (mm)
 * @param  offset_y_mm  工具末端相对于旋转中心的 Y 偏移 (mm)
 * @note   补偿会在每次 fragment 执行时自动应用;
 *         未调用时默认使用 ACTUATOR_OFFSET_X_MM / ACTUATOR_OFFSET_Y_MM
 */
void actuator_set_offset(float offset_x_mm, float offset_y_mm);

/**
 * @brief  获取当前 XY 偏移量
 * @param  offset_x_mm  [out] X 偏移 (mm), 可为 NULL
 * @param  offset_y_mm  [out] Y 偏移 (mm), 可为 NULL
 */
void actuator_get_offset(float *offset_x_mm, float *offset_y_mm);

/**
 * @brief  对目标坐标应用 Z 旋转偏移补偿
 * @param  target_x      [in/out] 目标 X 坐标 (mm)
 * @param  target_y      [in/out] 目标 Y 坐标 (mm)
 * @param  angle_deg     当前旋转角度 (度)
 * @note   内部使用, 也可由上层在手动模式调用
 */
void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
                                        float angle_deg);

/**
 * @brief  执行一个完整 Cycle (遍历所有 Fragment, 逐段运动)
 * @param  cycle  协议下发的 Cycle 数据
 * @return 0 成功, -1 参数无效
 */
int actuator_execute_cycle(const edc_cycle_t *cycle);

/**
 * @brief  急停全部 4 路电机
 */
void actuator_stop_all(void);

/**
 * @brief  舵机: 抬起 (上限位)
 */
void actuator_servo_up(void);

/**
 * @brief  舵机: 放下 (下限位)
 */
void actuator_servo_down(void);

/**
 * @brief  舵机: 设置任意角度
 * @param  angle  角度 0.0 ~ 180.0
 */
void actuator_servo_set_angle(float angle);

/* ---- 单电机手动控制 (Web 调试用, Y 轴自动同步) ---- */

/**
 * @brief  单电机使能控制 (Y 轴电机各自独立)
 * @param  addr   电机地址 (1~4)
 * @param  state  使能状态
 */
void actuator_motor_enable(uint8_t addr, bool state);

/**
 * @brief  单电机速度模式 (Y 轴地址 3/4 自动同步)
 * @param  addr  电机地址 (1~4)
 * @param  dir   方向, 0=CW, 非0=CCW
 * @param  vel   速度 0~5000 RPM
 * @param  acc   加速度 0~255
 */
void actuator_motor_vel(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc);

/**
 * @brief  单电机位置模式 (Y 轴地址 3/4 自动同步)
 * @param  addr  电机地址 (1~4)
 * @param  dir   方向, 0=CW, 非0=CCW
 * @param  vel   速度 0~5000 RPM
 * @param  acc   加速度 0~255
 * @param  clk   脉冲数
 * @param  raF   相对/绝对, false=相对, true=绝对
 */
void actuator_motor_pos(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
                        uint32_t clk, bool raF);

/**
 * @brief  单电机角度直控 (Y 轴地址 3/4 自动同步)
 * @param  addr      电机地址 (1~4)
 * @param  angle_deg 目标角度 (°), 内部换算为脉冲 (3200=360°)
 * @param  vel       速度 0~5000 RPM
 * @param  acc       加速度 0~255
 * @param  raF       相对/绝对, false=相对, true=绝对
 */
void actuator_motor_angle(uint8_t addr, float angle_deg, uint16_t vel,
                          uint8_t acc, bool raF);

/**
 * @brief  单电机直线距离控制 (X/Y 轴, Y 轴地址 3/4 自动同步)
 * @param  addr        电机地址 (2=X, 3/4=Y)
 * @param  distance_mm 直线移动距离 (mm), 丝杆导程 8mm/圈
 * @param  vel         速度 0~5000 RPM
 * @param  acc         加速度 0~255
 * @param  raF         相对/绝对, false=相对, true=绝对
 */
void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel,
                       uint8_t acc, bool raF);

/**
 * @brief  单电机立即停止 (Y 轴地址 3/4 自动同步)
 * @param  addr  电机地址 (1~4)
 */
void actuator_motor_stop(uint8_t addr);
/**
 * @brief  获取最近一次执行的 Cycle 快照 (供外部读取, 如 Web 面板)
 * @return 有效时返回指针, 否则 NULL
 */
const edc_cycle_t *actuator_get_last_cycle(void);

/**
 * @brief  最近 Cycle 是否有效
 */
bool actuator_is_last_cycle_valid(void);

/**
 * @brief  清除最近 Cycle 记录 (用于等待新 Cycle)
 */
void actuator_clear_last_cycle(void);

#ifdef __cplusplus
}
#endif

#endif /* ACTUATOR_H */