#include "include/PID.hpp"
#include <chrono>
#include <algorithm>
#include <spdlog/spdlog.h>

#define MS_IN_S 1000.0
#define US_IN_S (1000.0 * MS_IN_S)

using namespace std;


PID::PID() : P(0), I(0), D(0), setpoint_ramp(0), limit(0), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) {};

PID::PID(double P, double I, double D, double ramp, double limit)
    : P(P), I(I), D(D), setpoint_ramp(ramp), limit(limit), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) {
  timestamp_prev = clock::now();
}

double PID::operator()(double setpoint, double process_variable) {
  // calculate the time from the last call
  auto timestamp_now = clock::now();
  double delta_seconds = chrono::duration_cast<chrono::duration<double, std::micro>>(timestamp_now - timestamp_prev).count() / US_IN_S;

  if (setpoint_ramp > 0) {
    double setpoint_rate = (setpoint - setpoint_prev) / delta_seconds;
    if (setpoint_rate > setpoint_ramp) {
      setpoint = setpoint_prev + setpoint_ramp * delta_seconds;
    } else if (setpoint_rate < -setpoint_ramp) {
      setpoint = setpoint_prev - setpoint_ramp * delta_seconds;
    }
  }

  double error = setpoint - process_variable;

  double proportional = P * error;
  // Tustin transform of the integral part
  double integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
  double derivative = D * ((error - error_prev) / delta_seconds);
  integral = std::clamp(integral, -limit, limit);

  double output = proportional + integral + derivative;

  // anti-windup - limit the output variable
  output = std::clamp(output, -limit, limit);
  spdlog::debug("PID: E{:03.4f} P{:03.4f} I{:03.4f} D{:03.4f} O{:03.4f} EP{:03.4f} DS{:03.4f}, SP{:03.4f} PV{:03.4f}",
                error, proportional, integral, derivative, output, error_prev, delta_seconds, setpoint, process_variable
  );

  integral_prev = integral;
  setpoint_prev = setpoint;
  error_prev = error;
  timestamp_prev = timestamp_now;
  return output;
}
