#include "WCPData/ToyPointCloud.h"

using namespace WCP;

WCP::ToyPointCloud::ToyPointCloud(double angle_u, double angle_v, double angle_w)
  : angle_u(angle_u)
  , angle_v(angle_v)
  , angle_w(angle_w)
{
  index = 0;
}

WCP::ToyPointCloud::~ToyPointCloud(){
  if (index!=0){
    delete index;
    delete index_u;
    delete index_v;
    delete index_w;
  }
  map_mcell_indices.clear();
  cloud.pts.clear();
  cloud_u.pts.clear();
  cloud_v.pts.clear();
  cloud_w.pts.clear();
}

std::pair<int,double> WCP::ToyPointCloud::get_closest_point_along_vec(Point& p_test1, TVector3& dir, double test_dis, double dis_step, double angle_cut, double dis_cut){
  if (dir.Mag()!=1)
    dir.SetMag(1);
  
  bool flag = false;
  Point p_test;
  double min_dis = 1e9;
  double min_dis1 = 1e9;
  int min_index = -1;
  for (int i=0; i!= int(test_dis/dis_step)+1;i++){
    p_test.x = p_test1.x + dir.X() * i * dis_step;
    p_test.y = p_test1.y + dir.Y() * i * dis_step;
    p_test.z = p_test1.z + dir.Z() * i * dis_step;
    
    WCP::WCPointCloud<double>::WCPoint& closest_pt = get_closest_wcpoint(p_test);
    double dis = sqrt(pow(p_test.x - closest_pt.x,2)+pow(p_test.y - closest_pt.y,2)+pow(p_test.z - closest_pt.z,2));
    double dis1 = sqrt(pow(p_test1.x - closest_pt.x,2)+pow(p_test1.y - closest_pt.y,2)+pow(p_test1.z - closest_pt.z,2));

    // if (fabs(p_test1.x-199.942*units::cm) < 1*units::cm &&
    // 	fabs(p_test1.y+19.4682*units::cm) < 1*units::cm &&
    // 	fabs(p_test1.z-127*units::cm) < 1*units::cm)
    //   std::cout << i << " " << dis/units::cm << " " << std::min(dis1 * tan(angle_cut/180.*3.1415926),dis_cut)/units::cm << " " << p_test.x/units::cm << " " << p_test.y/units::cm << " " << p_test.z/units::cm << std::endl;
	      
    
    if (dis < std::min(dis1 * tan(angle_cut/180.*3.1415926),dis_cut)){
      if (dis < min_dis){
	min_dis = dis;
	min_index = closest_pt.index;
	min_dis1 = dis1;
      }
      if (dis < 3*units::cm)
	return std::make_pair(closest_pt.index,dis1);
    }
  }

  return std::make_pair(min_index,min_dis1);
}

