#include <boost/dll/import.hpp>
#include <iostream>
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <tf3/buffer_core.h>
#include <tf3/time.h>
#include "nav_msgs/OccupancyGrid.h"
#include <map_msgs/OccupancyGridUpdate.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/critical_layer.h>
#include <costmap_2d/preferred_layer.h>
using namespace costmap_2d;

int main(int argc, char* argv[]) {

    std::string global_frame = "map";
    bool rolling_window = false;
    bool track_unknown = false;
    LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
    
    // ✅ FIX 1: RESIZE costmap TRƯỚC KHI sử dụng
    // Costmap mặc định có size = 0x0, resolution = 0.0 -> sẽ gây crash
    layered_costmap.resizeMap(
        100,    // size_x: số ô theo chiều X
        100,    // size_y: số ô theo chiều Y
        0.05,   // resolution: 0.05m/ô (5cm)
        0.0,    // origin_x
        0.0,    // origin_y
        false   // size_locked
    );
    std::cout << "Costmap resized to: " 
              << layered_costmap.getCostmap()->getSizeInCellsX() 
              << "x" << layered_costmap.getCostmap()->getSizeInCellsY()
              << ", resolution: " << layered_costmap.getCostmap()->getResolution() 
              << "m/cell" << std::endl;
    
    tf3::BufferCore tf_buffer;
    
    // // ✅ FIX 2: Lưu tất cả plugins để tránh bị destroy
    // std::vector<costmap_2d::PluginLayerPtr> plugin_instances;
    
    // // // ✅ FIX 3: Load và add static_layer
    // // auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    // //     "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // // );
    // // costmap_2d::PluginLayerPtr plugin1 = creator();
    // // std::cout << "Plugin created static_layer successfully" << std::endl;
    // // plugin1->initialize(&layered_costmap, "static_layer", &tf_buffer);
    // // layered_costmap.addPlugin(plugin1);  // ✅ FIX: ADD vào LayeredCostmap
    // // plugin_instances.push_back(plugin1);  // ✅ FIX: Lưu lại để tránh bị destroy
    // // std::cout << "Static layer initialized and added. Use count: " << plugin1.use_count() << std::endl;


    // // // ✅ FIX 5: Load và add obstacle_layer
    // // auto creator2 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    // //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // // );
    // // costmap_2d::PluginLayerPtr plugin3 = creator2();
    // // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
    // // plugin3->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
    // // layered_costmap.addPlugin(plugin3);  // ✅ FIX: ADD vào LayeredCostmap
    // // plugin_instances.push_back(plugin3);  // ✅ FIX: Lưu lại
    // // std::cout << "Obstacle layer initialized and added. Use count: " << plugin3.use_count() << std::endl;


    // // ✅ FIX 4: Load và add inflation_layer
    // auto creator1 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // costmap_2d::PluginLayerPtr plugin2 = creator1();
    // std::cout << "✅ Plugin created inflation_layer successfully" << std::endl;
    // plugin2->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
    // plugin2->onFootprintChanged();  // ✅ FIX: Cần gọi cho inflation_layer
    // layered_costmap.addPlugin(plugin2);  // ✅ FIX: ADD vào LayeredCostmap
    // plugin_instances.push_back(plugin2);  // ✅ FIX: Lưu lại
    // std::cout << "Inflation layer initialized and added. Use count: " << plugin2.use_count() << std::endl;

    // auto creator3 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // costmap_2d::PluginLayerPtr plugin4 = creator3();
    // std::cout << "Plugin created preferred_layer successfully" << std::endl;
    // plugin4->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
    // layered_costmap.addPlugin(plugin4);  // ✅ FIX: ADD vào LayeredCostmap
    // plugin_instances.push_back(plugin4);  // ✅ FIX: Lưu lại
    

    // // ✅ Kiểm tra số lượng plugins
    // std::cout << "Total plugins added: " << layered_costmap.getPlugins()->size() << std::endl;
    
    // // ✅ Test updateMap (nếu cần)
    // // layered_costmap.updateMap(0.0, 0.0, 0.0);  // robot_x, robot_y, robot_yaw
    
    // std::cout << "All plugins initialized and added successfully!" << std::endl;
    
    // // ✅ FIX: Xóa reference đến layered_costmap trong các plugins trước khi destroy
    // // Để tránh destructor của plugin cố gắng truy cập layered_costmap đã bị destroy
    // // for (auto& plugin : plugin_instances) {
    // //     // Reset pointer để tránh truy cập vào object đã bị destroy
    // //     // Lưu ý: Cần kiểm tra xem Layer có hàm reset pointer không
    // //     // Nếu không có, cần thêm vào Layer class
    // // }
    
    // // ✅ FIX: Clear plugin_instances TRƯỚC KHI layered_costmap bị destroy
    // // Thứ tự destroy: plugin_instances -> plugins (trong shared_ptr) -> destructor của plugin
    // plugin_instances.clear();
    // std::cout << "Plugin instances cleared" << std::endl;
    // // std::cout << "=== Costmap Plugin Test ===" << std::endl;

    // // // 1️⃣ Khởi tạo LayeredCostmap
    // // std::string global_frame = "map";
    // // bool rolling_window = true;
    // // bool track_unknown = true;
    // // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);

    // // // ✅ Resize map trước khi init plugin
    // // layered_costmap.resizeMap(
    // //     100,    // size_x
    // //     100,    // size_y
    // //     0.05,   // resolution (m/cell)
    // //     0.0,    // origin_x
    // //     0.0,    // origin_y
    // //     false   // size_locked
    // // );

    // // std::cout << "Costmap resized to: "
    // //           << layered_costmap.getCostmap()->getSizeInCellsX()
    // //           << "x" << layered_costmap.getCostmap()->getSizeInCellsY()
    // //           << ", resolution: " << layered_costmap.getCostmap()->getResolution()
    // //           << " m/cell" << std::endl;

    // // // 2️⃣ TF buffer
    // // tf3::BufferCore tf_buffer;

    // // // 3️⃣ Thiết lập footprint robot trước khi load inflation_layer
    // // std::vector<geometry_msgs::Point> footprint = {
    // //     {0.1, 0.1, 0.0},
    // //     {0.1, -0.1, 0.0},
    // //     {-0.1, -0.1, 0.0},
    // //     {-0.1, 0.1, 0.0}
    // // };
    // // // layered_costmap.setUnpaddedRobotFootprint(footprint);

    // 4️⃣ Danh sách plugin để load (thứ tự an toàn)
    struct PluginInfo { std::string path; std::string name; };
    std::vector<PluginInfo> plugins_to_load = {
        {"./costmap_2d/libstatic_layer.so", "static_layer"},
        {"./costmap_2d/libinflation_layer.so", "inflation_layer"},
        {"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
        {"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
    };

    // // // 5️⃣ Vector lưu shared_ptr plugin
    std::vector<PluginLayerPtr> plugin_instances;

    for (auto& info : plugins_to_load)
    {
        auto creator = boost::dll::import_alias<PluginLayerPtr()>(
            info.path, "create_plugin", boost::dll::load_mode::append_decorations
        );

        PluginLayerPtr plugin = creator();
        std::cout << "Plugin created: " << info.name << std::endl;

        plugin->initialize(&layered_costmap, info.name, &tf_buffer);

        // Nếu là inflation_layer, cần gọi onFootprintChanged()
        if(info.name == "inflation_layer")
        {
            plugin->onFootprintChanged();
            std::cout << "Inflation layer footprint updated." << std::endl;
        }

        plugin_instances.push_back(plugin);
        layered_costmap.addPlugin(plugin); 
        std::cout << info.name << " initialized, use_count=" << plugin.use_count() << std::endl;
    }

    std::cout << "All plugins loaded successfully." << std::endl;

    // 6️⃣ Test update map với dummy grid
    // nav_msgs::OccupancyGrid grid;
    // grid.info.width = 10;
    // grid.info.height = 10;
    // grid.info.resolution = 0.05;
    // grid.data.resize(grid.info.width * grid.info.height, 0); // free space

    // for(auto& plugin : plugin_instances)
    // {
    //     plugin->dataCallBack(grid, "map");
    // }

    // std::cout << "Map update simulated." << std::endl;

    // // // 7️⃣ Clear plugin_instances trước khi LayeredCostmap bị destroy
    // // plugin_instances.clear();
    // // std::cout << "Plugin instances cleared safely." << std::endl;

    // // return 0;

    // auto creator1 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // costmap_2d::PluginLayerPtr plugin1 = creator1();
    // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
    // plugin1->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
    // layered_costmap.addPlugin(plugin1); 


    // auto creator2 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // costmap_2d::PluginLayerPtr plugin2 = creator2();
    // std::cout << "Plugin created static_layer successfully" << std::endl;
    // plugin2->initialize(&layered_costmap, "static_layer", &tf_buffer);
    // layered_costmap.addPlugin(plugin2); 


    // auto creator3 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // costmap_2d::PluginLayerPtr plugin3 = creator3();
    // std::cout << "Plugin created inflation_layer successfully" << std::endl;
    // plugin3->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
    // layered_costmap.addPlugin(plugin3); 

    // auto creator4 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // costmap_2d::PluginLayerPtr plugin4 = creator4();
    // std::cout << "Plugin created preferred_layer successfully" << std::endl;
    // plugin4->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
    // layered_costmap.addPlugin(plugin4); 

    // while(true)
    // {
    robot::Duration(10).sleep();
    // }

    return 0;
}







    // auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // costmap_2d::PluginLayerPtr plugin = creator();
    // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
    // plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);


    // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // plugin = creator();
    // std::cout << "Plugin created static_layer successfully" << std::endl;
    // plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);


    // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // plugin = creator();
    // std::cout << "Plugin created inflation_layer successfully" << std::endl;
    // plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer);


    // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
    //     "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
    // );
    // plugin = creator();
    // std::cout << "Plugin created preferred_layer successfully" << std::endl;
    // plugin->initialize(&layered_costmap, "preferred_layer", &tf_buffer);






// #include <boost/dll/import.hpp>
// #include <iostream>
// #include <costmap_2d/costmap_layer.h>
// #include <costmap_2d/layered_costmap.h>
// #include <tf3/buffer_core.h>
// #include <tf3/time.h>
// #include "nav_msgs/OccupancyGrid.h"
// #include <map_msgs/OccupancyGridUpdate.h>
// #include <costmap_2d/obstacle_layer.h>
// #include <costmap_2d/inflation_layer.h>
// #include <costmap_2d/critical_layer.h>
// #include <costmap_2d/preferred_layer.h>
// using namespace costmap_2d;

// int main(int argc, char* argv[]) {

//     auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
//         "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
//     );

//     costmap_2d::PluginLayerPtr plugin = creator();
//     std::cout << "Plugin created static_layer successfully" << std::endl;
    
//     std::string global_frame = "map";
//     bool rolling_window = true;
//     bool track_unknown = true;
//     LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
    
//     tf3::BufferCore tf_buffer;
//     // tf3::Duration cache_time(10.0);
//     // tf3::BufferCore tf_buffer(cache_time);
    
//     plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
    
//     std::cout << "Plugin initialized" << std::endl;
    
//     // plugin->activate();
//     nav_msgs::OccupancyGrid grid;

//     grid.info.resolution = 0.05f;
//     grid.info.width = 2;
//     grid.info.height = 2;

//     grid.data.resize(grid.info.width * grid.info.height, -1);
//     grid.data[0] = 0; 
//     grid.data[1] = 100;
//     grid.data[2] = 10;
//     grid.data[3] = 0;
//     plugin->dataCallBack(grid, "map");

//     map_msgs::OccupancyGridUpdate update;

//     update.x = 1;
//     update.y = 1;
//     update.width = 2;
//     update.height = 2;

//     update.data.resize(update.width * update.height, -1);
//     update.data[0] = 0; 
//     update.data[1] = 100;
//     update.data[2] = 10;
//     update.data[3] = 0;
//     plugin->dataCallBack(update, "map_update");


//     // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
//     //     "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
//     // );

//     // plugin = creator();
//     // std::cout << "Plugin created obstacle_layer successfully" << std::endl;

//     // plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);

//     // sensor_msgs::LaserScan scan;

//     // // --- Header ---
//     // scan.header.seq = 1;
//     // scan.header.stamp = robot::Time::now();  // nếu bạn chưa có Time class, gán tạm số
//     // scan.header.frame_id = "laser_frame";

//     // // --- Cấu hình góc ---
//     // scan.angle_min = -M_PI;             // -180 độ
//     // scan.angle_max = M_PI;              // +180 độ
//     // scan.angle_increment = M_PI / 180;  // 1 độ mỗi tia

//     // // --- Cấu hình thời gian ---
//     // scan.time_increment = 0.0001f;  // mỗi tia cách nhau 0.1 ms
//     // scan.scan_time = 0.04f;         // tổng thời gian quét 40 ms (~25Hz)

//     // // --- Giới hạn đo ---
//     // scan.range_min = 0.1f;
//     // scan.range_max = 10.0f;

//     // // --- Tạo dữ liệu ---
//     // int num_readings = static_cast<int>((scan.angle_max - scan.angle_min) / scan.angle_increment);
//     // scan.ranges.resize(num_readings);
//     // scan.intensities.resize(num_readings);

//     // for (int i = 0; i < num_readings; ++i)
//     // {
//     //     float angle = scan.angle_min + i * scan.angle_increment;

//     //     // Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc
//     //     scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle);

//     //     // Giả lập cường độ phản xạ
//     //     scan.intensities[i] = 100.0f + 20.0f * std::cos(angle);
//     // }

//     // // --- In thử 10 giá trị đầu ---
//     // // for (int i = 0; i < 10; ++i)
//     // // {
//     // //     std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment)
//     // //               << " rad | Range " << scan.ranges[i]
//     // //               << " m | Intensity " << scan.intensities[i]
//     // //               << std::endl;
//     // // }
//     // // plugin->deactivate();
//     // plugin->dataCallBack(scan, "laser_valid");

//     // sensor_msgs::PointCloud cloud;

//     // // 2. Thiết lập header
//     // cloud.header.stamp.sec = 1234567890;   // hoặc ros::Time::now().sec
//     // cloud.header.stamp.nsec = 0;
//     // cloud.header.frame_id = "laser_frame";

//     // // 3. Thêm một vài điểm
//     // geometry_msgs::Point32 pt1;
//     // pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f;

//     // geometry_msgs::Point32 pt2;
//     // pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f;

//     // geometry_msgs::Point32 pt3;
//     // pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f;

//     // cloud.points.push_back(pt1);
//     // cloud.points.push_back(pt2);
//     // cloud.points.push_back(pt3);

//     // // 4. Thêm dữ liệu channels (tùy chọn)
//     // cloud.channels.resize(1);
//     // cloud.channels[0].name = "intensity";
//     // cloud.channels[0].values.push_back(0.5f);
//     // cloud.channels[0].values.push_back(1.0f);
//     // cloud.channels[0].values.push_back(0.8f);

//     // plugin->dataCallBack(cloud, "pcl_cb");
    
//     std::cout << "Plugin activated successfully" << std::endl;
//     creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
//         "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
//     );

//     plugin = creator();
//     std::cout << "Plugin created inflation_layer successfully" << std::endl;
    
//     plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
//     plugin->onFootprintChanged();
    
//     std::cout << "Plugin initialized" << std::endl;


//     creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
//         "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
//     );

//     plugin = creator();
//     std::cout << "Plugin created preferred_layer successfully" << std::endl;

//     plugin->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
//     plugin->dataCallBack(grid, "map");

    
//     creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
//         "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
//     );

//     plugin = creator();
//     std::cout << "Plugin created obstacle_layer successfully" << std::endl;

//     plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
    
//     std::cout << "Plugin initialized" << std::endl;
// }