#!/usr/bin/env python3
"""
Generates SkillManager C++ sources (skill_manager.cpp / skill_manager.hpp) from
robot_skills.yaml.

The script mirrors the current hand-tuned implementation by emitting the exact
registrars, traits, and includes used in the repository. Update the metadata
below when a new skill interface is introduced so regeneration stays lossless.
"""

import argparse
import os
import subprocess
import sys
from functools import lru_cache
from pathlib import Path
from string import Template
from typing import Dict, List, Mapping, Optional, Sequence, Set, Tuple

try:
    import yaml
except ImportError as exc:  # pragma: no cover - environment guard
    print("Missing dependency: PyYAML. Install with: pip install pyyaml", file=sys.stderr)
    raise

REPO_ROOT = Path(__file__).resolve().parents[2]
DEFAULT_ACTION_PACKAGE = "interfaces"
DEFAULT_SERVICE_PACKAGE = "interfaces"


def camel_to_snake(name: str) -> str:
    chars: List[str] = []
    for idx, ch in enumerate(name):
        if ch.isupper() and idx > 0:
            chars.append("_")
        chars.append(ch.lower())
    return "".join(chars)


def compute_search_roots() -> Tuple[Path, ...]:
    roots: List[Path] = []
    roots.append(REPO_ROOT)
    src_root = REPO_ROOT / "src"
    if src_root.exists():
        roots.append(src_root)
    for prefix in os.environ.get("AMENT_PREFIX_PATH", "").split(os.pathsep):
        if not prefix:
            continue
        share_path = Path(prefix) / "share"
        if share_path.exists():
            roots.append(share_path)
    unique: List[Path] = []
    seen = set()
    for root in roots:
        try:
            resolved = root.resolve()
        except OSError:
            continue
        if resolved in seen:
            continue
        seen.add(resolved)
        unique.append(resolved)
    return tuple(unique)


SEARCH_ROOTS = compute_search_roots()


@lru_cache(maxsize=None)
def find_interface_file(base: str, kind: str) -> Optional[Path]:
    ext = "action" if kind == "action" else "srv"
    snake = camel_to_snake(base)
    patterns = [f"{base}.{ext}"]
    if snake and snake != base.lower():
        patterns.append(f"{snake}.{ext}")
    for root in SEARCH_ROOTS:
        for pattern in patterns:
            try:
                for candidate in root.rglob(pattern):
                    return candidate
            except (OSError, PermissionError):
                continue
    return None


def infer_package(base: str, kind: str) -> str:
    path = find_interface_file(base, kind)
    if path:
        parent = path.parent
        expected_dir = "action" if kind == "action" else "srv"
        if parent.name.lower() == expected_dir:
            package_dir = parent.parent
        else:
            package_dir = parent
        if package_dir and package_dir.name:
            return package_dir.name
    return DEFAULT_ACTION_PACKAGE if kind == "action" else DEFAULT_SERVICE_PACKAGE


def parse_action_result_fields(base: str) -> Dict[str, str]:
    path = find_interface_file(base, "action")
    fields: Dict[str, str] = {}
    if not path or not path.exists():
        return fields
    text = path.read_text(encoding="utf-8")
    blocks = text.split('---')
    if len(blocks) < 2:
        return fields
    result_block = blocks[1]
    for line in result_block.splitlines():
        line = line.split('#', 1)[0].strip()
        if not line:
            continue
        parts = line.split()
        if len(parts) >= 2:
            fields[parts[1]] = parts[0]
    return fields


def determine_action_fields(base: str) -> Tuple[str, str]:
    fields = parse_action_result_fields(base)
    success_field = "success" if fields.get("success") else "success"
    message_field = "message" if fields.get("message") else "message"
    return success_field, message_field


def parse_message_styles_from_text(text: str) -> Dict[str, str]:
  styles: Dict[str, str] = {}
  current_skill: Optional[str] = None

  for raw_line in text.splitlines():
    line = raw_line.strip()
    if not line:
      continue
    if line.startswith("template<>"):
      current_skill = None
      continue
    if "skill_name" in line and '"' in line:
      parts = line.split('"')
      if len(parts) >= 3:
        current_skill = parts[1]
      continue
    if line.startswith("static std::string message") and current_skill:
      style = "single" if "{" in line and "}" in line else "multi"
      styles[current_skill] = style

  return styles


