#ifndef ROBOT_NODE_H_INCLUDED_H
#define ROBOT_NODE_H_INCLUDED_H

#include <yaml-cpp/yaml.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
#include <memory>
#include <string>
#include <map>
#include <vector>


namespace robot
{
  /**
   * @class NodeHandle
   * @brief Main interface for accessing and setting parameters using YAML-based parameter server.
   *
   * NodeHandle provides a ROS-like interface for parameter management, using YAML files as the backend.
   * It supports namespaces, nested parameters, and automatic loading of YAML configuration files.
   *
   * @note This class uses YAML::Node internally to store and manage parameters, providing a hierarchical
   * parameter structure similar to ROS parameter server.
   */
  class NodeHandle
  {
  public:
    /**
     * @brief Type alias for remappings (map of string to string).
     *
     * Used for topic and parameter name remapping, similar to ROS remapping mechanism.
     */
    using M_string = std::map<std::string, std::string>;

    /**
     * @brief Type alias for shared pointer to NodeHandle.
     *
     * Convenience type for managing NodeHandle instances with shared ownership.
     */
    using Ptr = std::shared_ptr<NodeHandle>;

    /**
     * @brief Default constructor with optional namespace and remappings.
     *
     * Creates a root-level NodeHandle or one scoped to the specified namespace.
     * If namespace is "~" or empty, operates at root level. Automatically loads
     * YAML configuration files from the config directory if available.
     *
     * @param ns The namespace for this NodeHandle (default: root).
     *           Use "~" for private namespace (maps to config directory).
     * @param remappings Map of parameter name remappings (default: empty).
     */
    NodeHandle(const std::string &ns = std::string(), const M_string &remappings = M_string());

    /**
     * @brief Constructor from parent NodeHandle with new namespace.
     * 
     * Creates a new NodeHandle that is scoped to a child namespace of the parent.
     * The new NodeHandle will have access to parameters under the specified namespace.
     * 
     * @param parent The parent NodeHandle to inherit from.
     * @param ns The namespace to scope this NodeHandle to (relative to parent).
     *           Leading '/' is automatically stripped.
     */
    NodeHandle(const NodeHandle &parent, const std::string &ns);

    /**
     * @brief Constructor from parent NodeHandle with new namespace and remappings.
     * 
     * Creates a new NodeHandle with a child namespace and optional remappings.
     * 
     * @param parent The parent NodeHandle to inherit from.
     * @param ns The namespace to scope this NodeHandle to (relative to parent).
     *           Leading '/' is automatically stripped.
     * @param remappings Map of parameter name remappings (currently not fully implemented).
     */
    NodeHandle(const NodeHandle &parent, const std::string &ns, const M_string &remappings);

    /**
     * @brief Copy constructor.
     *
     * Creates a copy of the NodeHandle with the same namespace scope.
     * Both instances will reference the same underlying parameter tree.
     *
     * @param parent The NodeHandle to copy from.
     */
    NodeHandle(const NodeHandle &parent);

    /**
     * @brief Assignment operator.
     *
     * Assigns the namespace and parameter scope from another NodeHandle.
     *
     * @param parent The NodeHandle to assign from.
     * @return Reference to this NodeHandle.
     */
    NodeHandle &operator=(const NodeHandle &parent);

    /**
     * @brief Destructor.
     *
     * Cleans up the NodeHandle instance. Note that static members (root_ and config_directory_)
     * persist across NodeHandle instances.
     */
    ~NodeHandle();

    /**
     * @brief Check if a parameter exists.
     *
     * Checks whether a parameter with the given key exists in the parameter server.
     * Supports nested keys using '/' separator (e.g., "parent/child").
     *
     * @param key The parameter key to check. Can be nested using '/' separator.
     * @return true if the parameter exists, false otherwise.
     */
    bool hasParam(const std::string &key) const;

    /**
     * @brief Get a boolean value from the parameter server.
     *
     * Retrieves a boolean parameter value. Supports string-to-boolean conversion
     * for values like "true", "false", "1", "0", "yes", "no", "on", "off".
     *
     * @param key The parameter key (supports nested keys with '/' separator).
     * @param b Storage for the retrieved value. Set to default_value if key not found.
     * @param default_value Default value to use if parameter doesn't exist (default: false).
     * @return true if the parameter value was retrieved successfully, false otherwise.
     *
     * @note If you want to provide a default value in case the key does not exist, use param() instead.
     */
    bool getParam(const std::string &key, bool &b, bool default_value = false) const;

    /**
     * @brief Get a double value from the parameter server.
     *
     * @param key The parameter key (supports nested keys with '/' separator).
     * @param d Storage for the retrieved value. Set to default_value if key not found.
     * @param default_value Default value to use if parameter doesn't exist (default: 0.0).
     * @return true if the parameter value was retrieved successfully, false otherwise.
     */
    bool getParam(const std::string &key, double &d, double default_value = 0.0) const;

