#ifndef THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_PID_H_
#define THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_PID_H_

#include <cmath>
#include <limits>

#include "thermal_config.pb.h"

namespace milotic_tlbmc {
namespace thermal {

/*
 * `PidThermalLoop` is a commonly used thermal control algorithm based on
 * proportional, integral, and derivative terms. The algorithm implemented here
 * is a feedforward PID:
 * https://blog.incatools.com/benefits-feedforward-pid-controllers
 *
 * This class is not thread-safe.
 *
 * This class is created based on the PID structs at
 * https://github.com/openbmc/phosphor-pid-control/blob/master/pid/ec/pid.hpp
 *
 * More resources can be found at:
 * https://www.mathworks.com/help/control/ug/proportional-integral-derivative-pid-controllers.html
 * https://en.wikipedia.org/wiki/Proportional%E2%80%93integral%E2%80%93derivative_controller
 */
class PidThermalLoop {
 public:
  PidThermalLoop() = default;

  explicit PidThermalLoop(const PidLoopConfig& pid_params)
      : pid_params_(pid_params) {
    if (pid_params.sample_time_sec() <= 0) {
      pid_params_.set_sample_time_sec(1.0);
    }
  }

  /*
   * If the hysteresis check passes, `ExecutePidLoop` performs exactly one
   * iteration of a PID loop with the current input; otherwise, the last input
   * will be used for the calculation.
   *
   * A hysteresis check is performed by comparing the current input with the
   * last input. If the difference between the current input and the last
   * input is no less than the hysteresis value, the check will pass.
   *
   * If `pid_params_.check_hysteresis_with_setpoint()` is true, the hysteresis
   * check will be performed by comparing the current input with the setpoint,
   * instead of comparing it with the last input.
   *
   * Hysteresis check will be skipped if
   * `pid_params_.check_hysteresis_with_setpoint()` is false and none of the
   * hysteresis values is significant.
   *
   * A hysteresis value needs to be at least 0.5 to be significant.
   */
  double ExecutePidLoop(double input, double setpoint,
                        bool enable_calculation_log = false);

  /*
   * `CalculatePidOutput` performs exactly one iteration of a PID loop.
   *
   * The states from the last iterations (e.g., `integral_`, `last_output_`, and
   * `last_error_`) will be used here, so we need to maintain the states, making
   * this function non-const.
   */
  double CalculatePidOutput(double input, double setpoint,
                            bool enable_calculation_log = false);

  void SetInitialized(bool initialized) { initialized_ = initialized; }
  bool GetInitialized() const { return initialized_; }

  void SetCheckHysteresisWithSetpoint(bool check_hysteresis_with_setpoint) {
    pid_params_.set_check_hysteresis_with_setpoint(
        check_hysteresis_with_setpoint);
  }
  bool GetCheckHysteresisWithSetpoint() const {
    return pid_params_.check_hysteresis_with_setpoint();
  }

  void SetIntegral(double integral) { integral_ = integral; }
  double GetIntegral() const { return integral_; }

  void SetLastInput(double last_input) { last_input_ = last_input; }
  double GetLastInput() const { return last_input_; }

  void SetLastOutput(double last_output) { last_output_ = last_output; }
  double GetLastOutput() const { return last_output_; }

  void SetLastError(double last_error) { last_error_ = last_error; }
  double GetLastError() const { return last_error_; }

  void SetSampleTime(double sample_time) {
    pid_params_.set_sample_time_sec(sample_time);
  }
  double GetSampleTime() const { return pid_params_.sample_time_sec(); }

  void SetCoeffProportional(double coeff_proportional) {
    pid_params_.set_coeff_proportional(coeff_proportional);
  }
  double GetCoeffProportional() const {
    return pid_params_.coeff_proportional();
  }

  void SetCoeffIntegral(double coeff_integral) {
    pid_params_.set_coeff_integral(coeff_integral);
  }
  double GetCoeffIntegral() const { return pid_params_.coeff_integral(); }

  void SetCoeffDerivative(double coeff_derivative) {
    pid_params_.set_coeff_derivative(coeff_derivative);
  }
  double GetCoeffDerivative() const { return pid_params_.coeff_derivative(); }

