#include <robot/node_handle.h>
#include <robot/console.h>
#include <sstream>
#include <algorithm>
#include <cctype>
#include <fstream>
#include <filesystem>
#include <functional>
#include <iostream>
#include <cstdlib>
#include <cstring>
#include <unistd.h>
#include <limits.h>
#ifdef __linux__
#include <linux/limits.h>
#endif
#include <robot_xmlrpcpp/XmlRpcValue.h>

namespace robot
{

  NodeHandle::~NodeHandle() {}

  // Constructor with namespace and optional remappings
  NodeHandle::NodeHandle(const std::string &ns, const M_string &remappings)
  {
    // Apply remappings if provided
    (void)remappings; // Suppress unused parameter warning for now

    // Step 1: If root_ is not defined, load config files into root_
    if (!root_.IsDefined() || !root_.IsMap() || root_.size() == 0)
    {
      // Find and set config directory first
      if (config_directory_.empty())
      {
        std::string config_dir = findConfigDirectory();
        if (!config_dir.empty())
        {
          config_directory_ = config_dir;
        }
      }

      // Initialize root_ as empty map
      root_ = YAML::Node(YAML::NodeType::Map);

      // Auto-load config files and merge into root_
      if (!config_directory_.empty() && std::filesystem::exists(config_directory_) && std::filesystem::is_directory(config_directory_))
      {
        int loaded_count = loadYamlFilesFromDirectory(config_directory_);
        if (loaded_count > 0)
        {
          std::cout << "[NodeHandle] Auto-loaded " << loaded_count
                    << " YAML file(s) from: " << config_directory_ << std::endl;
        }
      }
    }

    // Step 2: Update node_handle_ from root_ based on namespace_
    // Normalize namespace: strip leading '/' if present
    std::string ns_normalized = ns;
    if (!ns_normalized.empty() && ns_normalized.front() == '/')
    {
      ns_normalized = ns_normalized.substr(1);
    }
    if (ns_normalized == "~" || ns_normalized.empty())
    {
      // If namespace is root, node_handle_ should be the full root_ (direct reference)
      // node_handle_ = root_;
      namespace_ = "/";
    }
    else
    {
      // Try to scope node_handle_ to namespace_ from root_
      std::stringstream ns_ss(ns_normalized);
      std::string ns_segment;
      std::vector<std::string> segs;

      while (std::getline(ns_ss, ns_segment, '/'))
      {
        if (!ns_segment.empty())
        {
          segs.push_back(ns_segment);
        }
      }
      if (!segs.empty())
      {
        std::vector<YAML::Node> curs;
        curs.push_back(root_);
        bool ns_found = true;
        for (int i = 0; i < segs.size() - 1; i++)
        {
          const auto &k = segs[i];
          if (k.empty())
            continue;
          if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
          {
            ns_found = false;
            break;
          }
          curs.push_back(curs.back()[k]);
        }        
        if (ns_found && curs.back().IsDefined())
        {
          // Scope node_handle_ to the found namespace (direct reference, syncs with root_)
          // node_handle_ = curs.back();
          namespace_ = ns_normalized;
        }
        else
        {
          // Namespace not found: use root_ as fallback
          // node_handle_ = root_;
          namespace_ = "/"; // indicate root when ns not found
        }
      }
      else
      {
        // Namespace not found: use root_ as fallback
        // node_handle_ = root_;
        namespace_ = "/"; // indicate root when ns not found
      }
    }
  }

  // Constructor from parent NodeHandle with new namespace (2 parameters - delegates to 3 parameter version)
  // This is needed to ensure the symbol is exported correctly in the shared library
  NodeHandle::NodeHandle(const NodeHandle &parent, const std::string &ns)
    : NodeHandle(parent, ns, M_string())
  {
  }

  // Constructor from parent NodeHandle with new namespace and remappings
  NodeHandle::NodeHandle(const NodeHandle &parent, const std::string &ns, const M_string &remappings)
  {
    // Apply remappings if provided
    (void)remappings; // Suppress unused parameter warning for now

    // Step 1: If root_ is not defined, load config files into root_
    if (!root_.IsDefined() || !root_.IsMap() || root_.size() == 0)
    {
      // Find and set config directory first
      if (config_directory_.empty())
      {
        std::string config_dir = findConfigDirectory();
        if (!config_dir.empty())
        {
          config_directory_ = config_dir;
        }
      }

      // Initialize root_ as empty map
      root_ = YAML::Node(YAML::NodeType::Map);
      log_warning("Root is not defined, initializing root_ as empty map");

      // Auto-load config files and merge into root_
      if (!config_directory_.empty() && std::filesystem::exists(config_directory_) && std::filesystem::is_directory(config_directory_))
      {
        int loaded_count = loadYamlFilesFromDirectory(config_directory_);
        if (loaded_count > 0)
        {
          std::cout << "[NodeHandle] Auto-loaded " << loaded_count
                    << " YAML file(s) from: " << config_directory_ << std::endl;
        }
      }
    }
    // Normalize child namespace (remove leading '/')
    std::string child_ns = ns;
    while (!child_ns.empty() && child_ns.front() == '/')
    {
      child_ns.erase(child_ns.begin());
    }
    std::string parent_ns = parent.getNamespace();
    while (!parent_ns.empty() && parent_ns.front() == '/')
    {
      parent_ns.erase(parent_ns.begin());
    }
    std::string ns_normalized = parent_ns + "/" + child_ns;
    if (ns_normalized != "~" && !ns_normalized.empty())
    {
      // Try to scope node_handle_ to namespace_ from root_
      std::stringstream ns_ss(ns_normalized);
      std::string ns_segment;
      std::vector<std::string> segs;
      while (std::getline(ns_ss, ns_segment, '/'))
      {
        if (!ns_segment.empty())
        {
          segs.push_back(ns_segment);
        }
      }
      if (!segs.empty())
      {
        std::vector<YAML::Node> curs = {root_};
        bool ns_found = true;
        for (int i = 0; i < segs.size() - 1; i++)
        {
          const auto k = segs[i];
          if (k.empty())
            continue;
          if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
          {
            robot::log_warning_at(__FILE__, __LINE__, "%s %x %x", k.c_str(), curs.back()[k].IsDefined(), curs.back()[k].IsMap());
            ns_found = false;
            break;
          }
          curs.push_back(curs.back()[k]);
        }

        if (ns_found && curs.back().IsDefined() && curs.size() >= 1)
        {
          // Scope node_handle_ to the found namespace (direct reference, syncs with root_)
          // node_handle_ = curs.back()[segs.back()];
          namespace_ = ns_normalized;
        }
        else
        {
          // Namespace not found: use root_ as fallback
          // node_handle_ = parent.getNodeHandle();
          namespace_ = parent.getNamespace(); // indicate root when ns not found
        }
      }
    }
  }