def load_existing_message_styles(header_path: Path) -> Dict[str, str]:
  if not header_path.is_absolute():
    header_path = (REPO_ROOT / header_path).resolve()

  texts: List[str] = []

  try:
    rel_path = header_path.relative_to(REPO_ROOT)
  except ValueError:
    rel_path = None

  if rel_path is not None:
    git_path = rel_path.as_posix()
    try:
      result = subprocess.run(
        ["git", "-C", str(REPO_ROOT), "show", f"HEAD:{git_path}"],
        check=True,
        capture_output=True,
        text=True,
      )
      if result.stdout:
        texts.append(result.stdout)
    except (subprocess.SubprocessError, FileNotFoundError):
      pass

  if header_path.exists():
    try:
      texts.append(header_path.read_text(encoding="utf-8"))
    except OSError:
      pass

  for text in texts:
    styles = parse_message_styles_from_text(text)
    if styles:
      return styles

  return {}


def parse_registrar_styles_from_text(text: str) -> Dict[str, str]:
  styles: Dict[str, str] = {}
  current_base: Optional[str] = None

  for raw_line in text.splitlines():
    stripped = raw_line.strip()
    if stripped.startswith('{"') and '", [this]' in stripped:
      parts = stripped.split('"')
      if len(parts) >= 2:
        current_base = parts[1]
      continue
    if current_base and "ActionClientRegistry::GoalHandle<" in stripped and "::WrappedResult" in stripped:
      style = "same_line" if "&" in stripped else "next_line"
      styles[current_base] = style
      current_base = None

  return styles


def load_wrapped_result_styles(cpp_path: Path) -> Dict[str, str]:
  if not cpp_path.is_absolute():
    cpp_path = (REPO_ROOT / cpp_path).resolve()

  texts: List[str] = []

  try:
    rel_path = cpp_path.relative_to(REPO_ROOT)
  except ValueError:
    rel_path = None

  if rel_path is not None:
    git_path = rel_path.as_posix()
    try:
      result = subprocess.run(
        ["git", "-C", str(REPO_ROOT), "show", f"HEAD:{git_path}"],
        check=True,
        capture_output=True,
        text=True,
      )
      if result.stdout:
        texts.append(result.stdout)
    except (subprocess.SubprocessError, FileNotFoundError):
      pass

  if cpp_path.exists():
    try:
      texts.append(cpp_path.read_text(encoding="utf-8"))
    except OSError:
      pass

  for text in texts:
    styles = parse_registrar_styles_from_text(text)
    if styles:
      return styles

  return {}


DEFAULT_ACTION_TRAIT_TEMPLATE = Template(
  """template<>
struct SkillActionTrait<${type}>
{
  static constexpr const char * skill_name = \"${base}\";
  static bool success(const ${type}::Result & r) {return r.${success_field};}
${message_block}
};"""
)


DEFAULT_ACTION_REGISTRAR_TEMPLATE = Template("""        if (action_clients_) {
          action_clients_->register_client<${base}>(
            topic,
            []() {${base}::Goal g; return g;},
            nullptr,
            [this,
            internal_skill](const ActionClientRegistry::GoalHandle<${base}>::WrappedResult${ampersand_suffix}
            ${res_prefix}res) {
              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
              res.result->${success_field})
              {
                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());
              }
            }
          );
        }""")


DEFAULT_SERVICE_REGISTRAR_TEMPLATE = Template("""        auto client = node_->create_client<${type}>(topic);
        service_clients_.push_back(client);
        service_client_map_[internal_skill] = client;
        service_client_map_[topic] = client;""")


