#include "WCPNanoflann/nanoflann.h" 
#include "WCPData/WCPointCloud.h"

#include "TRandom.h"

using namespace WCP;

int main()
{
  WCPointCloud<double> cloud;
  cloud.pts.resize(1000);
  for (size_t i = 0; i < 1000; i++)
    {
      cloud.pts[i].x = gRandom->Uniform(-100,100);
      cloud.pts[i].y = gRandom->Uniform(-100,100);
      cloud.pts[i].z = gRandom->Uniform(-100,100);
      SlimMergeGeomCell *cell = new SlimMergeGeomCell(i);
      cloud.pts[i].mcell = cell;
    }

  typedef nanoflann::KDTreeSingleIndexAdaptor<
    nanoflann::L2_Simple_Adaptor<double, WCPointCloud<double> > ,
    WCPointCloud<double>,
    3 /* dim */
    > my_kd_tree_t;

  my_kd_tree_t   index(3 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
  index.buildIndex();

  const double query_pt[3] = { 0.5, 0.5, 0.5};

  {
    size_t num_results = 5;
    std::vector<size_t>   ret_index(num_results);
    std::vector<double>   out_dist_sqr(num_results);
    
    num_results = index.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]);
    
    // In case of less points in the tree than requested:
    ret_index.resize(num_results);
    out_dist_sqr.resize(num_results);
    
    std::cout << "knnSearch(): num_results=" << num_results << "\n";
    for (size_t i = 0; i < num_results; i++){
      // int abc = cloud.pts[ret_index[i]].mcell;
      std::cout << "idx["<< i << "]=" << ret_index[i] << " dist["<< i << "]=" << out_dist_sqr[i] << " " << sqrt(pow(cloud.pts[ret_index[i]].x-0.5,2)+pow(cloud.pts[ret_index[i]].y-0.5,2)+pow(cloud.pts[ret_index[i]].z-0.5,2)) << " " << cloud.pts[ret_index[i]].mcell->GetIdent() << std::endl;
    }
    std::cout << "\n";
    
  }

  // ----------------------------------------------------------------
  // radiusSearch():  Perform a search for the N closest points
  // ----------------------------------------------------------------
  {
  	const double search_radius = 100;
  	std::vector<std::pair<size_t,double> >   ret_matches;
  
  	nanoflann::SearchParams params;
  	//params.sorted = false;
  
  	const size_t nMatches = index.radiusSearch(&query_pt[0], search_radius, ret_matches, params);
  
	std::cout << "radiusSearch(): radius=" << search_radius << " -> " << nMatches << " matches\n";
  	for (size_t i = 0; i < nMatches; i++)
	  std::cout << "idx["<< i << "]=" << ret_matches[i].first << " dist["<< i << "]=" << ret_matches[i].second << " " << cloud.pts[ret_matches[i].first].mcell->GetIdent()  << std::endl;
	std::cout << "\n";
  }

  
  return 0;
}