  // Copy constructor
  NodeHandle::NodeHandle(const NodeHandle &parent) 
  {
    this->namespace_ = parent.namespace_;
    if(!namespace_.empty() && namespace_ != "/")
    {
      // Try to scope node_handle_ to namespace_ from root_
      std::stringstream ns_ss(namespace_);
      std::string ns_segment;
      std::vector<std::string> segs;
      while (std::getline(ns_ss, ns_segment, '/'))
      {
        if (!ns_segment.empty())
        {
          segs.push_back(ns_segment);
        }
      }
      if (!segs.empty())
      {
        std::vector<YAML::Node> curs;
        curs.push_back(parent.root_);
        bool ns_found = true;
        for (int i = 0; i < segs.size() - 1; i++)
        {

          const auto &k = segs[i];
          if (k.empty())
            continue;
          if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
          {
            ns_found = false;
            break;
          }
          curs.push_back(curs.back()[k]);
        }
        if (ns_found && curs.back().IsDefined() && curs.size() >= 1)
        {
          // Scope node_handle_ to the found namespace (direct reference, syncs with root_)
          // node_handle_[segs.back()] = curs.back()[segs.back()];
        }
        else
        {
          // Namespace not found: use root_ as fallback
          // node_handle_ = parent.getNodeHandle();
        }
      }
    }
  }

  // Assignment operator
  NodeHandle &NodeHandle::operator=(const NodeHandle &parent)
  {
    if (this != &parent)
    {
      namespace_ = parent.getNamespace();
      // node_handle_ = parent.getNodeHandle();
    }

    return *this;
  }

  bool NodeHandle::hasParam(const std::string &key) const
  {
    // Safety check: validate key parameter
    if (key.empty())
    {
      return false;
    }

    std::string full_key = namespace_ + "/" + key;
    if (!root_.IsDefined() || !root_.IsMap())
    {
      return false;
    }

    // Split key by '/' to handle nested keys
    std::stringstream ss(full_key);
    std::string segment;
    std::vector<std::string> segments;

    while (std::getline(ss, segment, '/'))
    {
      if (!segment.empty())
      {
        segments.push_back(segment);
      }
    }

    if (segments.empty())
    {
      // Empty key refers to the whole tree; treat as existing only if map is non-empty
      return false;
    }

    const YAML::Node *current = &root_;

    for (size_t i = 0; i < segments.size(); ++i)
    {
      // Safety check: ensure current is valid
      if (!current || !current->IsDefined())
      {
        return false;
      }

      if (current->IsMap())
      {
        // Search for the segment without inserting default nodes
        const YAML::Node *found_child = nullptr;
        
        try
        {
          for (YAML::const_iterator it = current->begin(); it != current->end(); ++it)
          {
            try
            {
              // Safety check: validate iterator nodes
              if (!it->first.IsDefined())
              {
                continue;
              }
              
              // Safely convert key to string
              std::string key_str;
              try
              {
                key_str = it->first.as<std::string>();
              }
              catch (...)
              {
                // Skip nodes that can't be converted to string
                continue;
              }
              
              if (key_str == segments[i])
              {
                // Safety check: validate value node
                if (it->second.IsDefined())
                {
                  found_child = &(it->second);
                  break;
                }
              }
            }
            catch (...)
            {
              // Skip invalid nodes and continue iteration
              continue;
            }
          }
        }
        catch (...)
        {
          // If iteration fails, return false
          return false;
        }
        
        if (!found_child || !found_child->IsDefined())
        {
          return false;
        }
        current = found_child;
        continue;
      }

      if (current->IsSequence())
      {
        // For sequences, interpret segment as index if possible
        try
        {
          size_t idx = static_cast<size_t>(std::stoul(segments[i]));
          if (idx >= current->size())
          {
            return false;
          }
          const YAML::Node &child = (*current)[idx];
          if (!child.IsDefined())
          {
            return false;
          }
          current = &child;
          continue;
        }
        catch (...)
        {
          // Segment is not an index
          return false;
        }
      }

      // Neither map nor sequence: cannot traverse further
      return false;
    }
    
    // Safety check: ensure current is valid before returning
    return current && current->IsDefined();
  }

  YAML::Node NodeHandle::getNestedValue(const std::string &key) const
  {
    // Safety check: validate key parameter
    if (key.empty())
    {
      return YAML::Node();
    }

    // Safety check: validate root node
    if (!root_.IsDefined() || !root_.IsMap())
    {
      return YAML::Node();
    }

    // Split key by '/' to handle nested keys
    std::stringstream ss(namespace_ + "/" + key);
    std::string segment;
    std::vector<std::string> segments;

    while (std::getline(ss, segment, '/'))
    {
      if (!segment.empty())
      {
        segments.push_back(segment);
      }
    }

    if (segments.empty())
    {
      return YAML::Node();
    }

    // Traverse the tree without modifying node_handle_
    // Use const reference to the original node_handle_ to avoid any modifications
    const YAML::Node *current = &root_;

    for (size_t i = 0; i < segments.size(); ++i)
    {
      // Safety check: ensure current is valid
      if (!current || !current->IsDefined() || !current->IsMap())
      {
        return YAML::Node();
      }

      // Access child node without mutating the map (avoid operator[] default insertion)
      const YAML::Node *found_child = nullptr;
      
      try
      {
        for (YAML::const_iterator it = current->begin(); it != current->end(); ++it)
        {
          try
          {
            // Safety check: validate iterator nodes
            if (!it->first.IsDefined())
            {
              continue;
            }
            
            // Safely convert key to string
            std::string key_str;
            try
            {
              key_str = it->first.as<std::string>();
            }
            catch (...)
            {
              // Skip nodes that can't be converted to string
              continue;
            }
            
            if (key_str == segments[i])
            {
              // Safety check: validate value node
              if (it->second.IsDefined())
              {
                found_child = &(it->second);
                break;
              }
            }
          }
          catch (...)
          {
            // Skip invalid nodes and continue iteration
            continue;
          }
        }
      }
      catch (...)
      {
        // If iteration fails, return empty node
        return YAML::Node();
      }
      
      if (!found_child || !found_child->IsDefined())
      {
        return YAML::Node();
      }

      // Move to next level
      current = found_child;
    }

    // Safety check: ensure current is valid before cloning
    if (!current || !current->IsDefined())
    {
      return YAML::Node();
    }

    // Return a clone of the final node to ensure it's independent
    try
    {
      return YAML::Clone(*current);
    }
    catch (...)
    {
      // If cloning fails, return empty node
      return YAML::Node();
    }
  }