def build_default_action_spec(
  base: str,
  message_style: Optional[str],
  registrar_style: Optional[str],
) -> dict:
  package = infer_package(base, "action")
  success_field, message_field = determine_action_fields(base)
  type_name = f"{package}::action::{base}"
  snake = camel_to_snake(base)
  include_path = f"{package}/action/{snake}.hpp"

  style = message_style or "multi"
  if style == "single":
    message_block = (
      f"  static std::string message(const {type_name}::Result & r) "
      f"{{return r.{message_field};}}"
    )
  else:
    message_block = (
      f"  static std::string message(const {type_name}::Result & r)\n"
      "  {\n"
      f"    return r.{message_field};\n"
      "  }"
    )

  registrar_style_value = registrar_style or "next_line"
  if registrar_style_value == "same_line":
    ampersand_suffix = " &"
    res_prefix = ""
  else:
    ampersand_suffix = ""
    res_prefix = "& "

  trait_block = DEFAULT_ACTION_TRAIT_TEMPLATE.substitute(
    type=type_name,
    base=base,
    success_field=success_field,
    message_block=message_block,
  )
  registrar = DEFAULT_ACTION_REGISTRAR_TEMPLATE.substitute(
    base=base,
    success_field=success_field,
    ampersand_suffix=ampersand_suffix,
    res_prefix=res_prefix,
  )
  using_stmt = f"using {type_name};" if package == "interfaces" else None
  return {
    "base": base,
    "type": type_name,
    "tuple_entry": type_name,
    "header_include": include_path,
    "cpp_include": include_path,
    "using": using_stmt,
    "trait_block": trait_block,
    "registrar": registrar,
  }


def build_default_service_spec(base: str) -> dict:
    package = infer_package(base, "service")
    type_name = f"{package}::srv::{base}"
    snake = camel_to_snake(base)
    include_path = f"{package}/srv/{snake}.hpp"
    using_stmt = f"using {type_name};" if package == "interfaces" else None
    registrar = DEFAULT_SERVICE_REGISTRAR_TEMPLATE.substitute(type=type_name)
    return {
        "base": base,
        "header_include": None,
        "cpp_include": include_path,
        "using": using_stmt,
        "registrar": registrar,
    }


def build_slam_mode_spec(base: str) -> dict:
    package = infer_package(base, "action")
    type_name = f"{package}::action::{base}"
    snake = camel_to_snake(base)
    trait_block = Template("""\ntemplate<>
struct SkillActionTrait<${type}>
{
  static constexpr const char * skill_name = \"${base}\";
  static bool success(const ${type}::Result & r)
  {
    return r.success;
  }
  static std::string message(const ${type}::Result & r)
  {
    return r.message;
  }
};""").substitute(type=type_name, base=base)
    registrar = """        if (!action_clients_) {return;}
        auto make_goal = [this]() {
            brain_interfaces::action::SlamMode::Goal goal;
            const std::string enable_param = "slam_mode.enable_mapping";
            if (!node_->has_parameter(enable_param)) {
              node_->declare_parameter<bool>(enable_param, true);
            }
            goal.enable_mapping = node_->get_parameter(enable_param).as_bool();
            return goal;
          };
        auto on_result = [this, internal_skill](
            const ActionClientRegistry::GoalHandle<brain_interfaces::action::SlamMode>::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 if (res.code == rclcpp_action::ResultCode::CANCELED) {
              RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
            } else {
              RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
            }
          };
        action_clients_->register_client<brain_interfaces::action::SlamMode>(
          topic,
          make_goal,
          nullptr,
          on_result,
          nullptr);"""
    include_path = f"{package}/action/{snake}.hpp"
    return {
        "base": base,
        "type": type_name,
        "tuple_entry": type_name,
        "header_include": include_path,
        "cpp_include": include_path,
        "using": None,
        "trait_block": trait_block,
        "registrar": registrar,
    }