std::tuple<int,int,double> WCP::ToyPointCloud::get_closest_points(ToyPointCloud *point_cloud){

  //std::cout << cloud.pts.size() << " " << point_cloud->get_cloud().pts.size() << std::endl;
   
  
  WCP::WCPointCloud<double>::WCPoint p1 = cloud.pts.front();
  WCP::WCPointCloud<double>::WCPoint p2 = point_cloud->get_cloud().pts.front();
  WCP::WCPointCloud<double>::WCPoint p1_save;
  WCP::WCPointCloud<double>::WCPoint p2_save;
  double min_dis = 1e9;
  
  int prev_index1 = -1;
  int prev_index2 = -1;
  while(p1.index!=prev_index1 || p2.index!=prev_index2){
    prev_index1 = p1.index;
    prev_index2 = p2.index;
    p2 = point_cloud->get_closest_wcpoint(p1);
    p1 = get_closest_wcpoint(p2);
  }
  double dis = sqrt(pow(p1.x-p2.x,2)+pow(p1.y-p2.y,2)+pow(p1.z-p2.z,2));
  if (dis < min_dis){
    min_dis = dis;
    p1_save = p1;
    p2_save = p2;
  }

  prev_index1 = -1;
  prev_index2 = -1;
  p1 = cloud.pts.back();
  p2 = point_cloud->get_cloud().pts.front();
  while(p1.index!=prev_index1 || p2.index!=prev_index2){
    prev_index1 = p1.index;
    prev_index2 = p2.index;
    p2 = point_cloud->get_closest_wcpoint(p1);
    p1 = get_closest_wcpoint(p2);
  }
  dis = sqrt(pow(p1.x-p2.x,2)+pow(p1.y-p2.y,2)+pow(p1.z-p2.z,2));
  if (dis < min_dis){
    min_dis = dis;
    p1_save = p1;
    p2_save = p2;
  }

  prev_index1 = -1;
  prev_index2 = -1;
  p1 = cloud.pts.back();
  p2 = point_cloud->get_cloud().pts.front();
  while(p1.index!=prev_index1 || p2.index!=prev_index2){
    prev_index1 = p1.index;
    prev_index2 = p2.index;
    p1 = get_closest_wcpoint(p2);
    p2 = point_cloud->get_closest_wcpoint(p1);
  }
  dis = sqrt(pow(p1.x-p2.x,2)+pow(p1.y-p2.y,2)+pow(p1.z-p2.z,2));
  if (dis < min_dis){
    min_dis = dis;
    p1_save = p1;
    p2_save = p2;
  }

  prev_index1 = -1;
  prev_index2 = -1;
  p1 = cloud.pts.back();
  p2 = point_cloud->get_cloud().pts.back();
  while(p1.index!=prev_index1 || p2.index!=prev_index2){
    prev_index1 = p1.index;
    prev_index2 = p2.index;
    p1 = get_closest_wcpoint(p2);
    p2 = point_cloud->get_closest_wcpoint(p1);
  }
  dis = sqrt(pow(p1.x-p2.x,2)+pow(p1.y-p2.y,2)+pow(p1.z-p2.z,2));
  if (dis < min_dis){
    min_dis = dis;
    p1_save = p1;
    p2_save = p2;
  }

  
  
  
  return std::make_tuple(p1_save.index,p2_save.index,min_dis);
}

WCP::WCPointCloud<double>::WCPoint& WCP::ToyPointCloud::get_closest_wcpoint(WCP::WCPointCloud<double>::WCPoint& wcp){
  Point p(wcp.x,wcp.y,wcp.z);
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,1);
  return cloud.pts[results.front().first];
}

WCP::WCPointCloud<double>::WCPoint& WCP::ToyPointCloud::get_closest_wcpoint(WCP::Point& p){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,1);
  return cloud.pts[results.front().first];
}


void WCP::ToyPointCloud::AddPoint(WCPointCloud<double>::WCPoint& wcp, WC2DPointCloud<double>::WC2DPoint& wcp_u, WC2DPointCloud<double>::WC2DPoint& wcp_v, WC2DPointCloud<double>::WC2DPoint& wcp_w){
  SlimMergeGeomCell *mcell = wcp.mcell;
  int index = wcp.index;
  
  if (map_mcell_indices.find(mcell)==map_mcell_indices.end()){
    std::vector<int> temp_indices;
    temp_indices.push_back(index);
    map_mcell_indices[mcell] = temp_indices;
  }else{
    map_mcell_indices[mcell].push_back(index);
  }
  

  cloud.pts.push_back(wcp);
  cloud_u.pts.push_back(wcp_u);
  cloud_v.pts.push_back(wcp_v);
  cloud_w.pts.push_back(wcp_w);
  
}


void WCP::ToyPointCloud::AddPoint(Point& p, std::tuple<int,int,int>& wires_index, SlimMergeGeomCell *mcell){
  WCPointCloud<double>::WCPoint point;
  point.x = p.x;
  point.y = p.y;
  point.z = p.z;

  point.index_u = std::get<0>(wires_index);
  point.index_v = std::get<1>(wires_index);
  point.index_w = std::get<2>(wires_index);
    
  point.mcell = mcell;
  point.index =cloud.pts.size();

  WC2DPointCloud<double>::WC2DPoint point_u;
  point_u.x = p.x;
  point_u.y = cos(angle_u) * p.z - sin(angle_u) *p.y;
  point_u.index = cloud_u.pts.size();
  
  WC2DPointCloud<double>::WC2DPoint point_v;
  point_v.x = p.x;
  point_v.y = cos(angle_v) * p.z - sin(angle_v) *p.y;
  point_v.index = cloud_v.pts.size();
  
  WC2DPointCloud<double>::WC2DPoint point_w;
  point_w.x = p.x;
  point_w.y = cos(angle_w) * p.z - sin(angle_w) *p.y;
  point_w.index = cloud_w.pts.size();
  
  if (map_mcell_indices.find(mcell)==map_mcell_indices.end()){
    std::vector<int> temp_indices;
    temp_indices.push_back(point.index);
    map_mcell_indices[mcell] = temp_indices;
  }else{
    map_mcell_indices[mcell].push_back(point.index);
  }
  

  cloud.pts.push_back(point);
  cloud_u.pts.push_back(point_u);
  cloud_v.pts.push_back(point_v);
  cloud_w.pts.push_back(point_w);
  
}

