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

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

#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 "absl/types/span.h"
#include <nlohmann/json.hpp>
#include "tlbmc/thermal/zone_manager.h"

namespace milotic_tlbmc {
namespace thermal {

absl::StatusOr<std::unique_ptr<FirstOrderAdrcController>>
FirstOrderAdrcController::Create(FirstOrderAdrcControllerParameters params) {
  if (params.input_sensors.empty()) {
    return absl::InvalidArgumentError(absl::StrFormat(
        "No input sensor is assigned to FirstOrderAdrc controller `%s`!",
        params.id));
  }

  auto first_order_adrc_controller =
      absl::WrapUnique(new FirstOrderAdrcController(
          params.id, params.zone_manager,
          std::move(params.first_order_adrc_loop), params.setpoint,
          std::move(params.input_sensors)));

  return first_order_adrc_controller;
}

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

  ProcessOutput(GetFirstOrderAdrcLoop()->ExecuteFirstOrderAdrcLoop(
      setpoint, input, zone_owner_->GetDebugEnabled()));
}

double FirstOrderAdrcController::ProcessInput() {
  std::optional<double> input_value =
      ProcessSensorInputs(input_sensors_, zone_owner_);

  // If no input is acceptable, return `setpoint_` to indicate the invalidness.
  // This will make the ADRC error to be zero.
  if (!input_value.has_value()) {
    input_value = GetSetpoint();
    zone_owner_->TryLogThermalDebugMessage(absl::StrFormat(
        "FirstOrderAdrc controller %s chose the setpoint %lf as the input "
        "value, as there is no valid input sensor reading\n",
        GetId(), GetSetpoint()));
  } else {
    zone_owner_->TryLogThermalDebugMessage(absl::StrFormat(
        "FirstOrderAdrc controller %s chose the input value %lf\n", GetId(),
        *input_value));
  }

  return *input_value;
}

void FirstOrderAdrcController::ProcessOutput(double output) {
  zone_owner_->AddSetpoint(GetId(), output);
  zone_owner_->TryLogThermalDebugMessage(absl::StrFormat(
      "FirstOrderAdrc controller %s outputs setpoint: %lf\n", GetId(), output));
  if (zone_owner_->GetDebugEnabled()) {
    zone_owner_->AppendSampledData(absl::StrCat(output, ","));
  }
}

std::optional<double> FirstOrderAdrcController::ProcessSensorInputs(
    absl::Span<const std::string> sensors, ZoneManager* zone_manager) {
  std::optional<double> input_value = std::nullopt;

  for (const std::string& sensor : sensors) {
    double sensor_value = zone_manager->GetSensorReadings(sensor).scaled_value;

    // Sensor readings must not be NaN.
    if (!std::isfinite(sensor_value)) {
      continue;
    }

    input_value = input_value.has_value() ? std::max(*input_value, sensor_value)
                                          : sensor_value;
  }

  return input_value;
}

nlohmann::json FirstOrderAdrcController::ToJson() const {
  nlohmann::json json;
  json["SampleTimeSec"] = first_order_adrc_loop_->GetSampleTimeSec();
  json["SettleTimeSec"] = first_order_adrc_loop_->GetSettleTimeSec();
  json["B0"] = first_order_adrc_loop_->GetB0();
  json["ObserverBandwidthFactor"] =
      first_order_adrc_loop_->GetObserverBandwidthFactor();
  json["OutLimitMax"] = first_order_adrc_loop_->GetOutputLimitMax();
  json["OutLimitMin"] = first_order_adrc_loop_->GetOutputLimitMin();
  json["SetPoint"] = GetSetpoint();
  json["Inputs"] = nlohmann::json::array();
  for (const std::string& input : input_sensors_) {
    json["Inputs"].push_back(input);
  }
  return json;
}

}  // namespace thermal
}  // namespace milotic_tlbmc