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


extern CAN_transmiter CAN0_transmiter;
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) {}

// PID 控制器函数
float PIDController::operator()(float error) {
	// P环
	float proportional = P * error;

	// Tustin 散点积分（I环）
	// 添加积分限幅，防止积分饱和
	float integral =
		integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
	integral = _constrain(integral, -limit, limit);

	// D环（微分环节）
	float derivative = D * (error - error_prev) / TIME_SLICE;

	// 将P,I,D三环的计算值加起来
	float output = proportional + integral + derivative;
	output = _constrain(output, -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 设置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; }