  YAML::Node NodeHandle::getNestedValueDebug(const std::string &key) const
  {
    std::string full_key = namespace_ + "/" + key;
    if (!root_.IsDefined() || !root_.IsMap())
    {
      return YAML::Node();
    }

    // Split key by '/' to handle nested keys
    std::stringstream ss(key);
    std::string segment;
    std::vector<std::string> segments;

    while (std::getline(ss, segment, '/'))
    {
      if (!segment.empty())
      {
        segments.push_back(segment);
      }
    }

    if (segments.empty())
    {
      return YAML::Node();
    }

    const YAML::Node *current = &root_;

    for (size_t i = 0; i < segments.size(); ++i)
    {
      if (!current->IsMap())
      {
        return YAML::Node();
      }

      // Access child node without mutating the map (avoid operator[] default insertion)
      YAML::Node segment_node;
      bool found_child = false;
      for (auto it = current->begin(); it != current->end(); ++it)
      {
        if (it->first.as<std::string>() == segments[i])
        {
          segment_node = it->second;
          found_child = true;
          break;
        }
      }
      if (!found_child)
      {
        return YAML::Node();
      }
      if (!segment_node.IsDefined())
      {
        return YAML::Node();
      }

      current = &segment_node;
    }
    return YAML::Clone(*current);
  }

  YAML::Node NodeHandle::getParamValue(const std::string &key) const
  {
    return getNestedValue(key);
  }

  std::string NodeHandle::getNamespace() const
  {
    // namespace_ is already set to config directory path if name was "~" or ""
    return namespace_;
  }

  void NodeHandle::printParams() const
  {
    log_info("\n[NodeHandle] ========== Root Parameters ==========");
    log_info("Namespace: %s", this->getNamespace().c_str());
    printParams(root_);
  }

  void NodeHandle::printParams(YAML::Node node) const
  {
    if (!node.IsDefined())
    {
      std::cout << "[NodeHandle] Node is not defined!" << std::endl;
      return;
    }

    if (!node.IsMap())
    {
      std::cout << "[NodeHandle] Node is not a map!" << std::endl;
      return;
    }

    std::cout << "[NodeHandle] Total top-level keys: " << node.size() << std::endl;
    std::cout << std::endl;

    // Helper lambda to print nested structure with indentation
    std::function<void(const YAML::Node &, const std::string &, int)> printNode =
        [&printNode](const YAML::Node &node, const std::string &prefix, int depth)
    {
      if (depth > 10)
        return; // Limit depth to avoid infinite recursion

      std::string indent(depth * 2, ' '); // 2 spaces per depth level

      if (node.IsMap())
      {
        for (auto it = node.begin(); it != node.end(); ++it)
        {
          std::string key = it->first.as<std::string>();
          const YAML::Node &value = it->second;
          std::string full_key = prefix.empty() ? key : prefix + "/" + key;
          if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos)
            continue;
          if (value.IsMap())
          {
            std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl;
            printNode(value, full_key, depth + 1);
          }
          else if (value.IsSequence())
          {
            std::cout << "[NodeHandle] " << indent << full_key << " (sequence, " << value.size() << " items):" << std::endl;
            // Print sequence items
            for (size_t i = 0; i < value.size(); ++i)
            {
              const YAML::Node &item = value[i];
              if (item.IsMap())
              {
                std::cout << "[NodeHandle] " << indent << "  [" << i << "]:" << std::endl;
                printNode(item, full_key + "[" + std::to_string(i) + "]", depth + 2);
              }
              else if (item.IsScalar())
              {
                std::cout << "[NodeHandle] " << indent << "  [" << i << "] = " << item.as<std::string>() << std::endl;
              }
            }
          }
          else if (value.IsScalar())
          {
            try
            {
              std::cout << "[NodeHandle] " << indent << full_key << " = " << value.as<std::string>() << std::endl;
            }
            catch (...)
            {
              std::cout << "[NodeHandle] " << indent << full_key << " = (unable to convert to string)" << std::endl;
            }
          }
          else if (value.IsNull())
          {
            std::cout << "[NodeHandle] " << indent << full_key << " = null" << std::endl;
          }
        }
      }
    };

