#ifndef ROBOT_COSTMAP_2D_UTILS_H
#define ROBOT_COSTMAP_2D_UTILS_H
#include <yaml-cpp/yaml.h>
#include <filesystem>
#include <string>
#include <iostream>

namespace robot_costmap_2d
{
    template<typename T>
    T loadParam(const YAML::Node& node, const std::string& key, const T& default_value)
    {
        if (node[key] && node[key].IsDefined())
            return node[key].as<T>();
        return default_value;
    }

    inline std::vector<robot_geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<robot_geometry_msgs::Point>& default_value)
    {
        if( !node || !node.IsDefined())
            return default_value;

        std::vector<robot_geometry_msgs::Point> fp;

        for (const auto& p : node) {
            if (p.size() != 2)
                throw std::runtime_error("Footprint point must be [x, y]");
            robot_geometry_msgs::Point point;
            point.x = p[0].as<double>();
            point.y = p[1].as<double>();
            point.z = 0.0;
            fp.push_back(point);
        }
        std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl;

        return fp;
    }


    inline std::string getSourceFile(const std::string& root, const std::string& config_file_name)
    {
        std::string path_source = " ";
        std::string sub_folder = "config";
        namespace fs = std::filesystem;

        fs::path cfg_dir = fs::path(root) / sub_folder;

        if (fs::exists(cfg_dir) && fs::is_directory(cfg_dir))
        {
            for (const auto& entry : fs::recursive_directory_iterator(cfg_dir))
            {
                if (entry.is_regular_file() && entry.path().filename() == config_file_name)
                {
                    path_source = entry.path().string();
                    std::cout << "Path source: " << path_source << std::endl;
                    break;  // tìm thấy thì dừng
                }
            }
        }
        if(path_source == " ")
            std::cout<< config_file_name << " file not found at path: "<< cfg_dir << std::endl;
        return path_source;
    }

}

#endif  // ROBOT_COSTMAP_2D_UTILS_H