#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.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/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <Eigen/Geometry>
#define foreach BOOST_FOREACH

tf::Transform trans;
// trans.setOrigin( tf::Vector3(1.3435, 0.0, -1.3435) );
// tf::Quaternion q;
// q.setRPY(0, -0.698132, 0);
// trans.setRotation(q);
// const std::string target_frame = "base_link";
const std::string target_frame = "camera_color_optical_frame";
// float pitch = -0.698132;
// Eigen::Affine3f trans = Eigen::Affine3f::Identity();
// trans.translation() << 1.3435, 0.0, -1.3435;
// trans.rotate (Eigen::AngleAxisf (pitch, Eigen::Vector3f::UnitY()));
// float pitch = -0.698132;
// Eigen::Affine3f trans = Eigen::Affine3f::Identity();
// trans.translation() << 1.3435, 0.0, -1.3435;
// trans.rotate (Eigen::AngleAxisf (pitch, Eigen::Vector3f::UnitY()));

ros::Publisher cloud_pub ;

// https://github.com/methylDragon/pcl-ros-tutorial/blob/master/PCL%20Reference%20with%20ROS.md
// http://wiki.ros.org/pcl/Tutorials/hydro?action=AttachFile&do=view&target=example_voxelgrid.cpp
// http://docs.ros.org/en/indigo/api/pcl_ros/html/namespacepcl__ros.html#aad20af010640a41f1898e75f6a6121ad
// https://answers.ros.org/question/225820/subscribe-to-sensor_msgpointcloud2/
 

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_in)                  
{
    // sensor_msgs::PointCloud2 cloud_out;
    // pcl_ros::transformPointCloud (target_frame, trans, cloud_in, cloud_out);
    // transformed_cloud.publish(cloud_out);
    pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
    pcl::PCLPointCloud2 cloud_filtered;

    pcl_conversions::toPCL(*cloud_in , *cloud);

    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.1, 0.1, 0.1);
    sor.filter (cloud_filtered);
    sensor_msgs::PointCloud2 cloud_o;
    sensor_msgs::PointCloud2 trans_cloud;
    pcl_conversions::moveFromPCL(cloud_filtered, cloud_o);

    trans.setOrigin( tf::Vector3(1.3435, 0.0, -1.3435) );
    tf::Quaternion q;
    q.setRPY(0, -0.698132, 0);
    trans.setRotation(q);

    pcl_ros::transformPointCloud(target_frame, trans, cloud_o, trans_cloud);

    cloud_pub.publish (trans_cloud);

}


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

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

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

    
    ros::spin ();

}