#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/data_convert.h>
#include <costmap_2d/utils.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <boost/dll/alias.hpp>


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

namespace costmap_2d
{

StaticLayer::StaticLayer()
{
  threshold_ = &lethal_threshold_;
}

StaticLayer::~StaticLayer()
{}

void StaticLayer::onInitialize()
{
  current_ = true;

  global_frame_ = layered_costmap_->getGlobalFrameID();
  getParams();

}

bool StaticLayer::getParams()
{
    try {
        YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
        YAML::Node layer = config["static_layer"];


        bool enabled = loadParam(layer, "enabled", true);
        bool first_map_only = loadParam(layer, "first_map_only", false);
        bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false);
        bool track_unknown_space = loadParam(layer, "track_unknown_space", true);
        bool use_maximum = loadParam(layer, "use_maximum", false);
        int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100);
        int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1);
        bool trinary_costmap = loadParam(layer, "trinary_costmap", true);
        std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map"));

        first_map_only_ = first_map_only;
        subscribe_to_updates_ = subscribe_to_updates;
        track_unknown_space_ = track_unknown_space;
        use_maximum_ = use_maximum;
        lethal_threshold_ = std::max(std::min(lethal_cost_threshold, 100), 0);
        unknown_cost_value_ = unknown_cost_value;
        trinary_costmap_ = trinary_costmap;
        base_frame_id_ = base_frame_id;

        if (enabled_ != enabled) {
            enabled_ = enabled;
            has_updated_data_ = true;
            x_ = y_ = 0;
            width_ = size_x_;
            height_ = size_y_;
        }
    } 
    catch (const YAML::BadFile& e) {
        std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
        return false;
    }

    return true;

}

void StaticLayer::matchSize()
{
  // If we are using rolling costmap, the static map size is
  //   unrelated to the size of the layered costmap
  if (!layered_costmap_->isRolling())
  {
    Costmap2D* master = layered_costmap_->getCostmap();
    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
              master->getOriginX(), master->getOriginY());
  }
}

