#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/utils.h>
#include <sensor_msgs/point_cloud2_iterator.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;

using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;

namespace costmap_2d
{

void ObstacleLayer::onInitialize()
{
  rolling_window_ = layered_costmap_->isRolling();
  

  ObstacleLayer::matchSize();
  current_ = true;
  stop_receiving_data_ = false;
  global_frame_ = layered_costmap_->getGlobalFrameID();
  getParams();
}


ObstacleLayer::~ObstacleLayer()
{}

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

      bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
      if (track_unknown_space)
        default_value_ = NO_INFORMATION;
      else
        default_value_ = FREE_SPACE;

      double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
      // get the topics that we'll subscribe to from the parameter server
      std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
      printf("    Subscribed to Topics: %s\n", topics_string.c_str());

      // get the parameters for the specific topic
      double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
      std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
      bool inf_is_valid = false, clearing=false, marking=true;
      topic = loadParam(layer,"topic", topic);
      sensor_frame = loadParam(layer,"sensor_frame", std::string(""));
      observation_keep_time = loadParam(layer,"observation_persistence", 0.0);
      expected_update_rate = loadParam(layer,"expected_update_rate", 0.0);
      data_type = loadParam(layer,"data_type", std::string("PointCloud"));
      min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0);
      max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
      inf_is_valid = loadParam(layer,"inf_is_valid", false);
      clearing = loadParam(layer,"clearing", false);
      marking = loadParam(layer,"marking", true);
      if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
      {
        printf("Only topics that use point clouds or laser scans are currently supported\n");
        throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
      }

      std::string raytrace_range_param_name, obstacle_range_param_name;

      double obstacle_range = 2.5;
      obstacle_range = loadParam(layer,"obstacle_range", obstacle_range);
      double raytrace_range = 3.0;
      raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);


      bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
      int combination_method = loadParam(layer, "combination_method", 1);

      // enabled_ = enabled;
      footprint_clearing_enabled_ = footprint_clearing_enabled;
      max_obstacle_height_ = max_obstacle_height;
      combination_method_ = combination_method;

      printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
                sensor_frame.c_str());

      // create an observation buffer
      observation_buffers_.push_back(
          boost::shared_ptr < ObservationBuffer
              > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
                                      max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
                                      sensor_frame, transform_tolerance)));
      if (marking)
        marking_buffers_.push_back(observation_buffers_.back());

      // check if we'll also add this buffer to our clearing observation buffers
      if (clearing)
        clearing_buffers_.push_back(observation_buffers_.back());

      printf(
          "Created an observation buffer for topic %s, global frame: %s, "
          "expected update rate: %.2f, observation persistence: %.2f",
          topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);

    } 
    catch (const YAML::BadFile& e) {
        std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
        return false;
    }

    return true;
}

void ObstacleLayer::handleImpl(const void* data, 
                              const std::type_info& type,
                              const std::string& topic)  
{
  if(!stop_receiving_data_)
  {
    if(observation_buffers_.empty()) return;
    boost::shared_ptr<costmap_2d::ObservationBuffer> buffer = observation_buffers_.back();
    if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") {
        laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
    } else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") {
        laserScanValidInfCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
    } else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") {
        pointCloudCallback(*static_cast<const sensor_msgs::PointCloud*>(data), buffer);
    } else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
        pointCloud2Callback(*static_cast<const sensor_msgs::PointCloud2*>(data), buffer);
    } else {
        std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
    }
  }
  else 
  {
    std::cout << "Stop receiving data!" << std::endl;
    return;
  }
}

void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message, 
                                      const boost::shared_ptr<ObservationBuffer>& buffer)
{
  // project the laser into a point cloud
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message.header;
  // printf("TEST PLUGIN OBSTACLE!!!\n");

  // project the scan into a point cloud
  try
  {
    projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
  }
  catch (tf2::TransformException &ex)
  {
    printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
             ex.what());
    projector_.projectLaser(message, cloud);
  }
  catch (std::runtime_error &ex)
  {
    printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
    return; //ignore this message
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud);
  buffer->unlock();
}

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message, 
                                              const boost::shared_ptr<ObservationBuffer>& buffer)
{
  // printf("TEST PLUGIN OBSTACLE 2!!!\n");
  // Filter positive infinities ("Inf"s) to max_range.
  float epsilon = 0.0001;  // a tenth of a millimeter
  sensor_msgs::LaserScan message = raw_message;
  for (size_t i = 0; i < message.ranges.size(); i++)
  {
    float range = message.ranges[ i ];
    if (!std::isfinite(range) && range > 0)
    {
      message.ranges[ i ] = message.range_max - epsilon;
    }
  }

  // project the laser into a point cloud
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message.header;

  // project the scan into a point cloud
  try
  {
    projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
  }
  catch (tf2::TransformException &ex)
  {
    printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
             global_frame_.c_str(), ex.what());
    projector_.projectLaser(message, cloud);
  }
  catch (std::runtime_error &ex)
  {
    printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
    return; //ignore this message
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud);
  buffer->unlock();
}

void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
                                       const boost::shared_ptr<ObservationBuffer>& buffer)
{
  // printf("TEST PLUGIN OBSTACLE 3!!!\n");
  sensor_msgs::PointCloud2 cloud2;

  if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
  {
    printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
    return;
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud2);
  buffer->unlock();
}

