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

#include <algorithm>
#include <limits>

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

namespace milotic_tlbmc {
namespace thermal {

double DffThermalLoop::ApplyLowPassFilterToDisturbance(
    double disturbance, bool enable_calculation_log) const {
  double filtered_disturbance = GetCoeffH1() * disturbance +
                                GetCoeffH2() * GetLastDisturbance() +
                                GetCoeffH3() * GetLastFilteredDisturbance();

  if (enable_calculation_log) {
    LOG(WARNING)
        << "[Thermal Debug]: "
        << absl::StrFormat(
               "After applying low-pass filter, filtered disturbance: %lf, "
               "with input disturbance: %lf, previous disturbance: %lf, and "
               "previous filtered disturbance: %lf",
               filtered_disturbance, disturbance, GetLastDisturbance(),
               GetLastFilteredDisturbance());
  }

  return filtered_disturbance;
}

double DffThermalLoop::ExecuteDffLoop(double disturbance,
                                      bool enable_calculation_log) {
  // Apply low pass filter on the disturbance measurement to attenuate
  // high-frequency components that the feedforward controller cannot
  // effectively handle.
  double filtered_disturbance =
      ApplyLowPassFilterToDisturbance(disturbance, enable_calculation_log);

  double dff_value =
      GetCoeffF1() * (filtered_disturbance - GetReferencePower()) +
      GetCoeffF2() * (GetLastFilteredDisturbance() - GetReferencePower()) +
      GetCoeffF3() * GetLastDffValue();

  SetLastDisturbance(disturbance);
  SetLastFilteredDisturbance(filtered_disturbance);
  SetLastDffValue(dff_value);

  double output = dff_value;

  if (enable_calculation_log) {
    LOG(WARNING) << "[Thermal Debug]: "
                 << absl::StrFormat(
                        "F1: %lf; F2: %lf; F3: %lf; H1: %lf; H2: %lf; H3: "
                        "%lf; output: %lf; disturbance: %lf; last "
                        "disturbance: %lf; last filtered disturbance: %lf; "
                        "last DFF value: %lf",
                        GetCoeffF1(), GetCoeffF2(), GetCoeffF3(), GetCoeffH1(),
                        GetCoeffH2(), GetCoeffH3(), output, disturbance,
                        GetLastDisturbance(), GetLastFilteredDisturbance(),
                        GetLastDffValue());
  }

  // Apply slew rate clamping
  double min_output = std::numeric_limits<double>::lowest();
  double max_output = std::numeric_limits<double>::max();
  if (GetSlewNeg() < -1.0) {
    min_output = GetLastOutput() + GetSlewNeg() * GetSampleTime();
  }
  if (GetSlewPos() > 1.0) {
    max_output = GetLastOutput() + GetSlewPos() * GetSampleTime();
  }
  output = std::clamp(output, min_output, max_output);

  if (enable_calculation_log) {
    LOG(WARNING)
        << "[Thermal Debug]: "
        << absl::StrFormat(
               "Slew rate clamped DFF output: %lf (slew_neg: %lf, slew_pos: "
               "%lf); last output: %lf",
               output, GetSlewNeg(), GetSlewPos(), GetLastOutput());
  }

  SetLastOutput(output);

  return output;
}

}  // namespace thermal
}  // namespace milotic_tlbmc