/**
 * @file Algorithm/pid.cpp
 * @brief 算法模块源文件
 * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
 */

#include "pid.h"
#define _constrain(amt, low, high)                                             \
	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#include "Can.h"
#include "config.h"
#include <cmath>

extern CAN_transmiter CAN0_transmiter;
// 微分项一阶低通离散系数：α = Ts / (τ + Ts)
constexpr float DERIV_LPF_ALPHA = TIME_SLICE / (DERIV_LPF_TAU + TIME_SLICE);

PIDController::PIDController(float P, float I, float D, float ramp, float limit)
	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
	  ,
	  limit(limit) // PID控制器输出限幅
	  ,
	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f),
	  measurement_prev(0.0f), derivative_filt(0.0f), deriv_init(false) {}

// 测量微分版本：D 项对测量量而非误差微分，目标阶跃不再产生微分踢
float PIDController::operator()(float error, float measurement) {
	// 首次调用只建立测量基线，避免 (meas-0)/Ts 的假微分尖峰
	if (!deriv_init) {
		measurement_prev = measurement;
		deriv_init = true;
	}
	float d_raw = -(measurement - measurement_prev) / TIME_SLICE;
	derivative_filt += DERIV_LPF_ALPHA * (d_raw - derivative_filt);
	measurement_prev = measurement;
	return step(error, D * derivative_filt);
}

// 公共计算路径：Tustin 积分 + 条件积分抗饱和 + 输出斜坡
float PIDController::step(float error, float derivative) {
	// P环
	float proportional = P * error;

	// Tustin 散点积分（I环）
	float integral =
		integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);

	// 将P,I,D三环的计算值加起来，与限幅比较得到未饱和/饱和输出
	float output_unsat = proportional + integral + derivative;
	float output = _constrain(output_unsat, -limit, limit);

	// 抗积分饱和（条件积分）：输出饱和且误差仍在推高饱和方向时冻结积分。
	// 不用大增益反算回退——深饱和时（P 项单独超限幅）反算会把积分压成反号，
	// 误差一缩小输出瞬间反向，实测表现为到位疯狂抖动/停不到位。
	if (output != output_unsat) {
		if ((output_unsat > limit && error > 0.0f) ||
			(output_unsat < -limit && error < 0.0f)) {
			integral = integral_prev; // 冻结：不再向饱和方向累积
		}
	}
	integral = _constrain(integral, -limit, limit);

	// 输出斜坡限制
	if (output_ramp > 0) {
		float max_delta = output_ramp * TIME_SLICE;
		output = output_prev +
				 fmaxf(-max_delta, fminf(max_delta, output - output_prev));
	}
	// 保存值（为了下一次循环）
	integral_prev = integral;
	output_prev = output;
	error_prev = error;

	return output;
}

/**
 * @brief 外部饱和泄放积分：前馈叠加后超出限幅时调用
 * @param[in] excess 超出限幅的部分（带符号）
 * @return void
 * @note 仅当积分与超限同号（积分在加剧超限）时泄放，且不泄过零点：
 *       防止前馈导致的饱和把积分抽干成反号，到位段失去保持力。
 */
void PIDController::bleed_integral(float excess) {
	if (excess > 0.0f && integral_prev > 0.0f) {
		float bleed = ANTI_WINDUP_GAIN * excess;
		integral_prev = (integral_prev > bleed) ? (integral_prev - bleed)
												: 0.0f;
	} else if (excess < 0.0f && integral_prev < 0.0f) {
		float bleed = ANTI_WINDUP_GAIN * (-excess);
		integral_prev = (integral_prev < -bleed) ? (integral_prev + bleed)
												 : 0.0f;
	}
}

/**
 * @brief 设置PID参数
 *
 * @param P: P参数
 * @param I: I参数
 * @param D: D参数
 * @return void
 */
void PIDController::set_PID(float P, float I, float D) {
	this->P = P;
	this->I = I;
	this->D = D;
}

/**
 * @brief 设置P参数
 *
 * @param P: P参数
 * @return void
 */
void PIDController::set_P(float pp) {
	this->P = pp;
}

/**
 * @brief 设置输出限幅
 *
 * @param limit: 输出限幅
 * @return void
 */
void PIDController::set_limit(float limit) { this->limit = limit; }

/**
 * @brief 重置PID控制器状态
 *
 * @return void
 */
void PIDController::reset() {
	error_prev = 0.0f;
	output_prev = 0.0f;
	integral_prev = 0.0f;
	measurement_prev = 0.0f;
	derivative_filt = 0.0f;
	deriv_init = false;
}

/**
 * @brief 仅清除PID积分项，保留输出斜坡和误差历史
 *
 * @return void
 */
void PIDController::reset_integral() { integral_prev = 0.0f; }