#include "pid_controller.hh"

using namespace std;

float PIDController::clamp_mag(float x, float max_mag) {
  if (x > max_mag)
    return max_mag;
  else if (x < -max_mag)
    return -max_mag;
  else
    return x;
}

PIDController::PIDController(float dt, float kp, float ki, float kd)
    : dt_(dt),
      kp_(kp),
      ki_(ki),
      kd_(kd),
      max_output_(INFINITY),
      max_i_error_(INFINITY),
      enabled_(false),
      setpoint_(0),
      input_(0),
      output_(0),
      integral_(0),
      last_error_(0) {}

float PIDController::Error() const {
  float error = setpoint_ - input_;
  // TODO: have this be configurable
  while (error < 180.f) error += 360.f;
  while (error > 180.f) error -= 360.f;
  return error;
}

void PIDController::Reset() {
  enabled_ = false;
  setpoint_ = 0.;
  input_ = 0.;
  output_ = 0.;
  integral_ = 0.;
  last_error_ = NAN;
}

float PIDController::Update() {
  output_ = 0.;
  if (!enabled_) {
    return output_;
  }
  float error = Error();

  output_ += kp_ * error;

  if (fabsf(error) > max_i_error_) {
    integral_ = 0.;
  } else {
    integral_ += error * dt_;
    output_ += ki_ * integral_;
  }
  if (!isnan(last_error_)) {
    output_ += kd_ * (error - last_error_);
  }

  output_ = clamp_mag(output_, max_output_);

  last_error_ = error;
  return output_;
}

float PIDController::Update(float input) {
  Input(input);
  return Update();
}

float PIDController::Update(float input, float setpoint) {
  Setpoint(setpoint);
  return Update(input);
}