def build_navigate_to_pose_spec(base: str) -> dict:
    package = infer_package(base, "action")
    type_name = f"{package}::action::{base}"
    snake = camel_to_snake(base)
    trait_block = Template("""\ntemplate<>
struct SkillActionTrait<${type}>
{
  static constexpr const char * skill_name = \"${base}\";
  static bool success(const ${type}::Result & r)
  {
    (void)r;
    // NavigateToPose::Result carries an empty payload; success is inferred from the
    // rclcpp_action::ResultCode before this helper is invoked.
    return true;
  }
  static std::string message(const ${type}::Result & r)
  {
    (void)r;
    return "NavigateToPose completed";
  }
};""").substitute(type=type_name, base=base)
    registrar = """        if (!action_clients_) {return;}
        auto make_goal = [this]() {
            nav2_msgs::action::NavigateToPose::Goal goal;
            const std::string pose_param = "navigate_to_pose.default_goal";
            const std::string frame_param = "navigate_to_pose.goal_frame";
            if (!node_->has_parameter(pose_param)) {
              node_->declare_parameter<std::vector<double>>(pose_param, {0.0, 0.0, 0.0});
            }
            if (!node_->has_parameter(frame_param)) {
              node_->declare_parameter<std::string>(frame_param, "map");
            }
            std::vector<double> values;
            try {
              values = node_->get_parameter(pose_param).as_double_array();
            } catch (const rclcpp::ParameterTypeException &)
            {
              RCLCPP_WARN(node_->get_logger(), "Parameter %s must be an array of doubles", pose_param.c_str());
            }
            const auto now = node_->get_clock()->now();
            goal.pose.header.stamp = now;
            goal.pose.header.frame_id = node_->get_parameter(frame_param).as_string();
            if (values.size() >= 2) {
              goal.pose.pose.position.x = values[0];
              goal.pose.pose.position.y = values[1];
            }
            double yaw = (values.size() >= 3) ? values[2] : 0.0;
            const double half = yaw * 0.5;
            goal.pose.pose.orientation.w = std::cos(half);
            goal.pose.pose.orientation.x = 0.0;
            goal.pose.pose.orientation.y = 0.0;
            goal.pose.pose.orientation.z = std::sin(half);
            return goal;
          };
        auto on_feedback = [this, internal_skill](
            ActionClientRegistry::GoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr,
            const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> feedback) {
            if (!feedback) {return;}
            RCLCPP_DEBUG(
              node_->get_logger(), "NavigateToPose feedback (%s): distance_remaining=%.2f", internal_skill.c_str(),
              feedback->distance_remaining);
          };
        auto on_result = [this, internal_skill](
            const ActionClientRegistry::GoalHandle<nav2_msgs::action::NavigateToPose>::WrappedResult & res) {
            switch (res.code) {
              case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
                break;
              case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
                break;
              case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(node_->get_logger(), "action aborted: %s", internal_skill.c_str());
                break;
              default:
                RCLCPP_ERROR(node_->get_logger(), "NavigateToPose result error for %s (code=%d)",
                  internal_skill.c_str(), static_cast<int>(res.code));
                break;
            }
          };
        action_clients_->register_client<nav2_msgs::action::NavigateToPose>(
          topic,
          make_goal,
          on_feedback,
          on_result);"""
    include_path = f"{package}/action/{snake}.hpp"
    return {
        "base": base,
        "type": type_name,
        "tuple_entry": type_name,
        "header_include": include_path,
        "cpp_include": include_path,
        "using": None,
        "trait_block": trait_block,
        "registrar": registrar,
    }


def build_vision_object_recognition_spec(base: str) -> dict:
    package = infer_package(base, "service")
    snake = camel_to_snake(base)
    include_path = f"{package}/srv/{snake}.hpp"
    using_stmt = f"using {package}::srv::{base};" if package == "interfaces" else None
    registrar = """        // 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;"""
    return {
        "base": base,
        "header_include": include_path,
        "cpp_include": include_path,
        "using": using_stmt,
        "registrar": registrar,
    }


ACTION_OVERRIDE_BUILDERS = {
    "SlamMode": build_slam_mode_spec,
    "NavigateToPose": build_navigate_to_pose_spec,
}

ACTION_OVERRIDE_ORDER = ["SlamMode", "NavigateToPose"]

SERVICE_OVERRIDE_BUILDERS = {
    "VisionObjectRecognition": build_vision_object_recognition_spec,
}