void WCP::ToyPointCloud::AddPoint(WCPointCloud<double>::WCPoint& p){
  WCPointCloud<double>::WCPoint point;
  point.x = p.x;
  point.y = p.y;
  point.z = p.z;

  point.index_u = 0;
  point.index_v = 0;
  point.index_w = 0;
    
  point.mcell = 0;
  point.index =cloud.pts.size();
  
  WC2DPointCloud<double>::WC2DPoint point_u;
  point_u.x = p.x;
  point_u.y = cos(angle_u) * p.z - sin(angle_u) *p.y;
  point_u.index = cloud_u.pts.size();
  WC2DPointCloud<double>::WC2DPoint point_v;
  point_v.x = p.x;
  point_v.y = cos(angle_v) * p.z - sin(angle_v) *p.y;
  point_v.index = cloud_v.pts.size();
  
  WC2DPointCloud<double>::WC2DPoint point_w;
  point_w.x = p.x;
  point_w.y = cos(angle_w) * p.z - sin(angle_w) *p.y;
  point_w.index = cloud_w.pts.size();
  
  cloud.pts.push_back(point);
  cloud_u.pts.push_back(point_u);
  cloud_v.pts.push_back(point_v);
  cloud_w.pts.push_back(point_w);
}

void WCP::ToyPointCloud::AddPoints(PointVector& ps){
  size_t current_size = cloud.pts.size();
  cloud.pts.resize(current_size + ps.size());
  cloud_u.pts.resize(current_size + ps.size());
  cloud_v.pts.resize(current_size + ps.size());
  cloud_w.pts.resize(current_size + ps.size());
  
  for (size_t i=0;i!=ps.size();i++){
    cloud.pts[current_size+i].x = ps.at(i).x;
    cloud.pts[current_size+i].y = ps.at(i).y;
    cloud.pts[current_size+i].z = ps.at(i).z;
    cloud.pts[current_size+i].index_u = 0;
    cloud.pts[current_size+i].index_v = 0;
    cloud.pts[current_size+i].index_w = 0;
    cloud.pts[current_size+i].mcell = 0;
    cloud.pts[current_size+i].index = current_size+i;

    cloud_u.pts[current_size+i].x = ps.at(i).x;
    cloud_u.pts[current_size+i].y = cos(angle_u) * ps.at(i).z - sin(angle_u) *ps.at(i).y;
    cloud_u.pts[current_size+i].index = current_size+i;
    
    cloud_v.pts[current_size+i].x = ps.at(i).x;
    cloud_v.pts[current_size+i].y = cos(angle_v) * ps.at(i).z - sin(angle_v) *ps.at(i).y;
    cloud_v.pts[current_size+i].index = current_size+i;
    
    cloud_w.pts[current_size+i].x = ps.at(i).x;
    cloud_w.pts[current_size+i].y = cos(angle_w) * ps.at(i).z - sin(angle_w) *ps.at(i).y;
    cloud_w.pts[current_size+i].index = current_size+i;
    

  }
}


