#include <costmap_2d/obstacle_layer.h>
// #include <costmap_2d/costmap_math.h>
// #include <tf2_ros/message_filter.h>

// #include <pluginlib/class_list_macros.hpp>
// #include <sensor_msgs/point_cloud2_iterator.h>

// PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)

// 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()
// {
//   ros::NodeHandle nh("~/" + name_), g_nh;
//   rolling_window_ = layered_costmap_->isRolling();

//   bool track_unknown_space;
//   nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
//   if (track_unknown_space)
//     default_value_ = NO_INFORMATION;
//   else
//     default_value_ = FREE_SPACE;

//   ObstacleLayer::matchSize();
//   current_ = true;

//   global_frame_ = layered_costmap_->getGlobalFrameID();
//   double transform_tolerance;
//   nh.param("transform_tolerance", transform_tolerance, 0.2);

//   std::string topics_string;
//   // get the topics that we'll subscribe to from the parameter server
//   nh.param("observation_sources", topics_string, std::string(""));
//   ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());

//   // now we need to split the topics based on whitespace which we can use a stringstream for
//   std::stringstream ss(topics_string);

//   std::string source;
//   while (ss >> source)
//   {
//     ros::NodeHandle source_node(nh, source);

//     // get the parameters for the specific topic
//     double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
//     std::string topic, sensor_frame, data_type;
//     bool inf_is_valid, clearing, marking;

//     source_node.param("topic", topic, source);
//     source_node.param("sensor_frame", sensor_frame, std::string(""));
//     source_node.param("observation_persistence", observation_keep_time, 0.0);
//     source_node.param("expected_update_rate", expected_update_rate, 0.0);
//     source_node.param("data_type", data_type, std::string("PointCloud"));
//     source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
//     source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
//     source_node.param("inf_is_valid", inf_is_valid, false);
//     source_node.param("clearing", clearing, false);
//     source_node.param("marking", marking, true);

//     if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
//     {
//       ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
//       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;

//     // get the obstacle range for the sensor
//     double obstacle_range = 2.5;
//     if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
//     {
//       source_node.getParam(obstacle_range_param_name, obstacle_range);
//     }

//     // get the raytrace range for the sensor
//     double raytrace_range = 3.0;
//     if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
//     {
//       source_node.getParam(raytrace_range_param_name, raytrace_range);
//     }

//     ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), 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)));

//     // check if we'll add this buffer to our marking observation buffers
//     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());

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

//     // create a callback for the topic
//     if (data_type == "LaserScan")
//     {
//       boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
//           > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));

//       boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
//         new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));

//       if (inf_is_valid)
//       {
//         filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
//       }
//       else
//       {
//         filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
//       }

//       observation_subscribers_.push_back(sub);
//       observation_notifiers_.push_back(filter);

//       observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
//     }
//     else if (data_type == "PointCloud")
//     {
//       boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
//           > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));

//       if (inf_is_valid)
//       {
//        ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
//       }

//         boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
//         > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
//         filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });

//       observation_subscribers_.push_back(sub);
//       observation_notifiers_.push_back(filter);
//     }
//     else
//     {
//       boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
//           > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));

//       if (inf_is_valid)
//       {
//        ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
//       }

//       boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
//       > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
//       filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });

//       observation_subscribers_.push_back(sub);
//       observation_notifiers_.push_back(filter);
//     }

//     if (sensor_frame != "")
//     {
//       std::vector < std::string > target_frames;
//       target_frames.push_back(global_frame_);
//       target_frames.push_back(sensor_frame);
//       observation_notifiers_.back()->setTargetFrames(target_frames);
//     }
//   }

//   dsrv_ = NULL;
//   setupDynamicReconfigure(nh);
// }

// void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
// {
//   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
//   dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
//       [this](auto& config, auto level){ reconfigureCB(config, level); };
//   dsrv_->setCallback(cb);
// }

// ObstacleLayer::~ObstacleLayer()
// {
//     if (dsrv_)
//         delete dsrv_;
// }
// void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
// {
//   enabled_ = config.enabled;
//   footprint_clearing_enabled_ = config.footprint_clearing_enabled;
//   max_obstacle_height_ = config.max_obstacle_height;
//   combination_method_ = config.combination_method;
// }

// void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
//                                       const boost::shared_ptr<ObservationBuffer>& buffer)
// {
//   // 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)
//   {
//     ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
//              ex.what());
//     projector_.projectLaser(*message, cloud);
//   }
//   catch (std::runtime_error &ex)
//   {
//     ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
//     return; //ignore this message
//   }

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

// void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
//                                               const boost::shared_ptr<ObservationBuffer>& buffer)
// {
//   // 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)
//   {
//     ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
//              global_frame_.c_str(), ex.what());
//     projector_.projectLaser(message, cloud);
//   }
//   catch (std::runtime_error &ex)
//   {
//     ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
//     return; //ignore this message
//   }

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

// void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
//                                                const boost::shared_ptr<ObservationBuffer>& buffer)
// {
//   sensor_msgs::PointCloud2 cloud2;

//   if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
//   {
//     ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
//     return;
//   }

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

// void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& 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_)
//       {
//         ROS_DEBUG("The point is too high");
//         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)
//       {
//         ROS_DEBUG("The point is too far away");
//         continue;
//       }

//       // now we need to compute the map coordinates for the observation
//       unsigned int mx, my;
//       if (!worldToMap(px, py, mx, my))
//       {
//         ROS_DEBUG("Computing map coords failed");
//         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))
//   {
//     ROS_WARN_THROTTLE(
//         1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
//         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()
// {
//   // if we're stopped we need to re-subscribe to topics
//   for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
//   {
//     if (observation_subscribers_[i] != NULL)
//       observation_subscribers_[i]->subscribe();
//   }

//   for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
//   {
//     if (observation_buffers_[i])
//       observation_buffers_[i]->resetLastUpdated();
//   }
// }
// void ObstacleLayer::deactivate()
// {
//   for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
//   {
//     if (observation_subscribers_[i] != NULL)
//       observation_subscribers_[i]->unsubscribe();
//   }
// }

// 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();
// }

// }  // namespace costmap_2d