HEADER_TEMPLATE = Template("""#pragma once

#include <rclcpp/rclcpp.hpp>
#include <yaml-cpp/yaml.h>
#include <string>
#include <unordered_map>
#include <vector>
#include <memory>

#include \"brain/action_registry.hpp\"
#include \"brain/bt_registry.hpp\"

$pre_interface_includes

#include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
$post_interface_includes

namespace brain
{

/**
 * @brief Specification of a skill as parsed from YAML.
 *
 * Fields capture metadata, dependency declarations and interface exposure. Interface strings use
 * the convention \"BaseName.action\" or \"BaseName.service\" to distinguish interface kinds.
 */
struct SkillSpec
{
  std::string name;
  std::string version;
  std::string description;
  std::vector<std::string> required_skills;
  std::vector<std::string> interfaces;  // e.g. \"BaseName.action\" / \"BaseName.service\"
};

/**
 * @brief Loads skill specifications and registers related action/service interfaces.
 *
 * Responsibilities:
 *  1. Parse a YAML description file (robot_skills.yaml or equivalent).
 *  2. Register action clients and service clients for declared interfaces.
 *  3. Provide enumeration utilities and type-safe access to service clients.
 *
 * BehaviorTree action node registration is handled elsewhere; this class only prepares interface bindings.
 */
class SkillManager
{
public:
  /**
   * @brief Construct with external registry references.
   * @param node ROS 2 node pointer (not owned).
   * @param action_clients Action client registry used to register skill actions.
   * @param bt Behavior tree registry reference (not used for node creation directly here).
   */
  SkillManager(rclcpp::Node * node, ActionClientRegistry * action_clients, BTRegistry * bt)
  : node_(node), action_clients_(action_clients), bt_(bt)
  {
    if (node_) {
      constexpr auto prefix_param = \"skill_topic_prefix\";
      if (!node_->has_parameter(prefix_param)) {
        node_->declare_parameter<std::string>(prefix_param, \"\");
      }
    }
  }

  /**
   * @brief Load skill specifications from a YAML file.
   * @param yaml_path File system path to YAML document.
   * @return true on success, false if parsing or validation fails.
   */
  bool load_from_file(const std::string & yaml_path);

  /**
   * @brief Access loaded skills mapping.
   * @return Map of skill name -> SkillSpec.
   */
  const std::unordered_map<std::string, SkillSpec> & skills() const {return skills_;}

  /**
   * @brief Enumerate action oriented skills.
   * @return Vector of (skill_name, action_base) pairs.
   */
  std::vector<std::pair<std::string, std::string>> enumerate_action_skills() const;

private:
  rclcpp::Node * node_ {nullptr};
  ActionClientRegistry * action_clients_ {nullptr};
  BTRegistry * bt_ {nullptr};
  std::unordered_map<std::string, SkillSpec> skills_;
  std::vector<rclcpp::ClientBase::SharedPtr> service_clients_; // 保存 service client 生命周期
  std::unordered_map<std::string, rclcpp::ClientBase::SharedPtr> service_client_map_; // skill name -> client

  void register_interfaces_(const SkillSpec & s);

public:
  /**
   * @brief Retrieve a typed service client for a named skill if available.
   * @tparam T Service type.
   * @param skill_name Registered skill name.
   * @return Shared pointer to service client or nullptr if not present or type mismatch.
   */
  template<typename T>
  std::shared_ptr<rclcpp::Client<T>> get_typed_service_client(const std::string & skill_name) const
  {
    auto it = service_client_map_.find(skill_name);
    if (it == service_client_map_.end()) {return nullptr;}
    return std::dynamic_pointer_cast<rclcpp::Client<T>>(it->second);
  }
};


/**
 * @brief Primary template (unspecialized) for skill action trait extraction.
 * Specializations must define: skill_name (constexpr const char*), success(result), message(result).
 */
template<typename ActionT>
struct SkillActionTrait; // intentionally undefined primary template

$trait_specializations

/**
 * @brief Tuple listing of all supported skill action types for dispatcher iteration.
 */
using SkillActionTypes = std::tuple<
  $skill_action_types
>;

/**
 * @brief Recursive compile-time dispatcher that matches a skill name to an action type and invokes send_and_wait.
 * @tparam I Current tuple index (internal use, defaults to 0).
 * @tparam TupleT Tuple type containing action types (e.g., SkillActionTypes).
 * @tparam RegistryT Registry providing template send_and_wait<ActionT>(...).
 * @param skill Canonical skill name to match (SkillActionTrait<ActionT>::skill_name).
 * @param topic Action topic name used when calling the registry.
 * @param reg Pointer to registry instance.
 * @param logger Logger for diagnostics.
 * @param timeout Wait timeout for the action result.
 * @param detail Populated with success/failure detail message.
 * @return true if execution succeeded and result success flag is true; false otherwise.
 */
template<size_t I = 0, typename TupleT, typename RegistryT>
bool dispatch_skill_action(
  const std::string & skill, const std::string & topic, RegistryT * reg,
  rclcpp::Logger logger, std::chrono::nanoseconds timeout, std::string & detail)
{
  if constexpr (I == std::tuple_size<TupleT>::value) {
    return false;
  } else {
    using ActionT = std::tuple_element_t<I, TupleT>;
    if (skill == SkillActionTrait<ActionT>::skill_name) {
      auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
      if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
        bool ok = SkillActionTrait<ActionT>::success(*opt->result);
        detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
          \"result flag false\");
        return ok;
      }
      detail = \"action failed\";
      return false;
    }
    return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, detail);
  }
}


} // namespace brain

""")

