// #include "common.h"
// #include "modules_common.h"
// #include "ptz0402_control.h"
// #include "gpio_common.h"
// #include <math.h>

// /* ===================== 硬件/PWM 参数 ===================== */
// #define PWM_PIN_up        21
// #define PWM_PIN_down      2

// #define SERVO_FREQ        50.0f
// #define PWM_RANGE         1024

// // 舵机脉冲宽度（ms）- 适配 270 度舵机
// #define MIN_PULSE_WIDTH   0.5f
// #define MAX_PULSE_WIDTH   2.5f

// /* ===================== 运动学参数（平滑控制核心） ===================== */
// #define CONTROL_PERIOD    0.02f      // 20ms 调用一次
// #define SERVO_MAX_SPEED   180.0f     // 最大角速度 °/s
// #define SERVO_ACCEL       800.0f     // 加速度 °/s²
// #define STOP_DEADZONE     0.2f      // 停止死区（度）

// /* ===================== 角度限位 ===================== */
// #define PAN_MIN_ANGLE     0.0f
// #define PAN_MAX_ANGLE     270.0f

// #define TILT_MIN_ANGLE    0.0f
// #define TILT_MAX_ANGLE    270.0f



// static ServoAxis servo_pan  = {90.0f, 90.0f, 0.0f, PAN_MIN_ANGLE, PAN_MAX_ANGLE};
// static ServoAxis servo_tilt = {90.0f, 90.0f, 0.0f, TILT_MIN_ANGLE, TILT_MAX_ANGLE};

// /* ===================== PWM 初始化 ===================== */
// void pwm_ptz0402_hz(void)
// {
//     // 计算时钟频率，确保输出 50Hz PWM
//     int pwm_clock = 24000000 / (SERVO_FREQ * 1000); 

//     pinMode(PWM_PIN_up, PWM_OUTPUT);
//     pinMode(PWM_PIN_down, PWM_OUTPUT);

//     pwmSetClock(PWM_PIN_up, pwm_clock);
//     pwmSetRange(PWM_PIN_up, PWM_RANGE);

//     pwmSetClock(PWM_PIN_down, pwm_clock);
//     pwmSetRange(PWM_PIN_down, PWM_RANGE);
// }

// /* ===================== 角度 → PWM 映射 ===================== */
// static float calculate_pulse_width(float angle)
// {
//     // 硬件物理保护：不允许超过 0-270 度范围
//     if (angle < 0) angle = 0;
//     if (angle > 270) angle = 270;

//     return ((MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) / 270.0f) * angle + MIN_PULSE_WIDTH;
// }

// static int calculate_duty_cycle(float pulse_width)
// {
//     float period = 1000.0f / SERVO_FREQ; // 20ms
//     return (int)((pulse_width / period) * PWM_RANGE);
// }

// static void servo_apply(int pin, float angle)
// {
//     float pulse_width = calculate_pulse_width(angle);
//     int pulse = calculate_duty_cycle(pulse_width);
//     pwmWrite(pin, pulse);
// }

// /* ===================== 核心：带减速预测的运动学更新 ===================== */
// static void servo_update(ServoAxis *s)
// {
//     float error = s->target - s->angle;
//     float abs_error = fabs(error);

//     // 1. 检查是否进入静止死区
//     // 只有在距离足够近且速度已经降下来时才真正停止，防止突然卡死
//     if (abs_error < STOP_DEADZONE && fabs(s->speed) < (SERVO_ACCEL * CONTROL_PERIOD)) {
//         s->angle = s->target;
//         s->speed = 0;
//         return;
//     }

//     // 2. 计算刹车距离 (物理公式: d = v^2 / 2a)
//     // 预测以当前加速度减速到 0 需要走多少度
//     float braking_dist = (s->speed * s->speed) / (2.0f * SERVO_ACCEL);

//     // 3. 速度规划
//     float desired_speed;
//     if (abs_error <= braking_dist) {
//         // 如果剩余距离已经小于等于刹车距离，目标速度应设为 0（开始减速）
//         desired_speed = 0;
//     } else {
//         // 否则，向目标方向以最大速度运行
//         desired_speed = (error > 0) ? SERVO_MAX_SPEED : -SERVO_MAX_SPEED;
//     }

