#ifndef PAST_CONTROL__TOOLS_H
#define PAST_CONTROL__TOOLS_H

#include <algorithm>
#include <chrono>

namespace past_control
{

inline double limit(double val, double min_val, double max_val)
{
  return std::min(std::max(val, min_val), max_val);
}

class PID
{
public:
  PID()
  : kp_(0.0), ki_(0.0), kd_(0.0),
    integral_max_(0.0), output_max_(0.0),
    integral_(0.0), prev_error_(0.0), first_run_(true),
    max_err_(0.0)
  {
  }

  void init(double kp, double ki, double kd,
            double integral_max, double output_max, double max_err = 0.0)
  {
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
    integral_max_ = integral_max;
    output_max_ = output_max;
    max_err_ = max_err;
    reset();
  }

  void reset()
  {
    integral_ = 0.0;
    prev_error_ = 0.0;
    first_run_ = true;
  }

  double update(double error, double dt = 0.02)
  {
    // Clamp error if max_err_ > 0
    if (max_err_ > 0.0) {
      error = limit(error, -max_err_, max_err_);
    }

    // Proportional
    double p_out = kp_ * error;

    // Integral (with clamping)
    integral_ += error * dt;
    integral_ = limit(integral_, -integral_max_, integral_max_);
    double i_out = ki_ * integral_;

    // Derivative (skip on first call)
    double d_out = 0.0;
    if (!first_run_) {
      double derivative = (error - prev_error_) / dt;
      d_out = kd_ * derivative;
    }
    first_run_ = false;
    prev_error_ = error;

    // Output clamping
    double output = p_out + i_out + d_out;
    output = limit(output, -output_max_, output_max_);

    return output;
  }

private:
  double kp_, ki_, kd_;
  double integral_max_, output_max_;
  double integral_;
  double prev_error_;
  bool first_run_;
  double max_err_;  // initialized to 0 in constructor
};

}  // namespace past_control

#endif  // PAST_CONTROL__TOOLS_H
