#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>  // For sensor_msgs::PointCloud2

#include "include/PointCloud.hpp"

void pointCloudReceived(const sensor_msgs::PointCloud2& msg, PointCloudHandler pointCloud);

int main(int argc, char **argv) {
  // Initialize the ROS system and become a node.
  ros::init(argc, argv, "pcl_obstacle_detector");
  ros::NodeHandle nh;
  ros::Subscriber sub_point_cloud;

  PointCloudHandler pointCloud;

  // 1. Subscribes to the point cloud topic /point_cloud
  // Type: sensor_msgs/PointCloud2
  sub_point_cloud = nh.subscribe("/point_cloud", 1000,
  &pointCloudReceived(pointCloud));

  // Let ROS take over.
  ros::spin();
}

void pointCloudReceived(const sensor_msgs::PointCloud2& msg,  PointCloudHandler pointCloud) {
  pointCloud.setMsgPointCloud(msg);
  // ROS_INFO_STREAM("End of pointCloudReceived");

  // 2. Processes the received point cloud to find clusters of points
  pointCloud.processReceivedPointCloud();

  // 3. Publishes the clusters as a detection array on a ROS topic
  pointCloud.pubClusters();
}