#include "tlbmc/thermal/controller/fan_pid_controller.h"

#include <algorithm>
#include <cmath>
#include <memory>
#include <optional>
#include <string>
#include <utility>
#include <vector>

#include "absl/log/log.h"
#include "absl/memory/memory.h"
#include "absl/status/status.h"
#include "absl/status/statusor.h"
#include "absl/strings/str_cat.h"
#include "absl/strings/str_format.h"
#include <nlohmann/json.hpp>
#include "tlbmc/thermal/controller/algorithm/pid.h"
#include "tlbmc/thermal/zone_manager.h"

namespace milotic_tlbmc {
namespace thermal {

absl::StatusOr<std::unique_ptr<FanPidController>> FanPidController::Create(
    FanPidControllerParameters params) {
  if (params.input_fans.empty()) {
    return absl::InvalidArgumentError(absl::StrFormat(
        "No input fan is assigned to fan PID controller `%s`!", params.id));
  }

  if (params.output_fans.empty()) {
    return absl::InvalidArgumentError(absl::StrFormat(
        "No output fan is assigned to fan PID controller `%s`!", params.id));
  }

  auto fan_pid_controller = absl::WrapUnique(new FanPidController(
      params.id, params.zone_manager, std::move(params.pid_loop),
      std::move(params.input_fans), std::move(params.output_fans)));

  return fan_pid_controller;
}

void FanPidController::ProcessThermalLoop() {
  double input = ProcessInput();
  double setpoint = GetSetpoint();

  zone_owner_->TryLogThermalDebugMessage(
      absl::StrFormat("Fan PID controller `%s` in zone `%d` takes "
                      "input RPM `%f` and setpoint `%f`",
                      GetId(), zone_owner_->GetId(), input, setpoint));

  ProcessOutput(GetPidLoop()->ExecutePidLoop(
      input, setpoint, zone_owner_->GetDebugPidEnabled()));
}

double FanPidController::ProcessInput() {
  std::optional<double> min_rpm;
  for (const std::string& fan_name : GetInputFans()) {
    double value = zone_owner_->GetSensorReadings(fan_name).scaled_value;
    if (!std::isfinite(value) || value < 1.0) {
      continue;
    }
    min_rpm = min_rpm ? std::min(value, *min_rpm) : value;
  }
  return min_rpm.value_or(kDefaultFanRpmValue);
}

void FanPidController::ProcessOutput(double pwm) {
  bool failsafe_transition = false;

  // Failsafe mode is only for non-tuning modes: In tuning mode, just print the
  // calculation results, instead of checking whether to enter the failsafe
  // mode.
  if (zone_owner_->GetTuningEnabled()) {
    ZoneManager::LogThermalMessage(
        absl::StrFormat("Fan PID controller `%s` in zone `%d` is in tuning "
                        "mode with output PWM `%f` by `%s`",
                        GetId(), zone_owner_->GetId(), pwm, GetId()));
  } else {
    bool current_failsafe_state = zone_owner_->GetFailsafeMode();

    if (previous_failsafe_state_ != current_failsafe_state) {
      previous_failsafe_state_ = current_failsafe_state;
      failsafe_transition = true;
    }

    if (current_failsafe_state) {
      double failsafe_percent = zone_owner_->GetFailsafePercent();
      // Ensure PWM is never lower than the failsafe PWM under failsafe mode.
      pwm = std::max(pwm, failsafe_percent);
    }

    if (zone_owner_->GetDebugEnabled()) {
      zone_owner_->TryLogThermalDebugMessage(
          absl::StrFormat("Fan PID controller `%s` in zone `%d` is in %s mode "
                          "with output PWM `%f`",
                          GetId(), zone_owner_->GetId(),
                          current_failsafe_state ? "failsafe" : "normal", pwm));
    } else if (failsafe_transition) {
      // Output failsafe log as long as the failsafe state changes, once only.
      ZoneManager::LogThermalMessage(
          absl::StrFormat("Fan PID controller `%s` in zone `%d` is in failsafe "
                          "mode with output PWM `%f`",
                          GetId(), zone_owner_->GetId(), pwm));
    }
  }

  // Update the real fan speeds.
  for (const std::string& fan_name : GetOutputFans()) {
    zone_owner_->UpdateLocallyRecordedFanPwmAndCheckFailsafe(fan_name, pwm);
  }
  if (zone_owner_->GetDebugEnabled()) {
    zone_owner_->AppendSampledData(absl::StrCat(pwm, ","));
  }
}

void FanPidController::SetCoefficients(
    const FanPidControllerCoefficients& coefficients) {
  if (coefficients.coeff_proportional.has_value()) {
    pid_loop_->SetCoeffProportional(*coefficients.coeff_proportional);
  }
  if (coefficients.coeff_integral.has_value()) {
    pid_loop_->SetCoeffIntegral(*coefficients.coeff_integral);
  }
  if (coefficients.coeff_derivative.has_value()) {
    pid_loop_->SetCoeffDerivative(*coefficients.coeff_derivative);
  }
  if (coefficients.feed_forward_offset.has_value()) {
    pid_loop_->SetFeedForwardOffset(*coefficients.feed_forward_offset);
  }
  if (coefficients.feed_forward_gain.has_value()) {
    pid_loop_->SetFeedForwardGain(*coefficients.feed_forward_gain);
  }
}

nlohmann::json FanPidController::ToJson() const {
  nlohmann::json json;
  json["DCoefficient"] = pid_loop_->GetCoeffDerivative();
  json["FFGainCoefficient"] = pid_loop_->GetFeedForwardGain();
  json["FFOffCoefficient"] = pid_loop_->GetFeedForwardOffset();
  json["ICoefficient"] = pid_loop_->GetCoeffIntegral();
  json["ILimitMax"] = pid_loop_->GetIntegralLimitMax();
  json["ILimitMin"] = pid_loop_->GetIntegralLimitMin();
  json["OutLimitMax"] = pid_loop_->GetOutputLimitMax();
  json["OutLimitMin"] = pid_loop_->GetOutputLimitMin();
  json["PCoefficient"] = pid_loop_->GetCoeffProportional();
  json["SlewNeg"] = pid_loop_->GetSlewNeg();
  json["SlewPos"] = pid_loop_->GetSlewPos();
  json["Inputs"] = nlohmann::json::array();
  for (const auto& input : input_fans_) {
    json["Inputs"].push_back(input);
  }
  return json;
}

}  // namespace thermal
}  // namespace milotic_tlbmc