#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <vector>
#include <ctime>


int main (int argc, char** argv)
{
    srand (time (NULL));  //seeds rand() with the system time 

    time_t begin,end;
    begin = clock();  //开始计时
    //-------------------------------------------------------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    // Generate pointcloud data
    cloud->width = 400000;
    cloud->height = 1;
    cloud->points.resize (cloud->width * cloud->height);

    // fills a PointCloud with random data
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
    }

    // creates kdtree object
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

    // sets our randomly created cloud as the input
    kdtree.setInputCloud (cloud);

    //create a “searchPoint” which is assigned random coordinates
    pcl::PointXYZ searchPoint;

    searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);


    // K nearest neighbor search
    int K = 1;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);

    std::cout << "K nearest neighbor search at (" << searchPoint.x 
        << " " << searchPoint.y 
        << " " << searchPoint.z
        << ") with K=" << K << std::endl;

    /***********************************************************************************************
    template<typename PointT> 
    virtual int pcl::KdTree< PointT >::nearestKSearch  ( const PointT &  p_q,  
                                                        int  k,  
                                                        std::vector< int > &  k_indices,  
                                                        std::vector< float > &  k_sqr_distances  
                                                        )  const [pure virtual] 

    Search for k-nearest neighbors for the given query point. 
    Parameters:
        [in] the given query point 
        [in] k the number of neighbors to search for  
        [out] the resultant indices of the neighboring points
        [out] the resultant squared distances to the neighboring points
    Returns:
        number of neighbors found 
    ********************************************************************************************/
    if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
    {
        for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
            std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x 
            << " " << cloud->points[ pointIdxNKNSearch[i] ].y 
            << " " << cloud->points[ pointIdxNKNSearch[i] ].z 
            << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    }
    //--------------------------------------------------------------------------------------------
    end = clock();  //结束计时
    double Times =  double(end - begin) / CLOCKS_PER_SEC; //将clock()函数的结果转化为以秒为单位的量

    std::cout<<"time: "<<Times<<"s"<<std::endl;

    return 0;
}