      /**
       * @brief Get a float value from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param f Storage for the retrieved value. Set to default_value if key not found.
       * @param default_value Default value to use if parameter doesn't exist (default: 0.0).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, float &f, float default_value = 0.0) const;

      /**
       * @brief Get an integer value from the parameter server.
       *
       * Supports hexadecimal format (e.g., "0x10").
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param i Storage for the retrieved value. Set to default_value if key not found.
       * @param default_value Default value to use if parameter doesn't exist (default: 0).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, int &i, int default_value = 0) const;

      /**
       * @brief Get a map of string-to-bool from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map Storage for the retrieved map. Set to default_value if key not found.
       * @param default_value Default map to use if parameter doesn't exist (default: empty map).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::map< std::string, bool > &map, std::map< std::string, bool > default_value = std::map< std::string, bool >()) const;

      /**
       * @brief Get a map of string-to-double from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map Storage for the retrieved map. Set to default_value if key not found.
       * @param default_value Default map to use if parameter doesn't exist (default: empty map).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::map< std::string, double > &map, std::map< std::string, double > default_value = std::map< std::string, double >()) const;

      /**
       * @brief Get a map of string-to-float from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map Storage for the retrieved map. Set to default_value if key not found.
       * @param default_value Default map to use if parameter doesn't exist (default: empty map).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::map< std::string, float > &map, std::map< std::string, float > default_value = std::map< std::string, float >()) const;

      /**
       * @brief Get a map of string-to-int from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map Storage for the retrieved map. Set to default_value if key not found.
       * @param default_value Default map to use if parameter doesn't exist (default: empty map).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::map< std::string, int > &map, std::map< std::string, int > default_value = std::map< std::string, int >()) const;

      /**
       * @brief Get a map of string-to-string from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map Storage for the retrieved map. Set to default_value if key not found.
       * @param default_value Default map to use if parameter doesn't exist (default: empty map).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::map< std::string, std::string > &map, std::map< std::string, std::string > default_value = std::map< std::string, std::string >()) const;

      /**
       * @brief Get a string value from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param s Storage for the retrieved value. Set to default_value if key not found.
       * @param default_value Default value to use if parameter doesn't exist (default: empty string).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::string &s, std::string default_value = "") const;

      /**
       * @brief Get a vector of boolean values from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec Storage for the retrieved vector. Set to default_value if key not found.
       * @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::vector< bool > &vec, std::vector< bool > default_value = std::vector< bool >()) const;

      /**
       * @brief Get a vector of double values from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec Storage for the retrieved vector. Set to default_value if key not found.
       * @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool 	getParam (const std::string &key, std::vector< double > &vec, std::vector< double > default_value = std::vector< double >()) const;

      /**
       * @brief Get a vector of float values from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec Storage for the retrieved vector. Set to default_value if key not found.
       * @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::vector< float > &vec, std::vector< float > default_value = std::vector< float >()) const;

      /**
       * @brief Get a vector of integer values from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec Storage for the retrieved vector. Set to default_value if key not found.
       * @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::vector< int > &vec, std::vector< int > default_value = std::vector< int >()) const;

      /**
       * @brief Get a vector of string values from the parameter server.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec Storage for the retrieved vector. Set to default_value if key not found.
       * @param default_value Default vector to use if parameter doesn't exist (default: empty vector).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, std::vector< std::string > &vec, std::vector< std::string > default_value = std::vector< std::string >()) const;

      /**
       * @brief Get a YAML::Node value from the parameter server.
       *
       * Returns the raw YAML node, which can represent any YAML structure (scalar, sequence, map).
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param v Storage for the retrieved YAML node. Set to default_value if key not found.
       * @param default_value Default YAML node to use if parameter doesn't exist (default: empty node).
       * @return true if the parameter value was retrieved successfully, false otherwise.
       */
      bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const;

      /**
       * @brief Template method to get a parameter value (without default).
       *
       * Gets a parameter value. If the parameter doesn't exist, returns a default-constructed value.
       * Use hasParam() to check existence before calling this method.
       *
       * @tparam T The type of the parameter (must be supported by getParam).
       * @param param_name The parameter key (supports nested keys with '/' separator).
       * @return The parameter value if found, otherwise a default-constructed value of type T.
       *
       * @code
       * if (nh.hasParam("my_param")) {
       *   double value = nh.param<double>("my_param");
       * }
       * @endcode
       */
      template<typename T >
      T 	param (const std::string &param_name) const;

