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

#include <cmath>

#include "thermal_config.pb.h"

namespace milotic_tlbmc {
namespace thermal {

double StepwiseThermalLoop::FindStepwiseOutput(double input) const {
  int low = 0;
  int high = GetCriticalPointsSize() - 1;

  // Find the first critical point whose `threshold` is larger than `input`.
  while (low <= high) {
    int mid = low + (high - low) / 2;

    StepwiseCriticalPoint critical_point = GetCriticalPointAt(mid);

    if (std::isnan(critical_point.threshold()) ||
        critical_point.threshold() > input) {
      high = mid - 1;
    } else {
      low = mid + 1;
    }
  }

  // Return the `stepwise_output` of the last critical point whose `threshold`
  // is smaller than or equal to `input`.
  // If no such critical point exists, return the `stepwise_output` of the first
  // critical point by default.
  int output_index = low - 1;
  if (output_index < 0 || output_index >= GetCriticalPointsSize()) {
    output_index = 0;
  }

  double output = GetCriticalPointAt(output_index).stepwise_output();
  if (std::isnan(output)) {
    output = 0;
  }
  return output;
}

double StepwiseThermalLoop::ExecuteStepwiseLoop(double input) {
  double output = GetLastOutput();
  double last_input = GetLastInput();

  if (!std::isnan(output) && (input - last_input) <= GetPositiveHysteresis() &&
      (last_input - input) <= GetNegativeHysteresis()) {
    return output;
  }

  output = FindStepwiseOutput(input);

  SetLastInput(input);
  SetLastOutput(output);
  return output;
}

}  // namespace thermal
}  // namespace milotic_tlbmc