void WCP::ToyPointCloud::AddPoints(PointVector& ps, std::vector<std::tuple<int,int,int>>& wires_indices, SlimMergeGeomCell *mcell){
  size_t current_size = cloud.pts.size();
  cloud.pts.resize(current_size + ps.size());
  cloud_u.pts.resize(current_size + ps.size());
  cloud_v.pts.resize(current_size + ps.size());
  cloud_w.pts.resize(current_size + ps.size());
  
  
  if (map_mcell_indices.find(mcell)==map_mcell_indices.end()){
    std::vector<int> temp_pts;
    map_mcell_indices[mcell] = temp_pts;
  }
  
  for (size_t i=0;i!=ps.size();i++){
    cloud.pts[current_size+i].x = ps.at(i).x;
    cloud.pts[current_size+i].y = ps.at(i).y;
    cloud.pts[current_size+i].z = ps.at(i).z;
    cloud.pts[current_size+i].index_u = std::get<0>(wires_indices.at(i)) ;
    cloud.pts[current_size+i].index_v = std::get<1>(wires_indices.at(i)) ;
    cloud.pts[current_size+i].index_w = std::get<2>(wires_indices.at(i)) ;
    cloud.pts[current_size+i].mcell = mcell;
    cloud.pts[current_size+i].index = current_size+i;

    cloud_u.pts[current_size+i].x = ps.at(i).x;
    cloud_u.pts[current_size+i].y = cos(angle_u) * ps.at(i).z - sin(angle_u) *ps.at(i).y;
    cloud_u.pts[current_size+i].index = current_size+i;
    
    cloud_v.pts[current_size+i].x = ps.at(i).x;
    cloud_v.pts[current_size+i].y = cos(angle_v) * ps.at(i).z - sin(angle_v) *ps.at(i).y;
    cloud_v.pts[current_size+i].index = current_size+i;
    
    cloud_w.pts[current_size+i].x = ps.at(i).x;
    cloud_w.pts[current_size+i].y = cos(angle_w) * ps.at(i).z - sin(angle_w) *ps.at(i).y;
    cloud_w.pts[current_size+i].index = current_size+i;
    

    map_mcell_indices[mcell].push_back(current_size+i);
  }
}

void WCP::ToyPointCloud::Print(){
  for (size_t i=0; i!=cloud.pts.size(); i++){
    std::cout << i << " " << cloud.pts[i].x << " " << cloud.pts[i].y << " " << cloud.pts[i].z << " " << cloud.pts[i].index_u << " " << cloud.pts[i].index_v << " " << cloud.pts[i].index_w << " " << cloud.pts[i].mcell << std::endl;
  }
}

