/**
 * @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 <rclcpp_action/rclcpp_action.hpp>

#include "interfaces/action/arm_space_control.hpp"
#include "interfaces/action/hand_control.hpp"
#include "interfaces/action/leg_control.hpp"
#include "interfaces/action/vision_grasp_object.hpp"
#include "interfaces/srv/vision_object_recognition.hpp"

using interfaces::action::ArmSpaceControl;
using interfaces::action::HandControl;
using interfaces::action::LegControl;
using interfaces::action::VisionGraspObject;
using interfaces::srv::VisionObjectRecognition;

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"]) {
        s.interfaces.push_back(v.as<std::string>());
      }
    }
    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;
}

/**
 * @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)
{
  // 由代码生成器根据 YAML 填充的注册器表（接口基名 -> 注册逻辑）
  static const std::unordered_map<std::string, std::function<void(const std::string &,
    const std::string &)>> action_registrars = {
    {"ArmSpaceControl", [this](const std::string & topic, const std::string & internal_skill) {
        if (action_clients_) {
          action_clients_->register_client<ArmSpaceControl>(
            topic,
            []() {ArmSpaceControl::Goal g; return g;},
            nullptr,
            [this,
            internal_skill](const ActionClientRegistry::GoalHandle<ArmSpaceControl>::WrappedResult
            & res) {
              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
              res.result->success)
              {
                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
              } else {
                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
              }
            }
          );
        }
      }},
    {"HandControl", [this](const std::string & topic, const std::string & internal_skill) {
        if (action_clients_) {
          action_clients_->register_client<HandControl>(
            topic,
            []() {HandControl::Goal g; return g;},
            nullptr,
            [this,
            internal_skill](const ActionClientRegistry::GoalHandle<HandControl>::WrappedResult &
            res) {
              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
              res.result->success)
              {
                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
              } else {
                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
              }
            }
          );
        }
      }},
    {"LegControl", [this](const std::string & topic, const std::string & internal_skill) {
        if (action_clients_) {
          action_clients_->register_client<LegControl>(
            topic,
            []() {LegControl::Goal g; return g;},
            nullptr,
            [this,
            internal_skill](const ActionClientRegistry::GoalHandle<LegControl>::WrappedResult &
            res) {
              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
              res.result->success)
              {
                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
              } else {
                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
              }
            }
          );
        }
      }},
    {"VisionGraspObject", [this](const std::string & topic, const std::string & internal_skill) {
        if (action_clients_) {
          action_clients_->register_client<VisionGraspObject>(
            topic,
            []() {VisionGraspObject::Goal g; return g;},
            nullptr,
            [this,
            internal_skill](const ActionClientRegistry::GoalHandle<VisionGraspObject>::WrappedResult
            & res) {
              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
              res.result->success)
              {
                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
              } else {
                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
              }
            }
          );
        }
      }},

  };
  static const std::unordered_map<std::string, std::function<void(const std::string &,
    const std::string &)>> service_registrars = {
    {"VisionObjectRecognition",
      [this](const std::string & topic, const std::string & internal_skill) {
        // NOTE: Previously the client was only stored under the snake_case topic key.
        // get_typed_service_client() queries by canonical skill name (CamelCase), which
        // caused lookups like "VisionObjectRecognition" to fail (nullptr) and produced
        // "Service client missing" at runtime. We now register under both the canonical
        // skill name and the resolved topic alias to remain backward compatible.
        auto client = node_->create_client<VisionObjectRecognition>(topic);
        service_clients_.push_back(client);
        // Store by canonical skill name
        service_client_map_[internal_skill] = client;
        // Also store by topic alias (snake_case) in case other code paths use it directly.
        service_client_map_[topic] = client;
      }},

  };

  for (const auto & iface : s.interfaces) {
    auto parsed = parse_interface_entry(iface);
    if (!parsed) {
      RCLCPP_WARN(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
      continue;
    }
  // Strategy updated: publish/resolve topics using snake_case form with leading slash.
  // Example: VisionObjectRecognition -> vision_object_recognition
  std::string topic_name = to_snake_case(s.name);
  if (parsed->kind == "action") {
      auto it = action_registrars.find(parsed->base);
      if (it != action_registrars.end()) {
        it->second(topic_name, s.name);
      } else {
        RCLCPP_WARN(
          node_->get_logger(), "No action registrar for interface base '%s'",
          parsed->base.c_str());
      }
    } else if (parsed->kind == "service") {
      auto it = service_registrars.find(parsed->base);
      if (it != service_registrars.end()) {
        it->second(topic_name, s.name);
      } else {
        RCLCPP_WARN(
          node_->get_logger(), "No service registrar for interface base '%s'",
          parsed->base.c_str());
      }
    }
  }
}

} // namespace brain