//     // 4. 根据加速度计算当前步进速度
//     if (s->speed < desired_speed) {
//         s->speed += SERVO_ACCEL * CONTROL_PERIOD;
//         if (s->speed > desired_speed && desired_speed >= 0) s->speed = desired_speed;
//     } else if (s->speed > desired_speed) {
//         s->speed -= SERVO_ACCEL * CONTROL_PERIOD;
//         if (s->speed < desired_speed && desired_speed <= 0) s->speed = desired_speed;
//     }

//     // 5. 更新实际位置
//     s->angle += s->speed * CONTROL_PERIOD;

//     // 6. 软限位二次约束：防止计算导致的越界抽动
//     if (s->angle < s->min_limit) {
//         s->angle = s->min_limit;
//         s->speed = 0;
//     } else if (s->angle > s->max_limit) {
//         s->angle = s->max_limit;
//         s->speed = 0;
//     }
// }

// /* ===================== 任务与线程逻辑 ===================== */
// void PTZ_pwm_task(void)
// {
//     servo_update(&servo_pan);
//     servo_update(&servo_tilt);

//     servo_apply(PWM_PIN_down, servo_pan.angle);
//     servo_apply(PWM_PIN_up,   servo_tilt.angle);
// }

// void ptz_pwm_task_function(void *arg)
// {
//     my_zlog_info("PTZ_pwm_task_function start");
//     while(1) {
//         PTZ_pwm_task();
//         delay_ms(20); // 50Hz 频率更新
//     }
// }

// /* 云台线程初始化 */
// void device_ptz_pwm_task_threadpoll_init()
// {
//     my_zlog_info("PTZ_pwm_thread_init start");
//     // 假设 g_pool_device_gpio_control_t 是全局线程池变量
//     g_pool_device_gpio_control_t = thread_pool_init(1, 1);
//     thread_pool_add_task(g_pool_device_gpio_control_t, ptz_pwm_task_function, NULL);
// }

// /* 全局初始化接口 */
// void ptz0402_pwm_init(void)
// {
//     pwm_ptz0402_hz(); // 必须先初始化硬件配置

//     servo_pan.angle  = servo_pan.target  = 90.0f;
//     servo_tilt.angle = servo_tilt.target = 90.0f;

//     servo_apply(PWM_PIN_down, servo_pan.angle);
//     servo_apply(PWM_PIN_up,   servo_tilt.angle);

//     device_ptz_pwm_task_threadpoll_init();
// }

// /* ===================== 控制接口（修改目标角度） ===================== */
// void PTZ_pwm_left(void)
// {
//     servo_pan.target -= 10.0f; // 步长可以适当加大，因为有平滑过渡
//     if (servo_pan.target < PAN_MIN_ANGLE)
//         servo_pan.target = PAN_MIN_ANGLE;
// }

// void PTZ_pwm_right(void)
// {
//     servo_pan.target += 10.0f;
//     if (servo_pan.target > PAN_MAX_ANGLE)
//         servo_pan.target = PAN_MAX_ANGLE;
// }

// void PTZ_pwm_up()
// {
//     servo_tilt.target += 10.0f;
//     if (servo_tilt.target > TILT_MAX_ANGLE)
//         servo_tilt.target = TILT_MAX_ANGLE;
// }

// void PTZ_pwm_down()
// {

//     servo_tilt.target -= 10.0f;
//     if (servo_tilt.target < TILT_MIN_ANGLE)
//         servo_tilt.target = TILT_MIN_ANGLE;
// }

// void ptz0402_pwm_stop(void)
// {
//     // 停止不是瞬间清零，而是将目标设为当前位置，让算法平滑减速
//     servo_pan.target  = servo_pan.angle;
//     servo_tilt.target = servo_tilt.angle;
// }
  
// /* ===================== 指令解析 ===================== */
// void ptz0402_pwm_change(unsigned char *buf)
// {
//     // buf[2] 通常作为有效性检测或速度系数
//     if (buf[2] == 0) return;

//     my_zlog_info("PTZ Receive CMD: %d", buf[1]);

//     switch (buf[1]) {
//         case 1: PTZ_pwm_up();    break;
//         case 2: PTZ_pwm_down();  break;
//         case 3: PTZ_pwm_right(); break;
//         case 4: PTZ_pwm_left();  break;
//         default: break;
//     }
// }