#include "tlbmc/thermal/controller/algorithm/pid.h"

#include <algorithm>
#include <cmath>
#include <limits>

#include "absl/log/log.h"
#include "absl/strings/str_format.h"

namespace milotic_tlbmc {
namespace thermal {

// The equations are based on the following (i.e., PID with feedforward):
// https://apmonitor.com/pdc/index.php/Main/TCLabPIDFeedforward
double PidThermalLoop::CalculateProportionalGain(double error_value) const {
  return GetCoeffProportional() * error_value;
}

double PidThermalLoop::CalculateIntegralGain(double error_value) const {
  double coeff_integral = GetCoeffIntegral();
  double integral_term = 0.0;
  if (std::fabs(coeff_integral) > 0.001) {
    integral_term =
        GetIntegral() + error_value * coeff_integral * GetSampleTime();
    integral_term =
        std::clamp(integral_term, GetIntegralLimitMin(), GetIntegralLimitMax());
  }
  return integral_term;
}

double PidThermalLoop::CalculateDerivativeGain(double error_value) const {
  return GetCoeffDerivative() *
         ((error_value - GetLastError()) / GetSampleTime());
}

double PidThermalLoop::CalculateFeedForward(double setpoint) const {
  return (setpoint + GetFeedForwardOffset()) * GetFeedForwardGain();
}

double PidThermalLoop::ClampOutputWithSlewRates(double pid_output) const {
  double last_output = GetLastOutput();
  double sample_time = GetSampleTime();

  double slew_neg = GetSlewNeg();
  double min_pid_output = std::numeric_limits<double>::lowest();
  // Slew rate with absolute value less than 1.0 is meaningless, and will be
  // ignored.
  if (std::fabs(slew_neg) >= 1.0) {
    min_pid_output = last_output + slew_neg * sample_time;
  }

  double slew_pos = GetSlewPos();
  double max_pid_output = std::numeric_limits<double>::max();
  if (std::fabs(slew_pos) >= 1.0) {
    max_pid_output = last_output + slew_pos * sample_time;
  }
  return std::clamp(pid_output, min_pid_output, max_pid_output);
}

double PidThermalLoop::CalculatePidOutput(double input, double setpoint,
                                          bool enable_calculation_log) {
  double error_value = setpoint - input;

  // Calculate Kp, Ki, Kd, Feed-Forward terms
  double proportional_term = CalculateProportionalGain(error_value);
  double integral_term = CalculateIntegralGain(error_value);
  double derivative_term = CalculateDerivativeGain(error_value);
  double feed_forward_term = CalculateFeedForward(setpoint);

  double pid_output =
      proportional_term + integral_term + derivative_term + feed_forward_term;

  if (enable_calculation_log) {
    LOG(WARNING)
        << "[Thermal Debug]: "
        << absl::StrFormat(
               "Kp: %lf; Ki: %lf; Kd: %lf; FfGain: %lf; FfOffset: %lf; "
               "PID output: %lf; proportional term: %lf; integral term: "
               "%lf; derivative term: %lf; feed forward term: %lf",
               GetCoeffProportional(), GetCoeffIntegral(), GetCoeffDerivative(),
               GetFeedForwardGain(), GetFeedForwardOffset(), pid_output,
               proportional_term, integral_term, derivative_term,
               feed_forward_term);
  }

  // Clamping the output with the lower/upper bounds and slew rates.
  // Skip output limit clamping for thermal PIDs here
  if (GetOutputLimitMax() != 0.0 || GetOutputLimitMin() != 0.0) {
    pid_output =
        std::clamp(pid_output, GetOutputLimitMin(), GetOutputLimitMax());
  }
  if (enable_calculation_log) {
    LOG(WARNING)
        << "[Thermal Debug]: "
        << absl::StrFormat(
               "Output limit clamped PID output: %lf (min: %lf, max: %lf)",
               pid_output, GetOutputLimitMin(), GetOutputLimitMax());
  }

  // If this is not the first iteration, applying slew rate to avoid
  // increasing or decreasing too fast
  if (GetInitialized() &&
      (std::fabs(GetSlewNeg()) >= 1.0 || std::fabs(GetSlewPos()) >= 1.0)) {
    pid_output = ClampOutputWithSlewRates(pid_output);

    // Back calculate integral term in case the output is limited.
    integral_term = pid_output - proportional_term;
    // Clamp again because having `pid_output` limited by slew rates may
    // result in a larger integral term
    integral_term =
        std::clamp(integral_term, GetIntegralLimitMin(), GetIntegralLimitMax());

    if (enable_calculation_log) {
      LOG(WARNING)
          << "[Thermal Debug]: "
          << absl::StrFormat(
                 "Slew rate clamped PID output: %lf (slew_neg: %lf, slew_pos: "
                 "%lf); back calculated integral term: %lf (min: %lf, max: "
                 "%lf)",
                 pid_output, GetSlewNeg(), GetSlewPos(), integral_term,
                 GetIntegralLimitMin(), GetIntegralLimitMax());
    }
  }

  SetIntegral(integral_term);
  SetInitialized(true);
  SetLastError(error_value);
  SetLastOutput(pid_output);

  return pid_output;
}

double PidThermalLoop::ExecutePidLoop(double input, double setpoint,
                                      bool enable_calculation_log) {
  // If hysteresis check with setpoint is enabled, check the bound using
  // `setpoint` instead of `last_input`. Even if the hysteresis bound value is
  // insignificant (i.e., |hysteresis_bound| < 0.5), this check should not be
  // skipped.
  if (GetCheckHysteresisWithSetpoint()) {
    double output;

    if (input > (setpoint + GetHysteresisPos())) {
      // The delta exceeds the positive hysteresis bound, perform one PID
      // iteration.
      output = CalculatePidOutput(input, setpoint, enable_calculation_log);
      SetLastInput(input);
    } else if (input < (setpoint - GetHysteresisNeg())) {
      // The delta is less than the negative hysteresis bound, reset PID.
      output = 0.0;
      SetLastInput(setpoint);
      SetIntegral(0.0);
    } else {
      // The delta is within the hysteresis bounds, use the last output.
      output = GetLastOutput();
      SetLastInput(input);
    }

    SetLastOutput(output);
    return output;
  }

  if (!IsWithinInputHysteresisBounds(input)) {
    SetLastInput(input);
  }

  return CalculatePidOutput(GetLastInput(), setpoint, enable_calculation_log);
}

}  // namespace thermal
}  // namespace milotic_tlbmc