/*********************************************************************************************************************
* TC264 智能车 - 四轮差速速度/转向PID控制模块 (pid_control.c)
*
* 适用: 四轮小车, 两个电机各驱动一侧两个轮子 (差速转向)
* 传感器: 2个编码器 + 1个陀螺仪(IMU) + 摄像头
*
* 控制结构:
*   前向运动:
*     目标速度 → 左/右速度环(内环) → 左/右PWM基础值
*
*   并行 (转向):
*     转向环 → PWM差速值 (陀螺仪 yaw rate 作为 D 项阻尼)
*
*   最终:
*     左轮PWM = 左速度环输出 - 转向环输出
*     右轮PWM = 右速度环输出 + 转向环输出
*
* 调用周期:
*   速度环:          100ms (CCU61_CH0 中断)
*   转向环:          帧周期 (cpu0_main 主循环, 或 5ms ISR 中更新 gyro D 项)
*********************************************************************************************************************/

#include "zf_common_headfile.h"
#include "pid_control.h"

/* ==================== PID 句柄 ==================== */
pid_t g_pid_speed_left;
pid_t g_pid_speed_right;
pid_t g_pid_steering;

/* ==================== 控制使能 ==================== */
bool g_control_enabled = FALSE;

/* ==================== 级联变量 ==================== */

/* 目标速度 (Base Spd → 速度环输入), 单位: 编码器脉冲/100ms */
float g_target_speed = 0.0f;

/* 最终下发到左右电机的 PWM 命令 (含死区补偿和转向差速) */
float g_motor_pwm_left_cmd = 0.0f;
float g_motor_pwm_right_cmd = 0.0f;

/* 陀螺仪 z 轴角速度 (°/s), 在 5ms ISR 中更新, 供转向环 D 项使用 */
float g_gyro_z_dps = 0.0f;

/* 转向差速分量 (转向环输出), 单位: PWM占空比 (叠加到左右轮) */
static float g_steering_offset = 0.0f;

static int8 g_speed_target_dir_last = 0;

/* ==================== 电机引脚 ==================== */
/*
 * 左电机: MOTOR_PWM1 = ATOM3_CH5_P11_10,  MOTOR_DIR1 = P11_9
 *         驱动左侧两个轮子
 * 右电机: MOTOR_PWM2 = ATOM2_CH7_P11_12,  MOTOR_DIR2 = P11_11
 *         驱动右侧两个轮子
 *
 * 方向极性 (需实测确认, 以下为默认约定):
 *   左电机: DIR1=1 前进,  DIR1=0 后退
 *   右电机: DIR2=1 后退,  DIR2=0 前进 (对向安装, 极性与左相反)
 */

void motor_set_left(int16 pwm)
{
    int16 abs_pwm;

    if(pwm > PWM_DUTY_MAX_MOTOR)  pwm = PWM_DUTY_MAX_MOTOR;
    if(pwm < -PWM_DUTY_MAX_MOTOR) pwm = -PWM_DUTY_MAX_MOTOR;

    abs_pwm = (pwm >= 0) ? pwm : -pwm;

    if(pwm > 0)
    {
        gpio_set_level(MOTOR_DIR2, 1);          /* 前进 */
        pwm_set_duty(MOTOR_PWM2, (uint32)abs_pwm);
    }
    else if(pwm < 0)
    {
        gpio_set_level(MOTOR_DIR2, 0);          /* 后退 */
        pwm_set_duty(MOTOR_PWM2, (uint32)abs_pwm);
    }
    else
    {
        pwm_set_duty(MOTOR_PWM2, 0);            /* 停止 */
    }
}

void motor_set_right(int16 pwm)
{
    int16 abs_pwm;

    if(pwm > PWM_DUTY_MAX_MOTOR)  pwm = PWM_DUTY_MAX_MOTOR;
    if(pwm < -PWM_DUTY_MAX_MOTOR) pwm = -PWM_DUTY_MAX_MOTOR;

    abs_pwm = (pwm >= 0) ? pwm : -pwm;

    if(pwm > 0)
    {
        gpio_set_level(MOTOR_DIR1, 1);          /* 前进 */
        pwm_set_duty(MOTOR_PWM1, (uint32)abs_pwm);
    }
    else if(pwm < 0)
    {
        gpio_set_level(MOTOR_DIR1, 0);          /* 后退 */
        pwm_set_duty(MOTOR_PWM1, (uint32)abs_pwm);
    }
    else
    {
        pwm_set_duty(MOTOR_PWM1, 0);            /* 停止 */
    }
}

