#pragma once

class PidController {
public:
  PidController(float kp, float ki, float kd, float outputMin, float outputMax)
      : kp_(kp),
        ki_(ki),
        kd_(kd),
        outputMin_(outputMin),
        outputMax_(outputMax) {}

  void setTunings(float kp, float ki, float kd) {
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
  }

  void setSetpoint(float setpoint) { setpoint_ = setpoint; }

  void reset() {
    integral_ = 0.0f;
    prevInput_ = 0.0f;
    firstSample_ = true;
  }

  float compute(float input, uint32_t nowMs) {
    if (firstSample_) {
      prevInput_ = input;
      prevTimeMs_ = nowMs;
      firstSample_ = false;
      return outputMin_;
    }

    const float dt = static_cast<float>(nowMs - prevTimeMs_) / 1000.0f;
    if (dt <= 0.0f) {
      return lastOutput_;
    }

    const float error = setpoint_ - input;
    integral_ += error * dt;

    // Anti-windup: clamp integral so output cannot exceed limits
    const float integralMax = (outputMax_ - outputMin_) / (ki_ > 0.0f ? ki_ : 1.0f);
    if (integral_ > integralMax) {
      integral_ = integralMax;
    } else if (integral_ < 0.0f) {
      integral_ = 0.0f;
    }

    const float derivative = (input - prevInput_) / dt;
    float output = kp_ * error + ki_ * integral_ - kd_ * derivative;

    if (output < outputMin_) {
      output = outputMin_;
    } else if (output > outputMax_) {
      output = outputMax_;
    }

    prevInput_ = input;
    prevTimeMs_ = nowMs;
    lastOutput_ = output;
    return output;
  }

  float lastOutput() const { return lastOutput_; }

  float kp() const { return kp_; }
  float ki() const { return ki_; }
  float kd() const { return kd_; }

private:
  float kp_;
  float ki_;
  float kd_;
  float outputMin_;
  float outputMax_;
  float setpoint_ = 0.0f;
  float integral_ = 0.0f;
  float prevInput_ = 0.0f;
  float lastOutput_ = 0.0f;
  uint32_t prevTimeMs_ = 0;
  bool firstSample_ = true;
};