CPP_TEMPLATE = Template("""/**
 * @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 <rclcpp_action/rclcpp_action.hpp>

$include_block

$using_block

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)
{
  static const std::unordered_map<std::string, std::function<void(const std::string &,
    const std::string &)>> action_registrars = {
$action_registrars
  };
  static const std::unordered_map<std::string, std::function<void(const std::string &,
    const std::string &)>> service_registrars = {
$service_registrars
  };

  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_WARN(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) {
        const std::string default_topic = join_topic(prefix_value, base_snake);
        const std::string param_name = base_snake + \".\" + kind_suffix + \"_name\";
        std::string topic_override;
        if (node_->get_parameter(param_name, topic_override)) {
          if (topic_override.empty()) {
            topic_override = default_topic;
          }
          return topic_override;
        }
        node_->declare_parameter<std::string>(param_name, default_topic);
        return default_topic;
      };

    if (parsed->kind == \"action\") {
      const std::string topic_name = resolve_topic_name(\"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\") {
      const std::string topic_name = resolve_topic_name(\"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

""")

def parse_interfaces(yaml_path: Path) -> Tuple[List[str], List[str]]:
  data = yaml.safe_load(yaml_path.read_text(encoding="utf-8"))
  if not isinstance(data, list):
    raise ValueError("YAML root must be a sequence of skills")

  actions: List[str] = []
  services: List[str] = []
  seen_actions: Set[str] = set()
  seen_services: Set[str] = set()
  for item in data:
    if not isinstance(item, dict):
      continue
    interfaces = item.get("interfaces") or []
    for entry in interfaces:
      if not isinstance(entry, str):
        continue
      pos = entry.rfind(".")
      if pos <= 0 or pos + 1 >= len(entry):
        continue
      base = entry[:pos]
      kind = entry[pos + 1 :].lower()
      if kind == "action":
        if base not in seen_actions:
          actions.append(base)
          seen_actions.add(base)
      elif kind == "service":
        if base not in seen_services:
          services.append(base)
          seen_services.add(base)
  return actions, services

def build_action_specs(
  actions_available: Sequence[str],
  message_styles: Mapping[str, str],
  registrar_styles: Mapping[str, str],
) -> List[dict]:
  specs: List[dict] = []
  available_set = set(actions_available)
  defaults = sorted(base for base in available_set if base not in ACTION_OVERRIDE_BUILDERS)
  for base in defaults:
    specs.append(
      build_default_action_spec(base, message_styles.get(base), registrar_styles.get(base))
    )

  for base in ACTION_OVERRIDE_ORDER:
    if base in available_set:
      specs.append(ACTION_OVERRIDE_BUILDERS[base](base))

  remaining = [
    base for base in available_set
    if base in ACTION_OVERRIDE_BUILDERS and base not in ACTION_OVERRIDE_ORDER
  ]
  for base in remaining:
    specs.append(ACTION_OVERRIDE_BUILDERS[base](base))

  return specs


def build_service_specs(services_available: Sequence[str]) -> List[dict]:
  specs: List[dict] = []
  handled: Set[str] = set()
  for base in services_available:
    if base in handled:
      continue
    builder = SERVICE_OVERRIDE_BUILDERS.get(base)
    specs.append(builder(base) if builder else build_default_service_spec(base))
    handled.add(base)
  return specs

def collect_includes(specs: Sequence[dict], key: str) -> List[str]:
    seen: Set[str] = set()
    includes: List[str] = []
    for spec in specs:
        path = spec.get(key)
        if not path or path in seen:
            continue
        includes.append(path)
        seen.add(path)
    return includes