static int16 motor_pwm_clamp_int32(int32 pwm)
{
    if(pwm > PWM_DUTY_MAX_MOTOR)  pwm = PWM_DUTY_MAX_MOTOR;
    if(pwm < -PWM_DUTY_MAX_MOTOR) pwm = -PWM_DUTY_MAX_MOTOR;

    return (int16)pwm;
}

static int8 speed_target_dir(float target_speed)
{
    if(target_speed > 1.0f)  return 1;
    if(target_speed < -1.0f) return -1;

    return 0;
}

static int16 speed_pwm_add_deadband(int16 pid_output, int8 target_dir)
{
    int32 pwm_cmd;

    if(target_dir > 0)
    {
        pwm_cmd = (int32)MOTOR_PWM_DEADBAND + (int32)pid_output;
        if(pwm_cmd < 0) pwm_cmd = 0;
    }
    else if(target_dir < 0)
    {
        pwm_cmd = -(int32)MOTOR_PWM_DEADBAND + (int32)pid_output;
        if(pwm_cmd > 0) pwm_cmd = 0;
    }
    else
    {
        pwm_cmd = 0;
    }

    return motor_pwm_clamp_int32(pwm_cmd);
}

/* ==================== PID 核心 ==================== */

void pid_init(pid_t *p, float kp, float ki, float kd,
              float out_max, float out_min, float integral_limit, uint8 mode)
{
    p->kp = kp;
    p->ki = ki;
    p->kd = kd;
    p->out_max = out_max;
    p->out_min = out_min;
    p->integral_limit = integral_limit;
    p->mode = mode;
    p->setpoint = 0.0f;
    p->feedback = 0.0f;
    p->error = 0.0f;
    p->integral = 0.0f;
    p->prev_error = 0.0f;
    p->prev_error2 = 0.0f;
    p->derivative = 0.0f;
    p->output = 0.0f;
}

float pid_compute(pid_t *p)
{
    p->error = p->setpoint - p->feedback;

    /* 积分分离 */
    if(p->integral_limit > 0.0f)
    {
        float abs_err = (p->error >= 0.0f) ? p->error : -p->error;
        if(abs_err > p->integral_limit)
        {
            p->integral = 0.0f;
        }
    }

    /* 积分累加 + 抗饱和 (仅在 ki != 0 时累加, 避免无效累积) */
    if(p->ki != 0.0f)
    {
        if((p->output < p->out_max) && (p->output > p->out_min))
        {
            p->integral += p->error;
        }
    }

    /* 微分先行 (仅在 kd != 0 时计算, 避免无效更新) */
    if(p->kd != 0.0f)
    {
        p->derivative = p->feedback - p->prev_error;
        p->prev_error = p->feedback;
    }

    /* 位置式 PID */
    p->output = p->kp * p->error
              + p->ki * p->integral
              - p->kd * p->derivative;

    /* 输出限幅 */
    if(p->output > p->out_max)  p->output = p->out_max;
    if(p->output < p->out_min)  p->output = p->out_min;

    return p->output;
}

/* 增量式 PID:
 *   Δu = kp*(e(k)-e(k-1)) + ki*e(k) + kd*(e(k)-2*e(k-1)+e(k-2))
 *   u(k) = clamp(u(k-1) + Δu)
 * 天然抗积分饱和, 输出平滑 */
float pid_compute_inc(pid_t *p)
{
    float delta;

    p->error = p->setpoint - p->feedback;

    /* 增量式不需要显式积分, ki 项天然隐含积分效果 */
    delta = p->kp * (p->error - p->prev_error)
          + p->ki * p->error
          + p->kd * (p->error - 2.0f * p->prev_error + p->prev_error2);

    /* 滚动历史误差 */
    p->prev_error2 = p->prev_error;
    p->prev_error  = p->error;

    p->output += delta;

    if(p->output > p->out_max)  p->output = p->out_max;
    if(p->output < p->out_min)  p->output = p->out_min;

    return p->output;
}