    printNode(node, "", 0);
    std::cout << "[NodeHandle] ================================================\n"
              << std::endl;
  }

  void NodeHandle::mergeYamlNode(const YAML::Node &source, YAML::Node &output)
  {
    if (!source.IsDefined())
    {
      return;
    }

    // If source is not a map, just replace node_handle_
    if (!source.IsMap())
    {
      output = source;
      return;
    }

    // Initialize node_handle_ as a map if it's not already
    if (!output.IsDefined() || !output.IsMap())
    {
      output = YAML::Node(YAML::NodeType::Map);
    }

    // Helper lambda to recursively merge two YAML nodes
    // Note: yaml-cpp's operator[] returns YAML::Node (not reference), so we need to work with copies
    std::function<void(YAML::Node &, const YAML::Node &)> mergeRecursive =
        [&mergeRecursive](YAML::Node &dest, const YAML::Node &src)
    {
      if (!src.IsMap())
      {
        dest = src;
        return;
      }

      // Ensure dest is a map
      if (!dest.IsMap())
      {
        dest = YAML::Node(YAML::NodeType::Map);
      }

      for (auto it = src.begin(); it != src.end(); ++it)
      {
        std::string key = it->first.as<std::string>();
        const YAML::Node &src_value = it->second;

        // Get current value in dest (if exists)
        YAML::Node dest_current = dest[key];

        // Check if key exists in dest and if both are maps
        if (dest_current.IsDefined() && dest_current.IsMap() && src_value.IsMap())
        {
          // Both are maps, recursively merge
          // Create a copy of dest[key] to work with, then assign back
          YAML::Node dest_value = dest_current;
          mergeRecursive(dest_value, src_value);
          dest[key] = dest_value;
        }
        else
        {
          // Overwrite with source value (or add if doesn't exist)
          dest[key] = src_value;
        }
      }
    };

    // Merge source into output
    mergeRecursive(output, source);
  }

  int NodeHandle::loadYamlFilesFromDirectory(const std::string &directory_path)
  {
    int loaded_count = 0;

    try
    {
      // Check if directory exists
      if (!std::filesystem::exists(directory_path) || !std::filesystem::is_directory(directory_path))
      {
        std::cerr << "Directory does not exist or is not a directory: " << directory_path << std::endl;
        return 0;
      }

      // Iterate through all files in the directory
      for (const auto &entry : std::filesystem::directory_iterator(directory_path))
      {
        if (entry.is_regular_file())
        {
          std::string file_path = entry.path().string();
          std::string extension = entry.path().extension().string();

          // Only process .yaml and .yml files
          if (extension == ".yaml" || extension == ".yml")
          {
            try
            {
              YAML::Node file_node = YAML::LoadFile(file_path);
              if (file_node.IsDefined())
              {
                mergeYamlNode(file_node, root_);
                loaded_count++;
              }
            }
            catch (const YAML::Exception &e)
            {
              std::cerr << "Error loading YAML file " << file_path << ": " << e.what() << std::endl;
            }
          }
        }
      }
    }
    catch (const std::exception &e)
    {
      std::cerr << "Error reading directory " << directory_path << ": " << e.what() << std::endl;
    }

    return loaded_count;
  }

  std::string NodeHandle::findConfigDirectory()
  {
    // If config directory is already set, use it
    if (!config_directory_.empty() && std::filesystem::exists(config_directory_))
    {
      return config_directory_;
    }

    // Try environment variable first
    const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
    if (env_config && std::filesystem::exists(env_config))
    {
      config_directory_ = std::string(env_config);
      return config_directory_;
    }

    // Try workspace path + /config
    const char *workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
    if (workspace_path)
    {
      std::string config_path = std::string(workspace_path) + "/config";
      if (std::filesystem::exists(config_path))
      {
        config_directory_ = config_path;
        return config_directory_;
      }
    }

    // Try to find config relative to executable path
    try
    {
      char exe_path[PATH_MAX];
      ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX);
      if (count != -1)
      {
        exe_path[count] = '\0';
        std::filesystem::path exe_dir = std::filesystem::path(exe_path).parent_path();

        // Try paths relative to executable: ../../config, ../config, etc.
        std::vector<std::filesystem::path> relative_paths = {
            exe_dir / "../../config",
            exe_dir / "../config",
            exe_dir / "config",
            exe_dir.parent_path() / "config",
            exe_dir.parent_path().parent_path() / "config"};

        for (const auto &rel_path : relative_paths)
        {
          if (std::filesystem::exists(rel_path) && std::filesystem::is_directory(rel_path))
          {
            config_directory_ = std::filesystem::canonical(rel_path).string();
            return config_directory_;
          }
        }
      }
    }
    catch (const std::exception &)
    {
      // Ignore errors when trying to find executable path
    }

    // Try hardcoded fallback paths
    std::vector<std::string> possible_paths = {
        "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/config",
        "../config",
        "../../config",
        "./config"};

    for (const auto &path : possible_paths)
    {
      if (std::filesystem::exists(path) && std::filesystem::is_directory(path))
      {
        try
        {
          config_directory_ = std::filesystem::canonical(path).string();
          return config_directory_;
        }
        catch (const std::exception &)
        {
          // If canonical fails, try the path as-is
          config_directory_ = path;
          return config_directory_;
        }
      }
    }

    // Return empty string if not found
    return "";
  }

  void NodeHandle::autoLoadConfigFiles()
  {
    // Auto-load if config_directory_ is set (regardless of namespace)
    // This ensures root_ always contains the full merged YAML tree
    if (config_directory_.empty())
    {
      // Config directory not set, can't auto-load
      return;
    }

    if (!std::filesystem::exists(config_directory_) || !std::filesystem::is_directory(config_directory_))
    {
      // Config directory doesn't exist, can't auto-load
      return;
    }

    std::string config_dir_to_load = config_directory_;

    // Load all YAML files from config directory
    int loaded_count = loadYamlFilesFromDirectory(config_dir_to_load);
    if (loaded_count <= 0)
    {
      std::cout << "[NodeHandle] No YAML files found or loaded from: " << config_dir_to_load << std::endl;
    }
  }

  void NodeHandle::setParamInternal(const std::string &key, const YAML::Node &value, YAML::NodeType::value expected_type)
  {
    YAML::Node existing = getNestedValue(key);

    // If key doesn't exist, create it
    if (!existing.IsDefined())
    {
      setParam(key, value);
      return;
    }
    // If key exists, check type compatibility
    // For scalar types, we allow conversion between compatible types
    if (expected_type == YAML::NodeType::Scalar && existing.IsScalar())
    {
      // Scalar types are compatible, just update the value
      setParam(key, value);
    }
    else if (existing.Type() == expected_type)
    {
      // Same type, update value
      setParam(key, value);
    }
    else
    {
      // Type mismatch - overwrite with new type (as per requirement)
      setParam(key, value);
    }
  }

  void NodeHandle::setParam(const std::string &key, const YAML::Node &value)
  {
    // Split key by '/' to handle nested keys
    std::stringstream ss(namespace_ + "/" + key);
    std::string segment;
    std::vector<std::string> segments;

    while (std::getline(ss, segment, '/'))
    {
      if (!segment.empty())
      {
        segments.push_back(segment);
      }
    }

    if (segments.empty())
    {
      return;
    }

    // Step 1: Calculate full key path in root_ (namespace_ + "/" + key)
    std::string full_key = key;
    if (!namespace_.empty() && namespace_ != "/")
    {
      if (full_key.empty())
      {
        full_key = namespace_;
      }
      else
      {
        full_key = namespace_ + "/" + full_key;
      }
    }
    
    std::stringstream s(full_key);
    std::string seg;
    std::vector<std::string> segs;
    while (std::getline(s, seg, '/'))
    {
      if (!seg.empty())
        segs.push_back(seg);
    }

    // Step 2: Update root_ with the full_key (preserve existing data)
    std::vector<YAML::Node> curs = {root_};
    if (!segs.empty())
    {
      for (size_t i = 0; i < segs.size() - 1; ++i)
      {
        const auto &k = segs[i];
        if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
        {
          curs.back()[k] = YAML::Node(YAML::NodeType::Map);
        }
        curs.push_back(curs.back()[k]);
      }
      curs.back()[segs.back()] = value;
    }
  }

  void NodeHandle::setParam(const std::string &key, bool b) const
  {
    const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(b), YAML::NodeType::Scalar);
  }

  void NodeHandle::setParam(const std::string &key, const char *s) const
  {
    const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(std::string(s)), YAML::NodeType::Scalar);
  }

  void NodeHandle::setParam(const std::string &key, const std::string &s) const
  {
    const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(s), YAML::NodeType::Scalar);
  }

  void NodeHandle::setParam(const std::string &key, int i) const
  {
    const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(i), YAML::NodeType::Scalar);
  }

  void NodeHandle::setParam(const std::string &key, double d) const
  {
    const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(d), YAML::NodeType::Scalar);
  }

  void NodeHandle::setParam(const std::string &key, const std::vector<bool> &vec) const
  {
    YAML::Node node(YAML::NodeType::Sequence);
    for (bool b : vec)
    {
      node.push_back(YAML::Node(b));
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
  }

  void NodeHandle::setParam(const std::string &key, const std::vector<int> &vec) const
  {
    YAML::Node node(YAML::NodeType::Sequence);
    for (int i : vec)
    {
      node.push_back(YAML::Node(i));
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
  }

  void NodeHandle::setParam(const std::string &key, const std::vector<float> &vec) const
  {
    YAML::Node node(YAML::NodeType::Sequence);
    for (float f : vec)
    {
      node.push_back(YAML::Node(static_cast<double>(f)));
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
  }

  void NodeHandle::setParam(const std::string &key, const std::vector<double> &vec) const
  {
    YAML::Node node(YAML::NodeType::Sequence);
    for (double d : vec)
    {
      node.push_back(YAML::Node(d));
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
  }

  void NodeHandle::setParam(const std::string &key, const std::vector<std::string> &vec) const
  {
    YAML::Node node(YAML::NodeType::Sequence);
    for (const std::string &s : vec)
    {
      node.push_back(YAML::Node(s));
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
  }

  void NodeHandle::setParam(const std::string &key, const std::map<std::string, bool> &map) const
  {
    YAML::Node node(YAML::NodeType::Map);
    for (const auto &pair : map)
    {
      node[pair.first] = YAML::Node(pair.second);
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
  }

  void NodeHandle::setParam(const std::string &key, const std::map<std::string, int> &map) const
  {
    YAML::Node node(YAML::NodeType::Map);
    for (const auto &pair : map)
    {
      node[pair.first] = YAML::Node(pair.second);
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
  }

  void NodeHandle::setParam(const std::string &key, const std::map<std::string, float> &map) const
  {
    YAML::Node node(YAML::NodeType::Map);
    for (const auto &pair : map)
    {
      node[pair.first] = YAML::Node(static_cast<double>(pair.second));
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
  }

  void NodeHandle::setParam(const std::string &key, const std::map<std::string, double> &map) const
  {
    YAML::Node node(YAML::NodeType::Map);
    for (const auto &pair : map)
    {
      node[pair.first] = YAML::Node(pair.second);
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
  }

  void NodeHandle::setParam(const std::string &key, const std::map<std::string, std::string> &map) const
  {
    YAML::Node node(YAML::NodeType::Map);
    for (const auto &pair : map)
    {
      node[pair.first] = YAML::Node(pair.second);
    }
    const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
  }

  void NodeHandle::setParam(const std::string &key, const robot_xmlrpcpp::XmlRpcValue &v) const
  {
    // Convert XmlRpcValue to YAML::Node
    // Create non-const copy to use conversion operators
    robot_xmlrpcpp::XmlRpcValue v_copy = v;
    YAML::Node node;
    try
    {
      switch (v.getType())
      {
      case robot_xmlrpcpp::XmlRpcValue::TypeBoolean:
      {
        bool b = v_copy;
        node = YAML::Node(b);
        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
      }
      break;
      case robot_xmlrpcpp::XmlRpcValue::TypeInt:
      {
        int i = v_copy;
        node = YAML::Node(i);
        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
      }
      break;
      case robot_xmlrpcpp::XmlRpcValue::TypeDouble:
      {
        double d = v_copy;
        node = YAML::Node(d);
        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
      }
      break;
      case robot_xmlrpcpp::XmlRpcValue::TypeString:
      {
        std::string s = v_copy;
        node = YAML::Node(s);
        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
      }
      break;
      case robot_xmlrpcpp::XmlRpcValue::TypeArray:
        {
        YAML::Node seq(YAML::NodeType::Sequence);
        for (int i = 0; i < v_copy.size(); ++i)
        {
          YAML::Node item;
          robot_xmlrpcpp::XmlRpcValue item_v = v_copy[i];
          if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeBoolean)
          {
            bool b = item_v;
            item = YAML::Node(b);
          }
          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt)
          {
            int i_val = item_v;
            item = YAML::Node(i_val);
          }
          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeDouble)
          {
            double d = item_v;
            item = YAML::Node(d);
          }
          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeString)
          {
            std::string s = item_v;
            item = YAML::Node(s);
          }
          seq.push_back(item);
        }
        const_cast<NodeHandle *>(this)->setParamInternal(key, seq, YAML::NodeType::Sequence);
      }
      break;
      case robot_xmlrpcpp::XmlRpcValue::TypeStruct:
      {
        YAML::Node map_node(YAML::NodeType::Map);
        // XmlRpcValue::TypeStruct doesn't have begin/end, need to use different approach
        // We'll need to iterate through the struct differently
        // For now, create empty map
        const_cast<NodeHandle *>(this)->setParamInternal(key, map_node, YAML::NodeType::Map);
      }
      break;
      default:
        // Unknown type, create empty node
        const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
        break;
      }
    }
    catch (...)
    {
      // On error, create empty node
      const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
    }
  }

  bool NodeHandle::getParam(const std::string &key, bool &b, bool default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined())
    {
      b = default_value;
      return false;
    }

    if (value.IsScalar())
    {
      try
      {
        if (value.Type() == YAML::NodeType::Scalar)
        {
          std::string str = value.as<std::string>();
          std::transform(str.begin(), str.end(), str.begin(), ::tolower);
          if (str == "true" || str == "1" || str == "yes" || str == "on")
          {
            b = true;
            return true;
          }
          else if (str == "false" || str == "0" || str == "no" || str == "off")
          {
            b = false;
            return true;
          }
        }
        b = value.as<bool>();
        return true;
      }
      catch (...)
      {
        b = default_value;
        return false;
      }
    }

    b = default_value;
    return false;
  }

  bool NodeHandle::getParam(const std::string &key, double &d, double default_value) const
  {
    // Safety check: validate key parameter
    if (key.empty())
    {
      d = default_value;
      return false;
    }
    YAML::Node value = getNestedValue(key);
    if (!value.IsDefined() || !value.IsScalar())
    {
      d = default_value;
      return false;
    }
    if(key == "local_costmap/inscribed_radius")
    {
      robot::log_warning_at(__FILE__, __LINE__, "3");
    }
    if(!value.IsDefined() || !value.IsScalar())
    if(key == "local_costmap/inscribed_radius")
    {
      robot::log_warning_at(__FILE__, __LINE__, "Inscribed radius: %f", default_value);
    }
    if(key == "local_costmap/inscribed_radius")
    {
      robot::log_warning_at(__FILE__, __LINE__, "4");
    }
    try
    {
      d = value.as<double>();
      return true;
    }
    catch (...)
    {
      d = default_value;
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, float &f, float default_value) const
  {
    double d;
    if (getParam(key, d, static_cast<double>(default_value)))
    {
      f = static_cast<float>(d);
      return true;
    }
    f = default_value;
    return false;
  }

  bool NodeHandle::getParam(const std::string &key, int &i, int default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsScalar())
    {
      i = default_value;
      return false;
    }

    try
    {
      std::string str = value.as<std::string>();
      // Handle hex format
      if (str.length() > 2 && str.substr(0, 2) == "0x")
      {
        i = std::stoi(str, nullptr, 16);
      }
      else
      {
        i = value.as<int>();
      }
      return true;
    }
    catch (...)
    {
      i = default_value;
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::string &s, std::string default_value) const
  {
    YAML::Node value = getNestedValue(key);

    // If key doesn't exist or is not a scalar, return default value
    if (!value.IsDefined() || !value.IsScalar())
    {
      s = default_value;
      return false;
    }

    try
    {
      s = value.as<std::string>();
      return true;
    }
    catch (...)
    {
      s = default_value;
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::vector<bool> &vec, std::vector<bool> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsSequence())
    {
      vec = default_value;
      return false;
    }

    vec.clear();
    try
    {
      for (size_t i = 0; i < value.size(); ++i)
      {
        if (value[i].IsScalar())
        {
          try
          {
            bool b = value[i].as<bool>();
            vec.push_back(b);
          }
          catch (...)
          {
            // Try as string and convert
            std::string str = value[i].as<std::string>();
            std::transform(str.begin(), str.end(), str.begin(), ::tolower);
            if (str == "true" || str == "1" || str == "yes" || str == "on")
            {
              vec.push_back(true);
            }
            else if (str == "false" || str == "0" || str == "no" || str == "off")
            {
              vec.push_back(false);
            }
            else
            {
              return false;
            }
          }
        }
        else
        {
          return false;
        }
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::vector<double> &vec, std::vector<double> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsSequence())
    {
      vec = default_value;
      return false;
    }

    vec.clear();
    try
    {
      for (size_t i = 0; i < value.size(); ++i)
      {
        vec.push_back(value[i].as<double>());
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::vector<float> &vec, std::vector<float> default_value) const
  {
    std::vector<double> dvec;
    std::vector<double> ddefault;
    ddefault.reserve(default_value.size());
    for (float f : default_value)
    {
      ddefault.push_back(static_cast<double>(f));
    }

    if (getParam(key, dvec, ddefault))
    {
      vec.clear();
      vec.reserve(dvec.size());
      for (double d : dvec)
      {
        vec.push_back(static_cast<float>(d));
      }
      return true;
    }
    vec = default_value;
    return false;
  }

  bool NodeHandle::getParam(const std::string &key, std::vector<int> &vec, std::vector<int> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsSequence())
    {
      vec = default_value;
      return false;
    }

    vec.clear();
    try
    {
      for (size_t i = 0; i < value.size(); ++i)
      {
        vec.push_back(value[i].as<int>());
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::vector<std::string> &vec, std::vector<std::string> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsSequence())
    {
      vec = default_value;
      return false;
    }

    vec.clear();
    try
    {
      for (size_t i = 0; i < value.size(); ++i)
      {
        vec.push_back(value[i].as<std::string>());
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::map<std::string, bool> &map, std::map<std::string, bool> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsMap())
    {
      map = default_value;
      return false;
    }

    map.clear();
    try
    {
      for (auto it = value.begin(); it != value.end(); ++it)
      {
        std::string key_str = it->first.as<std::string>();
        if (it->second.IsScalar())
        {
          try
          {
            bool b = it->second.as<bool>();
            map[key_str] = b;
          }
          catch (...)
          {
            // Try as string and convert
            std::string str = it->second.as<std::string>();
            std::transform(str.begin(), str.end(), str.begin(), ::tolower);
            if (str == "true" || str == "1" || str == "yes" || str == "on")
            {
              map[key_str] = true;
            }
            else if (str == "false" || str == "0" || str == "no" || str == "off")
            {
              map[key_str] = false;
            }
            else
            {
              return false;
            }
          }
        }
        else
        {
          return false;
        }
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::map<std::string, double> &map, std::map<std::string, double> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsMap())
    {
      map = default_value;
      return false;
    }

    map.clear();
    try
    {
      for (auto it = value.begin(); it != value.end(); ++it)
      {
        std::string key_str = it->first.as<std::string>();
        if (it->second.IsScalar())
        {
          map[key_str] = it->second.as<double>();
        }
        else
        {
          return false;
        }
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::map<std::string, float> &map, std::map<std::string, float> default_value) const
  {
    std::map<std::string, double> dmap;
    std::map<std::string, double> ddefault;
    for (const auto &pair : default_value)
    {
      ddefault[pair.first] = static_cast<double>(pair.second);
    }

    if (getParam(key, dmap, ddefault))
    {
      map.clear();
      for (const auto &pair : dmap)
      {
        map[pair.first] = static_cast<float>(pair.second);
      }
      return true;
    }
    map = default_value;
    return false;
  }

  bool NodeHandle::getParam(const std::string &key, std::map<std::string, int> &map, std::map<std::string, int> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsMap())
    {
      map = default_value;
      return false;
    }

    map.clear();
    try
    {
      for (auto it = value.begin(); it != value.end(); ++it)
      {
        std::string key_str = it->first.as<std::string>();
        if (it->second.IsScalar())
        {
          map[key_str] = it->second.as<int>();
        }
        else
        {
          return false;
        }
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, std::map<std::string, std::string> &map, std::map<std::string, std::string> default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined() || !value.IsMap())
    {
      map = default_value;
      return false;
    }

    map.clear();
    try
    {
      for (auto it = value.begin(); it != value.end(); ++it)
      {
        std::string key_str = it->first.as<std::string>();
        if (it->second.IsScalar())
        {
          map[key_str] = it->second.as<std::string>();
        }
        else
        {
          return false;
        }
      }
      return true;
    }
    catch (...)
    {
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, YAML::Node &v, YAML::Node default_value) const
  {
    YAML::Node value = getNestedValue(key);

    if (!value.IsDefined())
    {
      v = default_value;
      return false;
    }

    v = value;
    return true;
  }

  namespace
  {
    bool yamlToXmlRpc(const YAML::Node &y, robot_xmlrpcpp::XmlRpcValue &out)
    {
      if (!y.IsDefined())
        return false;
      if (y.IsScalar())
      {
        try
        {
          std::string s = y.as<std::string>();
          std::string lower = s;
          std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
          if (lower == "true" || lower == "1" || lower == "yes" || lower == "on")
          {
            out = robot_xmlrpcpp::XmlRpcValue(true);
            return true;
          }
          if (lower == "false" || lower == "0" || lower == "no" || lower == "off")
          {
            out = robot_xmlrpcpp::XmlRpcValue(false);
            return true;
          }
          try
          {
            int i = y.as<int>();
            out = robot_xmlrpcpp::XmlRpcValue(i);
            return true;
          }
          catch (...) {}
          try
          {
            double d = y.as<double>();
            out = robot_xmlrpcpp::XmlRpcValue(d);
            return true;
          }
          catch (...) {}
          out = robot_xmlrpcpp::XmlRpcValue(s);
          return true;
        }
        catch (...)
        {
          return false;
        }
      }
      if (y.IsSequence())
      {
        out = robot_xmlrpcpp::XmlRpcValue();
        out.setSize(static_cast<int>(y.size()));
        for (size_t i = 0; i < y.size(); ++i)
        {
          robot_xmlrpcpp::XmlRpcValue item;
          if (yamlToXmlRpc(y[i], item))
            out[static_cast<int>(i)] = item;
        }
        return true;
      }
      if (y.IsMap())
      {
        out = robot_xmlrpcpp::XmlRpcValue();
        for (YAML::const_iterator it = y.begin(); it != y.end(); ++it)
        {
          std::string k = it->first.as<std::string>();
          robot_xmlrpcpp::XmlRpcValue item;
          if (yamlToXmlRpc(it->second, item))
            out[k] = item;
        }
        return true;
      }
      return false;
    }
  }

  bool NodeHandle::getParam(const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const
  {
    YAML::Node value = getNestedValue(key);
    if (!value.IsDefined())
      return false;
    return yamlToXmlRpc(value, v);
  }

  template <typename T>
  T NodeHandle::param(const std::string &param_name) const
  {
    T param_val;
    T default_val = T{};  // Default-constructed value
    if (param(param_name, param_val, default_val))
    {
      return param_val;
    }
    return default_val;
  }

  template <typename T>
  T NodeHandle::param(const std::string &param_name, const T &default_val) const
  {
    T param_val;
    if (param(param_name, param_val, default_val))
    {
      return param_val;
    }
    return default_val;
  }

  template <typename T>
  bool NodeHandle::param(const std::string &param_name, T &param_val, const T &default_val) const
  {
    return getParam(param_name, param_val, default_val);
  }

  bool NodeHandle::searchParam(const std::string &key, std::string &result) const
  {
    result.clear();
    if (key.empty())
    {
      return false;
    }

    auto splitPath = [](const std::string &path) -> std::vector<std::string>
    {
      std::vector<std::string> parts;
      std::stringstream ss(path);
      std::string seg;
      while (std::getline(ss, seg, '/'))
      {
        if (!seg.empty())
        {
          parts.push_back(seg);
        }
      }
      return parts;
    };

    auto joinPath = [](const std::vector<std::string> &parts) -> std::string
    {
      std::string out;
      for (size_t i = 0; i < parts.size(); ++i)
      {
        if (i)
          out += "/";
        out += parts[i];
      }
      return out;
    };

    auto existsInRoot = [&](const std::string &full_key_no_leading_slash) -> bool
    {
      // Prefer searching in the global tree if available; otherwise fall back to
      // this NodeHandle's scoped view.
      const YAML::Node *search_root = nullptr;
      if (root_.IsDefined() && root_.IsMap())
      {
        search_root = &root_;
      }
      else
      {
        return false;
      }

      const std::vector<std::string> segments = splitPath(full_key_no_leading_slash);
      if (segments.empty())
      {
        return false;
      }

      const YAML::Node *current = search_root;
      for (const auto &seg : segments)
      {
        // Safety check: ensure current is valid
        if (!current || !current->IsDefined() || !current->IsMap())
        {
          return false;
        }

        const YAML::Node *found_child = nullptr;
        
        try
        {
          for (YAML::const_iterator it = current->begin(); it != current->end(); ++it)
          {
            try
            {
              // Safety check: validate iterator nodes
              if (!it->first.IsDefined())
              {
                continue;
              }
              
              // Safely convert key to string
              std::string key_str;
              try
              {
                key_str = it->first.as<std::string>();
              }
              catch (...)
              {
                // Skip nodes that can't be converted to string
                continue;
              }
              
              if (key_str == seg)
              {
                // Safety check: validate value node
                if (it->second.IsDefined())
                {
                  found_child = &(it->second);
                  break;
                }
              }
            }
            catch (...)
            {
              // Skip invalid nodes and continue iteration
              continue;
            }
          }
        }
        catch (...)
        {
          // If iteration fails, return false
          return false;
        }

        if (!found_child || !found_child->IsDefined())
        {
          return false;
        }

        current = found_child;
      }

      // Safety check: ensure current is valid before returning
      return current && current->IsDefined();
    };

    auto stripLeadingSlash = [](const std::string &s) -> std::string
    {
      std::string out = s;
      while (!out.empty() && out.front() == '/')
      {
        out.erase(out.begin());
      }
      return out;
    };

    const std::string key_no_slash = stripLeadingSlash(key);
    if (key_no_slash.empty())
    {
      return false;
    }

    // If caller passed an absolute key (started with '/'), we only check that exact path.
    // Use the key as-is (without adding this NodeHandle's namespace).
    if (!key.empty() && key.front() == '/')
    {
      if (existsInRoot(key_no_slash))
      {
        result = "/" + key_no_slash;
        return true;
      }
      return false;
    }

    // Search up the namespace tree. Example:
    //  existing: a/b
    //  namespace: a/c/d
    //  search "b" -> check a/c/d/b, a/c/b, a/b
    std::vector<std::string> ns_parts;
    if (!namespace_.empty() && namespace_ != "/")
    {
      ns_parts = splitPath(stripLeadingSlash(namespace_));
    }
    const std::vector<std::string> key_parts = splitPath(key_no_slash);

    for (int depth = static_cast<int>(ns_parts.size()); depth >= 0; --depth)
    {
      std::vector<std::string> candidate_parts;
      candidate_parts.reserve(static_cast<size_t>(depth) + key_parts.size());

      // Safety check: ensure depth doesn't exceed ns_parts size
      size_t safe_depth = static_cast<size_t>(depth);
      if (safe_depth > ns_parts.size())
      {
        safe_depth = ns_parts.size();
      }

      for (size_t i = 0; i < safe_depth; ++i)
      {
        candidate_parts.push_back(ns_parts[i]);
      }
      for (const auto &kp : key_parts)
      {
        candidate_parts.push_back(kp);
      }

      const std::string candidate = joinPath(candidate_parts);
      if (existsInRoot(candidate))
      {
        result = "/" + candidate;
        return true;
      }
    }
    return false;
  }

  const YAML::Node &NodeHandle::getNodeHandle() const
  {
    return root_;
  }

  // Explicit template instantiations for param function
  // String instantiations
  template std::string NodeHandle::param<std::string>(const std::string &param_name) const;
  template std::string NodeHandle::param<std::string>(const std::string &param_name, const std::string &default_val) const;
  template bool NodeHandle::param<std::string>(const std::string &param_name, std::string &param_val, const std::string &default_val) const;
  
  // Bool instantiations
  template bool NodeHandle::param<bool>(const std::string &param_name) const;
  template bool NodeHandle::param<bool>(const std::string &param_name, const bool &default_val) const;
  template bool NodeHandle::param<bool>(const std::string &param_name, bool &param_val, const bool &default_val) const;
  
  // Int instantiations
  template int NodeHandle::param<int>(const std::string &param_name) const;
  template int NodeHandle::param<int>(const std::string &param_name, const int &default_val) const;
  template bool NodeHandle::param<int>(const std::string &param_name, int &param_val, const int &default_val) const;
  
  // Double instantiations
  template double NodeHandle::param<double>(const std::string &param_name) const;
  template double NodeHandle::param<double>(const std::string &param_name, const double &default_val) const;
  template bool NodeHandle::param<double>(const std::string &param_name, double &param_val, const double &default_val) const;
  
  // Float instantiations
  template float NodeHandle::param<float>(const std::string &param_name) const;
  template float NodeHandle::param<float>(const std::string &param_name, const float &default_val) const;
  template bool NodeHandle::param<float>(const std::string &param_name, float &param_val, const float &default_val) const;
  
  // Vector<std::string> instantiations
  template std::vector<std::string> NodeHandle::param<std::vector<std::string>>(const std::string &param_name) const;
  template std::vector<std::string> NodeHandle::param<std::vector<std::string>>(const std::string &param_name, const std::vector<std::string> &default_val) const;
  template bool NodeHandle::param<std::vector<std::string>>(const std::string &param_name, std::vector<std::string> &param_val, const std::vector<std::string> &default_val) const;

  // Static member initialization
  std::string NodeHandle::config_directory_;
  YAML::Node NodeHandle::root_;

} // namespace robot