#include "pid_controller.h"
#include <cmath>

PIDController::PIDController(float proportionalGain, float integralGain, float derivativeGain,
    float iLimitLow, float iLimitHigh, float oLimitLow, float oLimitHigh, float timeStep)
    : kp(proportionalGain), ki(integralGain), kd(derivativeGain),
    integralLimitLow(iLimitLow), integralLimitHigh(iLimitHigh),
    outputLimitLow(oLimitLow), outputLimitHigh(oLimitHigh),
    dt(timeStep), prevError(0.0f), integral(0.0f), prevMeasurement(0.0f), firstRun(true) {
}

void PIDController::reset() {
    prevError = 0.0f;
    integral = 0.0f;
    prevMeasurement = 0.0f;
    firstRun = true;
}

float PIDController::compute(float setpoint, float measurement) {
    // 计算误差（期望角度 - 当前角度）
    float error = setpoint - measurement;

    // 比例项
    float proportionalTerm = kp * error;

    // 积分项（带抗积分饱和）
    integral += error * dt;

    // 积分限幅，防止积分饱和
    if (integral > integralLimitHigh) {
        integral = integralLimitHigh;
    }
    else if (integral < integralLimitLow) {
        integral = integralLimitLow;
    }

    float integralTerm = ki * integral;

    // 微分项（使用测量值的变化率而非误差变化率，以减少噪声影响）
    float derivativeTerm = 0.0f;
    if (!firstRun) {
        // 使用测量值的变化率计算微分
        float dMeasurement = measurement - prevMeasurement;
        derivativeTerm = -kd * (dMeasurement / dt);
    }
    firstRun = false;

    // 保存当前测量值用于下次计算
    prevMeasurement = measurement;

    // 计算总输出
    float output = proportionalTerm + integralTerm + derivativeTerm;

    // 输出限幅
    if (output > outputLimitHigh) {
        output = outputLimitHigh;
    }
    else if (output < outputLimitLow) {
        output = outputLimitLow;
    }

    return output;
}

void PIDController::getTunings(float& p, float& i, float& d) const {
    p = kp;
    i = ki;
    d = kd;
}

void PIDController::setTunings(float p, float i, float d) {
    kp = p;
    ki = i;
    kd = d;
}