#include "PidController.h"
#include <algorithm>
#include <cmath>

PidController::PidController(float kp, float ki, float kd) 
    : kp_(kp), ki_(ki), kd_(kd), last_error_(0.0f), integral_(0.0f), output_(0.0f), 
      output_min_(-100.0f), output_max_(100.0f), first_iteration_(true) {}

float PidController::update(float setpoint, float measured_value, float dt) {
    float error = setpoint - measured_value;
    
    // 处理积分饱和
    integral_ += error * dt;
    
    // 计算微分（使用前向差分）
    float derivative = 0.0f;
    if (!first_iteration_) {
        derivative = (error - last_error_) / dt;
    } else {
        first_iteration_ = false;
    }
    
    // PID计算
    float proportional = kp_ * error;
    float integral_contribution = ki_ * integral_;
    float derivative_contribution = kd_ * derivative;
    
    output_ = proportional + integral_contribution + derivative_contribution;
    
    // 限制输出
    output_ = std::max(output_min_, std::min(output_max_, output_));
    
    // 保存当前误差用于下次计算微分
    last_error_ = error;
    
    return output_;
}

void PidController::reset() {
    last_error_ = 0.0f;
    integral_ = 0.0f;
    output_ = 0.0f;
    first_iteration_ = true;
}

void PidController::setKp(float kp) {
    kp_ = kp;
}

void PidController::setKi(float ki) {
    ki_ = ki;
}

void PidController::setKd(float kd) {
    kd_ = kd;
}

void PidController::setParameters(float kp, float ki, float kd) {
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
}

float PidController::getKp() const {
    return kp_;
}

float PidController::getKi() const {
    return ki_;
}

float PidController::getKd() const {
    return kd_;
}

void PidController::setOutputLimits(float min, float max) {
    output_min_ = min;
    output_max_ = max;
}

float PidController::getOutput() const {
    return output_;
}

float PidController::getError() const {
    return last_error_;
}