void pid_reset(pid_t *p)
{
    p->integral = 0.0f;
    p->prev_error = 0.0f;
    p->prev_error2 = 0.0f;

    /* 增量式复位时清零累积输出, 避免突变 */
    if(p->mode == 1)
    {
        p->output = 0.0f;
    }
}

/* ==================== 速度环 (内环, 100ms) ==================== */
/*
 * 输入: enc1_count, enc2_count — 过去100ms编码器脉冲增量
 * 左右速度独立闭环, enc1 默认对应左侧电机, enc2 默认对应右侧电机
 * 输出: 直接驱动左右电机 PWM
 */
void cascade_speed_loop(int16 enc1_count, int16 enc2_count)
{
    float current_speed_left;
    float current_speed_right;
    int16 pwm_left;
    int16 pwm_right;
    int16 motor_pwm_left;
    int16 motor_pwm_right;
    int8 target_dir;

    if(!g_control_enabled) return;

    target_dir = speed_target_dir(g_target_speed);
    if(target_dir == 0)
    {
        pid_reset(&g_pid_speed_left);
        pid_reset(&g_pid_speed_right);
        g_speed_target_dir_last = 0;
        g_motor_pwm_left_cmd = 0.0f;
        g_motor_pwm_right_cmd = 0.0f;
        motor_set_left(0);
        motor_set_right(0);
        return;
    }

    if(target_dir != g_speed_target_dir_last)
    {
        pid_reset(&g_pid_speed_left);
        pid_reset(&g_pid_speed_right);
        g_speed_target_dir_last = target_dir;
    }

    current_speed_left  = (float)enc1_count;
    current_speed_right = (float)enc2_count;

    g_pid_speed_left.feedback = current_speed_left;
    g_pid_speed_left.setpoint = g_target_speed;
    pid_compute_inc(&g_pid_speed_left);

    g_pid_speed_right.feedback = current_speed_right;
    g_pid_speed_right.setpoint = g_target_speed;
    pid_compute_inc(&g_pid_speed_right);

    pwm_left  = speed_pwm_add_deadband((int16)g_pid_speed_left.output, target_dir);
    pwm_right = speed_pwm_add_deadband((int16)g_pid_speed_right.output, target_dir);

    motor_pwm_left  = motor_pwm_clamp_int32((int32)((float)pwm_left - g_steering_offset));
    motor_pwm_right = motor_pwm_clamp_int32((int32)((float)pwm_right + g_steering_offset));
    g_motor_pwm_left_cmd = (float)motor_pwm_left;
    g_motor_pwm_right_cmd = (float)motor_pwm_right;

    /* 叠加转向差速 → 左右电机 */
    motor_set_left(motor_pwm_left);
    motor_set_right(motor_pwm_right);
}

/* ==================== 转向环 (并行, 帧周期) ==================== */
/*
 * 输入: line_deviation — 摄像头中线偏差 (像素), 正=偏右需右转
 *       gyro_z         — IMU z轴角速度 (°/s), 用于 D 项阻尼
 * 输出: g_steering_offset — PWM差速值
 *
 * PD控制: P项根据中线偏差产生转向力, D项用gyro_z抑制过冲
 * 不加 I 项: 转向不需要消除静差 (弯道需要持续差速)
 */
void cascade_steering_loop(float line_deviation, float gyro_z)
{
    if(!g_control_enabled) return;

    /* 转向 PD:
     *   P项: 中线偏差 → 差速
     *   D项: gyro_z 反馈 → 阻尼 (车正在右转时 gyro_z>0, 需要抑制)
     */
    g_pid_steering.feedback = line_deviation;
    g_pid_steering.setpoint = 0.0f;           /* 目标: 中线居中 */
    pid_compute(&g_pid_steering);

    /* 叠加 gyro D 项 (在 PID 输出基础上减去角速度反馈) */
    g_steering_offset = g_pid_steering.output
                      - g_pid_steering.kd * gyro_z;

    /* 限幅 */
    if(g_steering_offset > g_pid_steering.out_max)
        g_steering_offset = g_pid_steering.out_max;
    if(g_steering_offset < g_pid_steering.out_min)
        g_steering_offset = g_pid_steering.out_min;
}