#include <boost/dll/import.hpp>
#include <iostream>
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <tf2/buffer_core.h>
#include <tf2/time.h>
#include "nav_msgs/OccupancyGrid.h"
using namespace costmap_2d;

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

    // Fix 1: Use correct path to the library
    auto creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
        "./costmap_2d/liblayers.so", "create_plugin", boost::dll::load_mode::append_decorations
    );

    costmap_2d::PluginCostmapLayerPtr plugin = creator();
    std::cout << "Plugin created successfully" << std::endl;
    
    // Fix 2: Initialize the plugin before calling activate()
    // activate() calls onInitialize() which requires layered_costmap_ to be set
    std::string global_frame = "map";
    bool rolling_window = false;
    bool track_unknown = true;
    LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
    
    // Fix 3: tf2::BufferCore constructor requires ros::Duration parameter (not tf2::Duration)
    tf2::BufferCore tf_buffer;
    // tf2::Duration cache_time(10.0);  // 10 seconds cache time
    // tf2::BufferCore tf_buffer(cache_time);
    
    // Initialize the layer with required parameters
    plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
    
    std::cout << "Plugin initialized" << std::endl;
    
    // Now it's safe to call activate()
    // 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;    // Free cell
    grid.data[1] = 100;  // Occupied cell
    grid.data[2] = 10;  // Occupied cell
    grid.data[3] = 0;    // Free cell
    plugin->incomingMap(grid);
    
    std::cout << "Plugin activated successfully" << std::endl;
}