#include <costmap_2d/critical_layer.h>
#include <boost/dll/alias.hpp>

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::CRITICAL_SPACE;

namespace costmap_2d
{

CriticalLayer::CriticalLayer(){}
CriticalLayer::~CriticalLayer(){}

unsigned char CriticalLayer::interpretValue(unsigned char value) 
{
  // printf("TEST PLUGIN CRITICAL\n");
  // check if the static value is above the unknown or lethal thresholds
  if(value >= *this->threshold_)
    return CRITICAL_SPACE;
  else 
    return NO_INFORMATION;
}

void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  // printf("TEST PLUGIN CRITICAL\n");
  if (!map_received_)
    return;
    
    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
    if (!use_maximum_)
      StaticLayer::CostmapLayer::updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); 
    else
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
    
}

// Export factory function
static std::shared_ptr<Layer> create_critical_plugin() {
    return std::make_shared<CriticalLayer>();
}

// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_critical_plugin, create_critical_layer)


}