#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/search/kdtree.h> 
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/io.h>
#include <pcl/common/common_headers.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <Eigen/Geometry>
#include <tf/transform_datatypes.h>
#include <pcl/io/vtk_io.h>
#define foreach BOOST_FOREACH


typedef pcl::PointXYZRGB PointT; 


ros::Publisher cloud_pub ;



//Transformation

void transformation(const pcl::PointCloud<PointT>::Ptr &ip,pcl::PointCloud<PointT>::Ptr &op )
{
float roll  = -3.14;
float pitch = 0.87;
float yaw   = 1.55;


Eigen::Affine3f trans = Eigen::Affine3f::Identity();


trans.translation() << -0.55, 0, 2;
trans.rotate (Eigen::AngleAxisf (roll, Eigen::Vector3f::UnitX()));
trans.rotate (Eigen::AngleAxisf (pitch, Eigen::Vector3f::UnitY()));
trans.rotate (Eigen::AngleAxisf (yaw, Eigen::Vector3f::UnitZ()));


pcl::transformPointCloud (*ip, *op, trans);
} 


//Surface meshing

// void greedy_projection() {
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// pcl::PCLPointCloud2 cloud_blob;

// }



//Callback for subscriber


void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{

  sensor_msgs::PointCloud2 map_cloud;
  std::cout << cloud_msg->header.frame_id << std::endl;

  // create pointer to Point Clouds in pcl
  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); 
  pcl::PointCloud<PointT>::Ptr transform_cloud(new pcl::PointCloud<PointT>); 
  pcl::fromROSMsg(*cloud_msg, *cloud);
  pcl::fromROSMsg(*cloud_msg, *transform_cloud);


  transformation(cloud,transform_cloud);
  

  pcl::toROSMsg(*transform_cloud, map_cloud);

  cloud_pub.publish (map_cloud);
}





int main(int argc, char **argv)
{
    ros::init(argc, argv, "pcd_transformer");
    ros::NodeHandle nh;

    ros::Subscriber cloud_in = nh.subscribe<sensor_msgs::PointCloud2>("/voxel_cloud", 1, cloud_cb);

    
    cloud_pub = nh.advertise<sensor_msgs::PointCloud2> ("map_cloud", 1);

    
    ros::spin ();

}