void WCP::ToyPointCloud::build_kdtree_index(){
  if (index!=(my_kd_tree_t*)0){
    delete index;
    delete index_u;
    delete index_v;
    delete index_w;
  }
  index = new my_kd_tree_t(3 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
  index->buildIndex();

  index_u = new my_kd_tree_2d_t(2, cloud_u, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
  index_u->buildIndex();

  index_v = new my_kd_tree_2d_t(2, cloud_v, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
  index_v->buildIndex();

  index_w = new my_kd_tree_2d_t(2, cloud_w, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
  index_w->buildIndex();
  
}



std::vector<std::pair<size_t,double>> WCP::ToyPointCloud::get_closest_index(WCP::Point& p, int N){
  std::vector<size_t> ret_index(N);
  std::vector<double> out_dist_sqr(N);
  
  double query_pt[3];
  query_pt[0] = p.x;
  query_pt[1] = p.y;
  query_pt[2] = p.z;  
  
  N = index->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  ret_index.resize(N);
  out_dist_sqr.resize(N);
  
  std::vector<std::pair<size_t,double>> results(N);
  for(size_t i=0; i!=N; i++){
    results.at(i) = std::make_pair(ret_index.at(i),out_dist_sqr.at(i));
  }
  
  return results;
}



std::vector<std::pair<size_t,double>> WCP::ToyPointCloud::get_closest_2d_index(double x, double y, int N, int plane){
  std::vector<size_t> ret_index(N);
  std::vector<double> out_dist_sqr(N);
  
  double query_pt[2];
  query_pt[0] = x;
  query_pt[1] = y;
  if (plane==0){
    N = index_u->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else if (plane==1){
    N = index_v->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else{
    N = index_w->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }
  ret_index.resize(N);
  out_dist_sqr.resize(N);
  
  std::vector<std::pair<size_t,double>> results(N);
  for(size_t i=0; i!=N; i++){
    results.at(i) = std::make_pair(ret_index.at(i),out_dist_sqr.at(i));
  }
  
  return results;
}

double WCP::ToyPointCloud::get_closest_2d_dis(double x, double y, int plane){
  double query_pt[2];
  query_pt[0] = x;
  query_pt[1] = y;
  int N = 1;
  std::vector<size_t> ret_index(N);
  std::vector<double> out_dist_sqr(N);
  if (plane==0){
    N = index_u->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else if (plane==1){
    N = index_v->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else{
    N = index_w->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }
  ret_index.resize(N);
  out_dist_sqr.resize(N);

  if (N>0)
    return sqrt(out_dist_sqr[0]);
  else
    return 1e9;
}

std::pair<double, int> WCP::ToyPointCloud::get_closest_2d_dis_index(double x, double y, int plane){
  double query_pt[2];
  query_pt[0] = x;
  query_pt[1] = y;
  int N = 1;
  std::vector<size_t> ret_index(N);
  std::vector<double> out_dist_sqr(N);
  if (plane==0){
    N = index_u->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else if (plane==1){
    N = index_v->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else{
    N = index_w->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }
  ret_index.resize(N);
  out_dist_sqr.resize(N);

  if (N>0)
    return std::make_pair(sqrt(out_dist_sqr[0]),ret_index[0]);
  else
    return std::make_pair(1e9,-1);
}

std::pair<int,double> WCP::ToyPointCloud::get_closest_2d_dis(WCP::Point& p, int plane){
   double x, y;
  if (plane==0){
    x = p.x;
    y = cos(angle_u) * p.z - sin(angle_u) *p.y;
  }else if (plane==1){
    x = p.x;
    y = cos(angle_v) * p.z - sin(angle_v) *p.y;
  }else{
    x = p.x;
    y = cos(angle_w) * p.z - sin(angle_w) *p.y;
  }

  double query_pt[2];
  query_pt[0] = x;
  query_pt[1] = y;
  int N = 1;
  std::vector<size_t> ret_index(N);
  std::vector<double> out_dist_sqr(N);
  if (plane==0){
    N = index_u->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else if (plane==1){
    N = index_v->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else{
    N = index_w->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }
  ret_index.resize(N);
  out_dist_sqr.resize(N);

  if (N>0)
    return std::make_pair(ret_index[0],sqrt(out_dist_sqr[0]));
  else
    return std::make_pair(-1,1e9);
}


std::vector<size_t> WCP::ToyPointCloud::get_closest_2d_index(Point& p, int N, int plane){
  double x, y;
  if (plane==0){
    x = p.x;
    y = cos(angle_u) * p.z - sin(angle_u) *p.y;
  }else if (plane==1){
    x = p.x;
    y = cos(angle_v) * p.z - sin(angle_v) *p.y;
  }else{
    x = p.x;
    y = cos(angle_w) * p.z - sin(angle_w) *p.y;
  }
  
  double query_pt[2];
  query_pt[0] = x;
  query_pt[1] = y;

  std::vector<size_t> ret_index(N);
  std::vector<double> out_dist_sqr(N);
  if (plane==0){
    N = index_u->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else if (plane==1){
    N = index_v->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }else{
    N = index_w->knnSearch(&query_pt[0], N, &ret_index[0], &out_dist_sqr[0]);
  }
  ret_index.resize(N);
  out_dist_sqr.resize(N);
  
  return ret_index;
}




std::vector<std::pair<size_t,double>> WCP::ToyPointCloud::get_closest_index(WCP::Point& p, double search_radius){
  double query_pt[3];
  query_pt[0] = p.x;
  query_pt[1] = p.y;
  query_pt[2] = p.z;
  std::vector<std::pair<size_t,double> >   ret_matches;
  nanoflann::SearchParams params;
  const size_t nMatches = index->radiusSearch(&query_pt[0], search_radius*search_radius, ret_matches, params);
  
  return ret_matches;
}


std::vector<std::pair<size_t,double>> WCP::ToyPointCloud::get_closest_2d_index(double x, double y, double search_radius, int plane){
  double query_pt[2];
  query_pt[0] = x;
  query_pt[1] = y;
  std::vector<std::pair<size_t,double> >   ret_matches;
  nanoflann::SearchParams params;
  if (plane ==0){
    const size_t nMatches = index_u->radiusSearch(&query_pt[0], search_radius*search_radius, ret_matches, params);
  }else if (plane==1){
    const size_t nMatches = index_v->radiusSearch(&query_pt[0], search_radius*search_radius, ret_matches, params);
  }else{
    const size_t nMatches = index_w->radiusSearch(&query_pt[0], search_radius*search_radius, ret_matches, params);
  }
  return ret_matches;
}

std::vector<size_t> WCP::ToyPointCloud::get_closest_2d_index(Point& p, double search_radius, int plane){
  double x, y;
  if (plane==0){
    x = p.x;
    y = cos(angle_u) * p.z - sin(angle_u) *p.y;
  }else if (plane==1){
    x = p.x;
    y = cos(angle_v) * p.z - sin(angle_v) *p.y;
  }else{
    x = p.x;
    y = cos(angle_w) * p.z - sin(angle_w) *p.y;
  }
  
  double query_pt[2];
  query_pt[0] = x;
  query_pt[1] = y;
  std::vector<std::pair<size_t,double>>   ret_matches;
  nanoflann::SearchParams params;
  if (plane ==0){
    const size_t nMatches = index_u->radiusSearch(&query_pt[0], search_radius*search_radius, ret_matches, params);
  }else if (plane==1){
    const size_t nMatches = index_v->radiusSearch(&query_pt[0], search_radius*search_radius, ret_matches, params);
  }else{
    const size_t nMatches = index_w->radiusSearch(&query_pt[0], search_radius*search_radius, ret_matches, params);
  }
  std::vector<size_t> ret_index(ret_matches.size());
  for (size_t i=0;i!=ret_matches.size();i++){
    ret_index.at(i) = ret_matches.at(i).first;
  }
  
  return ret_index;
}


std::map<WCP::SlimMergeGeomCell*, Point> WCP::ToyPointCloud::get_closest_mcell(WCP::Point& p, int N){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,N);
  
  std::map<WCP::SlimMergeGeomCell*, Point> mcell_point_map;
  std::map<WCP::SlimMergeGeomCell*, double> mcell_dis_map;
  
  for (auto it = results.begin(); it!= results.end(); it++){
    size_t index = (*it).first;
    double dis = (*it).second;
    SlimMergeGeomCell *mcell = cloud.pts[index].mcell;
    Point p;
    p.x = cloud.pts[index].x;
    p.y = cloud.pts[index].y;
    p.z = cloud.pts[index].z;
    
    if (mcell_dis_map.find(mcell)==mcell_dis_map.end()){
      mcell_dis_map[mcell]=dis;
      mcell_point_map[mcell] = p;
    }else{
      if (dis < mcell_dis_map[mcell]){
	mcell_dis_map[mcell]=dis;
	mcell_point_map[mcell]=p;
      }
    }
  }
  
  return mcell_point_map;
}


std::map<WCP::SlimMergeGeomCell*, Point> WCP::ToyPointCloud::get_closest_mcell(WCP::Point& p, double search_radius){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,search_radius);
  
  std::map<WCP::SlimMergeGeomCell*, Point> mcell_point_map;
  std::map<WCP::SlimMergeGeomCell*, double> mcell_dis_map;
  
  for (auto it = results.begin(); it!= results.end(); it++){
    size_t index = (*it).first;
    double dis = (*it).second;

    //    std::cout << index << " " << dis/units::cm << std::endl;
    
    SlimMergeGeomCell *mcell = cloud.pts[index].mcell;
    Point p1;
    p1.x = cloud.pts[index].x;
    p1.y = cloud.pts[index].y;
    p1.z = cloud.pts[index].z;

    // Point pc = mcell->center();
    // std::cout << index << " " << dis/units::cm << " " << sqrt(pow(p1.x-p.x,2)+pow(p1.y-p.y,2)+pow(p1.z-p.z,2))/units::cm << " " << sqrt(pow(pc.x-p.x,2)+pow(pc.y-p.y,2)+pow(pc.z-p.z,2))/units::cm << std::endl;
    
    if (mcell_dis_map.find(mcell)==mcell_dis_map.end()){
      mcell_dis_map[mcell]=dis;
      mcell_point_map[mcell] = p1;
    }else{
      if (dis < mcell_dis_map[mcell]){
	mcell_dis_map[mcell]=dis;
	mcell_point_map[mcell]=p1;
      }
    }
  }
  
  return mcell_point_map;
}

double WCP::ToyPointCloud::get_closest_dis(WCP::Point& p){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,1);
  return sqrt(results.front().second);
}
std::pair<double, WCP::Point> WCP::ToyPointCloud::get_closest_point(WCP::Point& p){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,1);
  Point p1;
  int index = results.front().first;
  p1.x =  cloud.pts[index].x;
  p1.y =  cloud.pts[index].y;
  p1.z =  cloud.pts[index].z;
  return std::make_pair(sqrt(results.front().second),p1);
}

std::vector<WCP::WCPointCloud<double>::WCPoint >  WCP::ToyPointCloud::get_closest_wcpoints(WCP::Point& p, int N){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,N);
  std::vector<WCP::WCPointCloud<double>::WCPoint > results_1;
  for (auto it = results.begin(); it!= results.end(); it++){
    size_t index = (*it).first;
    results_1.push_back(cloud.pts[index]);
  }
  return results_1;
}

WCP::Point WCP::ToyPointCloud::get_center_point_radius(WCP::Point& p, double radius){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,radius);
  WCP::Point center(0,0,0);
  int ncount = 0;
  for (auto it = results.begin(); it!= results.end(); it++){
    size_t index = (*it).first;
    center.x += cloud.pts[index].x;
    center.y += cloud.pts[index].y;
    center.z += cloud.pts[index].z;
    ncount ++;
  }
  if (ncount >0){
    center.x /= ncount;
    center.y /= ncount;
    center.z /= ncount;
  }
  return center; 
}

std::vector<WCP::WCPointCloud<double>::WCPoint >  WCP::ToyPointCloud::get_closest_wcpoints(WCP::Point& p, double radius){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,radius);
  std::vector<WCP::WCPointCloud<double>::WCPoint > results_1;
  for (auto it = results.begin(); it!= results.end(); it++){
    size_t index = (*it).first;
    results_1.push_back(cloud.pts[index]);
  }
  return results_1;
}


std::vector<std::pair<WCP::SlimMergeGeomCell*,Point>> WCP::ToyPointCloud::get_closest_points(WCP::Point& p, int N){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,N);
  std::vector<std::pair<WCP::SlimMergeGeomCell*,Point>> points;
  for (auto it = results.begin(); it!= results.end(); it++){
    size_t index = (*it).first;
    Point p;
    p.x = cloud.pts[index].x;
    p.y = cloud.pts[index].y;
    p.z = cloud.pts[index].z;
    SlimMergeGeomCell *mcell = cloud.pts[index].mcell;
    points.push_back(std::make_pair(mcell,p));
  }
  return points;
}
std::vector<std::pair<WCP::SlimMergeGeomCell*,Point>> WCP::ToyPointCloud::get_closest_points(WCP::Point& p, double search_radius){
  std::vector<std::pair<size_t,double>> results = get_closest_index(p,search_radius);
  std::vector<std::pair<WCP::SlimMergeGeomCell*,Point>> points;
  for (auto it = results.begin(); it!= results.end(); it++){
    size_t index = (*it).first;
    Point p;
    p.x = cloud.pts[index].x;
    p.y = cloud.pts[index].y;
    p.z = cloud.pts[index].z;
    SlimMergeGeomCell *mcell = cloud.pts[index].mcell;
    points.push_back(std::make_pair(mcell,p));
  }

  return points;
}



std::vector<WCPointCloud<double>::WCPoint> WCP::ToyPointCloud::get_hull(){
  quickhull::QuickHull<float> qh;
  std::vector<quickhull::Vector3<float>> pc;
  // std::cout << cloud.pts.size() << std::endl;
  for (size_t i=0;i!=cloud.pts.size();i++){
    // std::cout << cloud.pts.at(i).x << " " << cloud.pts.at(i).y << " " << cloud.pts.at(i).z << std::endl;
    pc.emplace_back(cloud.pts.at(i).x,cloud.pts.at(i).y,cloud.pts.at(i).z);
  }
  quickhull::ConvexHull<float> hull = qh.getConvexHull(pc,false,true);
  std::set<int> indices;
  
  for (size_t i=0;i!=hull.getIndexBuffer().size();i++){
    indices.insert(hull.getIndexBuffer().at(i));
  }
  
  std::vector<WCPointCloud<double>::WCPoint> results;
  //std::copy(indices.begin(),indices.end(),results.begin());
  for (auto it = indices.begin(); it!=indices.end(); it++){
    //std::cout << pc.at(*it).x << " " << pc.at(*it).y <<  " " << pc.at(*it).z << std::endl;
  // Point p;
  // p.x = cloud.pts.at(*it).x;
  // p.y = cloud.pts.at(*it).y;
  // p.z = cloud.pts.at(*it).z;
  // SlimMergeGeomCell *mcell = cloud.pts.at(*it).mcell;
  // results.push_back(std::make_pair(mcell,p));
  //results.push_back(*it);
    results.push_back(cloud.pts.at(*it));
  }
  return results;
}
