#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <pcl/features/normal_3d.h>

#include <pcl/search/kdtree.h>
#include <pcl/search/search.h>
#include <pcl/filters/filter.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>

#include <boost/make_shared.hpp>

#include <iostream>
#include <vector>


int main(int argc, char **argv){

    //load pcd files into clouds
    //test at first with correctly rotated clouds and then with rotated clouds
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourceCloud (new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetCloud (new pcl::PointCloud<pcl::PointXYZRGB>());

    int errNo1 = pcl::io::loadPCDFile("bun01.pcd", *sourceCloud);
    int errNo2 = pcl::io::loadPCDFile("bun02.pcd", *targetCloud);

    if(errNo1){
        PCL_ERROR ("oops, some error occured loading file .pcd file");
        return (-1);
    }

    if(errNo2){
        PCL_ERROR ("oops, some error occured loading .pcd file");
        return (-1);
    }


    // Create the normal estimation class, and pass the input dataset to it
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    ne.setInputCloud(sourceCloud);

    // Create an empty kdtree representation, and pass it to the normal estimation object.
    // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given)
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
    ne.setSearchMethod (tree);

    // Output datasets
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);

    Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity ();
    Eigen::Matrix4f pairTransform = Eigen::Matrix4f::Identity();

    // Use all neighbors in a sphere of radius 30cm
    ne.setRadiusSearch (0.30);
    // Compute the features
    ne.compute (*normals);

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> rgbSrc(sourceCloud, 125, 50, 255);   //purple color
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> rgbTgt(sourceCloud, 0, 0, 255);
    //test with normals
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("Normal viewer"));

    viewer->setBackgroundColor (0, 0, 0);

    int level = 1;
    double scale = 0.01;

    viewer->addPointCloud(sourceCloud, rgbSrc, "sample cloud");
    //uncomment if you want to see normals
    //viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(inputCloud, normals, level, scale, "normals");

    viewer->addCoordinateSystem (0.15);

    /*pcl::ModelCoefficients coeffs;

    coeffs.values.push_back(1.0);
    coeffs.values.push_back(0.0);
    coeffs.values.push_back(0.1);
    coeffs.values.push_back(1.0);

    viewer->addPlane(coeffs, "plane");*/

    //next register and transform on each iteration
    pcl::IterativeClosestPointNonLinear<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr
            reg (new pcl::IterativeClosestPointNonLinear<pcl::PointXYZRGB, pcl::PointXYZRGB>());

    reg->setTransformationEpsilon(1e-6);
    reg->setMaxCorrespondenceDistance (0.1);

    while(!viewer->wasStopped()){
        viewer->spinOnce(10);
    }

    /* if(kdtree->radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)){

         for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
             std::cout << "    "  <<   bunnySrc->points[ pointIdxRadiusSearch[i] ].x
             << " " << bunnySrc->points[ pointIdxRadiusSearch[i] ].y
             << " " << bunnySrc->points[ pointIdxRadiusSearch[i] ].z
             << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
             }
         }*/

    return (0);
}