def order_cpp_includes(paths: Sequence[str]) -> List[str]:
    groups: Tuple[List[str], ...] = ([], [], [], [], [])
    for path in paths:
        if path.startswith("interfaces/action/"):
            groups[0].append(path)
        elif path.startswith("interfaces/srv/"):
            groups[1].append(path)
        elif path.startswith("brain_interfaces/action/"):
            groups[2].append(path)
        elif path.startswith("brain_interfaces/srv/"):
            groups[3].append(path)
        else:
            groups[4].append(path)

    ordered: List[str] = []
    for group in groups:
        ordered.extend(group)
    return ordered


def render_header(actions: Sequence[dict], services: Sequence[dict]) -> str:
    include_paths = collect_includes(list(actions) + list(services), "header_include")
    pre_includes = [inc for inc in include_paths if inc.startswith("interfaces/")]
    post_includes = [inc for inc in include_paths if not inc.startswith("interfaces/")]
    trait_blocks = "\n".join(spec["trait_block"].rstrip() for spec in actions)
    skill_tuple = ",\n  ".join(spec["tuple_entry"] for spec in actions)

    return HEADER_TEMPLATE.substitute(
        pre_interface_includes="\n".join(f'#include "{inc}"' for inc in pre_includes),
        post_interface_includes="\n".join(f'#include "{inc}"' for inc in post_includes),
        trait_specializations=trait_blocks,
        skill_action_types=skill_tuple,
    )


def render_cpp(actions: Sequence[dict], services: Sequence[dict]) -> str:
    include_paths = order_cpp_includes(collect_includes(list(actions) + list(services), "cpp_include"))
    include_block = "\n".join(f'#include "{inc}"' for inc in include_paths)
    using_sources = [spec for spec in list(actions) + list(services) if spec.get("using")]
    using_lines = "\n".join(spec["using"] for spec in using_sources)

    action_entries = [
        '    {"%s", [this](const std::string & topic, const std::string & internal_skill) {\n%s\n      }},'
        % (spec["base"], spec["registrar"].rstrip())
        for spec in actions
    ]
    service_entries = [
        '    {"%s",\n      [this](const std::string & topic, const std::string & internal_skill) {\n%s\n      }},'
        % (spec["base"], spec["registrar"].rstrip())
        for spec in services
    ]

    action_block = "\n".join(action_entries) + "\n"
    service_block = "\n".join(service_entries) + "\n"

    return CPP_TEMPLATE.substitute(
        include_block=include_block,
        using_block=using_lines,
        action_registrars=action_block,
        service_registrars=service_block,
    )

def generate(yaml_path: Path, include_dir: Path, src_dir: Path) -> None:
  actions_available, services_available = parse_interfaces(yaml_path)
  if not actions_available:
    raise ValueError("At least one action interface is required in robot_skills.yaml")

  hdr_path = include_dir / "skill_manager.hpp"
  cpp_path = src_dir / "skill_manager.cpp"
  message_styles = load_existing_message_styles(hdr_path)
  registrar_styles = load_wrapped_result_styles(cpp_path)

  actions = build_action_specs(actions_available, message_styles, registrar_styles)
  services = build_service_specs(services_available)

  header_code = render_header(actions, services)
  cpp_code = render_cpp(actions, services)

  include_dir.mkdir(parents=True, exist_ok=True)
  src_dir.mkdir(parents=True, exist_ok=True)

  hdr_path.write_text(header_code.rstrip() + "\n", encoding="utf-8")
  cpp_path.write_text(cpp_code.rstrip() + "\n", encoding="utf-8")

  print(f"Generated: {hdr_path}")
  print(f"Generated: {cpp_path}")

def main() -> None:
    parser = argparse.ArgumentParser(description="Generate SkillManager sources from YAML")
    parser.add_argument("--yaml", dest="yaml_path", default="src/brain/config/robot_skills.yaml")
    parser.add_argument("--include-dir", dest="include_dir", default="src/brain/include/brain")
    parser.add_argument("--src-dir", dest="src_dir", default="src/brain/src")
    args = parser.parse_args()

    yaml_path = Path(args.yaml_path)
    include_dir = Path(args.include_dir)
    src_dir = Path(args.src_dir)

    if not yaml_path.exists():
        print(f"YAML not found: {yaml_path}", file=sys.stderr)
        sys.exit(1)

    generate(yaml_path, include_dir, src_dir)

if __name__ == "__main__":
    main()
