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

#include <algorithm>
#include <cmath>
#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<SecondOrderAdrcController>>
SecondOrderAdrcController::Create(SecondOrderAdrcControllerParameters params) {
  if (params.input_sensors.empty()) {
    return absl::InvalidArgumentError(absl::StrFormat(
        "No input sensor is assigned to SecondOrderAdrc controller `%s`!",
        params.id));
  }

  auto second_order_adrc_controller =
      absl::WrapUnique(new SecondOrderAdrcController(
          params.id, params.zone_manager,
          std::move(params.second_order_adrc_loop), params.setpoint,
          std::move(params.input_sensors),
          std::move(params.disturbance_sensor_lists)));

  return second_order_adrc_controller;
}

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

  ProcessOutput(GetSecondOrderAdrcLoop()->ExecuteSecondOrderAdrcLoop(
      setpoint, input, input_disturbances_, zone_owner_->GetDebugEnabled()));
}

double SecondOrderAdrcController::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(
        "SecondOrderAdrc 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(
        "SecondOrderAdrc controller %s chose the input value %lf\n", GetId(),
        *input_value));
  }

  return *input_value;
}

void SecondOrderAdrcController::ProcessInputDisturbances() {
  // Set `input_disturbances_` to 0 by default, in case there is no valid sensor
  // reading, which will result in zero disturbance errors.
  std::fill(input_disturbances_.begin(), input_disturbances_.end(), 0.0);
  for (int i = 0; i < disturbance_sensor_lists_.size(); ++i) {
    std::optional<double> disturbance_value =
        ProcessSensorInputs(disturbance_sensor_lists_[i], zone_owner_);

    if (!disturbance_value.has_value()) {
      continue;
    }

    input_disturbances_[i] = *disturbance_value;
  }
}

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

std::optional<double> SecondOrderAdrcController::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 SecondOrderAdrcController::ToJson() const {
  nlohmann::json json;
  json["SampleTimeSec"] = second_order_adrc_loop_->GetSampleTimeSec();
  json["SettleTimeSec"] = second_order_adrc_loop_->GetSettleTimeSec();
  json["B0"] = second_order_adrc_loop_->GetB0();
  json["ObserverBandwidthFactor"] =
      second_order_adrc_loop_->GetObserverBandwidthFactor();
  json["OutLimitMax"] = second_order_adrc_loop_->GetOutputLimitMax();
  json["OutLimitMin"] = second_order_adrc_loop_->GetOutputLimitMin();
  json["SetPoint"] = GetSetpoint();
  json["Inputs"] = nlohmann::json::array();
  for (const std::string& input : input_sensors_) {
    json["Inputs"].push_back(input);
  }
  json["Disturbances"] = nlohmann::json::array();
  for (const std::vector<std::string>& disturbance_sensor_list :
       disturbance_sensor_lists_) {
    nlohmann::json disturbance_json;
    disturbance_json["InputSensors"] = nlohmann::json::array();
    for (const std::string& input_sensor : disturbance_sensor_list) {
      disturbance_json["InputSensors"].push_back(input_sensor);
    }
    json["Disturbances"].push_back(disturbance_json);
  }
  return json;
}

}  // namespace thermal
}  // namespace milotic_tlbmc