#include <cmath>

#include <loam_velodyne/common.h>
#include <nav_msgs/Odometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
//#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/ndt.h>
#include <geometry_msgs/PoseStamped.h>
#include <ndt_omp.h>

using std::sin;
using std::cos;
using std::atan2;
bool first = true;
ros::Publisher pub1, pub2;
sensor_msgs::PointCloud2 first_cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>());
void ndtcallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
    sensor_msgs::PointCloud2 res_cloud;
    if(first)
    {
        first_cloud = *msg;
        first_cloud.header.frame_id = "/laser";
        pcl::fromROSMsg(first_cloud, *target_cloud);
        first = false;
        return;
    }
    pcl::fromROSMsg(*msg, *input_cloud);
    int n = 3;
    ndt_omp->setInputTarget(target_cloud);
    ndt_omp->setInputCloud(input_cloud);
    ndt_omp->setNumThreads(n);
    ndt_omp->setTransformationEpsilon(0.002);
    ndt_omp->setStepSize(0.05);
    ndt_omp->setResolution(0.3);
    ndt_omp->setMaximumIterations(100);
    ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
    Eigen::AngleAxisf init_rotation(0, Eigen::Vector3f::UnitZ()); 
    Eigen::Translation3f init_translation(0, 0, 0); 
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ndt_omp->align(*output_cloud, init_guess);
    Eigen::Matrix4f globalPose=ndt_omp->getFinalTransformation();
    std::cout<<globalPose<<std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ndtomp_test");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("cloud2", 1000, ndtcallback);
    pub1 = nh.advertise<sensor_msgs::PointCloud2>("cloud_1",1000);
    pub2 = nh.advertise<sensor_msgs::PointCloud2>("cloud_2",1000);
    ros::spin();
    return 0;
}
