/**
 * bldc_pid.c — 三环 PI/P 控制器实现
 *
 * 三环关系:
 *   位置环(P) → 速度参考 → 速度环(PI) → 电流参考 → 电流环(PI) → PWM占空比
 *   频率50Hz      频率500Hz             频率20kHz
 *
 * 全部 Q15 定点数, 电流用 mA 单位, 转速用 RPM 单位
 */

#include "bldc_pid.h"

/* ==================== PID 控制器实例 ==================== */
/*
 * 参数调试起点 (需根据实际电机调整):
 *   电流环 Kp=0.3, Ki=0.05
 *   速度环 Kp=0.5, Ki=0.02
 *   位置环 Kp=1.0
 */

/* 电流环 PI — 20kHz 运行 */
PiController g_pid_current = {
    .kp           = Q15_FLOAT(0.3f),
    .ki           = Q15_FLOAT(0.05f),
    .integral     = 0,
    .integral_max = Q15_FLOAT(1.0f),    /* 积分限幅 100% 占空比 */
    .out_min      = 0,                   /* 正向驱动, 最小 0% */
    .out_max      = Q15_FLOAT(1.0f),     /* 最大 100% 占空比 */
};

/* 速度环 PI — 500Hz 运行 */
PiController g_pid_speed = {
    .kp           = Q15_FLOAT(0.5f),
    .ki           = Q15_FLOAT(0.02f),
    .integral     = 0,
    .integral_max = Q15_FLOAT(1.2f),    /* 积分限幅(120%额定电流) */
    .out_min      = 0,
    .out_max      = Q15_FLOAT(1.0f),     /* 电流参考最大 100% */
};

/* 位置环 P — 50Hz 运行 */
int16_t g_pid_pos_kp = Q15_FLOAT(1.0f);

/* ==================== 初始化 ==================== */
void PID_Init(void)
{
    /* 重置积分项 */
    PID_ResetIntegral(&g_pid_current);
    PID_ResetIntegral(&g_pid_speed);
}

/* ==================== PI 控制器一步运算 ==================== */
/*
 * 输入:  error_q15 — 误差的 Q15 表示
 *                   电流环: error = I_ref - I_bus
 *                   速度环: error = speed_ref - speed_actual
 * 输出:  Q15 值, 范围 [out_min, out_max]
 *
 * 抗积分饱和: 当输出限幅时, 积分项不再累加 (条件积分)
 */
int16_t PID_Update(PiController *pid, int16_t error_q15)
{
    /* 1. 计算比例项 */
    int16_t p_term = Q15_MUL(pid->kp, error_q15);

    /* 2. 计算积分项 (条件积分, 反积分饱和) */
    int16_t i_term = 0;
    int16_t new_integral = pid->integral + Q15_MUL(pid->ki, error_q15);

    /* 积分限幅 */
    if (new_integral > pid->integral_max) {
        new_integral = pid->integral_max;
    } else if (new_integral < -pid->integral_max) {
        new_integral = -pid->integral_max;
    }

    i_term = new_integral;
    pid->integral = new_integral;

    /* 3. 计算输出 = P + I */
    int32_t output = (int32_t)p_term + (int32_t)i_term;

    /* 4. 输出限幅 */
    if (output > pid->out_max) {
        output = pid->out_max;
        /* 反积分饱和: 输出限幅时积分的增量退掉 */
        pid->integral = pid->integral_max;
    } else if (output < pid->out_min) {
        output = pid->out_min;
        pid->integral = 0;  /* 反向限幅时清零积分(电流环不反转) */
    }

    return (int16_t)output;
}

/* ==================== 电流环封装 ==================== */
/*
 * 输入电流以 mA 为单位, 归一化到 Q15
 * I_ref_q15 = I_ref_mA / I_max_mA * Q15_ONE
 * feedback_q15 = I_bus_mA / I_max_mA * Q15_ONE
 */
int16_t PID_CurrentRun(int16_t i_ref_q15, int16_t i_fb_q15)
{
    int16_t error = i_ref_q15 - i_fb_q15;
    int16_t output = PID_Update(&g_pid_current, error);

    /* 输出映射到 PWM 占空比 */
    /* output 范围 [0, Q15_ONE], 映射到 [PWM_DUTY_MIN, PWM_DUTY_MAX] */
    uint32_t duty = (uint32_t)PWM_DUTY_MIN +
        ((uint32_t)(PWM_DUTY_MAX - PWM_DUTY_MIN) * (uint32_t)output) / (uint32_t)Q15_ONE;

    return (int16_t)duty;
}

/* ==================== 速度环封装 ==================== */
/*
 * target_rpm, actual_rpm 均以 Q15 表示
 * 输出 Q15 值, 范围 [0, Q15_ONE] — 作为电流环 I_ref
 */
int16_t PID_SpeedRun(int16_t target_rpm_q15, int16_t actual_rpm_q15)
{
    int16_t error = target_rpm_q15 - actual_rpm_q15;
    int16_t output = PID_Update(&g_pid_speed, error);

    /* 输出限幅到 [0, Q15_ONE] 作为 I_ref */
    if (output < 0) output = 0;

    return output;
}