      /**
       * @brief Template method to get a parameter value with default.
       *
       * Convenience method that returns the parameter value directly, or the default if not found.
       * This is the recommended way to get parameters when you have a default value.
       *
       * @tparam T The type of the parameter (must be supported by getParam).
       * @param param_name The parameter key (supports nested keys with '/' separator).
       * @param default_val The default value to return if parameter doesn't exist.
       * @return The parameter value if found, otherwise default_val.
       *
       * @code
       * int max_iterations = nh.param("max_iterations", 100);
       * std::string frame_id = nh.param("frame_id", std::string("base_link"));
       * @endcode
       */
      template<typename T >
      T 	param (const std::string &param_name, const T &default_val) const;

      /**
       * @brief Template method to get a parameter value with default (output parameter version).
       *
       * Similar to param() but uses an output parameter instead of return value.
       *
       * @tparam T The type of the parameter (must be supported by getParam).
       * @param param_name The parameter key (supports nested keys with '/' separator).
       * @param param_val Output parameter to store the retrieved value.
       * @param default_val The default value to use if parameter doesn't exist.
       * @return true if the parameter was found, false if default was used.
       */
      template<typename T >
      bool param (const std::string &param_name, T &param_val, const T &default_val) const;

      /**
       * @brief Search for a parameter and return the result.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param result The result of the search.
       * @return true if the parameter was found, false otherwise.
       */
      bool searchParam (const std::string &key, std::string &result) const;

      /**
       * @brief Set a parameter from a YAML::Node.
       *
       * Sets a parameter value using a YAML node. This is the most flexible setParam method
       * as it can handle any YAML structure (scalar, sequence, map).
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param value The YAML node containing the value to set.
       */
      void setParam(const std::string &key, const YAML::Node &value);

      /**
       * @brief Set a boolean parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param b The boolean value to set.
       */
      void setParam (const std::string &key, bool b) const;

      /**
       * @brief Set a string parameter from C-style string.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param s The C-style string value to set.
       */
      void setParam (const std::string &key, const char *s) const;

      /**
       * @brief Set a map of string-to-bool parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map The map of string-to-bool values to set.
       */
      void setParam (const std::string &key, const std::map< std::string, bool > &map) const;

      /**
       * @brief Set a map of string-to-double parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map The map of string-to-double values to set.
       */
      void setParam (const std::string &key, const std::map< std::string, double > &map) const;

      /**
       * @brief Set a map of string-to-float parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map The map of string-to-float values to set.
       */
      void setParam (const std::string &key, const std::map< std::string, float > &map) const;

      /**
       * @brief Set a map of string-to-int parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map The map of string-to-int values to set.
       */
      void setParam (const std::string &key, const std::map< std::string, int > &map) const;

      /**
       * @brief Set a map of string-to-string parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param map The map of string-to-string values to set.
       */
      void setParam (const std::string &key, const std::map< std::string, std::string > &map) const;

      /**
       * @brief Set a string parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param s The string value to set.
       */
      void setParam (const std::string &key, const std::string &s) const;

      /**
       * @brief Set a vector of boolean values parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec The vector of boolean values to set.
       */
      void setParam (const std::string &key, const std::vector< bool > &vec) const;

      /**
       * @brief Set a vector of double values parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec The vector of double values to set.
       */
      void setParam (const std::string &key, const std::vector< double > &vec) const;

      /**
       * @brief Set a vector of float values parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec The vector of float values to set.
       */
      void setParam (const std::string &key, const std::vector< float > &vec) const;

      /**
       * @brief Set a vector of integer values parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec The vector of integer values to set.
       */
      void setParam (const std::string &key, const std::vector< int > &vec) const;

      /**
       * @brief Set a vector of string values parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param vec The vector of string values to set.
       */
      void setParam (const std::string &key, const std::vector< std::string > &vec) const;

      /**
       * @brief Set a parameter from robot_xmlrpcpp::XmlRpcValue.
       *
       * Converts an XmlRpcValue to YAML format and sets it as a parameter.
       * Supports boolean, int, double, string, array, and struct types.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param v The XmlRpcValue to convert and set.
       */
      void setParam (const std::string &key, const robot_xmlrpcpp::XmlRpcValue &v) const;

      /**
       * @brief Set a double parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param d The double value to set.
       */
      void setParam (const std::string &key, double d) const;

      /**
       * @brief Set an integer parameter.
       *
       * @param key The parameter key (supports nested keys with '/' separator).
       * @param i The integer value to set.
       */
      void setParam (const std::string &key, int i) const;

    /**
     * @brief Get a nested parameter value by key path.
     *
     * Retrieves a YAML node from the parameter tree using a key path.
     * Supports nested keys using '/' separator (e.g., "parent/child/grandchild").
     *
     * @param key The parameter key path (supports nested keys with '/' separator).
     * @return YAML::Node containing the parameter value, or empty node if not found.
     */
    YAML::Node getParamValue(const std::string &key) const;

