/*********************************************************************
 * recovery_core — validate + đọc RecoveryConfig.
 *
 * Author: DuongTD
 *********************************************************************/
#include <recovery_core/recovery_config.h>

#include <cmath>
#include <string>

#include <robot/robot.h>

namespace recovery_core
{
namespace
{

constexpr double kDefaultControlFrequency = 20.0;
constexpr double kDefaultTimeout = 0.0;

void appendError(std::string* error, const std::string& message)
{
  if (error == nullptr)
  {
    return;
  }

  if (!error->empty())
  {
    *error += "; ";
  }
  *error += message;
}

bool invalidControlFrequency(double value)
{
  return !std::isfinite(value) || value <= 0.0;
}

bool invalidTimeout(double value)
{
  return !std::isfinite(value) || value < 0.0;
}

}  // namespace

bool RecoveryConfig::validate(std::string* error) const
{
  if (error != nullptr)
  {
    error->clear();
  }

  bool valid = true;
  if (invalidControlFrequency(control_frequency))
  {
    appendError(error, "control_frequency must be finite and > 0 Hz");
    valid = false;
  }

  if (invalidTimeout(timeout))
  {
    appendError(error, "timeout must be finite and >= 0 s");
    valid = false;
  }

  return valid;
}

RecoveryConfig RecoveryConfig::fromNodeHandle(robot::NodeHandle& nh)
{
  RecoveryConfig config;
  nh.param("control_frequency", config.control_frequency, kDefaultControlFrequency);
  nh.param("timeout", config.timeout, kDefaultTimeout);

  std::string error;
  if (config.validate(&error))
  {
    return config;
  }

  robot::log_warning("[recovery_core] Invalid common recovery config: %s. "
                     "Replacing invalid values with defaults.",
                     error.c_str());

  if (invalidControlFrequency(config.control_frequency))
  {
    config.control_frequency = kDefaultControlFrequency;
  }
  if (invalidTimeout(config.timeout))
  {
    config.timeout = kDefaultTimeout;
  }

  if (!config.validate(nullptr))
  {
    return RecoveryConfig{};
  }
  return config;
}

}  // namespace recovery_core