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

using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE;

namespace costmap_2d
{

PreferredLayer::PreferredLayer(){}
PreferredLayer::~PreferredLayer(){}

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

  double scale = (double) value / *this->threshold_;
  return scale * LETHAL_OBSTACLE;
}

// Export factory function
static PluginLayerPtr create_preferred_plugin() {
    return std::make_shared<PreferredLayer>();
}

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

}