unsigned char StaticLayer::interpretValue(unsigned char value)
{
  // check if the static value is above the unknown or lethal thresholds
  if (track_unknown_space_ && value == unknown_cost_value_)
    return NO_INFORMATION;
  else if (!track_unknown_space_ && value == unknown_cost_value_)
    return FREE_SPACE;
  else if (value >= lethal_threshold_)
    return LETHAL_OBSTACLE;
  else if (trinary_costmap_)
    return FREE_SPACE;

  double scale = (double) value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

void StaticLayer::handleImpl(const void* data, 
                            const std::type_info& type, 
                            const std::string& topic)  
{
    if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") {
        incomingMap(*static_cast<const nav_msgs::OccupancyGrid*>(data));
    } else if (type == typeid(map_msgs::OccupancyGridUpdate) && topic == "map_update") {
        incomingUpdate(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
    } else {
        std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
    }
}

void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
  if(!map_shutdown_)
  {
    std::cout << "Received new map!" << std::endl;

    unsigned int size_x = new_map.info.width, size_y = new_map.info.height;

    printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);

    // resize costmap if size, resolution or origin do not match
    Costmap2D* master = layered_costmap_->getCostmap();
    if (!layered_costmap_->isRolling() &&
        (master->getSizeInCellsX() != size_x ||
        master->getSizeInCellsY() != size_y ||
        master->getResolution() != new_map.info.resolution ||
        master->getOriginX() != new_map.info.origin.position.x ||
        master->getOriginY() != new_map.info.origin.position.y))
    {
      // Update the size of the layered costmap (and all layers, including this one)
      printf("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
      layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
                                  new_map.info.origin.position.y,
                                  true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
    }
    else if (size_x_ != size_x || size_y_ != size_y ||
            resolution_ != new_map.info.resolution ||
            origin_x_ != new_map.info.origin.position.x ||
            origin_y_ != new_map.info.origin.position.y)
    {
      // only update the size of the costmap stored locally in this layer
      printf("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
      resizeMap(size_x, size_y, new_map.info.resolution,
                new_map.info.origin.position.x, new_map.info.origin.position.y);
    }

    unsigned int index = 0;

    // initialize the costmap with static data
    for (unsigned int i = 0; i < size_y; ++i)
    {
      for (unsigned int j = 0; j < size_x; ++j)
      {
        unsigned char value = new_map.data[index];
        costmap_[index] = interpretValue(value);
        ++index;
      }
    }
    map_frame_ = new_map.header.frame_id;

    // we have a new map, update full size of map
    x_ = y_ = 0;
    width_ = size_x_;
    height_ = size_y_;
    map_received_ = true;
    has_updated_data_ = true;
    
  }
  else
  {
    printf("Stop receive new map!");
  }
  // shutdown the map subscrber if firt_map_only_ flag is on
  if (first_map_only_)
  {
    printf("Shutting down the map subscriber. first_map_only flag is on");
    map_shutdown_ = true;
    // map_sub_.shutdown();
  }
}

void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
{
  if(!map_update_shutdown_)
  {
    std::cout << "Update new map!" << std::endl;
    unsigned int di = 0;
    for (unsigned int y = 0; y < update.height ; y++)
    {
      unsigned int index_base = (update.y + y) * size_x_;
      for (unsigned int x = 0; x < update.width ; x++)
      {
        unsigned int index = index_base + x + update.x;
        costmap_[index] = interpretValue(update.data[di++]);
      }
    }
    x_ = update.x;
    y_ = update.y;
    width_ = update.width;
    height_ = update.height;
    has_updated_data_ = true;
  }
}

void StaticLayer::activate()
{
  map_shutdown_ = false;
  map_update_shutdown_ = false;
  onInitialize();
}

void StaticLayer::deactivate()
{
  map_shutdown_ = true;
  if (subscribe_to_updates_)
    map_update_shutdown_ = true;
}

void StaticLayer::reset()
{
  if (first_map_only_)
  {
    map_shutdown_ = false;
    map_update_shutdown_ = false;
    has_updated_data_ = true;
  }
  else
  {
    onInitialize();
  }
  printf("RESET MAP");
}

void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                               double* max_x, double* max_y)
{

  if( !layered_costmap_->isRolling() ){
    if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
      return;
  }

  useExtraBounds(min_x, min_y, max_x, max_y);

  double wx, wy;

  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);

  mapToWorld(x_ + width_, y_ + height_, wx, wy);
  *max_x = std::max(wx, *max_x);
  *max_y = std::max(wy, *max_y);

  has_updated_data_ = false;
}

void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!map_received_)
    return;

  if (!layered_costmap_->isRolling())
  {
    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
    if (!use_maximum_)
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); 
    else
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
  }
  else
  {
    // If rolling window, the master_grid is unlikely to have same coordinates as this layer
    unsigned int mx, my;
    double wx, wy;
    // Might even be in a different frame
    // geometry_msgs::TransformStamped transform;
    tf2::TransformStampedMsg transformMsg;
    try
    {
      // transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
      bool status =tf_->canTransform(map_frame_, global_frame_, tf2::Time());
      if(!status) throw tf2::TransformException("[static_layer] Cannot transform");
      transformMsg = tf_->lookupTransform(map_frame_, 
                                          global_frame_,
                                          tf2::Time());
    }
    catch (tf2::TransformException ex)
    {
      printf("%s", ex.what());
      return;
    }
    // Copy map data given proper transformations
    tf2::Transform tf2_transform;
    tf2_transform = convertToTf2Transform(transformMsg.transform);
    // tf2::convert(transformMsg.transform, tf2_transform);
    for (unsigned int i = min_i; i < max_i; ++i)
    {
      for (unsigned int j = min_j; j < max_j; ++j)
      {
        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Transform from global_frame_ to map_frame_
        tf2::Vector3 p(wx, wy, 0);
        p = tf2_transform*p;
        // Set master_grid with cell from map
        if (worldToMap(p.x(), p.y(), mx, my))
        {
          if (!use_maximum_)
            master_grid.setCost(i, j, getCost(mx, my));
          else
            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
        }
      }
    }
  }
}

// Export factory function
static PluginCostmapLayerPtr create_static_plugin() {
    return std::make_shared<StaticLayer>();
}

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

}  // namespace costmap_2d