    /**
     * @brief Get the underlying YAML node handle.
     *
     * Returns a const reference to the internal YAML node that stores all parameters
     * for this NodeHandle's namespace scope.
     *
     * @return Const reference to the YAML node handle.
     */
    const YAML::Node &getNodeHandle() const;

    /**
     * @brief Merge a YAML node into another node (recursively merges maps).
     *
     * Recursively merges the source YAML node into the output node.
     * If both nodes contain maps with the same key, the maps are merged recursively.
     * Scalar values and sequences are overwritten.
     *
     * @param source The source YAML node to merge from.
     * @param output The output YAML node to merge into (modified in place).
     */
    void mergeYamlNode(const YAML::Node &source, YAML::Node &output);

    /**
     * @brief Load all YAML files from a directory and merge them.
     *
     * Scans the specified directory for .yaml and .yml files, loads them,
     * and merges them into the root parameter tree. Files are processed in
     * filesystem order.
     *
     * @param directory_path Path to the directory containing YAML files.
     * @return Number of files successfully loaded.
     */
    int loadYamlFilesFromDirectory(const std::string &directory_path);

    /**
     * @brief Get the namespace of this NodeHandle.
     *
     * Returns the namespace string that this NodeHandle is scoped to.
     * If the namespace was "~" or empty, returns "/" (root namespace).
     *
     * @return The namespace string (e.g., "/robot/base" or "/" for root).
     */
    std::string getNamespace() const;

    /**
     * @brief Print all parameters in the current namespace scope (debug method).
     *
     * Debug utility that prints all parameters in the node_handle_ to stdout.
     * Shows the hierarchical structure with indentation. Useful for debugging
     * parameter loading and namespace scoping.
     */
    void printParams() const;

  private:
    /**
     * @brief Helper method to split key path and get nested value.
     *
     * Internal method that traverses the parameter tree using a key path
     * (e.g., "parent/child") and returns the nested YAML node.
     *
     * @param key The parameter key path (supports nested keys with '/' separator).
     * @return YAML::Node containing the nested value, or empty node if not found.
     */
    YAML::Node getNestedValue(const std::string &key) const;

    /**
     * @brief Debug version of getNestedValue with verbose output.
     *
     * Same as getNestedValue() but with additional debug output for troubleshooting.
     *
     * @param key The parameter key path (supports nested keys with '/' separator).
     * @return YAML::Node containing the nested value, or empty node if not found.
     */
    YAML::Node getNestedValueDebug(const std::string &key) const;

    /**
     * @brief Helper method to set parameter with type checking.
     *
     * Internal method that sets a parameter value with optional type checking.
     * If the key already exists with a different type, it will be overwritten.
     *
     * @param key The parameter key (supports nested keys with '/' separator).
     * @param value The YAML node value to set.
     * @param expected_type The expected YAML node type (for type checking).
     */
    void setParamInternal(const std::string &key, const YAML::Node &value, YAML::NodeType::value expected_type);

    /**
     * @brief Helper method to find config directory automatically.
     *
     * Static method that attempts to locate the config directory using various
     * strategies: environment variables, relative paths from executable, hardcoded paths.
     *
     * @return The config directory path if found, empty string otherwise.
     */
    static std::string findConfigDirectory();

    /**
     * @brief Auto-load YAML files from config directory.
     *
     * Internal method that automatically loads all YAML files from the config
     * directory and merges them into the root_ parameter tree. Called during
     * NodeHandle construction if config_directory_ is set.
     */
    void autoLoadConfigFiles();

    /**
     * @brief Debug method to print all parameters in a YAML node.
     *
     * Recursive helper method that prints the structure and values of a YAML node
     * with indentation to show hierarchy. Used by printAllParams().
     *
     * @param node The YAML node to print.
     */
    void printParams(YAML::Node node) const;

    /**
     * @brief The namespace this NodeHandle is scoped to.
     *
     * Stores the namespace string. If namespace is "~" or empty, it's set to "/" (root).
     */
    std::string namespace_;

    /**
     * @brief Mutable storage for parameters in this namespace scope.
     *
     * YAML node that contains all parameters accessible through this NodeHandle.
     * This is a reference into the static root_ tree, scoped to the namespace_.
     */
    // YAML::Node node_handle_;

    /**
     * @brief Static root parameter tree shared by all NodeHandle instances.
     *
     * The root YAML node containing the entire parameter tree. All NodeHandle
     * instances share this static tree, with each instance scoped to a namespace.
     */
    static YAML::Node root_;

    /**
     * @brief Static config directory path for automatic YAML loading.
     *
     * Directory path from which YAML configuration files are automatically loaded.
     * Set via setConfigDirectory() or found automatically via findConfigDirectory().
     */
    static std::string config_directory_;
  };
} // namespace robot

#endif // ROBOT_NODE_H_INCLUDED_H