#include "obstacle_nav2/nav2_profile_loader.hpp"

#include <algorithm>
#include <cctype>
#include <cstdint>
#include <regex>
#include <stdexcept>
#include <string>
#include <vector>

#include "yaml-cpp/yaml.h"

namespace obstacle_nav2
{
namespace
{

std::string lowerCopy(std::string value)
{
  std::transform(
    value.begin(), value.end(), value.begin(),
    [](unsigned char c) {return static_cast<char>(std::tolower(c));});
  return value;
}

bool isBoolScalar(const std::string & value)
{
  const auto lower = lowerCopy(value);
  return lower == "true" || lower == "false";
}

bool isIntegerScalar(const std::string & value)
{
  static const std::regex pattern(R"(^[-+]?[0-9]+$)");
  return std::regex_match(value, pattern);
}

bool isDoubleScalar(const std::string & value)
{
  static const std::regex pattern(
    R"(^[-+]?(([0-9]+\.?[0-9]*)|(\.[0-9]+))([eE][-+]?[0-9]+)?$)");
  return std::regex_match(value, pattern);
}

rclcpp::Parameter scalarToParameter(const std::string & name, const YAML::Node & node)
{
  const auto value = node.Scalar();
  if (isBoolScalar(value)) {
    return rclcpp::Parameter(name, node.as<bool>());
  }
  if (isIntegerScalar(value)) {
    return rclcpp::Parameter(name, static_cast<int64_t>(node.as<int64_t>()));
  }
  if (isDoubleScalar(value)) {
    return rclcpp::Parameter(name, node.as<double>());
  }
  return rclcpp::Parameter(name, value);
}

rclcpp::Parameter sequenceToParameter(const std::string & name, const YAML::Node & node)
{
  std::vector<std::string> values;
  values.reserve(node.size());
  for (const auto & item : node) {
    if (!item.IsScalar()) {
      throw std::runtime_error("Parameter array '" + name + "' contains a non-scalar value");
    }
    values.push_back(item.Scalar());
  }

  const auto all_bool = std::all_of(values.begin(), values.end(), isBoolScalar);
  if (all_bool) {
    std::vector<bool> parsed;
    parsed.reserve(values.size());
    for (const auto & value : values) {
      parsed.push_back(YAML::Load(value).as<bool>());
    }
    return rclcpp::Parameter(name, parsed);
  }

  const auto all_integer = std::all_of(values.begin(), values.end(), isIntegerScalar);
  if (all_integer) {
    std::vector<int64_t> parsed;
    parsed.reserve(values.size());
    for (const auto & value : values) {
      parsed.push_back(YAML::Load(value).as<int64_t>());
    }
    return rclcpp::Parameter(name, parsed);
  }

  const auto all_double = std::all_of(values.begin(), values.end(), isDoubleScalar);
  if (all_double) {
    std::vector<double> parsed;
    parsed.reserve(values.size());
    for (const auto & value : values) {
      parsed.push_back(YAML::Load(value).as<double>());
    }
    return rclcpp::Parameter(name, parsed);
  }

  return rclcpp::Parameter(name, values);
}

rclcpp::Parameter yamlToParameter(const std::string & name, const YAML::Node & node)
{
  if (node.IsScalar()) {
    return scalarToParameter(name, node);
  }
  if (node.IsSequence()) {
    return sequenceToParameter(name, node);
  }
  throw std::runtime_error("Parameter '" + name + "' must be a scalar or sequence");
}

void appendFlattenedParameters(
  const YAML::Node & parameters,
  const std::string & prefix,
  std::vector<rclcpp::Parameter> & output)
{
  if (!parameters.IsMap()) {
    throw std::runtime_error("ros__parameters must contain a parameter map");
  }

  for (const auto & parameter_entry : parameters) {
    const auto key = parameter_entry.first.as<std::string>();
    if (key.empty()) {
      throw std::runtime_error("ros__parameters contains an empty parameter name");
    }

    const auto parameter_name = prefix.empty() ? key : prefix + "." + key;
    const auto value = parameter_entry.second;
    if (value.IsMap()) {
      appendFlattenedParameters(value, parameter_name, output);
    } else {
      output.push_back(yamlToParameter(parameter_name, value));
    }
  }
}

NodeParameterSet loadNodeParameterSet(
  const std::string & node_name,
  const YAML::Node & parameters)
{
  if (node_name.empty()) {
    throw std::runtime_error("Nav2 profile contains an empty node name");
  }
  if (!parameters.IsMap()) {
    throw std::runtime_error("Node '" + node_name + "' must contain a parameter map");
  }

  NodeParameterSet node_set;
  node_set.node_name = node_name;
  for (const auto & parameter_entry : parameters) {
    const auto parameter_name = parameter_entry.first.as<std::string>();
    if (parameter_name.empty()) {
      throw std::runtime_error("Node '" + node_name + "' contains an empty parameter name");
    }
    node_set.parameters.push_back(yamlToParameter(parameter_name, parameter_entry.second));
  }
  return node_set;
}

bool isNativeRuntimeProfileNode(const std::string & path)
{
  return path == "controller_server" || path == "velocity_smoother";
}

void collectRosParameterNodes(
  const YAML::Node & yaml_node,
  const std::string & path,
  Nav2Profile & profile)
{
  if (!yaml_node.IsMap()) {
    return;
  }

  const auto ros_parameters = yaml_node["ros__parameters"];
  if (ros_parameters) {
    if (path.empty()) {
      throw std::runtime_error("ros__parameters must be nested under a node name");
    }

    NodeParameterSet node_set;
    node_set.node_name = path.front() == '/' ? path : "/" + path;
    appendFlattenedParameters(ros_parameters, "", node_set.parameters);
    if (isNativeRuntimeProfileNode(path)) {
      profile.nodes.push_back(node_set);
    }
    return;
  }

  for (const auto & child_entry : yaml_node) {
    const auto key = child_entry.first.as<std::string>();
    const auto child_path = path.empty() ? key : path + "/" + key;
    collectRosParameterNodes(child_entry.second, child_path, profile);
  }
}

}  // namespace

Nav2Profile loadNav2Profile(const std::string & path)
{
  const auto root = YAML::LoadFile(path);
  const auto nodes = root["nodes"];

  Nav2Profile profile;
  if (nodes && nodes.IsMap()) {
    for (const auto & node_entry : nodes) {
      const auto node_name = node_entry.first.as<std::string>();
      profile.nodes.push_back(loadNodeParameterSet(node_name, node_entry.second));
    }
    return profile;
  }

  collectRosParameterNodes(root, "", profile);
  if (profile.nodes.empty()) {
    throw std::runtime_error(
      "Nav2 profile '" + path + "' must contain a nodes map or ros__parameters blocks");
  }

  return profile;
}

std::vector<rclcpp::Parameter> filterDeclaredParameters(
  const std::vector<rclcpp::Parameter> & parameters,
  const std::set<std::string> & declared_names)
{
  std::vector<rclcpp::Parameter> filtered;
  filtered.reserve(parameters.size());
  for (const auto & parameter : parameters) {
    if (declared_names.count(parameter.get_name()) > 0) {
      filtered.push_back(parameter);
    }
  }
  return filtered;
}

}  // namespace obstacle_nav2
