#ifndef _PID_CONTROL_H_
#define _PID_CONTROL_H_

#include "zf_common_typedef.h"

/*============================================================
 * 电机引脚定义 (与 cpu0_main.c 保持一致)
 *============================================================*/
#define MOTOR_DIR1                  (P11_9)
#define MOTOR_PWM1                  (ATOM3_CH5_P11_10)
#define MOTOR_DIR2                  (P11_11)
#define MOTOR_PWM2                  (ATOM2_CH7_P11_12)

/*============================================================
 * PID 单环结构体 (支持位置式/增量式, 带积分分离 + 输出限幅)
 *
 * mode: 0=位置式 (output = f(error) 直接输出)
 *       1=增量式 (output += Δf(error) 累积输出, 天然抗饱和)
 *============================================================*/
typedef struct {
    float kp, ki, kd;           /* PID 系数               */
    float setpoint;             /* 目标值 (外部写入)       */
    float feedback;             /* 反馈值 (外部写入)       */
    float error;                /* 当前误差 (只读)         */
    float integral;             /* 积分累加值 (位置式用)    */
    float prev_error;           /* e(k-1)                 */
    float prev_error2;          /* e(k-2), 增量式 D 项用   */
    float derivative;           /* 本次微分值 (只读)       */
    float output;               /* 本次输出 (只读)         */
    float out_max, out_min;     /* 输出限幅                */
    float integral_limit;       /* 积分分离阈值             */
    uint8 mode;                 /* 0=位置式, 1=增量式      */
} pid_t;

/*============================================================
 * 四轮车速度/转向 PID 全局句柄
 *
 * 前向运动:
 *   目标速度 → 左/右速度环 → 左/右PWM基础值
 *
 * 并行方向 (转向):
 *   转向环 → PWM差速值 (用陀螺仪 yaw rate 做 D 项反馈)
 *
 * 最终输出:
 *   左轮PWM = 左速度环输出 - PWM差速值
 *   右轮PWM = 右速度环输出 + PWM差速值
 *============================================================*/
extern pid_t g_pid_speed_left;  /* 左轮速度环 (内环, 100ms)      */
extern pid_t g_pid_speed_right; /* 右轮速度环 (内环, 100ms)      */
extern pid_t g_pid_steering;    /* 转向环 (并行,   帧周期)       */

/*============================================================
 * 电机输出接口 (每个电机驱动一侧两个轮子, 差速转向)
 *============================================================*/
#define PWM_DUTY_MAX_MOTOR   ((int16)9990)
#define MOTOR_PWM_DEADBAND   ((int16)3000)
#define SPEED_PID_OUT_MAX    ((float)(PWM_DUTY_MAX_MOTOR - MOTOR_PWM_DEADBAND))

void motor_set_left (int16 pwm);    /* 左电机: 正=前进, 负=后退 */
void motor_set_right(int16 pwm);    /* 右电机: 正=前进, 负=后退 */

/*============================================================
 * PID 核心函数
 *============================================================*/
void pid_init(pid_t *p, float kp, float ki, float kd,
              float out_max, float out_min, float integral_limit, uint8 mode);

float pid_compute(pid_t *p);        /* 位置式 */

float pid_compute_inc(pid_t *p);    /* 增量式 */

void pid_reset(pid_t *p);

/*============================================================
 * 控制环总控
 *============================================================*/

/* 速度环 (内环): 每100ms调用, 编码器速度 → PWM基础值
 *   target_speed 由 Base Spd 菜单参数设定 (全局变量)
 *   输出: 驱动左右电机 PWM
 */
void cascade_speed_loop(int16 enc1_count, int16 enc2_count);

/* 转向环 (并行): 每帧调用, 摄像头中线偏差 → PWM差速
 *   gyro_z 作为 D 项反馈, 抑制转向过冲和蛇形
 */
void cascade_steering_loop(float line_deviation, float gyro_z);

/* 控制使能 */
extern bool g_control_enabled;

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

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

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

#endif /* _PID_CONTROL_H_ */