/**
 * @file skill_manager.cpp
 * @brief Implements SkillManager YAML loading and interface client registration logic.
 *
 * Originally generated by an internal script (gen_skill_manager.py). Manual edits now focus on
 * documentation and style consistency. If regeneration is reintroduced, ensure the generator
 * preserves Doxygen blocks or performs a merge.
 */
//#include <ament_index_cpp/get_package_share_directory.hpp>
#include "brain/skill_manager.hpp"

#include <optional>
#include <cctype>
#include <cmath>
#include <utility>
#include <rclcpp_action/rclcpp_action.hpp>

namespace brain
{

/**
 * @brief Convert a string to lowercase (ASCII only).
 * @param s Input string (copied).
 * @return Lowercase transformed string.
 */
static inline std::string to_lower(std::string s)
{
  for (char & c : s) {
    c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
  }
  return s;
}

/**
 * @brief Convert a CamelCase / TitleCase identifier to snake_case.
 * @param s Input string.
 * @return snake_case string.
 */
static inline std::string to_snake_case(const std::string & s)
{
  std::string out;
  out.reserve(s.size() * 2);
  for (size_t i = 0; i < s.size(); ++i) {
    unsigned char uc = static_cast<unsigned char>(s[i]);
    if (::isupper(uc)) {
      if (i > 0) {out.push_back('_');}
      out.push_back(static_cast<char>(::tolower(uc)));
    } else {
      out.push_back(static_cast<char>(uc));
    }
  }
  return out;
}

/**
 * @brief Parsed representation of an interface token BaseName.kind.
 */
struct ParsedInterface { std::string base; std::string kind; };

/**
 * @brief Parse an interface entry of the form BaseName.kind.
 * @param entry Raw interface token.
 * @return ParsedInterface on success; std::nullopt if malformed.
 */
static std::optional<ParsedInterface> parse_interface_entry(const std::string & entry)
{
  const auto pos = entry.rfind('.');
  if (pos == std::string::npos || pos == 0 || pos + 1 >= entry.size()) {
    return std::nullopt;
  }
  ParsedInterface p;
  p.base = entry.substr(0, pos);
  p.kind = to_lower(entry.substr(pos + 1));
  return p;
}

/**
 * @brief Load skill specifications from a YAML file.
 *
 * Expected top-level YAML node is a sequence; each element maps onto SkillSpec fields. Interfaces
 * are registered immediately upon successful parsing. Duplicate skill names overwrite earlier
 * entries (last one wins).
 * @param yaml_path Path to YAML document.
 * @return true if parsing succeeded and at least zero skills were processed (false only on format error).
 */
bool SkillManager::load_from_file(const std::string & yaml_path)
{
  YAML::Node root = YAML::LoadFile(yaml_path);
  if (!root || !root.IsSequence()) {
    RCLCPP_ERROR(node_->get_logger(), "Invalid skills YAML: expected sequence at root");
    return false;
  }

  for (const auto & item : root) {
    if (!item.IsMap()) {continue;}
    SkillSpec s;
    s.name = item["name"].as<std::string>("");
    s.version = item["version"].as<std::string>("");
    s.description = item["description"].as<std::string>("");
    if (item["required_skills"]) {
      for (const auto & v : item["required_skills"]) {
        s.required_skills.push_back(v.as<std::string>());
      }
    }
    if (item["interfaces"]) {
      for (const auto & v : item["interfaces"]) {
        if (v.IsScalar()) {
          // Simple form: "BaseName.kind"
          const auto token = v.as<std::string>();
          s.interfaces.push_back(token);
        } else if (v.IsMap()) {
          // Extended form: { name: BaseName.kind, default_topic: <str> }
          const auto name_node = v["name"];
          if (!name_node || !name_node.IsScalar()) {
            RCLCPP_ERROR(node_->get_logger(), "Invalid interface entry (missing 'name'): %s", YAML::Dump(v).c_str());
            continue;
          }
          const std::string token = name_node.as<std::string>("");
          if (token.empty()) { continue; }
          s.interfaces.push_back(token);
          const auto def_node = v["default_topic"];
          if (def_node && def_node.IsScalar()) {
            const std::string def_topic = def_node.as<std::string>("");
            if (!def_topic.empty()) {
              s.interface_default_topics[token] = def_topic;
            }
          }
        } else {
          RCLCPP_ERROR(node_->get_logger(), "Unsupported interface entry type: %s", YAML::Dump(v).c_str());
        }
      }
    }
    if (s.name.empty()) {continue;}
    skills_[s.name] = s;
    register_interfaces_(s);
  }

  RCLCPP_INFO(node_->get_logger(), "Loaded %zu skills from %s", skills_.size(), yaml_path.c_str());
  return true;
}

/**
 * @brief Enumerate skills that declare at least one action interface.
 * @return Vector of (skill_name, interface_base) pairs (one per qualifying skill).
 */
std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_skills() const
{
  std::vector<std::pair<std::string, std::string>> out;
  for (const auto & kv : skills_) {
    const auto & s = kv.second;
    for (const auto & iface : s.interfaces) {
      auto p = parse_interface_entry(iface);
      if (p && p->kind == "action") {out.emplace_back(s.name, p->base); break;}
    }
  }
  return out;
}

using ActionRegistrar = void (*)(SkillManager * self, const std::string & topic, const std::string & internal_skill);
using ServiceRegistrar = void (*)(SkillManager * self, const std::string & topic, const std::string & internal_skill);

// Registrar function templates (no captures, usable as function pointers)
template<typename ActionT>
static void action_register(SkillManager * self, const std::string & topic, const std::string & internal_skill)
{
  self->register_action_for_base<ActionT>(SkillActionTrait<ActionT>::skill_name, topic, internal_skill);
}

template<typename ActionT>
static constexpr std::pair<const char *, ActionRegistrar> make_action_entry()
{
  return {SkillActionTrait<ActionT>::skill_name, &action_register<ActionT>};
}

template<typename ServiceT>
static void service_register(SkillManager * self, const std::string & topic, const std::string & internal_skill)
{
  self->register_service_client<ServiceT>(topic, internal_skill);
}

template<typename ServiceT>
static constexpr std::pair<const char *, ServiceRegistrar> make_service_entry(const char * name)
{
  return {name, &service_register<ServiceT>};
}

// Helper to build the service map by expanding tuple indices (C++17-compatible).
template<typename TupleT, std::size_t... I>
static std::unordered_map<std::string, ServiceRegistrar> build_service_map(std::index_sequence<I...>) {
  std::unordered_map<std::string, ServiceRegistrar> m;
  (void)std::array<int, sizeof...(I)>{{(m.emplace(
    std::string(SkillServiceTrait<std::tuple_element_t<I, TupleT>>::skill_name),
    &service_register<std::tuple_element_t<I, TupleT>>), 0)...}};
  return m;
}

// Helper to build the map by expanding tuple indices. Implemented as a template
// function rather than a templated lambda for C++17 compatibility.
template<typename TupleT, std::size_t... I>
static std::unordered_map<std::string, ActionRegistrar> build_action_map(std::index_sequence<I...>) {
  std::unordered_map<std::string, ActionRegistrar> m;
  // Expand the pack by using a dummy array and the comma operator to call emplace for each index.
  // This avoids constructing nested initializer_list objects and is C++17 compatible.
  (void)std::array<int, sizeof...(I)>{{(m.emplace(
    std::string(SkillActionTrait<std::tuple_element_t<I, TupleT>>::skill_name),
    &action_register<std::tuple_element_t<I, TupleT>>), 0)...}};
  return m;
}

// Factory methods to access static registries
static const std::unordered_map<std::string, ActionRegistrar> & get_action_registrars()
{
  // Build the registrar map by iterating the SkillActionTypes tuple at compile time.
  using TupleT = SkillActionTypes;
  constexpr std::size_t N = std::tuple_size<TupleT>::value;
  static const std::unordered_map<std::string, ActionRegistrar> kMap = build_action_map<TupleT>(std::make_index_sequence<N>{});
  return kMap;
}

static const std::unordered_map<std::string, ServiceRegistrar> & get_service_registrars()
{
  using TupleT = SkillServiceTypes;
  constexpr std::size_t N = std::tuple_size<TupleT>::value;
  static const std::unordered_map<std::string, ServiceRegistrar> kMap = build_service_map<TupleT>(std::make_index_sequence<N>{});
  return kMap;
}

// Build maps of default topics for actions and services (name -> default topic string).
template<typename TupleT, std::size_t... I>
static std::unordered_map<std::string, std::string> build_action_default_topics(std::index_sequence<I...>) {
  std::unordered_map<std::string, std::string> m;
  (void)std::array<int, sizeof...(I)>{{(m.emplace(
    std::string(SkillActionTrait<std::tuple_element_t<I, TupleT>>::skill_name),
    std::string(SkillActionTrait<std::tuple_element_t<I, TupleT>>::default_topic)), 0)...}};
  return m;
}

static const std::unordered_map<std::string, std::string> & get_action_default_topics()
{
  using TupleT = SkillActionTypes;
  constexpr std::size_t N = std::tuple_size<TupleT>::value;
  static const std::unordered_map<std::string, std::string> kMap = build_action_default_topics<TupleT>(std::make_index_sequence<N>{});
  return kMap;
}

template<typename TupleT, std::size_t... I>
static std::unordered_map<std::string, std::string> build_service_default_topics(std::index_sequence<I...>) {
  std::unordered_map<std::string, std::string> m;
  (void)std::array<int, sizeof...(I)>{{(m.emplace(
    std::string(SkillServiceTrait<std::tuple_element_t<I, TupleT>>::skill_name),
    std::string(SkillServiceTrait<std::tuple_element_t<I, TupleT>>::default_topic)), 0)...}};
  return m;
}

static const std::unordered_map<std::string, std::string> & get_service_default_topics()
{
  using TupleT = SkillServiceTypes;
  constexpr std::size_t N = std::tuple_size<TupleT>::value;
  static const std::unordered_map<std::string, std::string> kMap = build_service_default_topics<TupleT>(std::make_index_sequence<N>{});
  return kMap;
}

// Utility: join map keys for diagnostics
template<typename T>
static std::string join_keys(const std::unordered_map<std::string, T> & m)
{
  std::string out;
  bool first = true;
  for (const auto & kv : m) {
    if (!first) {out += ", ";}
    first = false;
    out += kv.first;
  }
  return out;
}

template<size_t I = 0>
static void validate_action_registrars_complete(rclcpp::Logger logger)
{
  if constexpr (I < std::tuple_size<SkillActionTypes>::value) {
    using ActionT = std::tuple_element_t<I, SkillActionTypes>;
    const char * name = SkillActionTrait<ActionT>::skill_name;
    if (get_action_registrars().find(name) == get_action_registrars().end()) {
      RCLCPP_ERROR(logger, "Missing action registrar for '%s'", name);
    }
    validate_action_registrars_complete<I + 1>(logger);
  }
}

/**
 * @brief Register action and service interfaces declared by a SkillSpec.
 *
 * Action registrars create action clients with result callbacks logging success/failure. Service
 * registrars create and retain service clients (stored for lifetime management). Topic naming:
 * currently uses the skill's canonical name (CamelCase) directly.
 * @param s Skill specification.
 */
void SkillManager::register_interfaces_(const SkillSpec & s)
{
  // One-time runtime validation to ensure all actions in SkillActionTypes have a registrar.
  static bool s_validated = false;
  if (!s_validated) {
    validate_action_registrars_complete(node_->get_logger());
    s_validated = true;
  }

  auto join_topic = [](const std::string & prefix, const std::string & base) {
    if (prefix.empty()) {return base;}
    if (prefix == "/") {return std::string{"/"} + base;}
    if (prefix.back() == '/') {return prefix + base;}
    return prefix + "/" + base;
  };

  for (const auto & iface : s.interfaces) {
    auto parsed = parse_interface_entry(iface);
    if (!parsed) {
      RCLCPP_ERROR(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
      continue;
    }

    std::string prefix_value;
    node_->get_parameter("skill_topic_prefix", prefix_value);
    const std::string base_snake = to_snake_case(parsed->base);

    auto resolve_topic_name = [&](const std::string & kind_suffix) {
      // Compute the prior default (prefix + base_snake), but allow per-trait default topic when provided.
      const std::string prefix_default = join_topic(prefix_value, base_snake);
      const auto & action_defaults = get_action_default_topics();
      const auto & service_defaults = get_service_default_topics();
      std::string trait_default;
      if (kind_suffix == "action") {
        auto it = action_defaults.find(parsed->base);
        if (it != action_defaults.end()) { trait_default = it->second; }
      } else {
        auto it = service_defaults.find(parsed->base);
        if (it != service_defaults.end()) { trait_default = it->second; }
      }
      // YAML override: exact token (e.g., "Arm.action")
      std::string yaml_override;
      auto it_yaml = s.interface_default_topics.find(iface);
      if (it_yaml != s.interface_default_topics.end()) {
        yaml_override = it_yaml->second;
      }
      const bool has_trait_default = !trait_default.empty();
      const std::string computed_default = !yaml_override.empty() ? yaml_override : (has_trait_default ? trait_default : prefix_default);
      const std::string param_name = base_snake + "." + kind_suffix + "_name";
      std::string topic_override;
      if (node_->get_parameter(param_name, topic_override)) {
        // If user explicitly sets an empty parameter, fall back to computed_default.
        if (topic_override.empty()) { topic_override = computed_default; }
        return topic_override;
      }
      // Declare with computed default so it appears in parameter list; keep existing semantics.
      node_->declare_parameter<std::string>(param_name, computed_default);
      return computed_default;
    };

    if (parsed->kind == "action") {
      const std::string topic_name = resolve_topic_name("action");
      const auto & action_regs = get_action_registrars();
      auto it = action_regs.find(parsed->base);
      if (it != action_regs.end()) {
        it->second(this, topic_name, s.name);
      } else {
        RCLCPP_ERROR(node_->get_logger(), "No action registrar for interface base '%s'. Available: [%s]",
          parsed->base.c_str(), join_keys(action_regs).c_str());
      }
    } else if (parsed->kind == "srv") {
      const std::string topic_name = resolve_topic_name("srv");
      const auto & service_regs = get_service_registrars();
      auto it = service_regs.find(parsed->base);
      if (it != service_regs.end()) {
        it->second(this, topic_name, s.name);
      } else {
        RCLCPP_ERROR(node_->get_logger(), "No service registrar for interface base '%s'. Available: [%s]",
          parsed->base.c_str(), join_keys(service_regs).c_str());
      }
    }
  }
}

} // namespace brain