void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& message,
                                        const boost::shared_ptr<ObservationBuffer>& buffer)
{
  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(message);
  buffer->unlock();
}

void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                          double* min_y, double* max_x, double* max_y)
{
  if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
  useExtraBounds(min_x, min_y, max_x, max_y);

  bool current = true;
  std::vector<Observation> observations, clearing_observations;

  // get the marking observations
  current = current && getMarkingObservations(observations);

  // get the clearing observations
  current = current && getClearingObservations(clearing_observations);

  // update the global current status
  current_ = current;

  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }

  // place the new obstacles into a priority queue... each with a priority of zero to begin with
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const Observation& obs = *it;

    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);

    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");

    for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
    {
      double px = *iter_x, py = *iter_y, pz = *iter_z;

      // if the obstacle is too high or too far away from the robot we won't add it
      if (pz > max_obstacle_height_)
      {
        printf("The point is too high\n");
        continue;
      }

      // compute the squared distance from the hitpoint to the pointcloud's origin
      double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
          + (pz - obs.origin_.z) * (pz - obs.origin_.z);

      // if the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range)
      {
        printf("The point is too far away\n");
        continue;
      }

      // now we need to compute the map coordinates for the observation
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
        printf("Computing map coords failed\n");
        continue;
      }

      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                                    double* max_x, double* max_y)
{
    if (!footprint_clearing_enabled_) return;
    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);

    for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
    {
      touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
    }
}

void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (footprint_clearing_enabled_)
  {
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
  }
  
  switch (combination_method_)
  {
    case 0:  // Overwrite
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;
    case 1:  // Maximum
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
      break;
    default:  // Nothing
      break;
  }

}

void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
{
  if (marking)
    static_marking_observations_.push_back(obs);
  if (clearing)
    static_clearing_observations_.push_back(obs);
}

void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
{
  if (marking)
    static_marking_observations_.clear();
  if (clearing)
    static_clearing_observations_.clear();
}

bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
{
  bool current = true;
  // get the marking observations
  for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
  {
    marking_buffers_[i]->lock();
    marking_buffers_[i]->getObservations(marking_observations);
    current = marking_buffers_[i]->isCurrent() && current;
    marking_buffers_[i]->unlock();
  }
  marking_observations.insert(marking_observations.end(),
                              static_marking_observations_.begin(), static_marking_observations_.end());
  return current;
}

bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
{
  bool current = true;
  // get the clearing observations
  for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
  {
    clearing_buffers_[i]->lock();
    clearing_buffers_[i]->getObservations(clearing_observations);
    current = clearing_buffers_[i]->isCurrent() && current;
    clearing_buffers_[i]->unlock();
  }
  clearing_observations.insert(clearing_observations.end(),
                              static_clearing_observations_.begin(), static_clearing_observations_.end());
  return current;
}

void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
                                              double* max_x, double* max_y)
{
  double ox = clearing_observation.origin_.x;
  double oy = clearing_observation.origin_.y;
  const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);

  // get the map coordinates of the origin of the sensor
  unsigned int x0, y0;
  if (!worldToMap(ox, oy, x0, y0))
  {
    printf(
        "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
        ox, oy);
    return;
  }

  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
  double origin_x = origin_x_, origin_y = origin_y_;
  double map_end_x = origin_x + size_x_ * resolution_;
  double map_end_y = origin_y + size_y_ * resolution_;


  touch(ox, oy, min_x, min_y, max_x, max_y);

  // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
  sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
  sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");

  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
  {
    double wx = *iter_x;
    double wy = *iter_y;

    // now we also need to make sure that the enpoint we're raytracing
    // to isn't off the costmap and scale if necessary
    double a = wx - ox;
    double b = wy - oy;

    // the minimum value to raytrace from is the origin
    if (wx < origin_x)
    {
      double t = (origin_x - ox) / a;
      wx = origin_x;
      wy = oy + b * t;
    }
    if (wy < origin_y)
    {
      double t = (origin_y - oy) / b;
      wx = ox + a * t;
      wy = origin_y;
    }

    // the maximum value to raytrace to is the end of the map
    if (wx > map_end_x)
    {
      double t = (map_end_x - ox) / a;
      wx = map_end_x - .001;
      wy = oy + b * t;
    }
    if (wy > map_end_y)
    {
      double t = (map_end_y - oy) / b;
      wx = ox + a * t;
      wy = map_end_y - .001;
    }

    // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
    unsigned int x1, y1;

    // check for legality just in case
    if (!worldToMap(wx, wy, x1, y1))
      continue;

    unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
    MarkCell marker(costmap_, FREE_SPACE);
    // and finally... we can execute our trace to clear obstacles along that line
    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);

    updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
  }
}

void ObstacleLayer::activate()
{
  stop_receiving_data_ = false;
  for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
  {
    if (observation_buffers_[i])
      observation_buffers_[i]->resetLastUpdated();
  }
}
void ObstacleLayer::deactivate()
{
  stop_receiving_data_ = true;
}

void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
                                         double* min_x, double* min_y, double* max_x, double* max_y)
{
  double dx = wx-ox, dy = wy-oy;
  double full_distance = hypot(dx, dy);
  double scale = std::min(1.0, range / full_distance);
  double ex = ox + dx * scale, ey = oy + dy * scale;
  touch(ex, ey, min_x, min_y, max_x, max_y);
}

void ObstacleLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

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

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

}  // namespace costmap_2d