  void SetFeedForwardOffset(double feed_forward_offset) {
    pid_params_.set_feed_forward_offset(feed_forward_offset);
  }
  double GetFeedForwardOffset() const {
    return pid_params_.feed_forward_offset();
  }

  void SetFeedForwardGain(double feed_forward_gain) {
    pid_params_.set_feed_forward_gain(feed_forward_gain);
  }
  double GetFeedForwardGain() const { return pid_params_.feed_forward_gain(); }

  void SetIntegralLimitMax(double integral_limit_max) {
    pid_params_.set_integral_limit_max(integral_limit_max);
  }
  double GetIntegralLimitMax() const {
    return pid_params_.integral_limit_max();
  }

  void SetIntegralLimitMin(double integral_limit_min) {
    pid_params_.set_integral_limit_min(integral_limit_min);
  }
  double GetIntegralLimitMin() const {
    return pid_params_.integral_limit_min();
  }

  void SetOutputLimitMax(double output_limit_max) {
    pid_params_.set_output_limit_max(output_limit_max);
  }
  double GetOutputLimitMax() const { return pid_params_.output_limit_max(); }

  void SetOutputLimitMin(double output_limit_min) {
    pid_params_.set_output_limit_min(output_limit_min);
  }
  double GetOutputLimitMin() const { return pid_params_.output_limit_min(); }

  void SetSlewNeg(double slew_neg) { pid_params_.set_slew_neg(slew_neg); }
  double GetSlewNeg() const { return pid_params_.slew_neg(); }

  void SetSlewPos(double slew_pos) { pid_params_.set_slew_pos(slew_pos); }
  double GetSlewPos() const { return pid_params_.slew_pos(); }

  void SetHysteresisNeg(double hysteresis_neg) {
    pid_params_.set_hysteresis_neg(hysteresis_neg);
  }
  double GetHysteresisNeg() const { return pid_params_.hysteresis_neg(); }

  void SetHysteresisPos(double hysteresis_pos) {
    pid_params_.set_hysteresis_pos(hysteresis_pos);
  }
  double GetHysteresisPos() const { return pid_params_.hysteresis_pos(); }

 protected:
  // Calculate proportional gain based on Kp.
  double CalculateProportionalGain(double error_value) const;
  // Calculate integral gain based on Ki.
  double CalculateIntegralGain(double error_value) const;
  // Calculate derivative gain based on Kd.
  double CalculateDerivativeGain(double error_value) const;
  // Calculate feed-forward term based on feed-forward gain and offset.
  double CalculateFeedForward(double setpoint) const;
  // Clamp the output based on slew rates.
  double ClampOutputWithSlewRates(double pid_output) const;

  // Hysteresis values need to be at least 0.5 to be significant.
  bool IsHysteresisCheckEnabled() const {
    return pid_params_.check_hysteresis_with_setpoint() ||
           GetHysteresisPos() >= 0.5 || GetHysteresisNeg() >= 0.5;
  }
  // Perform input hysteresis check if significant hysteresis params are set:
  //    1. Is `last_input_` invalid (e.g., NaN, INF, or -INF)?
  //    2. Is the delta between `input` and `last_input_` outside of the
  //       hysteresis bounds?
  // If any condition is satisfied, perform one PID iteration using `input`;
  // otherwise, use `last_input_`.
  bool IsWithinInputHysteresisBounds(double input) const {
    return IsHysteresisCheckEnabled() && std::isfinite(GetLastInput()) &&
           input - GetLastInput() <= GetHysteresisPos() &&
           GetLastInput() - input <= GetHysteresisNeg();
  }

 private:
  bool initialized_ = false;  // Is this PID finished with the first iteration?

  double integral_ = 0.0;  // Integral of error

  double last_input_ = std::numeric_limits<double>::quiet_NaN();
  double last_output_ = 0.0;
  double last_error_ = 0.0;

  PidLoopConfig pid_params_;  // The parameters for this PID thermal loop.
};

}  // namespace thermal
}  // namespace milotic_tlbmc

#endif  // THIRD_PARTY_MILOTIC_EXTERNAL_CC_TLBMC_THERMAL_CONTROLLER_PID_H_