#include "common.h"
#include "modules_common.h"
#include "ptz0404_control.h"
#include "gpio_common.h"
#include "ptz_common.h"
#include <math.h>

/* ===================== 运动学参数（分离配置） ===================== */
#define CONTROL_PERIOD    0.02f      // 20ms 调用一次

// --- 水平方向 (左右) 参数 ---
// 为了配合“立即停止”，建议适当加大加速度，让起步也更灵敏
#define PAN_MAX_SPEED     80.0f      // 左右最大速度 °/s
#define PAN_ACCEL         180.0f     // 左右加速度 °/s²

// --- 垂直方向 (上下) 参数 ---
#define TILT_MAX_SPEED    50.0f     // 上下最大速度 °/s
#define TILT_ACCEL        80.0f     // 上下加速度 °/s²

#define STOP_DEADZONE     0.2f       // 停止死区（度）

/* ===================== 角度限位 ===================== */
#define PAN_ANGLE_INITIAL 135.0f
#define PAN_MIN_ANGLE     90.0f
#define PAN_MAX_ANGLE     180.0f

#define TILT_ANGLE_INITIAL 70.0f
#define TILT_MIN_ANGLE    35.0f
#define TILT_MAX_ANGLE    90.0f

/* ===================== 对象初始化 ===================== */
static ServoAxis servo_pan = {
    .angle = PAN_ANGLE_INITIAL, .target = PAN_ANGLE_INITIAL, .speed = 0.0f,
    .min_limit = PAN_MIN_ANGLE, .max_limit = PAN_MAX_ANGLE,
    .max_speed_cfg = PAN_MAX_SPEED, 
    .accel_cfg = PAN_ACCEL
};

static ServoAxis servo_tilt = {
    .angle = TILT_ANGLE_INITIAL, .target = TILT_ANGLE_INITIAL, .speed = 0.0f,
    .min_limit = TILT_MIN_ANGLE, .max_limit = TILT_MAX_ANGLE,
    .max_speed_cfg = TILT_MAX_SPEED, 
    .accel_cfg = TILT_ACCEL
};

/* ===================== PWM 初始化 ===================== */
void pwm_ptz0404_hz(void)
{
    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 ptz0404_calculate_pulse_width(float angle)
{
    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 ptz0404_calculate_duty_cycle(float pulse_width)
{
    float period = 1000.0f / SERVO_FREQ;
    return (int)((pulse_width / period) * PWM_RANGE);
}

static void ptz0404_servo_apply(int pin, float angle)
{
    float pulse_width = ptz0404_calculate_pulse_width(angle);
    int pulse = ptz0404_calculate_duty_cycle(pulse_width);
    pwmWrite(pin, pulse);
}

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

    // 1. 检查是否进入静止死区
    if (abs_error < STOP_DEADZONE && fabs(s->speed) < (s->accel_cfg * CONTROL_PERIOD)) {
        s->angle = s->target;
        s->speed = 0;
        return;
    }

    // 2. 计算刹车距离 (d = v^2 / 2a)
    float braking_dist = (s->speed * s->speed) / (2.0f * s->accel_cfg);

    // 3. 速度规划
    float desired_speed;
    if (abs_error <= braking_dist) {
        desired_speed = 0;
    } else {
        desired_speed = (error > 0) ? s->max_speed_cfg : -s->max_speed_cfg;
    }

    // 4. 根据加速度计算当前步进速度
    float speed_step = s->accel_cfg * CONTROL_PERIOD;
    
    if (s->speed < desired_speed) {
        s->speed += speed_step;
        if (s->speed > desired_speed && desired_speed >= 0) s->speed = desired_speed;
    } else if (s->speed > desired_speed) {
        s->speed -= speed_step;
        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 ptz0404_pwm_task(void)
{
    ptz0404_servo_update(&servo_pan);
    ptz0404_servo_update(&servo_tilt);

    ptz0404_servo_apply(PWM_PIN_down, servo_pan.angle);
    ptz0404_servo_apply(PWM_PIN_up,   servo_tilt.angle);
}

void ptz0404_pwm_task_function(void *arg)
{
    my_zlog_info("PTZ_pwm_task_function start");
    while(1) {
        ptz0404_pwm_task();
        delay_ms(20); 
    }
}

void ptz0404_pwm_init(void)
{
    pwm_ptz0404_hz(); 

    servo_pan.angle  = servo_pan.target  = PAN_ANGLE_INITIAL;
    servo_tilt.angle = servo_tilt.target = TILT_ANGLE_INITIAL;
    servo_pan.speed = 0;
    servo_tilt.speed = 0;

    ptz0404_servo_apply(PWM_PIN_down, servo_pan.angle);
    ptz0404_servo_apply(PWM_PIN_up,   servo_tilt.angle);

    device_ptz_pwm_task_threadpoll_init();
}

/* ===================== 控制接口 ===================== */
void ptz0404_pwm_left(void)
{
    servo_pan.target -= 10.0f; 
    if (servo_pan.target < PAN_MIN_ANGLE)
        servo_pan.target = PAN_MIN_ANGLE;
}

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

void ptz0404_pwm_up()
{
    servo_tilt.target -= 10.0f;
    if (servo_tilt.target < TILT_MIN_ANGLE) 
        servo_tilt.target = TILT_MIN_ANGLE;
}

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

/* ===================== 停止逻辑（关键修改） ===================== */
void ptz0404_pwm_stop(void)
{
    // --- 左右 (Pan) 立即停止 ---
    // 1. 设置目标为当前角度（不再继续走）
    servo_pan.target  = servo_pan.angle;
    // 2. 【核心】强制速度清零。
    //    如果不清零，算法会根据加速度计算刹车距离并滑行一段。
    //    强制清零后，位置更新立即停止，实现“急停”。
    servo_pan.speed   = 0.0f; 

    // --- 上下 (Tilt) 保持平滑缓冲 ---
    // 只重置目标，让算法自动平滑减速到停止
    servo_tilt.target = servo_tilt.angle;
}
  
/* ===================== 指令解析 ===================== */
void ptz0404_pwm_change(int *buf)
{
    // 假设 buf[1] 是命令字，通常协议里会有停止命令（例如 0 或 5）
    // 如果收到 0 或者未定义的命令，应该调用 stop
     if (buf[2] == 0) {
        ptz0404_pwm_stop();
        return; // 处理完停止直接退出，不检查 buf[2]
    }
    
    my_zlog_info("PTZ Receive CMD: %d", buf[1]);

    switch (buf[1]) {
        case 1: ptz0404_pwm_up();    break;
        case 2: ptz0404_pwm_down();  break;
        case 3: ptz0404_pwm_right(); break;
        case 4: ptz0404_pwm_left();  break;
        // 添加停止处理 (假设 0 是停止，具体看您的协议)
        default:  break; // 其他情况也停止，保证安全
    }
}