#include "pid.h"

PID PIDController::pid = {0, 0, 0, 0, 0};

PIDConfig PIDController::pidConfig = {
    PID_KP,
    PID_KI,
    PID_KD};

void PIDController::reset()
{
  pid.error = 0;
  pid.last_error = 0;
  pid.integral = 0;
  pid.derivative = 0;
  pid.lastTime = millis();
}

void PIDController::setConfig(float Kp, float Ki, float Kd,
                              float outputMax, float outputMin,
                              float integralMax)
{
  pidConfig.Kp = Kp;
  pidConfig.Ki = Ki;
  pidConfig.Kd = Kd;
  pidConfig.outputMax = outputMax;
  pidConfig.outputMin = outputMin;
  pidConfig.integralMax = integralMax;
}

float PIDController::update(float target, float current)
{
  // 计算时间间隔
  unsigned long now = millis();
  float dt = (now - pid.lastTime) / 1000.0f; // 转换为秒
  pid.lastTime = now;

  // 如果时间间隔过大，说明可能是第一次运行或长时间未运行
  if (dt > 1.0f)
  {
    dt = 0.01f; // 使用默认值
  }

  // 计算误差
  pid.error = target - current;

  // 计算积分项
  pid.integral += pid.error * dt;

  // 积分限幅
  if (pid.integral > pidConfig.integralMax)
  {
    pid.integral = pidConfig.integralMax;
  }
  else if (pid.integral < -pidConfig.integralMax)
  {
    pid.integral = -pidConfig.integralMax;
  }

  // 计算微分项（使用错误微分而不是输出微分，避免微分突跳）
  pid.derivative = (pid.error - pid.last_error) / dt;
  pid.last_error = pid.error;

  // 计算PID输出
  float output = pidConfig.Kp * pid.error +
                 pidConfig.Ki * pid.integral +
                 pidConfig.Kd * pid.derivative;

  // 输出限幅
  if (output > pidConfig.outputMax)
  {
    output = pidConfig.outputMax;
  }
  else if (output < pidConfig.outputMin)
  {
    output = pidConfig.outputMin;
  }

  return output;
}