#include "WCPData/PR3DCluster.h"
#include "WCPData/TPCParams.h"
#include "WCPData/Singleton.h"

#include "TMatrixDEigen.h"
#include "TH2F.h"
#include "TVector3.h"

#include <boost/graph/connected_components.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>

using namespace WCP;

#include "PR3DCluster_dQ_dx_fit.h"
#include "PR3DCluster_trajectory_fit.h"

PR3DCluster::PR3DCluster(int cluster_id)
    : cluster_id(cluster_id)
{
    point_cloud = 0;
    graph = 0;
    source_wcp_index = -1;
    flag_fine_tracking = false;
    flag_PCA = false;
}

PR3DCluster::~PR3DCluster()
{
    if (point_cloud != (ToyPointCloud *)0)
        delete point_cloud;
    if (graph != (MCUGraph *)0)
        delete graph;
}

void PR3DCluster::Del_graph()
{
    if (graph != (MCUGraph *)0)
    {
        delete graph;
        graph = 0;

        //    std::cout << "Del graph! " << cluster_id << std::endl;
    }
}

void PR3DCluster::get_projection(std::vector<int> &proj_channel, std::vector<int> &proj_timeslice, std::vector<int> &proj_charge, std::vector<int> &proj_charge_err, std::vector<int> &proj_flag, std::map<int, std::map<const GeomWire *, SMGCSelection>> &global_wc_map)
{
    // std::vector<int> proj_charge_err;

    std::set<SlimMergeGeomCell *> cluster_mcells_set;
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = *it;
        cluster_mcells_set.insert(mcell);
    }

    std::set<std::pair<int, int>> saved_time_channel;

    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = *it;
        int time_slice = mcell->GetTimeSlice();

        std::map<const GeomWire *, SMGCSelection> &timeslice_wc_map = global_wc_map[time_slice];

        GeomWireSelection &uwires = mcell->get_uwires();
        GeomWireSelection &vwires = mcell->get_vwires();
        GeomWireSelection &wwires = mcell->get_wwires();

        bool flag_reg_save = true;

        std::vector<WirePlaneType_t> bad_planes = mcell->get_bad_planes();
        if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(0)) == bad_planes.end())
        {
            int num_shared_wires = 0;
            for (int i = 0; i != uwires.size(); i++)
            {
                const GeomWire *wire = uwires.at(i);

                if (timeslice_wc_map[wire].size() > 1)
                {
                    for (auto it1 = timeslice_wc_map[wire].begin(); it1 != timeslice_wc_map[wire].end(); it1++)
                    {
                        SlimMergeGeomCell *mcell1 = *it1;
                        if (cluster_mcells_set.find(mcell1) == cluster_mcells_set.end())
                            num_shared_wires++;
                    }
                    //
                }
            }
            if (num_shared_wires > 0.15 * uwires.size() && num_shared_wires > 1)
                flag_reg_save = false;
        }
        else
        {
            flag_reg_save = false;
        }

        if (flag_reg_save)
        {
            for (int i = 0; i != uwires.size(); i++)
            {
                const GeomWire *wire = uwires.at(i);
                int ch = wire->channel();
                // regular cases ...
                int charge = mcell->Get_Wire_Charge(wire);
                int charge_err = mcell->Get_Wire_Charge_Err(wire);

                if (saved_time_channel.find(std::make_pair(time_slice, ch)) == saved_time_channel.end())
                {
                    proj_channel.push_back(ch);
                    proj_timeslice.push_back(time_slice);
                    proj_charge.push_back(charge);
                    proj_charge_err.push_back(charge_err);
                    proj_flag.push_back(1);
                    saved_time_channel.insert(std::make_pair(time_slice, ch));
                }
            }
        }
        else
        {
            for (int i = 0; i != uwires.size(); i++)
            {
                const GeomWire *wire = uwires.at(i);
                int ch = wire->channel();
                int temp_flag = 1;
                int charge = mcell->Get_Wire_Charge(wire);
                int charge_err = mcell->Get_Wire_Charge_Err(wire);

                if (charge <= 0)
                {
                    charge = mcell->get_q() * 1.0 / uwires.size();
                    charge_err = sqrt(pow(charge * 0.1, 2) + pow(600, 2)); // assume 30% error
                    temp_flag = 0;
                }

                //	if(cluster_id==18)
                // std::cout << ch << " " << time_slice << " " << charge << std::endl;
                //	if (saved_time_channel.find(std::make_pair(time_slice,ch))==saved_time_channel.end()){
                proj_channel.push_back(ch);
                proj_timeslice.push_back(time_slice);
                proj_charge.push_back(charge);
                proj_charge_err.push_back(charge_err);
                proj_flag.push_back(temp_flag);
                // saved_time_channel.insert(std::make_pair(time_slice,ch));
                //}
            }
        }

        flag_reg_save = true;
        if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(1)) == bad_planes.end())
        {
            int num_shared_wires = 0;
            for (int i = 0; i != vwires.size(); i++)
            {
                const GeomWire *wire = vwires.at(i);

                for (auto it1 = timeslice_wc_map[wire].begin(); it1 != timeslice_wc_map[wire].end(); it1++)
                {
                    SlimMergeGeomCell *mcell1 = *it1;
                    if (cluster_mcells_set.find(mcell1) == cluster_mcells_set.end())
                        num_shared_wires++;
                }

                //	if (timeslice_wc_map[wire].size()>1)
                //  num_shared_wires ++;
            }
            if (num_shared_wires > 0.15 * vwires.size() && num_shared_wires > 1)
                flag_reg_save = false;
        }
        else
        {
            flag_reg_save = false;
        }

        if (flag_reg_save)
        {
            for (int i = 0; i != vwires.size(); i++)
            {
                const GeomWire *wire = vwires.at(i);
                int ch = wire->channel();
                int charge = mcell->Get_Wire_Charge(wire);
                int charge_err = mcell->Get_Wire_Charge_Err(wire);

                if (saved_time_channel.find(std::make_pair(time_slice, ch)) == saved_time_channel.end())
                {
                    proj_channel.push_back(ch);
                    proj_timeslice.push_back(time_slice);
                    proj_charge.push_back(charge);
                    proj_charge_err.push_back(charge_err);
                    proj_flag.push_back(1);
                    saved_time_channel.insert(std::make_pair(time_slice, ch));
                }
            }
        }
        else
        {
            for (int i = 0; i != vwires.size(); i++)
            {
                const GeomWire *wire = vwires.at(i);
                int ch = wire->channel();
                int temp_flag = 1;
                int charge = mcell->Get_Wire_Charge(wire);
                int charge_err = mcell->Get_Wire_Charge_Err(wire);
                if (charge <= 0)
                {
                    charge = mcell->get_q() * 1.0 / vwires.size();
                    charge_err = sqrt(pow(charge * 0.1, 2) + pow(600, 2));
                    temp_flag = 0;
                }

                // if (saved_time_channel.find(std::make_pair(time_slice,ch))==saved_time_channel.end()){
                proj_channel.push_back(ch);
                proj_timeslice.push_back(time_slice);
                proj_charge.push_back(charge);
                proj_charge_err.push_back(charge_err);
                proj_flag.push_back(temp_flag);
                // saved_time_channel.insert(std::make_pair(time_slice,ch));
                // }
            }
        }

        flag_reg_save = true;
        if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(2)) == bad_planes.end())
        {
            int num_shared_wires = 0;
            for (int i = 0; i != wwires.size(); i++)
            {
                const GeomWire *wire = wwires.at(i);

                for (auto it1 = timeslice_wc_map[wire].begin(); it1 != timeslice_wc_map[wire].end(); it1++)
                {
                    SlimMergeGeomCell *mcell1 = *it1;
                    if (cluster_mcells_set.find(mcell1) == cluster_mcells_set.end())
                        num_shared_wires++;
                }

                //	if (timeslice_wc_map[wire].size()>1)
                // num_shared_wires ++;
            }
            if (num_shared_wires > 0.15 * wwires.size() && num_shared_wires > 1)
                flag_reg_save = false;
        }
        else
        {
            flag_reg_save = false;
        }

        if (flag_reg_save)
        {
            for (int i = 0; i != wwires.size(); i++)
            {
                const GeomWire *wire = wwires.at(i);
                int ch = wire->channel();
                int charge = mcell->Get_Wire_Charge(wire);
                int charge_err = mcell->Get_Wire_Charge_Err(wire);

                if (saved_time_channel.find(std::make_pair(time_slice, ch)) == saved_time_channel.end())
                {
                    proj_channel.push_back(ch);
                    proj_timeslice.push_back(time_slice);
                    proj_charge.push_back(charge);
                    proj_charge_err.push_back(charge_err);
                    proj_flag.push_back(1);
                    saved_time_channel.insert(std::make_pair(time_slice, ch));
                }
            }
        }
        else
        {
            for (int i = 0; i != wwires.size(); i++)
            {
                const GeomWire *wire = wwires.at(i);
                int ch = wire->channel();
                int temp_flag = 1;
                int charge = mcell->Get_Wire_Charge(wire);
                int charge_err = mcell->Get_Wire_Charge_Err(wire);
                if (charge <= 0)
                {
                    charge = mcell->get_q() * 1.0 / wwires.size();
                    charge_err = sqrt(pow(charge * 0.1, 2) + pow(100, 2));
                    temp_flag = 0;
                }
                //	if (saved_time_channel.find(std::make_pair(time_slice,ch))==saved_time_channel.end()){
                proj_channel.push_back(ch);
                proj_timeslice.push_back(time_slice);
                proj_charge.push_back(charge);
                proj_charge_err.push_back(charge_err);
                proj_flag.push_back(temp_flag);
                //	  saved_time_channel.insert(std::make_pair(time_slice,ch));
                //	}
            }
        }
    } // loop over mcells

    for (auto it = collected_charge_map.begin(); it != collected_charge_map.end(); it++)
    {
        int time_slice = it->first.first;
        int ch = it->first.second;
        int charge = it->second.first;
        int charge_err = it->second.second;
        proj_channel.push_back(ch);
        proj_timeslice.push_back(time_slice);
        proj_charge.push_back(charge);
        proj_charge_err.push_back(charge_err);
        proj_flag.push_back(1);
    }
}

// bool PR3DCluster::check_neutrino_candidate(WCPointCloud<double>::WCPoint& wcp1 ,WCPointCloud<double>::WCPoint& wcp2, Point& p_vertex, double& angle_change){
//   Create_graph();
//   dijkstra_shortest_paths(wcp1);
//   cal_shortest_path(wcp2);

//   // fine_tracking();

//   PointVector path_wcps_vec;
//   // if (fine_tracking_path.size()==0){
//   double low_dis_limit = 0.5*units::cm;
//   for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
//     if (path_wcps_vec.size()==0){
//       Point p((*it).x,(*it).y,(*it).z);
//       path_wcps_vec.push_back(p);
//     }else{
//       double dis = sqrt(pow((*it).x - path_wcps_vec.back().x,2)
// 			+pow((*it).y - path_wcps_vec.back().y,2)
// 			+pow((*it).z - path_wcps_vec.back().z,2));
//       if (dis > low_dis_limit){
// 	Point p((*it).x,(*it).y,(*it).z);
// 	path_wcps_vec.push_back(p);
//       }
//     }
//   }
//   // }else{
//   //   path_wcps_vec = fine_tracking_path;
//   // }

//   // if (cluster_id == 13){
//   //   std::cout << wcp1.x/units::cm << " " << wcp1.y/units::cm << " " << wcp1.z/units::cm << " " << wcp2.x/units::cm << " " << wcp2.y/units::cm << " " << wcp2.z/units::cm << std::endl;
//   int count = 0;
//   TVector3 drift_dir(1,0,0);
//   for (size_t i=5;i+5<path_wcps_vec.size();i++){
//     TVector3 dir1(path_wcps_vec.at(i).x - path_wcps_vec.at(i-5).x,
// 		  path_wcps_vec.at(i).y - path_wcps_vec.at(i-5).y,
// 		  path_wcps_vec.at(i).z - path_wcps_vec.at(i-5).z);
//     TVector3 dir2(path_wcps_vec.at(i).x - path_wcps_vec.at(i+5).x,
// 		  path_wcps_vec.at(i).y - path_wcps_vec.at(i+5).y,
// 		  path_wcps_vec.at(i).z - path_wcps_vec.at(i+5).z);

//     TVector3 dir3, dir4, dir5, dir6;
//     {
//       PointVector pts;
//       double temp_x = 0;
//       double temp_y = 0;
//       double temp_z = 0;
//       double temp_count = 0;
//       for (size_t j=1;j!=15;j++){
//        	if (i>=j){
// 	  Point pt(path_wcps_vec.at(i-j).x,path_wcps_vec.at(i-j).y,path_wcps_vec.at(i-j).z);

// 	  if (j<=12&&j>2){
// 	    temp_x += pt.x;
// 	    temp_y += pt.y;
// 	    temp_z += pt.z;
// 	    temp_count ++;
// 	  }
// 	  pts.push_back(pt);
// 	}
// 	Point pt(path_wcps_vec.at(i).x,path_wcps_vec.at(i).y,path_wcps_vec.at(i).z);
// 	dir3 = calc_PCA_dir(pt,pts);
// 	dir5.SetXYZ(temp_x/temp_count - path_wcps_vec.at(i).x,
// 		    temp_y/temp_count - path_wcps_vec.at(i).y,
// 		    temp_z/temp_count - path_wcps_vec.at(i).z);
// 	if (dir3.Angle(dir1)>3.1415926/2.)
// 	  dir3 *= -1;
//       }
//     }
//     {
//       PointVector pts;
//       double temp_x = 0;
//       double temp_y = 0;
//       double temp_z = 0;
//       double temp_count = 0;
//       for (size_t j=1;j!=15;j++){
// 	if (i+j<path_wcps_vec.size()){
// 	  Point pt(path_wcps_vec.at(i+j).x,path_wcps_vec.at(i+j).y,path_wcps_vec.at(i+j).z);
// 	  if (j<=12&&j>2){
// 	    temp_x += pt.x;
// 	    temp_y += pt.y;
// 	    temp_z += pt.z;
// 	    temp_count ++;
// 	  }
// 	  pts.push_back(pt);
// 	}
//       }
//       Point pt(path_wcps_vec.at(i).x,path_wcps_vec.at(i).y,path_wcps_vec.at(i).z);
//       dir4 = calc_PCA_dir(pt,pts);
//       dir6.SetXYZ(temp_x/temp_count - path_wcps_vec.at(i).x,
//       		  temp_y/temp_count - path_wcps_vec.at(i).y,
//       		  temp_z/temp_count - path_wcps_vec.at(i).z);
//       if (dir4.Angle(dir2)>3.1415926/2.)
// 	dir4 *= -1;
//     }

//     int cut1 = 0;
//     if ((3.1415926 - dir1.Angle(dir2))/3.1415926*180.>30) cut1++;
//     if ((3.1415926 - dir3.Angle(dir4))/3.1415926*180.>30) cut1++;
//     if ((3.1415926 - dir5.Angle(dir6))/3.1415926*180.>30) cut1++;
//     int cut2 = 0;
//     if (fabs(3.1415926/2.-drift_dir.Angle(dir1-dir2))/3.1415926*180. > 5) cut2++;
//     if (fabs(3.1415926/2.-drift_dir.Angle(dir3-dir4))/3.1415926*180. > 5) cut2++;
//     if (fabs(3.1415926/2.-drift_dir.Angle(dir5-dir6))/3.1415926*180. > 5) cut2++;

//     std::cout << i << " " << path_wcps_vec.at(i).x/units::cm << " " << path_wcps_vec.at(i).y/units::cm << " " << path_wcps_vec.at(i).z/units::cm << " " << (3.1415926 - dir1.Angle(dir2))/3.1415926*180. << " " << (3.1415926 - dir3.Angle(dir4))/3.1415926*180. << " " << (3.1415926 - dir5.Angle(dir6))/3.1415926*180. << " " << fabs(3.1415926/2.-drift_dir.Angle(dir1-dir2))/3.1415926*180. << " " << fabs(3.1415926/2.-drift_dir.Angle(dir3-dir4))/3.1415926*180. << " " << fabs(3.1415926/2.-drift_dir.Angle(dir5-dir6))/3.1415926*180. << " " << cut1 << " " << cut2 << std::endl;

//     // {
//     //   TVector3 dir_diff = dir3-dir4; // difference vector
//     //   TVector3 dir_cros = drift_dir.Cross(dir_diff); // find the crossing term
//     //   TVector3 dir_cros1 = dir_diff.Cross(dir_cros); // find the third dimension
//     //   TVector3 dir3_p = dir3 - dir3.Dot(dir_cros1)/dir_cros1.Mag2()*dir_cros1; // find projection ...
//     //   double angle1 = dir3_p.Angle(dir_diff)/3.1415926*180.;

//     //   //      double angle1 = dir3.Angle(dir_cros)/3.1415926*180.;
//     //   // double angle2 = dir4.Angle(dir_cros)/3.1415926*180.;

//     //   std::cout << i << " " << path_wcps_vec.at(i).x/units::cm << " " << path_wcps_vec.at(i).y/units::cm << " " << path_wcps_vec.at(i).z/units::cm << " " << (3.1415926-dir1.Angle(dir2))/3.1415926*180.<< " " << dir1.Angle(dir3)/3.1415926*180. << " " << dir2.Angle(dir4)/3.1415926*180 << " " << (3.1415926 - dir3.Angle(dir4))/3.1415926*180. <<" " << drift_dir.Angle(dir3-dir4)/3.1415926*180.<< " " << angle1 <<  std::endl;
//     // }
//     if (cut1>=3 && cut2>=2){
//       count ++;
//       if (count >=3){
// 	TVector3 temp1(path_wcps_vec.at(i).x-wcp1.x,
// 		       path_wcps_vec.at(i).y-wcp1.y,
// 		       path_wcps_vec.at(i).z-wcp1.z);
// 	TVector3 temp2(path_wcps_vec.at(i).x-wcp2.x,
// 		       path_wcps_vec.at(i).y-wcp2.y,
// 		       path_wcps_vec.at(i).z-wcp2.z);
// 	std::cout << "A: " << (3.1415926-temp1.Angle(temp2))/3.1415926*180. << " " << temp1.Mag()/units::cm << " " << temp2.Mag()/units::cm << std::endl;

// 	if ((3.1415926-temp1.Angle(temp2))/3.1415926*180. >30 ||
// 	    (3.1415926-temp1.Angle(temp2))/3.1415926*180. >25 && temp1.Mag()>15*units::cm && temp2.Mag()>15*units::cm){
// 	  p_vertex.x = path_wcps_vec.at(i).x;
// 	  p_vertex.y = path_wcps_vec.at(i).y;
// 	  p_vertex.z = path_wcps_vec.at(i).z;
// 	  angle_change = (3.1415926-temp1.Angle(temp2))/3.1415926*180.;
// 	  return true;
// 	}
// 	//
// 	//  	return true;
//       }
//     }else{
//       count = 0 ;
//     }
//       //
//   }
//     //}

//   return false;
// }

void PR3DCluster::adjust_wcpoints_parallel(WCPointCloud<double>::WCPoint &start_wcp, WCPointCloud<double>::WCPoint &end_wcp)
{
    // TVector3 dir(end_wcp.x - start_wcp.x, end_wcp.y - start_wcp.y, end_wcp.z - start_wcp.z);
    //  How to write this fast ???
    double low_x, high_x;
    low_x = start_wcp.x - 1 * units::cm;
    if (end_wcp.x - 1 * units::cm < low_x)
        low_x = end_wcp.x - 1 * units::cm;
    high_x = start_wcp.x + 1 * units::cm;
    if (end_wcp.x + 1 * units::cm > high_x)
        high_x = end_wcp.x + 1 * units::cm;

    WCPointCloud<double>::WCPoint low_u_wcp = start_wcp;
    WCPointCloud<double>::WCPoint low_v_wcp = start_wcp;
    WCPointCloud<double>::WCPoint low_w_wcp = start_wcp;
    WCPointCloud<double>::WCPoint high_u_wcp = start_wcp;
    WCPointCloud<double>::WCPoint high_v_wcp = start_wcp;
    WCPointCloud<double>::WCPoint high_w_wcp = start_wcp;

    for (size_t i = 0; i != point_cloud->get_cloud().pts.size(); i++)
    {
        if (point_cloud->get_cloud().pts.at(i).x > high_x ||
            point_cloud->get_cloud().pts.at(i).x < low_x)
            continue;
        if (point_cloud->get_cloud().pts.at(i).index_u < low_u_wcp.index_u)
            low_u_wcp = point_cloud->get_cloud().pts.at(i);
        if (point_cloud->get_cloud().pts.at(i).index_u > high_u_wcp.index_u)
            high_u_wcp = point_cloud->get_cloud().pts.at(i);

        if (point_cloud->get_cloud().pts.at(i).index_v < low_v_wcp.index_v)
            low_v_wcp = point_cloud->get_cloud().pts.at(i);
        if (point_cloud->get_cloud().pts.at(i).index_v > high_v_wcp.index_v)
            high_v_wcp = point_cloud->get_cloud().pts.at(i);

        if (point_cloud->get_cloud().pts.at(i).index_w < low_w_wcp.index_w)
            low_w_wcp = point_cloud->get_cloud().pts.at(i);
        if (point_cloud->get_cloud().pts.at(i).index_w > high_w_wcp.index_w)
            high_w_wcp = point_cloud->get_cloud().pts.at(i);
    }

    std::vector<size_t> indices, temp_indices;
    std::set<size_t> indices_set;
    Point test_p;

    bool flag_u = true, flag_v = true, flag_w = true;

    if (high_u_wcp.index_u - low_u_wcp.index_u < high_v_wcp.index_v - low_v_wcp.index_v)
    {
        if (high_u_wcp.index_u - low_u_wcp.index_u < high_w_wcp.index_w - low_w_wcp.index_w)
        {
            flag_u = false;
        }
        else
        {
            flag_w = false;
        }
    }
    else
    {
        if (high_v_wcp.index_v - low_v_wcp.index_v < high_w_wcp.index_w - low_w_wcp.index_w)
        {
            flag_v = false;
        }
        else
        {
            flag_w = false;
        }
    }

    if (flag_u)
    {
        test_p.x = low_u_wcp.x;
        test_p.y = low_u_wcp.y;
        test_p.z = low_u_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 0);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = high_u_wcp.x;
        test_p.y = high_u_wcp.y;
        test_p.z = high_u_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 0);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = start_wcp.x;
        test_p.y = start_wcp.y;
        test_p.z = start_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 0);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = end_wcp.x;
        test_p.y = end_wcp.y;
        test_p.z = end_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 0);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));
    }

    if (flag_v)
    {
        test_p.x = low_v_wcp.x;
        test_p.y = low_v_wcp.y;
        test_p.z = low_v_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 1);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = high_v_wcp.x;
        test_p.y = high_v_wcp.y;
        test_p.z = high_v_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 1);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = start_wcp.x;
        test_p.y = start_wcp.y;
        test_p.z = start_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 1);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = end_wcp.x;
        test_p.y = end_wcp.y;
        test_p.z = end_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 1);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));
    }

    if (flag_w)
    {
        test_p.x = low_w_wcp.x;
        test_p.y = low_w_wcp.y;
        test_p.z = low_w_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 2);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = high_w_wcp.x;
        test_p.y = high_w_wcp.y;
        test_p.z = high_w_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 2);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = start_wcp.x;
        test_p.y = start_wcp.y;
        test_p.z = start_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 2);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));

        test_p.x = end_wcp.x;
        test_p.y = end_wcp.y;
        test_p.z = end_wcp.z;
        temp_indices = point_cloud->get_closest_2d_index(test_p, 0.5 * units::cm, 2);
        std::copy(temp_indices.begin(), temp_indices.end(), inserter(indices_set, indices_set.begin()));
    }

    std::copy(indices_set.begin(), indices_set.end(), std::back_inserter(indices));

    // std::cout << start_wcp.index_u << " " << start_wcp.index_v << " " << start_wcp.index_w << " " << end_wcp.index_u << " " << end_wcp.index_v << " " << end_wcp.index_w << std::endl;
    // std::cout << low_u_wcp.index_u << " " << low_v_wcp.index_v << " " << low_w_wcp.index_w << " " << high_u_wcp.index_u << " " << high_v_wcp.index_v << " " << high_w_wcp.index_w << std::endl;

    // double old_dis = sqrt(pow(start_wcp.x-end_wcp.x,2)+pow(start_wcp.y-end_wcp.y,2)+pow(start_wcp.z-end_wcp.z,2));

    // std::cout << fabs(start_wcp.index_u - end_wcp.index_u) << " " <<  fabs(start_wcp.index_v - end_wcp.index_v) << " " << fabs(start_wcp.index_w - end_wcp.index_w) << " " << sqrt(pow(start_wcp.x-end_wcp.x,2)+pow(start_wcp.y-end_wcp.y,2)+pow(start_wcp.z-end_wcp.z,2))/units::cm << std::endl;

    WCPointCloud<double>::WCPoint new_start_wcp;
    WCPointCloud<double>::WCPoint new_end_wcp;

    //  std::cout << start_wcp.index << " " << end_wcp.index << std::endl;
    double sum_value = 0;
    for (size_t i = 0; i != indices.size(); i++)
    {
        //  std::cout << indices.at(i) << std::endl;
        for (size_t j = i + 1; j != indices.size(); j++)
        {
            double value = pow(point_cloud->get_cloud().pts.at(indices.at(i)).index_u - point_cloud->get_cloud().pts.at(indices.at(j)).index_u, 2) + pow(point_cloud->get_cloud().pts.at(indices.at(i)).index_v - point_cloud->get_cloud().pts.at(indices.at(j)).index_v, 2) + pow(point_cloud->get_cloud().pts.at(indices.at(i)).index_w - point_cloud->get_cloud().pts.at(indices.at(j)).index_w, 2);

            // double value = fabs(point_cloud->get_cloud().pts.at(indices.at(i)).index_u - point_cloud->get_cloud().pts.at(indices.at(j)).index_u) + fabs(point_cloud->get_cloud().pts.at(indices.at(i)).index_v - point_cloud->get_cloud().pts.at(indices.at(j)).index_v) + fabs(point_cloud->get_cloud().pts.at(indices.at(i)).index_w - point_cloud->get_cloud().pts.at(indices.at(j)).index_w);

            if (value > sum_value)
            {
                // old_dis = dis;
                if (point_cloud->get_cloud().pts.at(indices.at(i)).y > point_cloud->get_cloud().pts.at(indices.at(j)).y)
                {
                    new_start_wcp = point_cloud->get_cloud().pts.at(indices.at(i));
                    new_end_wcp = point_cloud->get_cloud().pts.at(indices.at(j));
                }
                else
                {
                    new_start_wcp = point_cloud->get_cloud().pts.at(indices.at(j));
                    new_end_wcp = point_cloud->get_cloud().pts.at(indices.at(i));
                }

                if (sqrt(pow(new_start_wcp.x - start_wcp.x, 2) + pow(new_start_wcp.y - start_wcp.y, 2) + pow(new_start_wcp.z - start_wcp.z, 2)) < 30 * units::cm && sqrt(pow(new_end_wcp.x - end_wcp.x, 2) + pow(new_end_wcp.y - end_wcp.y, 2) + pow(new_end_wcp.z - end_wcp.z, 2)) < 30 * units::cm)
                {
                    start_wcp = new_start_wcp;
                    end_wcp = new_end_wcp;
                    sum_value = value;
                }
            }
        }
    }

    //  std::cout << fabs(start_wcp.index_u - end_wcp.index_u) << " " <<  fabs(start_wcp.index_v - end_wcp.index_v) << " " << fabs(start_wcp.index_w - end_wcp.index_w) << " " << sqrt(pow(start_wcp.x-end_wcp.x,2)+pow(start_wcp.y-end_wcp.y,2)+pow(start_wcp.z-end_wcp.z,2))/units::cm << std::endl;
}

std::vector<PR3DCluster *> PR3DCluster::examine_x_boundary(double low_limit, double high_limit)
{
    double num_points[3] = {0, 0, 0};
    double x_max = -1e9;
    double x_min = 1e9;
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it);
        PointVector &pts = mcell->get_sampling_points();
        for (size_t i = 0; i != pts.size(); i++)
        {
            if (pts.at(i).x < low_limit)
            {
                num_points[0]++;
                if (pts.at(i).x > x_max)
                    x_max = pts.at(i).x;
            }
            else if (pts.at(i).x > high_limit)
            {
                num_points[2]++;
                if (pts.at(i).x < x_min)
                    x_min = pts.at(i).x;
            }
            else
            {
                num_points[1]++;
            }
        }
    }

    // std::cout << num_points[0] << " " << num_points[1] << " " << num_points[2] << std::endl;

    std::vector<PR3DCluster *> clusters;

    if (num_points[0] + num_points[2] < num_points[1] * 0.075)
    {
        PR3DCluster *cluster_1 = 0;
        PR3DCluster *cluster_2 = 0;
        PR3DCluster *cluster_3 = 0;
        if (x_max < low_limit - 1.0 * units::cm && x_max > -1e8)
        {
            // fill the small one ...
            cluster_1 = new PR3DCluster(1);
        }
        if (x_min > high_limit + 1.0 * units::cm && x_min < 1e8)
        {
            // fill the large one ...
            cluster_3 = new PR3DCluster(3);
        }
        if (cluster_1 != 0 || cluster_3 != 0)
        {
            cluster_2 = new PR3DCluster(2);
            for (auto it = mcells.begin(); it != mcells.end(); it++)
            {
                SlimMergeGeomCell *mcell = (*it);
                if (mcell->get_sampling_points().front().x < low_limit)
                {
                    if (cluster_1 != 0)
                    {
                        cluster_1->AddCell(mcell, mcell->GetTimeSlice());
                    }
                    else
                    {
                        cluster_2->AddCell(mcell, mcell->GetTimeSlice());
                    }
                }
                else if (mcell->get_sampling_points().front().x > high_limit)
                {
                    if (cluster_3 != 0)
                    {
                        cluster_3->AddCell(mcell, mcell->GetTimeSlice());
                    }
                    else
                    {
                        cluster_2->AddCell(mcell, mcell->GetTimeSlice());
                    }
                }
                else
                {
                    cluster_2->AddCell(mcell, mcell->GetTimeSlice());
                }
            }
            if (cluster_1 != 0)
                clusters.push_back(cluster_1);
            clusters.push_back(cluster_2);
            if (cluster_3 != 0)
                clusters.push_back(cluster_3);
        }
    }

    return clusters;
}

WCPointCloud<double>::WCPoint PR3DCluster::get_furthest_wcpoint(WCPointCloud<double>::WCPoint old_wcp, TVector3 dir, double step, int allowed_nstep)
{

    dir.SetMag(1);
    Point test_point;
    bool flag_continue = true;
    Point orig_point(old_wcp.x, old_wcp.y, old_wcp.z);
    TVector3 orig_dir = dir;
    orig_dir.SetMag(1);
    int counter = 0;
    TVector3 drift_dir(1, 0, 0);

    double old_dis = 15 * units::cm;

    while (flag_continue && counter < 400)
    {
        counter++;

        // first step
        test_point.x = old_wcp.x + dir.X() * step;
        test_point.y = old_wcp.y + dir.Y() * step;
        test_point.z = old_wcp.z + dir.Z() * step;
        WCPointCloud<double>::WCPoint new_wcp = point_cloud->get_closest_wcpoint(test_point);

        TVector3 dir1(new_wcp.x - old_wcp.x, new_wcp.y - old_wcp.y, new_wcp.z - old_wcp.z);
        double dis = dir1.Mag();                           // distance change
        double angle = dir1.Angle(dir) / 3.1415926 * 180.; // local angle change

        TVector3 dir2(new_wcp.x - orig_point.x, new_wcp.y - orig_point.y, new_wcp.z - orig_point.z); // start from the original point
        double dis1 = dir2.Mag();
        double angle1 = dir2.Angle(orig_dir) / 3.1415926 * 180.;

        TVector3 dir3(old_wcp.x - orig_point.x, old_wcp.y - orig_point.y, old_wcp.z - orig_point.z); // start from the original point

        bool flag_para = false;

        double angle_1 = fabs(dir1.Angle(drift_dir) - 3.1415926 / 2.) * 180. / 3.1415926;
        double angle_2 = fabs(dir.Angle(drift_dir) - 3.1415926 / 2.) / 3.1415926 * 180.;
        double angle_3 = fabs(dir2.Angle(drift_dir) - 3.1415926 / 2.) / 3.1415926 * 180.;
        double angle_4 = fabs(dir3.Angle(drift_dir) - 3.1415926 / 2.) / 3.1415926 * 180.;

        if (angle_1 < 5 && angle_2 < 5 || angle_3 < 2.5 && angle_4 < 2.5)
            flag_para = true;

        bool flag_forward = false;
        if (flag_para)
        {
            // parallel case
            if (angle < 60 &&
                dis > 0.2 * units::cm &&
                (angle < 45 || angle1 <= 5))
            {
                flag_forward = true;
            }
        }
        else
        {
            // non-parallel case
            if ((angle < 25 || dis < 1.2 * units::cm && angle < 60) &&                  // loose cut
                (angle < 15 || dis * sin(angle / 180. * 3.1415926) < 1.2 * units::cm || // tight cut
                 (angle < 21 && angle1 <= 2) ||
                 (angle1 <= 3 || dis1 * sin(angle1 / 180. * 3.1415926) < 3.6 * units::cm) && dis1 < 50 * units::cm) &&
                dis > 0.2 * units::cm)
            { // in case of good direction
                flag_forward = true;
            }
            else if ((angle_1 < 5 || angle_2 < 5) && (angle_1 + angle_2) < 15 && dis > 0.2 * units::cm &&
                     (angle < 60) && (angle < 45 || angle1 <= 5))
            {
                flag_forward = true;
            }
        }

        //  std::cout <<  " A " << old_wcp.x/units::cm << " " << old_wcp.y/units::cm << " " << old_wcp.z/units::cm << " " << dis1/units::cm << " " << angle << " " << dis/units::cm << " " << angle1 << " " << fabs(dir1.Angle(drift_dir)-3.1415926/2.)/3.1415926*180. << " " << fabs(dir.Angle(drift_dir)-3.1415926/2.)/3.1415926*180. << std::endl;

        if (flag_forward)
        {
            old_wcp = new_wcp;

            // std::cout << "A: " << " " << new_wcp.x/units::cm << " " << new_wcp.y/units::cm << " " << new_wcp.z/units::cm << " " << angle << " " << dis * sin(angle/180.*3.1415926)/units::cm << angle1 << " " << dis1 * sin(angle1/180.*3.1415926)/units::cm << " " << dis/units::cm << std::endl;

            if (dis > 3 * units::cm)
            {
                if (flag_para)
                {
                    dir = dir * old_dis + dir1 + orig_dir * 15 * units::cm; // if parallel, taking into account original direction ...
                }
                else
                {
                    dir = dir * old_dis + dir1;
                }
                dir.SetMag(1);
                old_dis = dis; //(old_dis*old_dis+dis*dis)/(old_dis + dis);
            }
        }
        else
        {
            //  failure & update direction
            flag_continue = false;

            test_point.x = old_wcp.x;
            test_point.y = old_wcp.y;
            test_point.z = old_wcp.z;

            TVector3 dir4;
            double eff_dis;
            if (flag_para)
            {
                dir4 = VHoughTrans(test_point, 100 * units::cm);
                eff_dis = 5 * units::cm;
            }
            else
            {
                dir4 = VHoughTrans(test_point, 30 * units::cm);
                eff_dis = 15 * units::cm;
            }
            dir4.SetMag(1);
            if (dir4.Angle(dir) > 3.1415926 / 2.)
                dir4 *= -1;

            // std::cout << dir.X() << " " << dir.Y() << " " << dir.Z() << " " << dir4.X() << " " << dir4.Y() << " " << dir4.Z() << " " << old_dis/units::cm << " " << dir4.Angle(dir)/3.1415926*180. << std::endl;

            if (flag_para)
            {
                dir = dir * old_dis + dir4 * eff_dis + orig_dir * 15 * units::cm;
                dir.SetMag(1);
                old_dis = eff_dis;
            }
            else
            {
                //	non-parallel case
                if (dir4.Angle(dir) < 25 / 180. * 3.1415926)
                {
                    dir = dir * old_dis + dir4 * eff_dis;
                    dir.SetMag(1);
                    old_dis = eff_dis;
                }
            }

            //  std::cout << dir.X() << " " << dir.Y() << " " << dir.Z() << " " << dir4.X() << " " << dir4.Y() << " " << dir4.Z() << std::endl;

            // start jump gaps
            for (int i = 0; i != allowed_nstep * 5; i++)
            {
                test_point.x = old_wcp.x + dir.X() * step * (1 + 1. / 5. * i);
                test_point.y = old_wcp.y + dir.Y() * step * (1 + 1. / 5. * i);
                test_point.z = old_wcp.z + dir.Z() * step * (1 + 1. / 5. * i);
                new_wcp = point_cloud->get_closest_wcpoint(test_point);
                double dis2 = sqrt(pow(new_wcp.x - test_point.x, 2) + pow(new_wcp.y - test_point.y, 2) + pow(new_wcp.z - test_point.z, 2));
                dir1.SetXYZ(new_wcp.x - old_wcp.x, new_wcp.y - old_wcp.y, new_wcp.z - old_wcp.z);
                dis = dir1.Mag();
                angle = dir1.Angle(dir) / 3.1415926 * 180.;

                dir2.SetXYZ(new_wcp.x - orig_point.x, new_wcp.y - orig_point.y, new_wcp.z - orig_point.z);
                dis1 = dir2.Mag();
                angle1 = dir2.Angle(orig_dir) / 3.1415926 * 180.;

                dir3.SetXYZ(old_wcp.x - orig_point.x, old_wcp.y - orig_point.y, old_wcp.z - orig_point.z);

                flag_para = false;
                double angle_1 = fabs(dir1.Angle(drift_dir) - 3.1415926 / 2.) * 180. / 3.1415926;
                double angle_2 = fabs(dir.Angle(drift_dir) - 3.1415926 / 2.) / 3.1415926 * 180.;
                double angle_3 = fabs(dir2.Angle(drift_dir) - 3.1415926 / 2.) / 3.1415926 * 180.;
                double angle_4 = fabs(dir3.Angle(drift_dir) - 3.1415926 / 2.) / 3.1415926 * 180.;

                //	std::cout << angle_1 << " " << angle_2 << std::endl;

                if (angle_1 < 7.5 && angle_2 < 7.5 || angle_3 < 5 && angle_4 < 5 && (angle_1 < 12.5 && angle_2 < 12.5))
                    flag_para = true;

                flag_forward = false;
                if (dis2 < 0.75 * step / 5. && dis > 0.2 * units::cm)
                    flag_forward = true;

                if (flag_para)
                {
                    if (dis > step * 0.8 &&
                        (angle < 45 || angle1 <= 5.5) &&
                        (angle < 60))
                        flag_forward = true;
                }
                else
                {
                    if (((angle < 20 && dis < 30 * units::cm ||
                          dis * sin(angle / 180. * 3.1415926) < 1.2 * units::cm ||
                          angle < 15 && dis < 45 * units::cm ||
                          angle < 10 ||
                          (angle <= 28 && angle_1 < 2 && dis < 10 * units::cm) ||
                          (angle <= 28 && angle1 <= 2)) ||
                         (angle1 <= 3 || dis1 * sin(angle1 / 180. * 3.1415926) < 6.0 * units::cm) && dis1 < 100 * units::cm) &&
                        dis > step * 0.8 && (angle < 30))
                    {
                        flag_forward = true;
                    }
                    else if ((angle_1 < 5 || angle_2 < 5) && (angle_1 + angle_2) < 15 && dis > step * 0.8 &&
                             (angle < 60) && (angle < 45 || angle1 <= 5))
                    {
                        flag_forward = true;
                    }
                }

                //	std::cout << i << " " << old_wcp.x/units::cm << " " << old_wcp.y/units::cm << " " << old_wcp.z/units::cm << " " << test_point.x/units::cm << " " << test_point.y/units::cm << " " << test_point.z/units::cm << " " << dis1/units::cm << " " << angle << " " << dis/units::cm << " " << angle1 << " " << fabs(dir1.Angle(drift_dir)-3.1415926/2.)/3.1415926*180. << " " << fabs(dir.Angle(drift_dir)-3.1415926/2.)/3.1415926*180. << " " << flag_para  << " " << angle_1 << " " << angle_2 << " " << angle_3 << " " << angle_4 << " " << new_wcp.x/units::cm << " " << new_wcp.y/units::cm << " " << new_wcp.z/units::cm << std::endl;

                if (flag_forward)
                {
                    old_wcp = new_wcp;

                    if (dis > 3 * units::cm)
                    {

                        //  std::cout << "B: " << dir.X() << " " << dir.Y() << " " << dir.Z() << " " << dir1.X() << " " << dir1.Y() << dir1.Z() << " " << new_wcp.x/units::cm << " " << new_wcp.y/units::cm << " " << new_wcp.z/units::cm << " " << dir1.Angle(dir)/3.1415926*180. << std::endl;

                        if (flag_para)
                        {
                            dir = dir * old_dis + dir1 + orig_dir * 15 * units::cm;
                        }
                        else
                        {
                            dir = dir * old_dis + dir1;
                        }
                        dir.SetMag(1);
                        old_dis = (old_dis * old_dis + dis * dis) / (old_dis + dis);
                        if (old_dis > 15 * units::cm)
                            old_dis = 15 * units::cm;
                    }

                    flag_continue = true;
                    break;
                }
            }
        }
    }

    return old_wcp;
}

void PR3DCluster::print()
{
    std::set<int> set_u, set_v, set_w, set_t;
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = *it;
        int time_slice = mcell->GetTimeSlice();
        set_t.insert(time_slice);
        GeomWireSelection uwires = mcell->get_uwires();
        GeomWireSelection vwires = mcell->get_vwires();
        GeomWireSelection wwires = mcell->get_wwires();
        for (auto it1 = uwires.begin(); it1 != uwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            set_u.insert(wire->index());
        }
        for (auto it1 = vwires.begin(); it1 != vwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            set_v.insert(wire->index());
        }
        for (auto it1 = wwires.begin(); it1 != wwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            set_w.insert(wire->index());
        }
    }
    std::cout << "Cluster: blob " << get_num_mcells() << " points "
              << get_num_points() << " uvwt "
              << *set_u.begin() << " " << *set_u.rbegin() << " "
              << *set_v.begin() << " " << *set_v.rbegin() << " "
              << *set_w.begin() << " " << *set_w.rbegin() << " "
              << *set_t.begin() * 4 << " " << *set_t.rbegin() * 4 << std::endl;
}

std::string PR3DCluster::dump_graph()
{
    if (graph == nullptr)
    {
        return "empty graph";
    }
    auto g = *graph;
    std::stringstream ss;

    const auto &points3d = get_point_cloud()->get_cloud().pts;
    ss << "MCUGraph:" << std::endl;
    ss << "Vertices: " << num_vertices(g) << std::endl;
    ss << "Edges: " << num_edges(g) << std::endl;

    ss << "Vertex Properties:" << std::endl;
    auto vrange = boost::vertices(g);
    for (auto vit = vrange.first; vit != vrange.second; ++vit)
    {
        auto v = *vit;
        ss << "Vertex " << v << ": Index = " << g[v].index << " (" << points3d[g[v].index].x << " " << points3d[g[v].index].y << " " << points3d[g[v].index].z << ")" << std::endl;
    }

    ss << "Edge Properties:" << std::endl;
    auto erange = boost::edges(g);
    for (auto eit = erange.first; eit != erange.second; ++eit)
    {
        auto e = *eit;
        auto src = source(e, g);
        auto tgt = target(e, g);
        ss << "Edge " << e << " ("
           << points3d[g[src].index].x << " " << points3d[g[src].index].y << " " << points3d[g[src].index].z << ") -> ()"
           << points3d[g[tgt].index].x << " " << points3d[g[tgt].index].y << " " << points3d[g[tgt].index].z << ") " << ": Distance = " << g[e].dist << std::endl;
    }
    return ss.str();
}

std::vector<int> PR3DCluster::get_uvwt_range()
{
    std::set<int> set_u, set_v, set_w, set_t;
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = *it;
        int time_slice = mcell->GetTimeSlice();
        set_t.insert(time_slice);
        GeomWireSelection uwires = mcell->get_uwires();
        GeomWireSelection vwires = mcell->get_vwires();
        GeomWireSelection wwires = mcell->get_wwires();
        for (auto it1 = uwires.begin(); it1 != uwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            set_u.insert(wire->index());
        }
        for (auto it1 = vwires.begin(); it1 != vwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            set_v.insert(wire->index());
        }
        for (auto it1 = wwires.begin(); it1 != wwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            set_w.insert(wire->index());
        }
    }
    std::vector<int> results;
    results.push_back(set_u.size());
    results.push_back(set_v.size());
    results.push_back(set_w.size());
    results.push_back(set_t.size());

    return results;
}

std::pair<int, int> PR3DCluster::get_num_points(Point &p, TVector3 &dir)
{
    int num_p1 = 0;
    int num_p2 = 0;

    // loop through all the points
    const int N = point_cloud->get_num_points();
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    for (int i = 0; i != N; i++)
    {
        TVector3 dir1(cloud.pts[i].x - p.x, cloud.pts[i].y - p.y, cloud.pts[i].z - p.z);
        if (dir1.Dot(dir) >= 0)
        {
            num_p1++;
        }
        else
        {
            num_p2++;
        }
    }

    return std::make_pair(num_p1, num_p2);
}

std::pair<int, int> PR3DCluster::get_num_points(Point &p, TVector3 &dir, double dis)
{
    int num_p1 = 0;
    int num_p2 = 0;

    // loop through all the points
    const int N = point_cloud->get_num_points();
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    for (int i = 0; i != N; i++)
    {
        TVector3 dir1(cloud.pts[i].x - p.x, cloud.pts[i].y - p.y, cloud.pts[i].z - p.z);
        if (dir1.Mag() < dis)
        {
            if (dir1.Dot(dir) >= 0)
            {
                num_p1++;
            }
            else
            {
                num_p2++;
            }
        }
    }

    return std::make_pair(num_p1, num_p2);
}

void PR3DCluster::Update_mcell_cluster_map(std::map<WCP::SlimMergeGeomCell *, WCP::PR3DCluster *> &mcell_cluster_map)
{
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it);
        mcell_cluster_map[mcell] = this;
    }
}

void PR3DCluster::Create_point_cloud(WCP::ToyPointCloud *global_point_cloud)
{
    if (point_cloud != (ToyPointCloud *)0)
        return;

    TPCParams &mp = Singleton<TPCParams>::Instance();
    double angle_u = mp.get_angle_u();
    double angle_v = mp.get_angle_v();
    double angle_w = mp.get_angle_w();

    point_cloud = new ToyPointCloud(angle_u, angle_v, angle_w);
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it);
        PointVector pts = mcell->get_sampling_points();

        if (global_point_cloud != (ToyPointCloud *)0)
            global_point_cloud->AddPoints(pts, mcell->get_sampling_points_wires(), mcell);

        point_cloud->AddPoints(pts, mcell->get_sampling_points_wires(), mcell);
    }
    point_cloud->build_kdtree_index();
    //  std::cout << point_cloud->get_num_points() << std::endl;
}

void PR3DCluster::Establish_close_connected_graph()
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();

    std::map<SlimMergeGeomCell *, std::map<int, std::set<int>>> map_mcell_uindex_wcps;
    std::map<SlimMergeGeomCell *, std::map<int, std::set<int>>> map_mcell_vindex_wcps;
    std::map<SlimMergeGeomCell *, std::map<int, std::set<int>>> map_mcell_windex_wcps;

    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it);
        std::map<int, std::set<int>> map_uindex_wcps;
        std::map<int, std::set<int>> map_vindex_wcps;
        std::map<int, std::set<int>> map_windex_wcps;
        std::vector<int> &wcps = point_cloud->get_mcell_indices(mcell);
        for (auto it1 = wcps.begin(); it1 != wcps.end(); it1++)
        {
            WCPointCloud<double>::WCPoint &wcp = cloud.pts[*it1];
            // int index = wcp.index;
            // std::cout << index << " " << wcp.x << " " << wcp.y << " " << wcp.z << " " << wcp.index_u << " " << wcp.index_v << " " << wcp.index_w << std::endl;

            auto v = vertex(wcp.index, *graph); // retrieve vertex descriptor
            (*graph)[v].index = wcp.index;
            if (map_uindex_wcps.find(wcp.index_u) == map_uindex_wcps.end())
            {
                std::set<int> wcps;
                wcps.insert(wcp.index);
                map_uindex_wcps[wcp.index_u] = wcps;
            }
            else
            {
                map_uindex_wcps[wcp.index_u].insert(wcp.index);
            }

            if (map_vindex_wcps.find(wcp.index_v) == map_vindex_wcps.end())
            {
                std::set<int> wcps;
                wcps.insert(wcp.index);
                map_vindex_wcps[wcp.index_v] = wcps;
            }
            else
            {
                map_vindex_wcps[wcp.index_v].insert(wcp.index);
            }

            if (map_windex_wcps.find(wcp.index_w) == map_windex_wcps.end())
            {
                std::set<int> wcps;
                wcps.insert(wcp.index);
                map_windex_wcps[wcp.index_w] = wcps;
            }
            else
            {
                map_windex_wcps[wcp.index_w].insert(wcp.index);
            }
        }
        map_mcell_uindex_wcps[mcell] = map_uindex_wcps;
        map_mcell_vindex_wcps[mcell] = map_vindex_wcps;
        map_mcell_windex_wcps[mcell] = map_windex_wcps;
    }

    int num_edges = 0;

    // create graph for points inside the same mcell
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it);
        std::vector<int> &wcps = point_cloud->get_mcell_indices(mcell);
        int max_wire_interval = mcell->get_max_wire_interval();
        int min_wire_interval = mcell->get_min_wire_interval();
        std::map<int, std::set<int>> *map_max_index_wcps;
        std::map<int, std::set<int>> *map_min_index_wcps;
        if (mcell->get_max_wire_type() == WirePlaneType_t(0))
        {
            map_max_index_wcps = &map_mcell_uindex_wcps[mcell];
        }
        else if (mcell->get_max_wire_type() == WirePlaneType_t(1))
        {
            map_max_index_wcps = &map_mcell_vindex_wcps[mcell];
        }
        else
        {
            map_max_index_wcps = &map_mcell_windex_wcps[mcell];
        }
        if (mcell->get_min_wire_type() == WirePlaneType_t(0))
        {
            map_min_index_wcps = &map_mcell_uindex_wcps[mcell];
        }
        else if (mcell->get_min_wire_type() == WirePlaneType_t(1))
        {
            map_min_index_wcps = &map_mcell_vindex_wcps[mcell];
        }
        else
        {
            map_min_index_wcps = &map_mcell_windex_wcps[mcell];
        }

        for (auto it1 = wcps.begin(); it1 != wcps.end(); it1++)
        {
            WCPointCloud<double>::WCPoint &wcp1 = cloud.pts[*it1];
            int index1 = wcp1.index;
            int index_max_wire;
            int index_min_wire;
            if (mcell->get_max_wire_type() == WirePlaneType_t(0))
            {
                index_max_wire = wcp1.index_u;
            }
            else if (mcell->get_max_wire_type() == WirePlaneType_t(1))
            {
                index_max_wire = wcp1.index_v;
            }
            else
            {
                index_max_wire = wcp1.index_w;
            }
            if (mcell->get_min_wire_type() == WirePlaneType_t(0))
            {
                index_min_wire = wcp1.index_u;
            }
            else if (mcell->get_min_wire_type() == WirePlaneType_t(1))
            {
                index_min_wire = wcp1.index_v;
            }
            else
            {
                index_min_wire = wcp1.index_w;
            }

            std::vector<std::set<int> *> max_wcps_set;
            std::vector<std::set<int> *> min_wcps_set;

            // go through the first map and find the ones satisfying the condition
            for (auto it2 = map_max_index_wcps->begin(); it2 != map_max_index_wcps->end(); it2++)
            {
                if (fabs(it2->first - index_max_wire) <= max_wire_interval)
                {
                    max_wcps_set.push_back(&(it2->second));
                }
            }
            // go through the second map and find the ones satisfying the condition
            for (auto it2 = map_min_index_wcps->begin(); it2 != map_min_index_wcps->end(); it2++)
            {
                if (fabs(it2->first - index_min_wire) <= min_wire_interval)
                {
                    min_wcps_set.push_back(&(it2->second));
                }
            }

            std::set<int> wcps_set1;
            std::set<int> wcps_set2;

            for (auto it2 = max_wcps_set.begin(); it2 != max_wcps_set.end(); it2++)
            {
                wcps_set1.insert((*it2)->begin(), (*it2)->end());
            }
            for (auto it3 = min_wcps_set.begin(); it3 != min_wcps_set.end(); it3++)
            {
                wcps_set2.insert((*it3)->begin(), (*it3)->end());
            }

            // std::cout << max_wcps_set.size() << " " << min_wcps_set.size() << std::endl;
            // for (auto it2 = max_wcps_set.begin(); it2!=max_wcps_set.end(); it2++){
            //	for (auto it3 = min_wcps_set.begin(); it3!=min_wcps_set.end(); it3++){
            {
                std::set<int> common_set;
                set_intersection(wcps_set1.begin(), wcps_set1.end(), wcps_set2.begin(), wcps_set2.end(), std::inserter(common_set, common_set.begin()));

                //	std::cout << "S0: " << common_set.size() << std::endl;

                for (auto it4 = common_set.begin(); it4 != common_set.end(); it4++)
                {
                    WCPointCloud<double>::WCPoint &wcp2 = cloud.pts[*it4];
                    if (wcp2.index != wcp1.index)
                    {
                        int index2 = wcp2.index;
                        //  std::cout << index1 << " " << index2 << std::endl;
                        // add edge ...
                        auto edge = add_edge(index1, index2, *graph);
                        if (edge.second)
                        {
                            (*graph)[edge.first].dist = sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) + pow(wcp1.z - wcp2.z, 2));
                            num_edges++;
                            // std::cout << wcp1.x << " " << wcp1.y << " " << wcp1.z << " " << wcp1.index_u << " " << wcp1.index_v << " " << wcp1.index_w << " " << wcp2.index_u << " " << wcp2.index_v << " " << wcp2.index_w << std::endl;
                        }
                    }
                }
                //}
            }
        }
    }

    //  std::cout << "Xin: " << num_edges << " " << N << std::endl;

    std::vector<int> time_slices;
    for (auto it1 = time_cells_set_map.begin(); it1 != time_cells_set_map.end(); it1++)
    {
        time_slices.push_back((*it1).first);
    }

    std::vector<std::pair<SlimMergeGeomCell *, SlimMergeGeomCell *>> connected_mcells;

    for (size_t i = 0; i != time_slices.size(); i++)
    {
        SMGCSet &mcells_set = time_cells_set_map[time_slices.at(i)];

        // create graph for points in mcell inside the same time slice
        if (mcells_set.size() >= 2)
        {
            for (auto it2 = mcells_set.begin(); it2 != mcells_set.end(); it2++)
            {
                SlimMergeGeomCell *mcell1 = *it2;
                auto it2p = it2;
                if (it2p != mcells_set.end())
                {
                    it2p++;
                    for (auto it3 = it2p; it3 != mcells_set.end(); it3++)
                    {
                        SlimMergeGeomCell *mcell2 = *(it3);
                        // std::cout << mcell1 << " " << mcell2 << " " << mcell1->Overlap_fast(mcell2,2) << std::endl;
                        if (mcell1->Overlap_fast(mcell2, 2))
                            connected_mcells.push_back(std::make_pair(mcell1, mcell2));
                    }
                }
            }
        }
        // create graph for points between connected mcells in adjacent time slices + 1, if not, + 2
        std::vector<SMGCSet> vec_mcells_set;
        if (i + 1 < time_slices.size())
        {
            if (time_slices.at(i + 1) - time_slices.at(i) == 1)
            {
                vec_mcells_set.push_back(time_cells_set_map[time_slices.at(i + 1)]);
                if (i + 2 < time_slices.size())
                    if (time_slices.at(i + 2) - time_slices.at(i) == 2)
                        vec_mcells_set.push_back(time_cells_set_map[time_slices.at(i + 2)]);
            }
            else if (time_slices.at(i + 1) - time_slices.at(i) == 2)
            {
                vec_mcells_set.push_back(time_cells_set_map[time_slices.at(i + 1)]);
            }
        }
        //    bool flag = false;
        for (size_t j = 0; j != vec_mcells_set.size(); j++)
        {
            //      if (flag) break;
            SMGCSet &next_mcells_set = vec_mcells_set.at(j);
            for (auto it1 = mcells_set.begin(); it1 != mcells_set.end(); it1++)
            {
                SlimMergeGeomCell *mcell1 = (*it1);
                for (auto it2 = next_mcells_set.begin(); it2 != next_mcells_set.end(); it2++)
                {
                    SlimMergeGeomCell *mcell2 = (*it2);
                    if (mcell1->Overlap_fast(mcell2, 2))
                    {
                        //	    flag = true; // correct???
                        connected_mcells.push_back(std::make_pair(mcell1, mcell2));
                    }
                }
            }
        }
    }

    // establish edge ...
    std::map<std::pair<int, int>, std::pair<int, double>> closest_index;

    // std::cout << connected_mcells.size() << std::endl;
    for (auto it = connected_mcells.begin(); it != connected_mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell1 = (*it).first;
        SlimMergeGeomCell *mcell2 = (*it).second;

        std::vector<int> &wcps1 = point_cloud->get_mcell_indices(mcell1);
        std::vector<int> &wcps2 = point_cloud->get_mcell_indices(mcell2);

        // test 2 against 1 ...
        int max_wire_interval = mcell1->get_max_wire_interval();
        int min_wire_interval = mcell1->get_min_wire_interval();
        std::map<int, std::set<int>> *map_max_index_wcps;
        std::map<int, std::set<int>> *map_min_index_wcps;

        if (mcell1->get_max_wire_type() == WirePlaneType_t(0))
        {
            map_max_index_wcps = &map_mcell_uindex_wcps[mcell2];
        }
        else if (mcell1->get_max_wire_type() == WirePlaneType_t(1))
        {
            map_max_index_wcps = &map_mcell_vindex_wcps[mcell2];
        }
        else
        {
            map_max_index_wcps = &map_mcell_windex_wcps[mcell2];
        }
        if (mcell1->get_min_wire_type() == WirePlaneType_t(0))
        {
            map_min_index_wcps = &map_mcell_uindex_wcps[mcell2];
        }
        else if (mcell1->get_min_wire_type() == WirePlaneType_t(1))
        {
            map_min_index_wcps = &map_mcell_vindex_wcps[mcell2];
        }
        else
        {
            map_min_index_wcps = &map_mcell_windex_wcps[mcell2];
        }

        for (auto it1 = wcps1.begin(); it1 != wcps1.end(); it1++)
        {
            WCPointCloud<double>::WCPoint &wcp1 = cloud.pts[*it1];
            int index1 = wcp1.index;
            int index_max_wire;
            int index_min_wire;
            if (mcell1->get_max_wire_type() == WirePlaneType_t(0))
            {
                index_max_wire = wcp1.index_u;
            }
            else if (mcell1->get_max_wire_type() == WirePlaneType_t(1))
            {
                index_max_wire = wcp1.index_v;
            }
            else
            {
                index_max_wire = wcp1.index_w;
            }
            if (mcell1->get_min_wire_type() == WirePlaneType_t(0))
            {
                index_min_wire = wcp1.index_u;
            }
            else if (mcell1->get_min_wire_type() == WirePlaneType_t(1))
            {
                index_min_wire = wcp1.index_v;
            }
            else
            {
                index_min_wire = wcp1.index_w;
            }
            std::vector<std::set<int> *> max_wcps_set;
            std::vector<std::set<int> *> min_wcps_set;
            // go through the first map and find the ones satisfying the condition
            for (auto it2 = map_max_index_wcps->begin(); it2 != map_max_index_wcps->end(); it2++)
            {
                if (fabs(it2->first - index_max_wire) <= max_wire_interval)
                {
                    max_wcps_set.push_back(&(it2->second));
                }
            }
            // go through the second map and find the ones satisfying the condition
            for (auto it2 = map_min_index_wcps->begin(); it2 != map_min_index_wcps->end(); it2++)
            {
                if (fabs(it2->first - index_min_wire) <= min_wire_interval)
                {
                    min_wcps_set.push_back(&(it2->second));
                }
            }

            std::set<int> wcps_set1;
            std::set<int> wcps_set2;

            for (auto it2 = max_wcps_set.begin(); it2 != max_wcps_set.end(); it2++)
            {
                wcps_set1.insert((*it2)->begin(), (*it2)->end());
            }
            for (auto it3 = min_wcps_set.begin(); it3 != min_wcps_set.end(); it3++)
            {
                wcps_set2.insert((*it3)->begin(), (*it3)->end());
            }

            //   for (auto it2 = max_wcps_set.begin(); it2!=max_wcps_set.end(); it2++){
            //	for (auto it3 = min_wcps_set.begin(); it3!=min_wcps_set.end(); it3++){
            {
                std::set<int> common_set;
                set_intersection(wcps_set1.begin(), wcps_set1.end(), wcps_set2.begin(), wcps_set2.end(), std::inserter(common_set, common_set.begin()));

                //	std::cout << "S1: " << common_set.size() << std::endl;
                //	  std::cout << common_set.size() << std::endl;

                //	std::map<int,std::pair<int,double> > closest_index;

                for (auto it4 = common_set.begin(); it4 != common_set.end(); it4++)
                {
                    WCPointCloud<double>::WCPoint &wcp2 = cloud.pts[*it4];
                    if (wcp2.index != wcp1.index)
                    {
                        int index2 = wcp2.index;
                        double dis = sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) + pow(wcp1.z - wcp2.z, 2));

                        if (closest_index.find(std::make_pair(index1, wcp2.mcell->GetTimeSlice())) == closest_index.end())
                        {
                            closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())] = std::make_pair(index2, dis);
                        }
                        else
                        {
                            if (dis < closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].second)
                                closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())] = std::make_pair(index2, dis);
                        }
                    }
                }

                //	std::cout << closest_index.size() << std::endl;
                // for (auto it4 = closest_index.begin(); it4!=closest_index.end(); it4++){
                //   int index2 = it4->second.first;
                //   double dis = it4->second.second;
                //   auto edge = add_edge(index1,index2,*graph);
                //   if (edge.second){
                //     (*graph)[edge.first].dist = dis;
                //     num_edges ++;
                //   }
                // }

                // for (auto it4 = common_set.begin(); it4!=common_set.end(); it4++){
                //   WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[*it4];
                //   if (wcp2.index != wcp1.index){
                //     int index2 = wcp2.index;
                //     auto edge = add_edge(index1,index2,*graph);
                //     if (edge.second){
                //       (*graph)[edge.first].dist = sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2));
                //       num_edges ++;
                //     }
                //   }
                // }
            }
            //}
        }

        // test 1 against 2 ...
        max_wire_interval = mcell2->get_max_wire_interval();
        min_wire_interval = mcell2->get_min_wire_interval();
        if (mcell2->get_max_wire_type() == WirePlaneType_t(0))
        {
            map_max_index_wcps = &map_mcell_uindex_wcps[mcell1];
        }
        else if (mcell2->get_max_wire_type() == WirePlaneType_t(1))
        {
            map_max_index_wcps = &map_mcell_vindex_wcps[mcell1];
        }
        else
        {
            map_max_index_wcps = &map_mcell_windex_wcps[mcell1];
        }
        if (mcell2->get_min_wire_type() == WirePlaneType_t(0))
        {
            map_min_index_wcps = &map_mcell_uindex_wcps[mcell1];
        }
        else if (mcell2->get_min_wire_type() == WirePlaneType_t(1))
        {
            map_min_index_wcps = &map_mcell_vindex_wcps[mcell1];
        }
        else
        {
            map_min_index_wcps = &map_mcell_windex_wcps[mcell1];
        }
        for (auto it1 = wcps2.begin(); it1 != wcps2.end(); it1++)
        {
            WCPointCloud<double>::WCPoint &wcp1 = cloud.pts[*it1];
            int index1 = wcp1.index;
            int index_max_wire;
            int index_min_wire;
            if (mcell2->get_max_wire_type() == WirePlaneType_t(0))
            {
                index_max_wire = wcp1.index_u;
            }
            else if (mcell2->get_max_wire_type() == WirePlaneType_t(1))
            {
                index_max_wire = wcp1.index_v;
            }
            else
            {
                index_max_wire = wcp1.index_w;
            }
            if (mcell2->get_min_wire_type() == WirePlaneType_t(0))
            {
                index_min_wire = wcp1.index_u;
            }
            else if (mcell2->get_min_wire_type() == WirePlaneType_t(1))
            {
                index_min_wire = wcp1.index_v;
            }
            else
            {
                index_min_wire = wcp1.index_w;
            }
            std::vector<std::set<int> *> max_wcps_set;
            std::vector<std::set<int> *> min_wcps_set;
            // go through the first map and find the ones satisfying the condition
            for (auto it2 = map_max_index_wcps->begin(); it2 != map_max_index_wcps->end(); it2++)
            {
                if (fabs(it2->first - index_max_wire) <= max_wire_interval)
                {
                    max_wcps_set.push_back(&(it2->second));
                }
            }
            // go through the second map and find the ones satisfying the condition
            for (auto it2 = map_min_index_wcps->begin(); it2 != map_min_index_wcps->end(); it2++)
            {
                if (fabs(it2->first - index_min_wire) <= min_wire_interval)
                {
                    min_wcps_set.push_back(&(it2->second));
                }
            }

            std::set<int> wcps_set1;
            std::set<int> wcps_set2;

            for (auto it2 = max_wcps_set.begin(); it2 != max_wcps_set.end(); it2++)
            {
                wcps_set1.insert((*it2)->begin(), (*it2)->end());
            }
            for (auto it3 = min_wcps_set.begin(); it3 != min_wcps_set.end(); it3++)
            {
                wcps_set2.insert((*it3)->begin(), (*it3)->end());
            }

            // for (auto it2 = max_wcps_set.begin(); it2!=max_wcps_set.end(); it2++){
            // 	for (auto it3 = min_wcps_set.begin(); it3!=min_wcps_set.end(); it3++){
            {
                std::set<int> common_set;
                set_intersection(wcps_set1.begin(), wcps_set1.end(), wcps_set2.begin(), wcps_set2.end(), std::inserter(common_set, common_set.begin()));

                //	std::cout << "S2: " << common_set.size() << std::endl;

                //	std::map<int,std::pair<int,double> > closest_index;

                for (auto it4 = common_set.begin(); it4 != common_set.end(); it4++)
                {
                    WCPointCloud<double>::WCPoint &wcp2 = cloud.pts[*it4];
                    if (wcp2.index != wcp1.index)
                    {
                        int index2 = wcp2.index;
                        double dis = sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) + pow(wcp1.z - wcp2.z, 2));

                        if (closest_index.find(std::make_pair(index1, wcp2.mcell->GetTimeSlice())) == closest_index.end())
                        {
                            closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())] = std::make_pair(index2, dis);
                        }
                        else
                        {
                            if (dis < closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].second)
                                closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())] = std::make_pair(index2, dis);
                        }
                    }
                }

                // std::cout << closest_index.size() << std::endl;
                //  for (auto it4 = closest_index.begin(); it4!=closest_index.end(); it4++){
                //    int index2 = it4->second.first;
                //    double dis = it4->second.second;
                //    auto edge = add_edge(index1,index2,*graph);
                //    if (edge.second){
                //      (*graph)[edge.first].dist = dis;
                //      num_edges ++;
                //    }
                //  }

                // for (auto it4 = common_set.begin(); it4!=common_set.end(); it4++){
                //   WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[*it4];
                //   if (wcp2.index != wcp1.index){
                //     int index2 = wcp2.index;
                //     auto edge = add_edge(index1,index2,*graph);
                //     if (edge.second){
                //       (*graph)[edge.first].dist = sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2));
                //       num_edges ++;
                //     }
                //   }
                // }
            }
            //      }
        }
    }

    for (auto it4 = closest_index.begin(); it4 != closest_index.end(); it4++)
    {
        int index1 = it4->first.first;
        int index2 = it4->second.first;
        double dis = it4->second.second;
        auto edge = add_edge(index1, index2, *graph);
        if (edge.second)
        {
            (*graph)[edge.first].dist = dis;
            num_edges++;
        }
    }
    // end of copying ...
}

void PR3DCluster::Connect_graph(WCP::ToyCTPointCloud &ct_point_cloud)
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCP::WC2DPointCloud<double> &cloud_u = point_cloud->get_cloud_u();
    WCP::WC2DPointCloud<double> &cloud_v = point_cloud->get_cloud_v();
    WCP::WC2DPointCloud<double> &cloud_w = point_cloud->get_cloud_w();

    // now form the connected components
    std::vector<int> component(num_vertices(*graph));
    const int num = connected_components(*graph, &component[0]);
    if (num > 1)
    {
        std::vector<ToyPointCloud *> pt_clouds;
        for (int j = 0; j != num; j++)
        {
            ToyPointCloud *pt_cloud = new ToyPointCloud();
            pt_clouds.push_back(pt_cloud);
        }

        std::vector<int>::size_type i;
        for (i = 0; i != component.size(); ++i)
        {
            pt_clouds.at(component[i])->AddPoint(cloud.pts[i], cloud_u.pts[i], cloud_v.pts[i], cloud_w.pts[i]);
            //   std::cout << "Vertex " << 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 << " " << cloud.pts[i].mcell->GetTimeSlice()  << " is in component " << component[i] << std::endl;
        }
        for (int j = 0; j != num; j++)
        {
            pt_clouds.at(j)->build_kdtree_index();
        }

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_mst(num, std::vector<std::tuple<int, int, double>>(num));

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir1(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir2(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir_mst(num, std::vector<std::tuple<int, int, double>>(num));

        for (int j = 0; j != num; j++)
        {
            for (int k = 0; k != num; k++)
            {
                index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_mst[j][k] = std::make_tuple(-1, -1, 1e9);

                index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir_mst[j][k] = std::make_tuple(-1, -1, 1e9);
            }
        }

        // check against the closest distance ...
        // no need to have MST ...
        for (int j = 0; j != num; j++)
        {
            for (int k = j + 1; k != num; k++)
            {
                index_index_dis[j][k] = pt_clouds.at(j)->get_closest_points(pt_clouds.at(k));

                // if (cluster_id==6)
                //   std::cout << j << " " << k << " " << num << " " << pt_clouds.at(j)->get_num_points() << " " << pt_clouds.at(k)->get_num_points() << std::endl;

                if (num < 100 && pt_clouds.at(j)->get_num_points() > 100 && pt_clouds.at(k)->get_num_points() > 100 &&
                        (pt_clouds.at(j)->get_num_points() + pt_clouds.at(k)->get_num_points()) > 400 ||
                    pt_clouds.at(j)->get_num_points() > 500 && pt_clouds.at(k)->get_num_points() > 500)
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    TVector3 dir1 = VHoughTrans(p1, 30 * units::cm, pt_clouds.at(j));
                    TVector3 dir2 = VHoughTrans(p2, 30 * units::cm, pt_clouds.at(k));
                    dir1 *= -1;
                    dir2 *= -1;

                    std::pair<int, double> result1 = pt_clouds.at(k)->get_closest_point_along_vec(p1, dir1, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);
                    // if (result1.first <0)
                    //   result1 = pt_clouds.at(k)->get_closest_point_along_vec(p1, dir1, 6*units::cm, 1*units::cm, 25, 1.5*units::cm);

                    // if (cluster_id==6)
                    //   std::cout << p1.x/units::cm << " " << p1.y/units::cm << " " << p1.z/units::cm << " " << dir1.X() << " " << dir1.Y() << " " << dir1.Z() << std::endl;

                    if (result1.first >= 0)
                    {
                        // Point test_p1(cloud.pts.at(std::get<0>(index_index_dis[j][k])).x,cloud.pts.at(std::get<0>(index_index_dis[j][k])).y,cloud.pts.at(std::get<0>(index_index_dis[j][k])).z);
                        // Point test_p2(cloud.pts.at(result1.first).x,cloud.pts.at(result1.first).y,cloud.pts.at(result1.first).z);
                        // double dis = sqrt(pow(test_p2.x-test_p1.x,2)+pow(test_p2.y-test_p1.y,2)+pow(test_p2.z-test_p1.z,2));
                        // int num_points = dis/(1.5*units::cm)+1;
                        // int num_cut_points = 0;
                        // for (size_t k1=0; k1!=num_points-1; k1++){
                        //   Point test_p3(test_p1.x + (test_p2.x-test_p1.x) * (k1+1)/num_points ,
                        // 		    test_p1.y + (test_p2.y-test_p1.y) * (k1+1)/num_points ,
                        // 		    test_p1.z + (test_p2.z-test_p1.z) * (k1+1)/num_points );
                        //   double dis1 = point_cloud->get_closest_dis(test_p3);
                        //   if (dis1 < 1*units::cm)
                        // 	num_cut_points ++;
                        // }
                        // // if (cluster_id==6)
                        // //   std::cout << num_cut_points << " " << num_points << " " << dis/units::cm << std::endl;

                        // if (num_cut_points <=8 && num_cut_points< 0.25 * num_points + 2 && dis > 1*units::cm)
                        index_index_dis_dir1[j][k] = std::make_tuple(std::get<0>(index_index_dis[j][k]), result1.first, result1.second);
                    }

                    std::pair<int, double> result2 = pt_clouds.at(j)->get_closest_point_along_vec(p2, dir2, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);
                    // if (result2.first <0)
                    //   result2 = pt_clouds.at(j)->get_closest_point_along_vec(p2, dir2, 7*units::cm, 1*units::cm, 12.5, 1.5*units::cm);

                    // if (cluster_id==6)
                    //   std::cout << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << " " << dir2.X() << " " << dir2.Y() << " " << dir2.Z() << std::endl;

                    if (result2.first >= 0)
                    {

                        // Point test_p1(cloud.pts.at(std::get<1>(index_index_dis[j][k])).x,cloud.pts.at(std::get<1>(index_index_dis[j][k])).y,cloud.pts.at(std::get<1>(index_index_dis[j][k])).z);
                        // Point test_p2(cloud.pts.at(result2.first).x,cloud.pts.at(result2.first).y,cloud.pts.at(result2.first).z);
                        // double dis = sqrt(pow(test_p2.x-test_p1.x,2)+pow(test_p2.y-test_p1.y,2)+pow(test_p2.z-test_p1.z,2));
                        // int num_points = dis/(1.5*units::cm)+1;
                        // int num_cut_points = 0;
                        // for (size_t k1=0; k1!=num_points-1; k1++){
                        //   Point test_p3(test_p1.x + (test_p2.x-test_p1.x) * (k1+1)/num_points ,
                        // 		    test_p1.y + (test_p2.y-test_p1.y) * (k1+1)/num_points ,
                        // 		    test_p1.z + (test_p2.z-test_p1.z) * (k1+1)/num_points );
                        //   double dis1 = point_cloud->get_closest_dis(test_p3);
                        //   if ( dis1 < 1*units::cm)
                        // 	num_cut_points ++;
                        // }

                        // // if (cluster_id==6)
                        // //   std::cout << num_cut_points << " " << num_points << " " << dis/units::cm << std::endl;

                        // if (num_cut_points <=8 && num_cut_points < 0.25 * num_points + 2 && dis > 1*units::cm)
                        index_index_dis_dir2[j][k] = std::make_tuple(result2.first, std::get<1>(index_index_dis[j][k]), result2.second);
                    }
                }

                // Now check the path ...
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    for (int ii = 0; ii != num_steps; ii++)
                    {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point(test_p))
                        {
                            num_bad++;
                            /* if (cluster->get_cluster_id()==11) */
                            /* 	std::cout << test_p.x/units::cm << " " << test_p.y/units::cm << " " << test_p.z/units::cm << std::endl; */
                        }
                    }

                    //  std::cout << cluster->get_cluster_id() << " " << j << " " << k << " " << num_bad << " " << num_steps << std::endl;

                    if (num_bad > 7 ||
                        num_bad > 2 && num_bad >= 0.75 * num_steps)
                    {
                        index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                    }

                    // if (cluster_id==13)
                    //   std::cout << cluster_id << " " << p1.x/units::cm << " " << p1.y/units::cm << " " << p1.z/units::cm << " " << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << " " << j << " " << k << " " << num_bad << " " << num_steps << std::endl;
                }

                // Now check the path ...
                if (std::get<0>(index_index_dis_dir1[j][k]) >= 0)
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis_dir1[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis_dir1[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    for (int ii = 0; ii != num_steps; ii++)
                    {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point(test_p))
                            num_bad++;
                    }

                    if (num_bad > 7 ||
                        num_bad > 2 && num_bad >= 0.75 * num_steps)
                    {
                        index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                    }
                    // if (cluster_id==13)
                    //   std::cout << "A: " << p1.x/units::cm << " " << p1.y/units::cm << " " << p1.z/units::cm << " " << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << " " << j << " " << k << " " << num_bad << " " << num_steps << std::endl;
                }

                // Now check the path ...
                if (std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis_dir2[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis_dir2[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    for (int ii = 0; ii != num_steps; ii++)
                    {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point(test_p))
                            num_bad++;
                    }

                    if (num_bad > 7 ||
                        num_bad > 2 && num_bad >= 0.75 * num_steps)
                    {
                        index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                    }
                    // if (cluster_id==13)
                    //   std::cout << "B: " << p1.x/units::cm << " " << p1.y/units::cm << " " << p1.z/units::cm << " " << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << " " << j << " " << k << " " << num_bad << " " << num_steps << std::endl;
                }
            }
        }

        // deal with MST of first type
        {
            const int N = num;
            boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                                  boost::no_property, boost::property<boost::edge_weight_t, double>>
                temp_graph(N);

            for (int j = 0; j != num; j++)
            {
                for (int k = j + 1; k != num; k++)
                {
                    int index1 = j;
                    int index2 = k;
                    if (std::get<0>(index_index_dis[j][k]) >= 0)
                        auto edge = add_edge(index1, index2, std::get<2>(index_index_dis[j][k]), temp_graph);
                }
            }

            {
                std::vector<int> possible_root_vertex;
                std::vector<int> component(num_vertices(temp_graph));
                const int num1 = connected_components(temp_graph, &component[0]);
                possible_root_vertex.resize(num1);
                std::vector<int>::size_type i;
                for (i = 0; i != component.size(); ++i)
                {
                    possible_root_vertex.at(component[i]) = i;
                }

                for (size_t i = 0; i != possible_root_vertex.size(); i++)
                {
                    std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(num_vertices(temp_graph));

                    prim_minimum_spanning_tree(temp_graph, &predecessors[0], boost::root_vertex(possible_root_vertex.at(i)));

                    for (size_t j = 0; j != predecessors.size(); ++j)
                    {
                        if (predecessors[j] != j)
                        {
                            if (j < predecessors[j])
                            {
                                index_index_dis_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                            }
                            else
                            {
                                index_index_dis_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                            }
                            // std::cout << j << " " << predecessors[j] << " " << std::endl;
                        }
                        else
                        {
                            // std::cout << j << " " << std::endl;
                        }
                    }
                }
            }
        }

        // MST of the direction ...
        {
            const int N = num;
            boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                                  boost::no_property, boost::property<boost::edge_weight_t, double>>
                temp_graph(N);

            for (int j = 0; j != num; j++)
            {
                for (int k = j + 1; k != num; k++)
                {
                    int index1 = j;
                    int index2 = k;
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0 || std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                        auto edge = add_edge(index1, index2, std::min(std::get<2>(index_index_dis_dir1[j][k]), std::get<2>(index_index_dis_dir2[j][k])), temp_graph);
                }
            }

            {
                std::vector<int> possible_root_vertex;
                std::vector<int> component(num_vertices(temp_graph));
                const int num1 = connected_components(temp_graph, &component[0]);
                possible_root_vertex.resize(num1);
                std::vector<int>::size_type i;
                for (i = 0; i != component.size(); ++i)
                {
                    possible_root_vertex.at(component[i]) = i;
                }

                for (size_t i = 0; i != possible_root_vertex.size(); i++)
                {
                    std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(num_vertices(temp_graph));
                    prim_minimum_spanning_tree(temp_graph, &predecessors[0], boost::root_vertex(possible_root_vertex.at(i)));
                    for (size_t j = 0; j != predecessors.size(); ++j)
                    {
                        if (predecessors[j] != j)
                        {
                            if (j < predecessors[j])
                            {
                                index_index_dis_dir_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                            }
                            else
                            {
                                index_index_dis_dir_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                            }
                            // std::cout << j << " " << predecessors[j] << " " << std::endl;
                        }
                        else
                        {
                            // std::cout << j << " " << std::endl;
                        }
                    }
                }
            }
        }

        for (int j = 0; j != num; j++)
        {
            for (int k = j + 1; k != num; k++)
            {
                if (std::get<2>(index_index_dis[j][k]) < 3 * units::cm)
                {
                    index_index_dis_mst[j][k] = index_index_dis[j][k];
                }

                // establish the path ...
                if (std::get<0>(index_index_dis_mst[j][k]) >= 0)
                {
                    auto edge = add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]), *graph);
                    if (edge.second)
                    {
                        if (std::get<2>(index_index_dis_mst[j][k]) > 5 * units::cm)
                        {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                        else
                        {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                    }
                }

                // if (std::get<0>(index_index_dis[j][k])>=0){
                //   auto edge = add_edge(std::get<0>(index_index_dis[j][k]),std::get<1>(index_index_dis[j][k]),*graph);
                //   if (edge.second){
                //     if (std::get<2>(index_index_dis[j][k])>5*units::cm){
                //       (*graph)[edge.first].dist = std::get<2>(index_index_dis[j][k]);
                //     }else{
                //       (*graph)[edge.first].dist = std::get<2>(index_index_dis[j][k]);
                //     }
                //   }
                // }
                if (std::get<0>(index_index_dis_dir_mst[j][k]) >= 0)
                {
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0)
                    {
                        auto edge = add_edge(std::get<0>(index_index_dis_dir1[j][k]), std::get<1>(index_index_dis_dir1[j][k]), *graph);
                        if (edge.second)
                        {
                            if (std::get<2>(index_index_dis_dir1[j][k]) > 5 * units::cm)
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]) * 1.1;
                            }
                            else
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]);
                            }
                        }
                    }
                    if (std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                    {
                        auto edge = add_edge(std::get<0>(index_index_dis_dir2[j][k]), std::get<1>(index_index_dis_dir2[j][k]), *graph);
                        if (edge.second)
                        {
                            if (std::get<2>(index_index_dis_dir2[j][k]) > 5 * units::cm)
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]) * 1.1;
                            }
                            else
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]);
                            }
                        }
                    }
                }

            } // k
        } // j

        // delete newly created point cloud
        for (int i = 0; i != num; i++)
        {
            delete pt_clouds.at(i);
        }
        pt_clouds.clear();
    }
}

void PR3DCluster::Connect_graph_overclustering_protection(WCP::ToyCTPointCloud &ct_point_cloud)
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCP::WC2DPointCloud<double> &cloud_u = point_cloud->get_cloud_u();
    WCP::WC2DPointCloud<double> &cloud_v = point_cloud->get_cloud_v();
    WCP::WC2DPointCloud<double> &cloud_w = point_cloud->get_cloud_w();

    // parallel case 1 and perpendicular case 2
    TVector3 drift_dir(1, 0, 0);
    // pronlonged case for U 3 and V 4 ...
    TVector3 U_dir(0, cos(60. / 180. * 3.1415926), sin(60. / 180. * 3.1415926));
    TVector3 V_dir(0, cos(60. / 180. * 3.1415926), -sin(60. / 180. * 3.1415926));
    TVector3 W_dir(0, 1, 0);

    // now form the connected components
    std::vector<int> component(num_vertices(*graph));
    const int num = connected_components(*graph, &component[0]);
    if (num > 1)
    {
        std::vector<ToyPointCloud *> pt_clouds;
        for (int j = 0; j != num; j++)
        {
            ToyPointCloud *pt_cloud = new ToyPointCloud();
            pt_clouds.push_back(pt_cloud);
        }

        std::vector<int>::size_type i;
        for (i = 0; i != component.size(); ++i)
        {
            pt_clouds.at(component[i])->AddPoint(cloud.pts[i], cloud_u.pts[i], cloud_v.pts[i], cloud_w.pts[i]);
        }
        for (int j = 0; j != num; j++)
        {
            pt_clouds.at(j)->build_kdtree_index();
        }

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_mst(num, std::vector<std::tuple<int, int, double>>(num));

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir1(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir2(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir_mst(num, std::vector<std::tuple<int, int, double>>(num));

        for (int j = 0; j != num; j++)
        {
            for (int k = 0; k != num; k++)
            {
                index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_mst[j][k] = std::make_tuple(-1, -1, 1e9);

                index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir_mst[j][k] = std::make_tuple(-1, -1, 1e9);
            }
        }

        // check against the closest distance ...
        // no need to have MST ...
        for (int j = 0; j != num; j++)
        {
            for (int k = j + 1; k != num; k++)
            {
                index_index_dis[j][k] = pt_clouds.at(j)->get_closest_points(pt_clouds.at(k));
                if (num < 100 && pt_clouds.at(j)->get_num_points() > 100 && pt_clouds.at(k)->get_num_points() > 100 &&
                        (pt_clouds.at(j)->get_num_points() + pt_clouds.at(k)->get_num_points()) > 400 ||
                    pt_clouds.at(j)->get_num_points() > 500 && pt_clouds.at(k)->get_num_points() > 500)
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    TVector3 dir1 = VHoughTrans(p1, 30 * units::cm, pt_clouds.at(j));
                    TVector3 dir2 = VHoughTrans(p2, 30 * units::cm, pt_clouds.at(k));
                    dir1 *= -1;
                    dir2 *= -1;

                    std::pair<int, double> result1 = pt_clouds.at(k)->get_closest_point_along_vec(p1, dir1, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);

                    if (result1.first >= 0)
                    {
                        index_index_dis_dir1[j][k] = std::make_tuple(std::get<0>(index_index_dis[j][k]), result1.first, result1.second);
                    }

                    std::pair<int, double> result2 = pt_clouds.at(j)->get_closest_point_along_vec(p2, dir2, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);
                    if (result2.first >= 0)
                    {
                        index_index_dis_dir2[j][k] = std::make_tuple(result2.first, std::get<1>(index_index_dis[j][k]), result2.second);
                    }
                }

                // Now check the path ...
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;

                    // int num_bad = 0;
                    // int num_bad1 = 0;
                    int num_bad[4] = {0, 0, 0, 0};  // more than one of three are bad
                    int num_bad1[4] = {0, 0, 0, 0}; // at least one of three are bad
                    int num_bad2[3] = {0, 0, 0};    // number of dead channels
                    for (int ii = 0; ii != num_steps; ii++)
                    {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);

                        std::vector<int> scores = ct_point_cloud.test_good_point(test_p);
                        // if ((p1.x>=130*units::cm && p1.x<=155*units::cm && p1.z > 815*units::cm && p1.z < 830*units::cm)&&
                        //   ((p2.x > 130*units::cm && p2.x <=155*units::cm && p2.z >815*units::cm && p2.z <830*units::cm))
                        // 	  ){
                        // 	std::cout << scores[0] << " " << scores[3] << " "
                        // 		  << scores[1] << " " << scores[4] << " "
                        // 		  << scores[2] << " " << scores[5] << std::endl;
                        //   }
                        if (scores[0] + scores[3] + scores[1] + scores[4] + (scores[2] + scores[5]) * 2 < 3)
                        { // num_bad
                            num_bad[0]++;
                        }
                        if (scores[0] + scores[3] == 0)
                            num_bad[1]++;
                        if (scores[1] + scores[4] == 0)
                            num_bad[2]++;
                        if (scores[2] + scores[5] == 0)
                            num_bad[3]++;

                        if (scores[3] != 0)
                            num_bad2[0]++;
                        if (scores[4] != 0)
                            num_bad2[1]++;
                        if (scores[5] != 0)
                            num_bad2[2]++;

                        if (scores[0] + scores[3] + scores[1] + scores[4] + (scores[2] + scores[5]) < 3)
                        { // num_bad1

                            num_bad1[0]++;
                        }
                        if (scores[0] + scores[3] == 0)
                            num_bad1[1]++;
                        if (scores[1] + scores[4] == 0)
                            num_bad1[2]++;
                        if (scores[2] + scores[5] == 0)
                            num_bad1[3]++;
                        // if (!ct_point_cloud.is_good_point_wc(test_p)) num_bad ++;
                        // if (!ct_point_cloud.is_good_point_wc(test_p,0.6*units::cm,1,0)) num_bad1 ++;
                    }

                    TVector3 tempV1(0, p2.y - p1.y, p2.z - p1.z);
                    TVector3 tempV5;
                    double angle1 = tempV1.Angle(U_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle1), 0);
                    angle1 = tempV5.Angle(drift_dir);
                    double angle2 = tempV1.Angle(V_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle2), 0);
                    angle2 = tempV5.Angle(drift_dir);
                    tempV5.SetXYZ(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);
                    double angle1p = tempV1.Angle(W_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle1p), 0);
                    angle1p = tempV5.Angle(drift_dir);
                    double angle3 = tempV5.Angle(drift_dir);

                    bool flag_strong_check = true;

                    //	  if (num_bad2[0]>0.95*num_steps || num_bad2[1] > 0.95*num_steps || num_bad2[2] > 0.95*num_steps){
                    // if (fabs(angle3-3.1415926/2.)<5/180.*3.1415926){
                    //   TVector3 tempV2 = VHoughTrans(p1, 15*units::cm);
                    //   TVector3 tempV3 = VHoughTrans(p2, 15*units::cm);
                    //   if ( fabs(tempV2.Angle(drift_dir)-3.1415926/2.)<5/180.*3.1415926 &&
                    // 	   fabs(tempV3.Angle(drift_dir)-3.1415926/2.)<5/180.*3.1415926)
                    // 	flag_strong_check = false;
                    // }else if ( angle1<5/180.*3.1415926  || angle2<5/180.*3.1415926 || angle1p < 5/180.*3.1415926){
                    //   flag_strong_check = false;
                    // }
                    // }else{
                    if (fabs(angle3 - 3.1415926 / 2.) < 10 / 180. * 3.1415926)
                    {
                        TVector3 tempV2 = VHoughTrans(p1, 15 * units::cm);
                        TVector3 tempV3 = VHoughTrans(p2, 15 * units::cm);
                        if (fabs(tempV2.Angle(drift_dir) - 3.1415926 / 2.) < 10 / 180. * 3.1415926 &&
                            fabs(tempV3.Angle(drift_dir) - 3.1415926 / 2.) < 10 / 180. * 3.1415926)
                            flag_strong_check = false;
                    }
                    else if (angle1 < 12.5 / 180. * 3.1415926 || angle2 < 12.5 / 180. * 3.1415926 || angle1p < 12.5 / 180. * 3.1415926)
                    {
                        flag_strong_check = false;
                    }
                    // }

                    if (flag_strong_check)
                    {
                        if (num_bad1[0] > 7 || num_bad1[0] > 2 && num_bad1[0] >= 0.75 * num_steps)
                        {
                            index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                        }

                        // if ((num_bad2[0]==num_steps && num_bad2[1] == num_steps ||
                        // 	 num_bad2[0]==num_steps && num_bad2[2] == num_steps ||
                        // 	 num_bad2[1]==num_steps && num_bad2[2] == num_steps ) &&
                        // 	dis > 5*units::cm && num_bad1[0] ==0 )
                        //   index_index_dis[j][k] = std::make_tuple(-1,-1,1e9);
                    }
                    else
                    {
                        if ((angle1 < 12.5 / 180. * 3.1415926 && angle2 < 12.5 / 180. * 3.1415926 ||
                             angle1p < 12.5 / 180. * 3.1415926 && angle1 < 12.5 / 180. * 3.1415926 ||
                             angle1p < 12.5 / 180. * 3.1415926 && angle2 < 12.5 / 180. * 3.1415926))
                        {
                            if (num_bad[0] > 7 || num_bad[0] > 2 && num_bad[0] >= 0.75 * num_steps)
                                index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                        else if (angle1 < 12.5 / 180. * 3.1415926 && (num_bad[2] + num_bad[3] > 9 || num_bad[2] + num_bad[3] > 2 && num_bad[2] + num_bad[3] >= 0.75 * num_steps || num_bad[3] >= 3))
                        {
                            index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                        else if (angle2 < 12.5 / 180. * 3.1415926 && (num_bad[1] + num_bad[3] > 9 || num_bad[1] + num_bad[3] > 2 && num_bad[1] + num_bad[3] >= 0.75 * num_steps || num_bad[3] >= 3))
                        {
                            index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                        else if (angle1p < 12.5 / 180. * 3.1415926 && (num_bad[2] + num_bad[1] > 9 || num_bad[2] + num_bad[1] > 2 && num_bad[2] + num_bad[1] >= 0.75 * num_steps))
                        {
                            index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                        else
                        {
                            if (num_bad[0] > 7 || num_bad[0] > 2 && num_bad[0] >= 0.75 * num_steps)
                            {
                                index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                            }
                        }
                    }
                    // if ((p1.x>=130*units::cm && p1.x<=155*units::cm && p1.z > 815*units::cm && p1.z < 830*units::cm)&&
                    //     ((p2.x > 130*units::cm && p2.x <=155*units::cm && p2.z >815*units::cm && p2.z <830*units::cm))
                    //     // ||
                    //     //(p2.x > 180*units::cm && p2.x <=220*units::cm && p2.z >680*units::cm && p2.z <720*units::cm) &&
                    //     //(!(p1.x>=180*units::cm && p1.x<=220*units::cm && p1.z > 680*units::cm && p1.z < 720*units::cm))
                    //     ){

                    //     std::cout << flag_strong_check << " " << j << " " << pt_clouds.at(j)->get_num_points() << " " << k << " " << pt_clouds.at(k)->get_num_points() << " " << num_bad[0] << " " << num_bad[1] << " " << num_bad[2] << " " << num_bad[3] << " " << num_bad1[0] << " " << num_steps << " " <<
                    //      p1 << " " << p2 << " " << std::get<0>(index_index_dis[j][k]) << " " <<
                    //      std::get<1>(index_index_dis[j][k]) << " " << std::get<2>(index_index_dis[j][k]) << " " << angle3/3.1415926*180. << " " << angle1/3.1415926*180. << " " << angle2/3.1415926*180. << " " << angle1p/3.1415926*180. << std::endl;
                    // }
                }

                // Now check the path ...
                if (std::get<0>(index_index_dis_dir1[j][k]) >= 0)
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis_dir1[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis_dir1[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    int num_bad1 = 0;
                    for (int ii = 0; ii != num_steps; ii++)
                    {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point_wc(test_p))
                            num_bad++;
                        if (!ct_point_cloud.is_good_point_wc(test_p, 0.6 * units::cm, 1, 0))
                            num_bad1++;
                    }

                    TVector3 tempV1(0, p2.y - p1.y, p2.z - p1.z);
                    TVector3 tempV5;
                    double angle1 = tempV1.Angle(U_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle1), 0);
                    angle1 = tempV5.Angle(drift_dir);
                    double angle2 = tempV1.Angle(V_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle2), 0);
                    angle2 = tempV5.Angle(drift_dir);
                    tempV5.SetXYZ(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);
                    double angle3 = tempV5.Angle(drift_dir);
                    double angle1p = tempV1.Angle(W_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle1p), 0);
                    angle1p = tempV5.Angle(drift_dir);

                    if (fabs(angle3 - 3.1415926 / 2.) < 10 / 180. * 3.1415926 || angle1 < 12.5 / 180. * 3.1415926 || angle2 < 12.5 / 180. * 3.1415926 || angle1p < 7.5 / 180. * 3.1415926)
                    {
                        // parallel or prolonged case
                        if (num_bad > 7 || num_bad > 2 && num_bad >= 0.75 * num_steps)
                        {
                            index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                    }
                    else
                    {
                        if (num_bad1 > 7 || num_bad1 > 2 && num_bad1 >= 0.75 * num_steps)
                        {
                            index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                    }
                }

                // Now check the path ...
                if (std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis_dir2[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis_dir2[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    int num_bad1 = 0;
                    for (int ii = 0; ii != num_steps; ii++)
                    {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point_wc(test_p))
                            num_bad++;
                        if (!ct_point_cloud.is_good_point_wc(test_p, 0.6 * units::cm, 1, 0))
                            num_bad1++;
                    }

                    TVector3 tempV1(0, p2.y - p1.y, p2.z - p1.z);
                    TVector3 tempV5;
                    double angle1 = tempV1.Angle(U_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle1), 0);
                    angle1 = tempV5.Angle(drift_dir);
                    double angle2 = tempV1.Angle(V_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle2), 0);
                    angle2 = tempV5.Angle(drift_dir);
                    tempV5.SetXYZ(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);
                    double angle3 = tempV5.Angle(drift_dir);
                    double angle1p = tempV1.Angle(W_dir);
                    tempV5.SetXYZ(fabs(p2.x - p1.x), sqrt(pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2)) * sin(angle1p), 0);
                    angle1p = tempV5.Angle(drift_dir);

                    if (fabs(angle3 - 3.1415926 / 2.) < 10 / 180. * 3.1415926 || angle1 < 12.5 / 180. * 3.1415926 || angle2 < 12.5 / 180. * 3.1415926 || angle1p < 7.5 / 180. * 3.1415926)
                    {
                        // parallel or prolonged case
                        if (num_bad > 7 || num_bad > 2 && num_bad >= 0.75 * num_steps)
                        {
                            index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                    }
                    else
                    {
                        if (num_bad1 > 7 || num_bad1 > 2 && num_bad1 >= 0.75 * num_steps)
                        {
                            index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                        }
                    }
                }
            }
        }

        // deal with MST of first type
        {
            const int N = num;
            boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                                  boost::no_property, boost::property<boost::edge_weight_t, double>>
                temp_graph(N);

            for (int j = 0; j != num; j++)
            {
                for (int k = j + 1; k != num; k++)
                {
                    int index1 = j;
                    int index2 = k;
                    if (std::get<0>(index_index_dis[j][k]) >= 0)
                    {
                        //	    std::cout << "A: " << index1 << " " << index2 << std::endl;
                        auto edge = add_edge(index1, index2, std::get<2>(index_index_dis[j][k]), temp_graph);
                    }
                }
            }

            {
                std::vector<int> possible_root_vertex;
                std::vector<int> component(num_vertices(temp_graph));
                const int num1 = connected_components(temp_graph, &component[0]);
                possible_root_vertex.resize(num1);
                std::vector<int>::size_type i;
                for (i = 0; i != component.size(); ++i)
                {
                    possible_root_vertex.at(component[i]) = i;
                }

                for (size_t i = 0; i != possible_root_vertex.size(); i++)
                {
                    std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(num_vertices(temp_graph));

                    prim_minimum_spanning_tree(temp_graph, &predecessors[0], boost::root_vertex(possible_root_vertex.at(i)));

                    for (size_t j = 0; j != predecessors.size(); ++j)
                    {
                        if (predecessors[j] != j)
                        {
                            if (j < predecessors[j])
                            {
                                index_index_dis_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                            }
                            else
                            {
                                index_index_dis_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                            }
                            //	      std::cout << j << " " << predecessors[j] << " " << std::endl;
                        }
                        else
                        {
                            // std::cout << j << " " << std::endl;
                        }
                    }
                }
            }
        }

        // MST of the direction ...
        {
            const int N = num;
            boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                                  boost::no_property, boost::property<boost::edge_weight_t, double>>
                temp_graph(N);

            for (int j = 0; j != num; j++)
            {
                for (int k = j + 1; k != num; k++)
                {
                    int index1 = j;
                    int index2 = k;
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0 || std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                        auto edge = add_edge(index1, index2, std::min(std::get<2>(index_index_dis_dir1[j][k]), std::get<2>(index_index_dis_dir2[j][k])), temp_graph);
                }
            }

            {
                std::vector<int> possible_root_vertex;
                std::vector<int> component(num_vertices(temp_graph));
                const int num1 = connected_components(temp_graph, &component[0]);
                possible_root_vertex.resize(num1);
                std::vector<int>::size_type i;
                for (i = 0; i != component.size(); ++i)
                {
                    possible_root_vertex.at(component[i]) = i;
                }
                for (size_t i = 0; i != possible_root_vertex.size(); i++)
                {
                    std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(num_vertices(temp_graph));
                    prim_minimum_spanning_tree(temp_graph, &predecessors[0], boost::root_vertex(possible_root_vertex.at(i)));
                    for (size_t j = 0; j != predecessors.size(); ++j)
                    {
                        if (predecessors[j] != j)
                        {
                            if (j < predecessors[j])
                            {
                                index_index_dis_dir_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                            }
                            else
                            {
                                index_index_dis_dir_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                            }
                            // std::cout << j << " " << predecessors[j] << " " << std::endl;
                        }
                        else
                        {
                            // std::cout << j << " " << std::endl;
                        }
                    }
                }
            }
        }

        for (int j = 0; j != num; j++)
        {
            for (int k = j + 1; k != num; k++)
            {
                if (std::get<2>(index_index_dis[j][k]) < 3 * units::cm)
                {
                    index_index_dis_mst[j][k] = index_index_dis[j][k];
                }

                // establish the path ...
                if (std::get<0>(index_index_dis_mst[j][k]) >= 0)
                {
                    auto edge = add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]), *graph);
                    // std::cout << j << " " << k << std::endl;
                    if (edge.second)
                    {
                        if (std::get<2>(index_index_dis_mst[j][k]) > 5 * units::cm)
                        {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                        else
                        {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                    }
                }

                if (std::get<0>(index_index_dis_dir_mst[j][k]) >= 0)
                {
                    // std::cout << j << " " << k << std::endl;
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0)
                    {
                        auto edge = add_edge(std::get<0>(index_index_dis_dir1[j][k]), std::get<1>(index_index_dis_dir1[j][k]), *graph);
                        if (edge.second)
                        {
                            if (std::get<2>(index_index_dis_dir1[j][k]) > 5 * units::cm)
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]) * 1.1;
                            }
                            else
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]);
                            }
                        }
                    }
                    if (std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                    {
                        // std::cout << j << " " << k << std::endl;
                        auto edge = add_edge(std::get<0>(index_index_dis_dir2[j][k]), std::get<1>(index_index_dis_dir2[j][k]), *graph);
                        if (edge.second)
                        {
                            if (std::get<2>(index_index_dis_dir2[j][k]) > 5 * units::cm)
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]) * 1.1;
                            }
                            else
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]);
                            }
                        }
                    }
                }

            } // k
        } // j

        //    std::cout << "AA" << std::endl;
        // delete newly created point cloud
        for (int i = 0; i != num; i++)
        {
            delete pt_clouds.at(i);
        }
        pt_clouds.clear();
    }
}

std::vector<SMGCSelection> PR3DCluster::Examine_graph(WCP::ToyCTPointCloud &ct_point_cloud)
{
    if (graph != (MCUGraph *)0)
        delete graph;
    if (point_cloud == (ToyPointCloud *)0)
        Create_point_cloud();

    // form connected_pieces ...
    const int N = point_cloud->get_num_points();
    graph = new MCUGraph(N);
    Establish_close_connected_graph();

    Connect_graph_overclustering_protection(ct_point_cloud);
    std::vector<int> component(num_vertices(*graph));
    const int num = connected_components(*graph, &component[0]);

    std::vector<SMGCSelection> sep_mcells;
    std::set<SlimMergeGeomCell *> used_mcells;
    for (int i = 0; i != num; i++)
    {
        SMGCSelection mcells;
        sep_mcells.push_back(mcells);
    }

    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    std::vector<int>::size_type i;
    for (i = 0; i != component.size(); ++i)
    {
        SlimMergeGeomCell *mcell = cloud.pts[i].mcell;
        if (used_mcells.find(mcell) == used_mcells.end())
        {
            used_mcells.insert(mcell);
            sep_mcells[component[i]].push_back(mcell);
        }
        // pt_clouds.at(component[i])->AddPoint(cloud.pts[i],cloud_u.pts[i],cloud_v.pts[i],cloud_w.pts[i]);
    }

    // std::cout << num << std::endl;
    // for (int i=0;i!=num;i++){
    //   std::cout << i << " " << sep_mcells.at(i).size() << std::endl;
    // }

    return sep_mcells;
}

void PR3DCluster::Create_graph(WCP::ToyCTPointCloud &ct_point_cloud)
{
    if (graph != (MCUGraph *)0)
        return;

    if (point_cloud == (ToyPointCloud *)0)
        Create_point_cloud();

    const int N = point_cloud->get_num_points();
    graph = new MCUGraph(N);

    Establish_close_connected_graph();
    Connect_graph(ct_point_cloud);
    Connect_graph();
}

void PR3DCluster::Connect_graph()
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCP::WC2DPointCloud<double> &cloud_u = point_cloud->get_cloud_u();
    WCP::WC2DPointCloud<double> &cloud_v = point_cloud->get_cloud_v();
    WCP::WC2DPointCloud<double> &cloud_w = point_cloud->get_cloud_w();

    std::vector<int> component(num_vertices(*graph));
    const int num = connected_components(*graph, &component[0]);

    if (num > 1)
    {
        // for separated kd tree to find the closest points between disconnected components,
        std::vector<ToyPointCloud *> pt_clouds;
        for (int j = 0; j != num; j++)
        {
            ToyPointCloud *pt_cloud = new ToyPointCloud();
            pt_clouds.push_back(pt_cloud);
        }

        std::vector<int>::size_type i;
        for (i = 0; i != component.size(); ++i)
        {
            pt_clouds.at(component[i])->AddPoint(cloud.pts[i], cloud_u.pts[i], cloud_v.pts[i], cloud_w.pts[i]);
            //   std::cout << "Vertex " << 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 << " " << cloud.pts[i].mcell->GetTimeSlice()  << " is in component " << component[i] << std::endl;
        }
        for (int j = 0; j != num; j++)
        {
            pt_clouds.at(j)->build_kdtree_index();
        }

        // std::cout << "Xin: " << num << std::endl;
        //  connect these graphs according to closest distance some how ...

        // std::tuple<int,int,double> index_index_dis[num][num];
        // std::tuple<int,int,double> index_index_dis_mst[num][num];
        // std::tuple<int,int,double> index_index_dis_dir1[num][num];
        // std::tuple<int,int,double> index_index_dis_dir2[num][num];

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_mst(num, std::vector<std::tuple<int, int, double>>(num));

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir1(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir2(num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir_mst(num, std::vector<std::tuple<int, int, double>>(num));

        for (int j = 0; j != num; j++)
        {
            for (int k = 0; k != num; k++)
            {
                index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_mst[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir_mst[j][k] = std::make_tuple(-1, -1, 1e9);
            }
        }

        // MST ...
        const int N = num;
        boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                              boost::no_property, boost::property<boost::edge_weight_t, double>>
            temp_graph(N);

        for (int j = 0; j != num; j++)
        {
            for (int k = j + 1; k != num; k++)
            {
                index_index_dis[j][k] = pt_clouds.at(j)->get_closest_points(pt_clouds.at(k));
                int index1 = j;
                int index2 = k;
                auto edge = add_edge(index1, index2, std::get<2>(index_index_dis[j][k]), temp_graph);
            }
        }

        {
            std::vector<int> possible_root_vertex;
            std::vector<int> component(num_vertices(temp_graph));
            const int num1 = connected_components(temp_graph, &component[0]);
            possible_root_vertex.resize(num1);
            std::vector<int>::size_type i;
            for (i = 0; i != component.size(); ++i)
            {
                possible_root_vertex.at(component[i]) = i;
            }

            for (size_t i = 0; i != possible_root_vertex.size(); i++)
            {
                std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(num_vertices(temp_graph));
                prim_minimum_spanning_tree(temp_graph, &predecessors[0], boost::root_vertex(possible_root_vertex.at(i)));
                for (size_t j = 0; j != predecessors.size(); ++j)
                {
                    if (predecessors[j] != j)
                    {
                        if (j < predecessors[j])
                        {
                            index_index_dis_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                        }
                        else
                        {
                            index_index_dis_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                        }
                        // std::cout << j << " " << predecessors[j] << " " << std::endl;
                    }
                    else
                    {
                        // std::cout << j << " " << std::endl;
                    }
                }
            }
        }
        // end of mst ...

        // short distance part
        for (int j = 0; j != num; j++)
        {
            for (int k = j + 1; k != num; k++)
            {
                // closest distance one ...
                if (std::get<2>(index_index_dis[j][k]) < 3 * units::cm)
                {
                    index_index_dis_mst[j][k] = index_index_dis[j][k];
                }

                if (num < 100)
                    if (pt_clouds.at(j)->get_num_points() > 100 && pt_clouds.at(k)->get_num_points() > 100 &&
                        (pt_clouds.at(j)->get_num_points() + pt_clouds.at(k)->get_num_points()) > 400)
                    {
                        WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis[j][k]));
                        WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis[j][k]));
                        Point p1(wp1.x, wp1.y, wp1.z);
                        Point p2(wp2.x, wp2.y, wp2.z);

                        TVector3 dir1 = VHoughTrans(p1, 30 * units::cm, pt_clouds.at(j));
                        TVector3 dir2 = VHoughTrans(p2, 30 * units::cm, pt_clouds.at(k));
                        dir1 *= -1;
                        dir2 *= -1;

                        std::pair<int, double> result1 = pt_clouds.at(k)->get_closest_point_along_vec(p1, dir1, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);

                        if (result1.first >= 0)
                        {
                            // Point test_p1(cloud.pts.at(std::get<0>(index_index_dis[j][k])).x,cloud.pts.at(std::get<0>(index_index_dis[j][k])).y,cloud.pts.at(std::get<0>(index_index_dis[j][k])).z);
                            // Point test_p2(cloud.pts.at(result1.first).x,cloud.pts.at(result1.first).y,cloud.pts.at(result1.first).z);
                            // double dis = sqrt(pow(test_p2.x-test_p1.x,2)+pow(test_p2.y-test_p1.y,2)+pow(test_p2.z-test_p1.z,2));
                            // int num_points = dis/(1.5*units::cm)+1;
                            // int num_cut_points = 0;
                            // for (size_t k1=0; k1!=num_points-1; k1++){
                            // 	Point test_p3(test_p1.x + (test_p2.x-test_p1.x) * (k1+1)/num_points ,
                            // 		      test_p1.y + (test_p2.y-test_p1.y) * (k1+1)/num_points ,
                            // 		      test_p1.z + (test_p2.z-test_p1.z) * (k1+1)/num_points );
                            // 	double dis1 = point_cloud->get_closest_dis(test_p3);
                            // 	if (dis1 < 1*units::cm)
                            // 	  num_cut_points ++;
                            // }
                            // if (num_cut_points <=8 && num_cut_points< 0.25 * num_points + 2 && dis > 5*units::cm)
                            index_index_dis_dir1[j][k] = std::make_tuple(std::get<0>(index_index_dis[j][k]), result1.first, result1.second);
                        }

                        std::pair<int, double> result2 = pt_clouds.at(j)->get_closest_point_along_vec(p2, dir2, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);

                        if (result2.first >= 0)
                        {

                            // Point test_p1(cloud.pts.at(std::get<1>(index_index_dis[j][k])).x,cloud.pts.at(std::get<1>(index_index_dis[j][k])).y,cloud.pts.at(std::get<1>(index_index_dis[j][k])).z);
                            // Point test_p2(cloud.pts.at(result2.first).x,cloud.pts.at(result2.first).y,cloud.pts.at(result2.first).z);
                            // double dis = sqrt(pow(test_p2.x-test_p1.x,2)+pow(test_p2.y-test_p1.y,2)+pow(test_p2.z-test_p1.z,2));
                            // int num_points = dis/(1.5*units::cm)+1;
                            // int num_cut_points = 0;
                            // for (size_t k1=0; k1!=num_points-1; k1++){
                            // 	Point test_p3(test_p1.x + (test_p2.x-test_p1.x) * (k1+1)/num_points ,
                            // 		      test_p1.y + (test_p2.y-test_p1.y) * (k1+1)/num_points ,
                            // 		      test_p1.z + (test_p2.z-test_p1.z) * (k1+1)/num_points );
                            // 	double dis1 = point_cloud->get_closest_dis(test_p3);
                            // 	if ( dis1 < 1*units::cm)
                            // 	  num_cut_points ++;
                            // }

                            // if (num_cut_points <=8 && num_cut_points < 0.25 * num_points + 2 && dis > 5*units::cm)
                            index_index_dis_dir2[j][k] = std::make_tuple(result2.first, std::get<1>(index_index_dis[j][k]), result2.second);
                        }
                    }
            }
        }

        // MST for the directionality ...
        {
            const int N = num;
            boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                                  boost::no_property, boost::property<boost::edge_weight_t, double>>
                temp_graph(N);

            for (int j = 0; j != num; j++)
            {
                for (int k = j + 1; k != num; k++)
                {
                    int index1 = j;
                    int index2 = k;
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0 || std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                        auto edge = add_edge(index1, index2, std::min(std::get<2>(index_index_dis_dir1[j][k]), std::get<2>(index_index_dis_dir2[j][k])), temp_graph);
                }
            }

            {
                std::vector<int> possible_root_vertex;
                std::vector<int> component(num_vertices(temp_graph));
                const int num1 = connected_components(temp_graph, &component[0]);
                possible_root_vertex.resize(num1);
                std::vector<int>::size_type i;
                for (i = 0; i != component.size(); ++i)
                {
                    possible_root_vertex.at(component[i]) = i;
                }
                for (size_t i = 0; i != possible_root_vertex.size(); i++)
                {
                    std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(num_vertices(temp_graph));
                    prim_minimum_spanning_tree(temp_graph, &predecessors[0], boost::root_vertex(possible_root_vertex.at(i)));
                    for (size_t j = 0; j != predecessors.size(); ++j)
                    {
                        if (predecessors[j] != j)
                        {
                            if (j < predecessors[j])
                            {
                                index_index_dis_dir_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                            }
                            else
                            {
                                index_index_dis_dir_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                            }
                            // std::cout << j << " " << predecessors[j] << " " << std::endl;
                        }
                        else
                        {
                            // std::cout << j << " " << std::endl;
                        }
                    }
                }
            }
        }

        // now complete graph according to the direction
        // according to direction ...
        for (int j = 0; j != num; j++)
        {
            for (int k = j + 1; k != num; k++)
            {
                if (std::get<0>(index_index_dis_mst[j][k]) >= 0)
                {
                    auto edge = add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]), *graph);
                    if (edge.second)
                    {
                        if (std::get<2>(index_index_dis_mst[j][k]) > 5 * units::cm)
                        {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                        else
                        {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                    }
                }

                if (std::get<0>(index_index_dis_dir_mst[j][k]) >= 0)
                {
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0)
                    {
                        auto edge = add_edge(std::get<0>(index_index_dis_dir1[j][k]), std::get<1>(index_index_dis_dir1[j][k]), *graph);
                        if (edge.second)
                        {
                            if (std::get<2>(index_index_dis_dir1[j][k]) > 5 * units::cm)
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]) * 1.2;
                                // }else if (std::get<2>(index_index_dis_dir1[j][k])>2*units::cm){
                                // 	(*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k])*1.1;
                            }
                            else
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]);
                            }
                        }
                    }
                    if (std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                    {
                        auto edge = add_edge(std::get<0>(index_index_dis_dir2[j][k]), std::get<1>(index_index_dis_dir2[j][k]), *graph);
                        if (edge.second)
                        {
                            if (std::get<2>(index_index_dis_dir2[j][k]) > 5 * units::cm)
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]) * 1.2;
                                // }else if(std::get<2>(index_index_dis_dir2[j][k])>2*units::cm){
                                // 	(*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k])*1.1;
                            }
                            else
                            {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]);
                            }
                        }
                    }
                }
            }
        }

        for (int i = 0; i != num; i++)
        {
            delete pt_clouds.at(i);
        }
    }
}

void PR3DCluster::Create_graph()
{
    std::cout << "Create Graph! " << cluster_id << " " << graph << std::endl;

    if (graph != (MCUGraph *)0)
        return;

    if (point_cloud == (ToyPointCloud *)0)
        Create_point_cloud();

    // create Graph ...
    const int N = point_cloud->get_num_points();
    graph = new MCUGraph(N);

    Establish_close_connected_graph();
    Connect_graph();

    // {
    //   std::vector<int> component(num_vertices(*graph));
    //   const int num = connected_components(*graph,&component[0]);
    //   if (num>1) std::cout << "Wrong! " << num << std::endl;
    // }
}

void PR3DCluster::dijkstra_shortest_paths(WCPointCloud<double>::WCPoint &wcp)
{
    if (graph == (MCUGraph *)0)
        Create_graph();
    if (wcp.index == source_wcp_index)
        return;
    source_wcp_index = wcp.index;
    parents.resize(num_vertices(*graph));
    distances.resize(num_vertices(*graph));

    auto v0 = vertex(wcp.index, *graph);
    boost::dijkstra_shortest_paths(*graph, v0,
                                   weight_map(get(&EdgeProp::dist, *graph))
                                       .predecessor_map(&parents[0])
                                       .distance_map(&distances[0]));
}

void PR3DCluster::dijkstra_shortest_paths(WCPointCloud<double>::WCPoint &wcp, WCP::ToyCTPointCloud &ct_point_cloud)
{
    if (graph == (MCUGraph *)0)
        Create_graph(ct_point_cloud);
    if (wcp.index == source_wcp_index)
        return;
    source_wcp_index = wcp.index;
    parents.resize(num_vertices(*graph));
    distances.resize(num_vertices(*graph));

    auto v0 = vertex(wcp.index, *graph);
    boost::dijkstra_shortest_paths(*graph, v0,
                                   weight_map(get(&EdgeProp::dist, *graph))
                                       .predecessor_map(&parents[0])
                                       .distance_map(&distances[0]));
}

void PR3DCluster::cal_shortest_path(WCPointCloud<double>::WCPoint &wcp_target)
{
    dest_wcp_index = wcp_target.index;
    path_wcps.clear();
    path_mcells.clear();

    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    int prev_i = -1;
    for (int i = dest_wcp_index; i != source_wcp_index; i = parents[i])
    {
        if (path_wcps.size() == 0)
        {
            path_wcps.push_front(cloud.pts[i]);
            path_mcells.push_front(cloud.pts[i].mcell);
        }
        else
        {
            path_wcps.push_front(cloud.pts[i]);
            if (cloud.pts[i].mcell != path_mcells.front())
                path_mcells.push_front(cloud.pts[i].mcell);
        }
        if (i == prev_i)
            break;
        prev_i = i;
    }
    path_wcps.push_front(cloud.pts[source_wcp_index]);
    if (cloud.pts[source_wcp_index].mcell != path_mcells.front())
        path_mcells.push_front(cloud.pts[source_wcp_index].mcell);
}

// void PR3DCluster::fine_tracking_old(int num_pts_cut){
//   // cut ...
//   if (path_wcps.size() < num_pts_cut) return;

//   WCP::WCPointCloud<double>& cloud = point_cloud->get_cloud();

//   // find the trajectory ...
//   //skip anypoint which is further away than 0.5 cm
//   double low_dis_limit = 0.5*units::cm;
//   std::vector<WCPointCloud<double>::WCPoint> path_wcps_vec;
//   std::vector<double> distances;
//   for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
//     if (path_wcps_vec.size()==0){
//       path_wcps_vec.push_back(*it);
//     }else{
//       double dis = sqrt(pow((*it).x - path_wcps_vec.back().x,2)
// 			+pow((*it).y - path_wcps_vec.back().y,2)
// 			+pow((*it).z - path_wcps_vec.back().z,2));
//       if (dis > low_dis_limit){
// 	path_wcps_vec.push_back(*it);
// 	distances.push_back(dis);
//       }
//     }
//     //    if (path_wcps_vec.size()==2) break;
//   }

//   // for (size_t i=0;i!=distances.size();i++){
//   //   std::cout << i << " " << path_wcps_vec.at(i).x/units::cm << " " << path_wcps_vec.at(i).y/units::cm << " " << path_wcps_vec.at(i).z/units::cm << " " << distances.at(i)/units::cm << std::endl;
//   // }

//   // figure out the charge ...
//   //form a map, (U,T) --> charge and error
//   //form a map, (V,T) --> charge and error
//   //form a map, (Z,T) --> charge and error
//   std::map<std::pair<int,int>,double> map_2D_ut_charge, map_2D_ut_charge_err;
//   std::map<std::pair<int,int>,double> map_2D_vt_charge, map_2D_vt_charge_err;
//   std::map<std::pair<int,int>,double> map_2D_wt_charge, map_2D_wt_charge_err;
//   for (auto it=mcells.begin();it!=mcells.end();it++){
//     SlimMergeGeomCell *mcell = (*it);
//     int time_slice = mcell->GetTimeSlice();
//     WireChargeMap& wire_charge_map = mcell->get_wirecharge_map();
//     WireChargeMap& wire_charge_err_map = mcell->get_wirechargeerr_map();
//     for (auto it = wire_charge_map.begin(); it!= wire_charge_map.end(); it++){
//       const GeomWire* wire = it->first;
//       double charge = it->second;
//       double charge_err = wire_charge_err_map[wire];
//       //std::cout << wire << " " << charge << " " << charge_err << std::endl;

//       // hack the charge ...
//       if (charge <=0){
// 	continue;
// 	//charge = 1000;
// 	//charge_err = 1000;
//       }

//       if (wire->iplane()==0){
// 	map_2D_ut_charge[std::make_pair(wire->index(),time_slice)] = charge;
// 	map_2D_ut_charge_err[std::make_pair(wire->index(),time_slice)] = charge_err;
//       }else if (wire->iplane()==1){
// 	map_2D_vt_charge[std::make_pair(wire->index(),time_slice)] = charge;
// 	map_2D_vt_charge_err[std::make_pair(wire->index(),time_slice)] = charge_err;
//       }else{
// 	map_2D_wt_charge[std::make_pair(wire->index(),time_slice)] = charge;
// 	map_2D_wt_charge_err[std::make_pair(wire->index(),time_slice)] = charge_err;
//       }

//     }
//     //    std::cout << wire_charge_map.size() << " " << wire_charge_err_map.size() << std::endl;
//   }

//   // Now prepare all the maps ...
//   // map 3D index to set of 2D points
//   std::map<int,std::set<std::pair<int,int>>> map_3D_2DU_set;
//   std::map<int,std::set<std::pair<int,int>>> map_3D_2DV_set;
//   std::map<int,std::set<std::pair<int,int>>> map_3D_2DW_set;
//   // map 2D points to 3D indices
//   std::map<std::pair<int,int>,std::set<int>> map_2DU_3D_set;
//   std::map<std::pair<int,int>,std::set<int>> map_2DV_3D_set;
//   std::map<std::pair<int,int>,std::set<int>> map_2DW_3D_set;

//   //std::cout << map_2DU_index.size() << " " << map_2DV_index.size() << " " << map_2DW_index.size() << std::endl;

//   TPCParams& mp = Singleton<TPCParams>::Instance();
//   double pitch_u = mp.get_pitch_u();
//   double pitch_v = mp.get_pitch_v();
//   double pitch_w = mp.get_pitch_w();
//   double angle_u = mp.get_angle_u();
//   double angle_v = mp.get_angle_v();
//   double angle_w = mp.get_angle_w();
//   double time_slice_width = mp.get_ts_width();
//   double first_u_dis = mp.get_first_u_dis();
//   double first_v_dis = mp.get_first_v_dis();
//   double first_w_dis = mp.get_first_w_dis();

//   float coef1 = 2 * pow(sin(angle_u),2);
//   float coef2 = 2 * (pow(sin(angle_u),2) - pow(cos(angle_u),2));

//   // graph based association ...
//   //std::cout << pitch_u/units::cm << " " << pitch_v/units::cm << " " << pitch_w/units::cm << " " << time_slice_width/units::cm << std::endl;
//   // Loop any point and try to find its 3-level neibours ...
//   typedef boost::property_map<MCUGraph, boost::vertex_index_t>::type IndexMap;
//   IndexMap index = get(boost::vertex_index,*graph);
//   typedef boost::graph_traits<MCUGraph>::adjacency_iterator adjacency_iterator;
//   int nlevel = 3;
//   double dis_cut,time_cut;
//   for (size_t i=0;i!=path_wcps_vec.size();i++){
//     int current_index = path_wcps_vec.at(i).index;
//     if (i==0){
//       dis_cut = std::min(distances.at(i) * 0.6,0.8*units::cm);
//     }else if (i==path_wcps_vec.size()-1){
//       dis_cut = std::min(distances.back() * 0.6,0.8*units::cm);
//     }else{
//       dis_cut = std::min(std::max(distances.at(i-1)*0.9,distances.at(i)*0.9),1.2*units::cm);
//     }

//     time_cut = 3; // allow +- 3 time slices and then distance cut ...
//     std::set<std::pair<int,int>> T2DU_set;
//     std::set<std::pair<int,int>> T2DV_set;
//     std::set<std::pair<int,int>> T2DW_set;
//     map_3D_2DU_set[i] = T2DU_set;
//     map_3D_2DV_set[i] = T2DV_set;
//     map_3D_2DW_set[i] = T2DW_set;

//     std::set<int> total_vertices_found;
//     std::set<int> vertices_to_be_examined;
//     std::set<int> vertices_saved_for_next;
//     total_vertices_found.insert(current_index);
//     vertices_to_be_examined.insert(current_index);

//     for (int j=0;j!=nlevel;j++){
//       for (auto it = vertices_to_be_examined.begin(); it!=vertices_to_be_examined.end(); it++){
//      	int temp_current_index = (*it);
//      	std::pair<adjacency_iterator, adjacency_iterator> neighbors = boost::adjacent_vertices(vertex(temp_current_index,*graph),*graph);
//      	for (; neighbors.first!=neighbors.second; ++neighbors.first){
//     	  //std::cout << *neighbors.first << " " << *neighbors.second << std::endl;
//     	  if (total_vertices_found.find(index(*neighbors.first))==total_vertices_found.end()){
// 	    total_vertices_found.insert(index(*neighbors.first));
// 	    vertices_saved_for_next.insert(index(*neighbors.first));
//     	  }
//     	}
//       }
//       vertices_to_be_examined = vertices_saved_for_next;
//     }
//     SMGCSet nearby_mcells_set;
//     for (auto it = total_vertices_found.begin(); it!=total_vertices_found.end(); it++){
//       SlimMergeGeomCell *mcell = cloud.pts[*it].mcell;
//       nearby_mcells_set.insert(mcell);
//     }
//     // std::cout << i << " " << total_vertices_found.size() << " " << nearby_mcells_set.size() << std::endl;

//     int cur_time_slice = cloud.pts[current_index].mcell->GetTimeSlice();
//     int cur_wire_u = cloud.pts[current_index].index_u;
//     int cur_wire_v = cloud.pts[current_index].index_v;
//     int cur_wire_w = cloud.pts[current_index].index_w;

//     // if (abs(cur_time_slice-1261)==0)
//     //   std::cout << "Center: " << i << " " << path_wcps_vec.at(i).mcell << " " << current_index <<  " " << cloud.pts[current_index].index << " " <<
//     // 	cloud.pts[current_index].mcell << " " << cloud.pts[current_index].x << " " << cur_time_slice << " " << cur_wire_u << " " << cur_wire_v << " " << cur_wire_v << std::endl;

//     // Now fill the other maps ...
//     for (auto it = nearby_mcells_set.begin(); it!=nearby_mcells_set.end(); it++){
//       SlimMergeGeomCell *mcell = *it;
//       int this_time_slice = mcell->GetTimeSlice();
//       double rem_dis_cut = pow(dis_cut,2) - pow((cur_time_slice - this_time_slice)*time_slice_width,2);
//       if (rem_dis_cut >0 && fabs(cur_time_slice-this_time_slice)<=time_cut){
//     	//	std::cout << this_time_slice << std::endl;
//     	GeomWireSelection uwires = mcell->get_uwires();
//     	GeomWireSelection vwires = mcell->get_vwires();
//     	GeomWireSelection wwires = mcell->get_wwires();

//     	float min_u_dis;
//     	if (cur_wire_u < uwires.front()->index()){
//     	  min_u_dis = uwires.front()->index()-cur_wire_u;
//     	}else if (cur_wire_u >= uwires.front()->index() &&
//     		  cur_wire_u <= uwires.back()->index()){
//     	  min_u_dis = 0;
//     	}else{
//     	  min_u_dis = cur_wire_u-uwires.back()->index();
//     	}
//     	float min_v_dis;
//     	if (cur_wire_v < vwires.front()->index()){
//     	  min_v_dis = vwires.front()->index()-cur_wire_v;
//     	}else if (cur_wire_v >= vwires.front()->index() &&
//     		  cur_wire_v <= vwires.back()->index()){
//     	  min_v_dis = 0;
//     	}else{
//     	  min_v_dis = cur_wire_v-vwires.back()->index();
//     	}
//     	float min_w_dis;
//     	if (cur_wire_w < wwires.front()->index()){
//     	  min_w_dis = wwires.front()->index()-cur_wire_w;
//     	}else if (cur_wire_w >= wwires.front()->index() &&
//     		  cur_wire_w <= wwires.back()->index()){
//     	  min_w_dis = 0;
//     	}else{
//     	  min_w_dis = cur_wire_w-wwires.back()->index();
//     	}

// 	// Note this assumes w is vertical wires, U and V have equal angle between then ...

//     	float range_u = rem_dis_cut*coef1 - pow(min_v_dis*pitch_v,2) - coef2*pow(min_w_dis*pitch_w,2);
//     	float range_v = rem_dis_cut*coef1 - pow(min_u_dis*pitch_u,2) - coef2*pow(min_w_dis*pitch_w,2);
//     	float range_w = (rem_dis_cut*coef1 - pow(min_u_dis*pitch_u,2) - pow(min_v_dis*pitch_v,2))/coef2;
// 	//std::cout << coef1 << " " << coef2 << std::endl;

//     	// if (abs(cur_time_slice-1261)==0)
//     	//   std::cout << min_u_dis << " " << min_v_dis << " " << min_w_dis << std::endl;

//     	if ( range_u > 0 && range_v >0 && range_w > 0){
//     	  float low_u_limit = cur_wire_u - sqrt(range_u)/pitch_u;
//     	  float high_u_limit = cur_wire_u + sqrt(range_u)/pitch_u;
//     	  float low_v_limit = cur_wire_v - sqrt(range_v)/pitch_v;
//     	  float high_v_limit = cur_wire_v + sqrt(range_v)/pitch_v;
//     	  float low_w_limit = cur_wire_w - sqrt(range_w)/pitch_w;
//     	  float high_w_limit = cur_wire_w + sqrt(range_w)/pitch_w;

//     	  // if (abs(cur_time_slice-1261)==0)
//     	  //   std::cout << low_u_limit << " " << high_u_limit << " " << low_v_limit << " " << high_v_limit << " " << low_w_limit << " " << high_w_limit << std::endl;

//     	  WireChargeMap& wire_charge_map = mcell->get_wirecharge_map();
//     	  for (auto it1 = wire_charge_map.begin(); it1!= wire_charge_map.end(); it1++){
// 	    const GeomWire *wire = it1->first;
// 	    if (it1->second >0){
// 	      //if (1>0){
// 	      if (wire->iplane()==0){
// 		// U plane ...
// 		if (wire->index() >= low_u_limit && wire->index() <= high_u_limit){
// 		  map_3D_2DU_set[i].insert(std::make_pair(wire->index(),this_time_slice));
// 		  if (map_2DU_3D_set.find(std::make_pair(wire->index(),this_time_slice))==map_2DU_3D_set.end()){
// 		    std::set<int>  temp_set;
// 		    temp_set.insert(i);
// 		    map_2DU_3D_set[std::make_pair(wire->index(),this_time_slice)] = temp_set;
// 		  }else{
// 		    map_2DU_3D_set[std::make_pair(wire->index(),this_time_slice)].insert(i);
// 		  }
// 		}
// 	      }else if (wire->iplane()==1){
// 		// V plane ...
// 		if (wire->index() >= low_v_limit && wire->index() <= high_v_limit){
// 		  map_3D_2DV_set[i].insert(std::make_pair(wire->index(),this_time_slice));
// 		  if (map_2DV_3D_set.find(std::make_pair(wire->index(),this_time_slice))==map_2DV_3D_set.end()){
// 		    std::set<int>  temp_set;
// 		    temp_set.insert(i);
// 		    map_2DV_3D_set[std::make_pair(wire->index(),this_time_slice)] = temp_set;
// 		  }else{
//     		  map_2DV_3D_set[std::make_pair(wire->index(),this_time_slice)].insert(i);
// 		  }
// 		}
// 	      }else{
// 		// W plane ...
// 		if (wire->index() >= low_w_limit && wire->index() <= high_w_limit){
// 		  map_3D_2DW_set[i].insert(std::make_pair(wire->index(),this_time_slice));
// 		  if (map_2DW_3D_set.find(std::make_pair(wire->index(),this_time_slice))==map_2DW_3D_set.end()){
// 		    std::set<int> temp_set;
// 		    temp_set.insert(i);
// 		    map_2DW_3D_set[std::make_pair(wire->index(),this_time_slice)] = temp_set;
// 		  }else{
// 		    map_2DW_3D_set[std::make_pair(wire->index(),this_time_slice)].insert(i);
// 		  }
// 		}
// 	      }
// 	    }
//     	  }
// 	}
//       }
//     }
//   } // i loop ...

//   // map 2D points to its index
//   std::vector<std::pair<std::pair<int,int>,int>> vec_2DU_index;
//   std::vector<std::pair<std::pair<int,int>,int>> vec_2DV_index;
//   std::vector<std::pair<std::pair<int,int>,int>> vec_2DW_index;

//   for (auto it = map_2DU_3D_set.begin(); it!= map_2DU_3D_set.end(); it++){
//     for (auto it1 =it->second.begin(); it1!=it->second.end(); it1++){
//       vec_2DU_index.push_back(std::make_pair(it->first,*it1));
//     }
//   }
//   for (auto it = map_2DV_3D_set.begin(); it!= map_2DV_3D_set.end(); it++){
//     for (auto it1 =it->second.begin(); it1!=it->second.end(); it1++){
//       vec_2DV_index.push_back(std::make_pair(it->first,*it1));
//     }
//   }
//   for (auto it = map_2DW_3D_set.begin(); it!= map_2DW_3D_set.end(); it++){
//     for (auto it1 =it->second.begin(); it1!=it->second.end(); it1++){
//       vec_2DW_index.push_back(std::make_pair(it->first,*it1));
//     }
//   }

//   //  std::cout << vec_2DU_index.size() << " " << vec_2DV_index.size() << " " << vec_2DW_index.size() << std::endl;

//   // for (auto it = map_2D_ut_charge.begin(); it!=map_2D_ut_charge.end();it++){
//   //   std::cout << "U_2: " << it->first.first << " " << it->first.second << std::endl;
//   // }
//   // for (auto it = map_2D_vt_charge.begin(); it!=map_2D_vt_charge.end();it++){
//   //   std::cout << "V_2: " << it->first.first << " " << it->first.second << std::endl;
//   // }
//   // for (auto it = map_2D_wt_charge.begin(); it!=map_2D_wt_charge.end();it++){
//   //   std::cout << "W_2: " << it->first.first << " " << it->first.second << std::endl;
//   // }
//   // for (size_t i=0;i!=path_wcps_vec.size();i++){
//   //   std::cout << i << " " << path_wcps_vec.at(i).mcell->GetTimeSlice() << std::endl;
//   //   std::cout << "U_3: " << path_wcps_vec.at(i).index_u << " " << path_wcps_vec.at(i).x << std::endl;
//   //   std::cout << "V_3: " << path_wcps_vec.at(i).index_v << " " << path_wcps_vec.at(i).x << std::endl;
//   //   std::cout << "W_3: " << path_wcps_vec.at(i).index_w << " " << path_wcps_vec.at(i).x << std::endl;
//   // }
//   // std::cout << map_2DU_index.size() << " " << map_2D_ut_charge.size() << " "
//   // 	    << map_2DV_index.size() << " " << map_2D_vt_charge.size() << " "
//   //   	    << map_2DW_index.size() << " " << map_2D_wt_charge.size() << " "
//   // 	    << path_wcps_vec.size() << std::endl;

//   // actual fit ... (charge division)
//   // Now start to prepare the matrix and solve it ...
//   // need to establish how to convert X position to T index
//   // T = slope_x * ( x + offset_x);
//   double slope_x = 1./time_slice_width;
//   double first_t_dis = path_wcps_vec.at(0).mcell->GetTimeSlice()*time_slice_width - path_wcps_vec.at(0).x;
//   double offset_t = first_t_dis / time_slice_width;

//   //  convert Z to W ...
//   double slope_zw = 1./pitch_w * cos(angle_w);
//   double slope_yw = 1./pitch_w * sin(angle_w);

//   double slope_yu = -1./pitch_u * sin(angle_u);
//   double slope_zu = 1./pitch_u * cos(angle_u);
//   double slope_yv = -1./pitch_v * sin(angle_v);
//   double slope_zv = 1./pitch_v * cos(angle_v);

//   //convert Y,Z to U,V
//   double offset_w = -first_w_dis/pitch_w;
//   double offset_u = -first_u_dis/pitch_u;
//   double offset_v = -first_v_dis/pitch_v;

//   //  for (size_t i=0;i!=path_wcps_vec.size();i++){
//     //std::cout << i << " " << path_wcps_vec.at(i).mcell->GetTimeSlice() - offset_t << " " << slope_x * path_wcps_vec.at(i).x << " " << path_wcps_vec.at(i).mcell->GetTimeSlice() - offset_t - slope_x * path_wcps_vec.at(i).x << std::endl;
//     //std::cout << i << " " << path_wcps_vec.at(i).index_w - offset_w << " " << slope_zw * path_wcps_vec.at(i).z << " " << path_wcps_vec.at(i).index_w - offset_w-slope_zw * path_wcps_vec.at(i).z <<  std::endl;
//     //std::cout << i << " " << path_wcps_vec.at(i).index_u - offset_u << " " << slope_yu * path_wcps_vec.at(i).y + slope_zu * path_wcps_vec.at(i).z << " " << path_wcps_vec.at(i).index_u - offset_u - (slope_yu * path_wcps_vec.at(i).y + slope_zu * path_wcps_vec.at(i).z) << std::endl;
//     // std::cout << i << " " << path_wcps_vec.at(i).index_v - offset_v << " " << slope_yv * path_wcps_vec.at(i).y + slope_zv * path_wcps_vec.at(i).z << " " << path_wcps_vec.at(i).index_v - offset_v - (slope_yv * path_wcps_vec.at(i).y + slope_zv * path_wcps_vec.at(i).z) << std::endl;
//   //}

//   // form matrix ...
//   int n_3D_pos = 3 * path_wcps_vec.size();
//   int n_2D_u = 2 * vec_2DU_index.size();
//   int n_2D_v = 2 * vec_2DV_index.size();
//   int n_2D_w = 2 * vec_2DW_index.size();
//   Eigen::VectorXd pos_3D(n_3D_pos), data_u_2D(n_2D_u), data_v_2D(n_2D_v), data_w_2D(n_2D_w);
//   Eigen::SparseMatrix<double> RU(n_2D_u, n_3D_pos) ;
//   Eigen::SparseMatrix<double> RV(n_2D_v, n_3D_pos) ;
//   Eigen::SparseMatrix<double> RW(n_2D_w, n_3D_pos) ;
//   Eigen::VectorXd pos_3D_init(n_3D_pos);
//   for (size_t i=0;i!=path_wcps_vec.size();i++){
//     pos_3D_init(3*i) = path_wcps_vec.at(i).x;
//     pos_3D_init(3*i+1) = path_wcps_vec.at(i).y;
//     pos_3D_init(3*i+2) = path_wcps_vec.at(i).z;
//   }

//   // fill in the measurement ...
//   for (size_t index = 0; index!=vec_2DU_index.size(); index++){
//     double charge = map_2D_ut_charge[vec_2DU_index.at(index).first];
//     double charge_err = map_2D_ut_charge_err[vec_2DU_index.at(index).first];
//     int n_divide = map_2DU_3D_set[vec_2DU_index.at(index).first].size();
//     double scaling = charge/charge_err/n_divide;
//     data_u_2D(2*index) =  scaling * (vec_2DU_index.at(index).first.first - offset_u);
//     data_u_2D(2*index+1) = scaling * (vec_2DU_index.at(index).first.second - offset_t);

//     int index_3D = vec_2DU_index.at(index).second; // 3*index_3D -->x  3*index_3D+1 --> y 3*index_3D+2 --> z
//     RU.insert(2*index,3*index_3D+1) = scaling * slope_yu; // Y--> U
//     RU.insert(2*index,3*index_3D+2) = scaling * slope_zu; // Z--> U
//     RU.insert(2*index+1,3*index_3D) = scaling * slope_x; // X --> T

//     // if (std::isnan(scaling * (vec_2DU_index.at(index).first.first - offset_u)) ||
//     // 	std::isnan(scaling * (vec_2DU_index.at(index).first.second - offset_t)) ||
//     // 	std::isnan(scaling * slope_yu) ||
//     // 	std::isnan(scaling * slope_zu) ||
//     // 	std::isnan(scaling * slope_x))
//     //   std::cout << "Wrong U" << " " << charge << " " << charge_err << " " << n_divide<< std::endl;

//     //std::cout << index << " " << index_3D << std::endl;
//   }
//   for (size_t index = 0; index!=vec_2DV_index.size(); index++){
//     double charge = map_2D_vt_charge[vec_2DV_index.at(index).first];
//     double charge_err = map_2D_vt_charge_err[vec_2DV_index.at(index).first];
//     int n_divide = map_2DV_3D_set[vec_2DV_index.at(index).first].size();
//     double scaling = charge/charge_err/n_divide;
//     data_v_2D(2*index) =  scaling * (vec_2DV_index.at(index).first.first - offset_v);
//     data_v_2D(2*index+1) = scaling * (vec_2DV_index.at(index).first.second - offset_t);

//     int index_3D = vec_2DV_index.at(index).second; // 3*index_3D -->x  3*index_3D+1 --> y 3*index_3D+2 --> z
//     RV.insert(2*index,3*index_3D+1) = scaling * slope_yv; // Y--> V
//     RV.insert(2*index,3*index_3D+2) = scaling * slope_zv; // Z--> V
//     RV.insert(2*index+1,3*index_3D) = scaling * slope_x; // X --> T
//     //std::cout << index << " " << index_3D << std::endl;

//     // if (std::isnan(scaling * (vec_2DV_index.at(index).first.first - offset_v)) ||
//     // 	std::isnan(scaling * (vec_2DV_index.at(index).first.second - offset_t)) ||
//     // 	std::isnan(scaling * slope_yv) ||
//     // 	std::isnan(scaling * slope_zv) ||
//     // 	std::isnan(scaling * slope_x))
//     //   std::cout << "Wrong V" << " " << charge << " " << charge_err << " " << n_divide << std::endl;
//   }
//   for (size_t index = 0; index!=vec_2DW_index.size(); index++){
//     double charge = map_2D_wt_charge[vec_2DW_index.at(index).first];
//     double charge_err = map_2D_wt_charge_err[vec_2DW_index.at(index).first];
//     int n_divide = map_2DW_3D_set[vec_2DW_index.at(index).first].size();
//     double scaling = charge/charge_err/n_divide;
//     data_w_2D(2*index) =  scaling * (vec_2DW_index.at(index).first.first - offset_w);
//     data_w_2D(2*index+1) = scaling * (vec_2DW_index.at(index).first.second - offset_t);

//     int index_3D = vec_2DW_index.at(index).second; // 3*index_3D -->x  3*index_3D+1 --> y 3*index_3D+2 --> z
//     RW.insert(2*index,3*index_3D+2) = scaling * slope_zw; // Z--> W
//     RW.insert(2*index+1,3*index_3D) = scaling * slope_x; // X --> T

//      // if (std::isnan(scaling * (vec_2DW_index.at(index).first.first - offset_w)) ||
//      // 	std::isnan(scaling * (vec_2DW_index.at(index).first.second - offset_t)) ||
//      // 	 std::isnan(scaling * slope_zw) ||
//      // 	std::isnan(scaling * slope_x))
//      //  std::cout << "Wrong W" << " " << charge << " " << charge_err << " " << n_divide << std::endl;
//   }

//  //  for (int k=0;k<RV.outerSize();++k){
//  //    for (Eigen::SparseMatrix<double>::InnerIterator it(RV,k); it; ++it){
//  //      std::cout << it.value() << " " << it.row() << " "<< it.col() << " " << it.index() << std::endl;
//  //    }
//  // }

//   Eigen::SparseMatrix<double> RUT = Eigen::SparseMatrix<double>(RU.transpose());
//   Eigen::SparseMatrix<double> RVT = Eigen::SparseMatrix<double>(RV.transpose());
//   Eigen::SparseMatrix<double> RWT = Eigen::SparseMatrix<double>(RW.transpose());

//   // double ave_distance = 0;
//   // for (size_t i=0;i!=distances.size();i++){
//   //   ave_distance += distances.at(i);
//   // }
//   // ave_distance /= distances.size();
//   // get initial chi2 ...
//   // Eigen::VectorXd chi_u = data_u_2D - RU * pos_3D_init;
//   // Eigen::VectorXd chi_v = data_v_2D - RV * pos_3D_init;
//   // Eigen::VectorXd chi_w = data_w_2D - RW * pos_3D_init;

//   // double chi2 = chi_u.squaredNorm() + chi_v.squaredNorm() + chi_w.squaredNorm();
//   // std::cout << sqrt(chi2 /(path_wcps_vec.size() * 1.)) << " " << lambda/units::cm*3 << std::endl;

//   double lambda = 2 // strength
//     * sqrt(9. //  average chi2 guessted
//  	   * (vec_2DU_index.size() + vec_2DV_index.size() + vec_2DW_index.size()) // how many of them
//  	   * 6 * 6 // charge/charge_err estimation ...
//  	   /(path_wcps_vec.size() * 1.)); //weighting
//   lambda = 0;
//   //double dis_range = 0.5*units::cm/sqrt(3.);
//   double angle_range = 0.25;

//   //std::cout << lambda/ dis_range << std::endl;

//   Eigen::SparseMatrix<double> FMatrix(n_3D_pos, n_3D_pos) ;
//   //Eigen::SparseMatrix<double> PMatrix(n_3D_pos, n_3D_pos) ;
//   // distances[i]
//   // 2nd order ...
//   for (size_t i=0;i!=path_wcps_vec.size();i++){
//     // PMatrix.insert(3*i,3*i)=1;
//     // PMatrix.insert(3*i+1,3*i+1)=1;
//     // PMatrix.insert(3*i+2,3*i+2)=1;
//     if (i==0){
//       FMatrix.insert(0,0) = -1./distances.at(0); // X
//       FMatrix.insert(0,3) = 1./distances.at(0);
//       FMatrix.insert(1,1) = -1./distances.at(0); // Y
//       FMatrix.insert(1,4) = 1./distances.at(0);
//       FMatrix.insert(2,2) = -1./distances.at(0); // Z
//       FMatrix.insert(2,5) = 1./distances.at(0);
//     }else if (i==path_wcps_vec.size()-1){
//       FMatrix.insert(3*i,3*i) = -1./distances.at(path_wcps_vec.size()-2); // X
//       FMatrix.insert(3*i,3*i-3) = 1./distances.at(path_wcps_vec.size()-2);
//       FMatrix.insert(3*i+1,3*i+1) = -1./distances.at(path_wcps_vec.size()-2);
//       FMatrix.insert(3*i+1,3*i-2) = 1./distances.at(path_wcps_vec.size()-2);
//       FMatrix.insert(3*i+2,3*i+2) = -1./distances.at(path_wcps_vec.size()-2);
//       FMatrix.insert(3*i+2,3*i-1) = 1./distances.at(path_wcps_vec.size()-2);
//     }else{
//       FMatrix.insert(3*i,3*i-3) = 1./distances.at(i-1)/(distances.at(i)+distances.at(i-1));
//       FMatrix.insert(3*i,3*i) = -1./distances.at(i-1)/distances.at(i);
//       FMatrix.insert(3*i,3*i+3) = 1./distances.at(i)/(distances.at(i)+distances.at(i-1));
//       FMatrix.insert(3*i+1,3*i-2) = 1./distances.at(i-1)/(distances.at(i)+distances.at(i-1));
//       FMatrix.insert(3*i+1,3*i+1) = -1./distances.at(i-1)/distances.at(i);
//       FMatrix.insert(3*i+1,3*i+4) = 1./distances.at(i)/(distances.at(i)+distances.at(i-1));
//       FMatrix.insert(3*i+2,3*i-1) = 1./distances.at(i-1)/(distances.at(i)+distances.at(i-1));
//       FMatrix.insert(3*i+2,3*i+2) = -1./distances.at(i-1)/distances.at(i);
//       FMatrix.insert(3*i+2,3*i+5) = 1./distances.at(i)/(distances.at(i)+distances.at(i-1));
//     }
//   }

//    //0th order
//   // for (size_t i=0;i!=path_wcps_vec.size();i++){
//   //   if (i==0){
//   //     FMatrix.insert(0,0) = -1;
//   //     FMatrix.insert(0,3) = 1.;
//   //     FMatrix.insert(1,1) = -1.;
//   //     FMatrix.insert(1,4) = 1.;
//   //     FMatrix.insert(2,2) = -1.;
//   //     FMatrix.insert(2,5) = 1.;
//   //   }else if (i==path_wcps_vec.size()-1){
//   //     FMatrix.insert(3*i,3*i) = -1.;
//   //     FMatrix.insert(3*i,3*i-3) = 1.;
//   //     FMatrix.insert(3*i+1,3*i+1) = -1.;
//   //     FMatrix.insert(3*i+1,3*i-2) = 1.;
//   //     FMatrix.insert(3*i+2,3*i+2) = -1.;
//   //     FMatrix.insert(3*i+2,3*i-1) = 1.;
//   //   }else{
//   //     FMatrix.insert(3*i,3*i) = -1.;
//   //     FMatrix.insert(3*i,3*i+3) = 1.;
//   //     FMatrix.insert(3*i+1,3*i+1) = -1.;
//   //     FMatrix.insert(3*i+1,3*i+4) = 1.;
//   //     FMatrix.insert(3*i+2,3*i+2) = -1.;
//   //     FMatrix.insert(3*i+2,3*i+5) = 1.;
//   //   }
//   // }

//   // 2nd order
//   // for (size_t i=0;i!=path_wcps_vec.size();i++){
//   //   if (i==0){
//   //     FMatrix.insert(0,0) = -1;
//   //     FMatrix.insert(0,3) = 1.;
//   //     FMatrix.insert(1,1) = -1.;
//   //     FMatrix.insert(1,4) = 1.;
//   //     FMatrix.insert(2,2) = -1.;
//   //     FMatrix.insert(2,5) = 1.;
//   //   }else if (i==path_wcps_vec.size()-1){
//   //     FMatrix.insert(3*i,3*i) = -1.;
//   //     FMatrix.insert(3*i,3*i-3) = 1.;
//   //     FMatrix.insert(3*i+1,3*i+1) = -1.;
//   //     FMatrix.insert(3*i+1,3*i-2) = 1.;
//   //     FMatrix.insert(3*i+2,3*i+2) = -1.;
//   //     FMatrix.insert(3*i+2,3*i-1) = 1.;
//   //   }else{
//   //     FMatrix.insert(3*i,3*i-3) = 1.;
//   //     FMatrix.insert(3*i,3*i) = -2.;
//   //     FMatrix.insert(3*i,3*i+3) = 1.;
//   //     FMatrix.insert(3*i+1,3*i-2) = 1.;
//   //     FMatrix.insert(3*i+1,3*i+1) = -2.;
//   //     FMatrix.insert(3*i+1,3*i+4) = 1.;
//   //     FMatrix.insert(3*i+2,3*i-1) = 1.;
//   //     FMatrix.insert(3*i+2,3*i+2) = -2.;
//   //     FMatrix.insert(3*i+2,3*i+5) = 1.;
//   //   }
//   // }

//   FMatrix *= lambda/angle_range ; // disable the angue cut ...

//   Eigen::SparseMatrix<double> FMatrixT = Eigen::SparseMatrix<double>(FMatrix.transpose());
//   // Eigen::SparseMatrix<double> PMatrixT = Eigen::SparseMatrix<double>(PMatrix.transpose());
//   Eigen::BiCGSTAB<Eigen::SparseMatrix<double>> solver;
//   Eigen::VectorXd b = RUT * data_u_2D + RVT * data_v_2D + RWT * data_w_2D;// + PMatrixT * pos_3D_init * pow(lambda/dis_range,2);

//   // for (int i=0;i!=n_3D_pos;i++){
//   //   std::cout << i << " " << b(i) << std::endl;
//   // }

//   Eigen::SparseMatrix<double> A =   RUT * RU + RVT * RV + RWT * RW + FMatrixT * FMatrix;// + PMatrixT * PMatrix * pow(lambda/dis_range,2);

//   // for (int k=0;k<A.outerSize(); ++k)
//   //   for (Eigen::SparseMatrix<double>::InnerIterator it(A,k); it; ++it){
//   //     if (it.value()!=0)
//   // 	std::cout << "Xin: " << it.value() << " "<< it.row() << " " << it.col() << " " << it.index() << std::endl;
//   //   }

//   solver.compute(A);

//   pos_3D = solver.solveWithGuess(b,pos_3D_init);

//   //  std::cout << "#iterations: " << solver.iterations() << std::endl;
//   //std::cout << "#estimated error: " << solver.error() << std::endl;

//   if (std::isnan(solver.error())){
//     pos_3D = solver.solve(b);
//     //std::cout << "#iterations: " << solver.iterations() << std::endl;
//     //std::cout << "#estimated error: " << solver.error() << std::endl;
//   }

//   //std::cout << path_wcps_vec.size() << " " << map_2DU_index.size() << " " << map_2DV_index.size() << " " << map_2DW_index.size() << std::endl;

//   if (std::isnan(solver.error())){
//     fine_tracking_path.clear();
//     if (fine_tracking_path.size()==0){
//       for (size_t i=0;i!=path_wcps_vec.size();i++){
// 	Point p;
// 	p.x = path_wcps_vec.at(i).x;
// 	p.y = path_wcps_vec.at(i).y;
// 	p.z = path_wcps_vec.at(i).z;
// 	fine_tracking_path.push_back(p);
//       }
//     }
//   }else{
//     flag_fine_tracking = true;
//     fine_tracking_path.clear();
//     for (size_t i=0;i!=path_wcps_vec.size();i++){
//       Point p;
//       p.x = pos_3D(3*i);
//       p.y = pos_3D(3*i+1);
//       p.z = pos_3D(3*i+2);
//       if (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z)){
//       }else{
// 	fine_tracking_path.push_back(p);
//       }
//     }
//     // std::cout << p.x << " " << p.y << " " << p.z << " " << path_wcps_vec.at(i).x << " " << path_wcps_vec.at(i).y << " " << path_wcps_vec.at(i).z << std::endl;
//   }

//   // Eigen::VectorXd data_v_2D_p1 = RVT * RV * pos_3D1;
//   // Eigen::VectorXd data_v_2D_p2 = RVT * RV * pos_3D;
//   // Eigen::VectorXd data_v_2D_p3 = RVT * data_v_2D;
//   // for (size_t i=0;i!=path_wcps_vec.size();i++){
//   //   std::cout << i << " " << data_v_2D_p3(3*i) << " " << data_v_2D_p1(3*i) << " " << data_v_2D_p2(3*i)
//   // 	      << " " << data_v_2D_p3(3*i+1) << " " << data_v_2D_p1(3*i+1) << " " << data_v_2D_p2(3*i+1)
//   // 	      << " " << data_v_2D_p3(3*i+2) << " " << data_v_2D_p1(3*i+2) << " " << data_v_2D_p2(3*i+2) << std::endl;
//   // }
//   // for (size_t i=0;i!= map_2DV_index.size();i++){
//   //   std::cout << i << " X " << data_v_2D(2*i) << " " << data_v_2D_p1(2*i) << " " << data_v_2D_p2(2*i) << " "
//   // 	      << data_v_2D(2*i+1) << " " << data_v_2D_p1(2*i+1) << " " << data_v_2D_p2(2*i+1) << " " << std::endl;
//   // }
//   // Eigen::VectorXd data_v_2D_p1 = RV * pos_3D1;
//   // for (size_t i=0;i!=n_2D_v;i++){
//   //   std::cout << data_v_2D(i) << " " << data_v_2D_p1(i) << std::endl;
//   // }

//   // for (int k=0;k<RV.outerSize();++k){
//   //   for (Eigen::SparseMatrix<double>::InnerIterator it(RV,k); it; ++it){
//   //     std::cout << it.value() << " " << it.row() << " "<< it.col() << " " << it.index() << std::endl;
//   //   }
//   // }

//   // for (size_t i=0;i!=path_wcps_vec.size();i++){
//   //   std::cout << pos_3D(3*i) << " " << pos_3D(3*i+1) << " " << pos_3D(3*i+2) << " " << path_wcps_vec.at(i).x << " " << path_wcps_vec.at(i).y << " " << path_wcps_vec.at(i).z << std::endl;
//   // }

//   // Eigen::VectorXd test1(3); test1(0)=1;test1(1)=2; test1(2)=3;
//   // Eigen::MatrixXd test2(2,3);
//   // test2(0,0)=1;
//   // test2(1,1) = 1; test2(1,2) = 1;
//   // Eigen::VectorXd test3 = test2 * test1;
//   // std::cout << test3(0) << " " << test3(1) << std::endl;

//   // copy the list into a vector...
//   //std::vector<WCPointCloud<double>::WCPoint> path_wcps_vec(std::begin(path_wcps), std::end(path_wcps));

//   // go along the vector, and create a new set of points, according to prechosen Dt
//   //double dt = 0.5*units::cm;

//   // max of( a certain distance (predefined) and three times the closest distance)
//   // save the corresponding mcells for these points as candidate

//   // current point, nearby points, and points satisfying a certain distance cut as candidates

//   // need a Kalman filter like alg to organize the points???
//   // std::vector<bool> flag_prev_dirs, flag_next_dirs;
//   // flag_prev_dirs.resize(path_wcps_vec.size(), true);
//   // flag_next_dirs.resize(path_wcps_vec.size(), true);

//   // TVector3 prev_pos,next_pos,dir,dir1;
//   // for (size_t j=0; j!=path_wcps_vec.size(); j++){
//   //   int current_index = j;

//   //   // calculate the direction vector
//   //   prev_pos.SetXYZ(0,0,0);
//   //   int num_pos = 0;
//   //   for (int i=std::max(0,current_index - 10);i!=current_index+1;i++){
//   //     prev_pos.SetXYZ(prev_pos.X() - path_wcps_vec.at(i).x,
//   // 		      prev_pos.Y() - path_wcps_vec.at(i).y,
//   // 		      prev_pos.Z() - path_wcps_vec.at(i).z);
//   //     num_pos ++;
//   //   }
//   //   prev_pos *= 1./num_pos;

//   //   next_pos.SetXYZ(0,0,0);
//   //   num_pos = 0;
//   //   for (int i=current_index; i!=std::min(current_index+11,int(path_wcps_vec.size()));i++){
//   //     next_pos.SetXYZ(next_pos.X() + path_wcps_vec.at(i).x,
//   // 		      next_pos.Y() + path_wcps_vec.at(i).y,
//   // 		      next_pos.Z() + path_wcps_vec.at(i).z
//   // 		      );
//   //     num_pos++;
//   //   }
//   //   next_pos *= 1./num_pos;
//   //   dir = next_pos - prev_pos;

//   //   if (j>0){
//   //     dir1.SetXYZ(path_wcps_vec.at(j).x - path_wcps_vec.at(j-1).x,
//   // 		  path_wcps_vec.at(j).y - path_wcps_vec.at(j-1).y,
//   // 		  path_wcps_vec.at(j).z - path_wcps_vec.at(j-1).z);
//   //     if (dir1.Dot(dir)<0) flag_prev_dirs.at(j) = false;
//   //   }
//   //   if (j+1<path_wcps_vec.size()){
//   //     dir1.SetXYZ(path_wcps_vec.at(j+1).x - path_wcps_vec.at(j).x,
//   // 		  path_wcps_vec.at(j+1).y - path_wcps_vec.at(j).y,
//   // 		  path_wcps_vec.at(j+1).z - path_wcps_vec.at(j).z);
//   //     if (dir1.Dot(dir)<0) flag_next_dirs.at(j) = false;
//   //   }

//   // }

//   // for (size_t i=0; i!=flag_prev_dirs.size(); i++){
//   //   std::cout << i << " " << flag_prev_dirs.at(i) << " " << flag_next_dirs.at(i) << " " <<
//   //     path_wcps_vec.at(i).x << " " << path_wcps_vec.at(i).y << " " << path_wcps_vec.at(i).z << std::endl;
//   // }

// }

void PR3DCluster::collect_charge_trajectory(ToyCTPointCloud &ct_point_cloud, double dis_cut, double range_cut)
{
    // clear up ...
    collected_charge_map.clear();

    std::set<std::pair<int, int>> existing_tcs;

    // std::cout << "mcells: " << mcells.size() << std::endl;

    // form a set cotaining everything inside the cluster
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it);
        int time_slice = mcell->GetTimeSlice();
        GeomWireSelection &uwires = mcell->get_uwires();
        GeomWireSelection &vwires = mcell->get_vwires();
        GeomWireSelection &wwires = mcell->get_wwires();
        for (auto it1 = uwires.begin(); it1 != uwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            existing_tcs.insert(std::make_pair(time_slice, wire->channel()));
        }
        for (auto it1 = vwires.begin(); it1 != vwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            existing_tcs.insert(std::make_pair(time_slice, wire->channel()));
        }
        for (auto it1 = wwires.begin(); it1 != wwires.end(); it1++)
        {
            const GeomWire *wire = (*it1);
            existing_tcs.insert(std::make_pair(time_slice, wire->channel()));
        }
    }

    // form a trajectory according to dis and fine tracking?
    PointVector traj_pts;
    //  PointVector& pts = get_fine_tracking_path();

    // not using fine tracking results ...
    PointVector pts;
    std::list<WCPointCloud<double>::WCPoint> &path_wcps = get_path_wcps();
    for (auto it = path_wcps.begin(); it != path_wcps.end(); it++)
    {
        Point p((*it).x, (*it).y, (*it).z);
        pts.push_back(p);
    }

    // std::cout << "trajectory points " << pts.size() << std::endl;

    // if (cluster_id==13){
    //   for (int i=0; i!=pts.size(); i++){
    //     std::cout << i << " " << pts.at(i).x/units::cm << " " << pts.at(i).y/units::cm << " "<< pts.at(i).z/units::cm << std::endl;
    //   }
    // }

    for (int i = 0; i != pts.size(); i++)
    {
        if (pts.at(i).y < -120 * units::cm || pts.at(i).y > 120 * units::cm ||
            pts.at(i).z < -5 * units::cm || pts.at(i).z > 1070 * units::cm)
            continue;

        if (traj_pts.size() == 0)
        {
            traj_pts.push_back(pts.at(i));
        }
        else
        {
            double dis = sqrt(pow(pts.at(i).x - pts.at(i - 1).x, 2) +
                              pow(pts.at(i).y - pts.at(i - 1).y, 2) +
                              pow(pts.at(i).z - pts.at(i - 1).z, 2));
            if (dis <= dis_cut)
            {
                traj_pts.push_back(pts.at(i));
            }
            else
            {
                int nseg = dis / dis_cut + 1;
                //	std::cout << i << " " << pts.at(i).x/units::cm << " " << pts.at(i).y/units::cm << " "<< pts.at(i).z/units::cm << " " << dis << " " << dis_cut << " " << nseg << std::endl;
                for (int j = 0; j != nseg; j++)
                {
                    Point temp_pt;
                    temp_pt.x = pts.at(i - 1).x + (pts.at(i).x - pts.at(i - 1).x) * (j + 1.) / nseg;
                    temp_pt.y = pts.at(i - 1).y + (pts.at(i).y - pts.at(i - 1).y) * (j + 1.) / nseg;
                    temp_pt.z = pts.at(i - 1).z + (pts.at(i).z - pts.at(i - 1).z) * (j + 1.) / nseg;
                    traj_pts.push_back(temp_pt);
                }
            }
        }
    }

    // collect the nearby points, and compare with existing maps
    for (size_t i = 0; i != traj_pts.size(); i++)
    {
        // std::cout << i << " " << traj_pts.at(i).x/units::cm << " " << traj_pts.at(i).y/units::cm << " " << traj_pts.at(i).z/units::cm << " " << range_cut << std::endl;
        WCP::CTPointCloud<double> nearby_points = ct_point_cloud.get_closest_points(traj_pts.at(i), range_cut, 0);

        //    std::cout << "0 " << nearby_points.pts.size() << std::endl;

        for (size_t j = 0; j != nearby_points.pts.size(); j++)
        {
            if (existing_tcs.find(std::make_pair(nearby_points.pts.at(j).time_slice, nearby_points.pts.at(j).channel)) == existing_tcs.end())
            {
                collected_charge_map[std::make_pair(nearby_points.pts.at(j).time_slice, nearby_points.pts.at(j).channel)] = std::make_pair(nearby_points.pts.at(j).charge, nearby_points.pts.at(j).charge_err);
            }
        }
        nearby_points = ct_point_cloud.get_closest_points(traj_pts.at(i), range_cut, 1);
        // std::cout << "1 " << nearby_points.pts.size() << std::endl;

        for (size_t j = 0; j != nearby_points.pts.size(); j++)
        {
            if (existing_tcs.find(std::make_pair(nearby_points.pts.at(j).time_slice, nearby_points.pts.at(j).channel)) == existing_tcs.end())
            {
                collected_charge_map[std::make_pair(nearby_points.pts.at(j).time_slice, nearby_points.pts.at(j).channel)] = std::make_pair(nearby_points.pts.at(j).charge, nearby_points.pts.at(j).charge_err);
            }
        }
        nearby_points = ct_point_cloud.get_closest_points(traj_pts.at(i), range_cut, 2);

        //    std::cout << "2 " << nearby_points.pts.size() << std::endl;

        for (size_t j = 0; j != nearby_points.pts.size(); j++)
        {
            if (existing_tcs.find(std::make_pair(nearby_points.pts.at(j).time_slice, nearby_points.pts.at(j).channel)) == existing_tcs.end())
            {
                collected_charge_map[std::make_pair(nearby_points.pts.at(j).time_slice, nearby_points.pts.at(j).channel)] = std::make_pair(nearby_points.pts.at(j).charge, nearby_points.pts.at(j).charge_err);
            }
        }
    }

    // std::cout << collected_charge_map.size() << std::endl;
}

TVector3 PR3DCluster::get_ft_dir_end(float mean_dis, float dis_cut)
{
    TVector3 dir(0, 0, 0);
    for (size_t i = 1; i < fine_tracking_path.size(); i++)
    {
        float dis = sqrt(pow(fine_tracking_path.at(i).x - fine_tracking_path.at(0).x, 2) + pow(fine_tracking_path.at(i).y - fine_tracking_path.at(0).y, 2) + pow(fine_tracking_path.at(i).z - fine_tracking_path.at(0).z, 2));
        if (dis < dis_cut)
        {
            dir.SetX(dir.X() + (fine_tracking_path.at(i).x - fine_tracking_path.at(0).x) * exp(-dis / mean_dis));
            dir.SetY(dir.Y() + (fine_tracking_path.at(i).y - fine_tracking_path.at(0).y) * exp(-dis / mean_dis));
            dir.SetZ(dir.Z() + (fine_tracking_path.at(i).z - fine_tracking_path.at(0).z) * exp(-dis / mean_dis));
        }
        else
        {
            break;
        }
    }

    return dir.Unit();
}

std::pair<Point, Point> PR3DCluster::get_two_extreme_points()
{
    Create_point_cloud();
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCPointCloud<double>::WCPoint extreme_wcp[6];
    for (int i = 0; i != 6; i++)
    {
        extreme_wcp[i] = cloud.pts[0];
    }
    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        if (cloud.pts[i].y > extreme_wcp[0].y)
            extreme_wcp[0] = cloud.pts[i];
        if (cloud.pts[i].y < extreme_wcp[1].y)
            extreme_wcp[1] = cloud.pts[i];

        if (cloud.pts[i].x > extreme_wcp[2].x)
            extreme_wcp[2] = cloud.pts[i];
        if (cloud.pts[i].x < extreme_wcp[3].x)
            extreme_wcp[3] = cloud.pts[i];

        if (cloud.pts[i].z > extreme_wcp[4].z)
            extreme_wcp[4] = cloud.pts[i];
        if (cloud.pts[i].z < extreme_wcp[5].z)
            extreme_wcp[5] = cloud.pts[i];
    }

    double max_dis = -1;
    WCPointCloud<double>::WCPoint wcp1, wcp2;
    for (int i = 0; i != 6; i++)
    {
        for (int j = i + 1; j != 6; j++)
        {
            double dis = sqrt(pow(extreme_wcp[i].x - extreme_wcp[j].x, 2) + pow(extreme_wcp[i].y - extreme_wcp[j].y, 2) + pow(extreme_wcp[i].z - extreme_wcp[j].z, 2));
            if (dis > max_dis)
            {
                max_dis = dis;
                wcp1 = extreme_wcp[i];
                wcp2 = extreme_wcp[j];
            }
        }
    }
    Point p1(wcp1.x, wcp1.y, wcp1.z);
    Point p2(wcp2.x, wcp2.y, wcp2.z);
    p1 = calc_ave_pos(p1, 5 * units::cm);
    p2 = calc_ave_pos(p2, 5 * units::cm);

    return std::make_pair(p1, p2);
}

bool PR3DCluster::judge_vertex(Point &p_test, double asy_cut, double occupied_cut)
{

    p_test = calc_ave_pos(p_test, 3 * units::cm);

    TVector3 dir = VHoughTrans(p_test, 15 * units::cm);

    // judge if this is end points
    std::pair<int, int> num_pts = get_num_points(p_test, dir, 25 * units::cm);

    if ((num_pts.first + num_pts.second) == 0)
        return false;

    double asy = fabs(num_pts.first - num_pts.second) / (num_pts.first + num_pts.second);

    // std::cout << asy << " " << std::endl;

    if (asy > asy_cut)
    {
        return true;
    }
    else
    {
        TPCParams &mp = Singleton<TPCParams>::Instance();
        double angle_u = mp.get_angle_u();
        double angle_v = mp.get_angle_v();
        double angle_w = mp.get_angle_w();
        // create a temp cloud ...
        ToyPointCloud temp_point_cloud(angle_u, angle_v, angle_w);
        dir.SetMag(1);
        PointVector pts;
        for (size_t i = 0; i != 40; i++)
        {
            Point pt(p_test.x + i * 0.5 * units::cm * dir.X(),
                     p_test.y + i * 0.5 * units::cm * dir.Y(),
                     p_test.z + i * 0.5 * units::cm * dir.Z());
            WCP::WCPointCloud<double>::WCPoint &wcp = point_cloud->get_closest_wcpoint(pt);

            if (sqrt(pow(wcp.x - pt.x, 2) + pow(wcp.y - pt.y, 2) + pow(wcp.z - pt.z, 2)) < std::max(1.8 * units::cm, i * 0.5 * units::cm * sin(18. / 180. * 3.1415926)))
            {
                pt.x = wcp.x;
                pt.y = wcp.y;
                pt.z = wcp.z;
            }
            pts.push_back(pt);
            if (i != 0)
            {
                Point pt1(p_test.x - i * 0.5 * units::cm * dir.X(),
                          p_test.y - i * 0.5 * units::cm * dir.Y(),
                          p_test.z - i * 0.5 * units::cm * dir.Z());
                WCP::WCPointCloud<double>::WCPoint &wcp1 = point_cloud->get_closest_wcpoint(pt1);
                if (sqrt(pow(wcp1.x - pt1.x, 2) + pow(wcp1.y - pt1.y, 2) + pow(wcp1.z - pt1.z, 2)) < std::max(1.8 * units::cm, i * 0.5 * units::cm * sin(18. / 180. * 3.1415926)))
                {
                    pt1.x = wcp1.x;
                    pt1.y = wcp1.y;
                    pt1.z = wcp1.z;
                }
                pts.push_back(pt1);
            }
        }
        temp_point_cloud.AddPoints(pts);
        temp_point_cloud.build_kdtree_index();

        int temp_num_total_points = 0;
        int temp_num_occupied_points = 0;

        const int N = point_cloud->get_num_points();
        WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
        for (int i = 0; i != N; i++)
        {
            TVector3 dir1(cloud.pts[i].x - p_test.x, cloud.pts[i].y - p_test.y, cloud.pts[i].z - p_test.z);

            if (dir1.Mag() < 15 * units::cm)
            {
                Point test_p1(cloud.pts[i].x, cloud.pts[i].y, cloud.pts[i].z);
                temp_num_total_points++;
                double dis[3];
                dis[0] = temp_point_cloud.get_closest_2d_dis(test_p1, 0).second;
                dis[1] = temp_point_cloud.get_closest_2d_dis(test_p1, 1).second;
                dis[2] = temp_point_cloud.get_closest_2d_dis(test_p1, 2).second;
                if (dis[0] <= 1.5 * units::cm && dis[1] <= 1.5 * units::cm && dis[2] <= 2.4 * units::cm ||
                    dis[0] <= 1.5 * units::cm && dis[2] <= 1.5 * units::cm && dis[1] <= 2.4 * units::cm ||
                    dis[2] <= 1.5 * units::cm && dis[1] <= 1.5 * units::cm && dis[0] <= 2.4 * units::cm)
                    temp_num_occupied_points++;

                // std::cout << temp_point_cloud.get_closest_2d_dis(test_p1,0).second /units::cm << " "
                // 	  << temp_point_cloud.get_closest_2d_dis(test_p1,1).second /units::cm << " "
                // 	  << temp_point_cloud.get_closest_2d_dis(test_p1,2).second /units::cm << " " << std::endl;
            }
        }

        // std::cout << asy << " " << temp_num_occupied_points << " " << temp_num_total_points << " " << temp_num_occupied_points * 1.0 / temp_num_total_points << " " << p_test.x/units::cm << " " << p_test.y/units::cm << " " << p_test.z/units::cm << std::endl;

        if (temp_num_occupied_points < temp_num_total_points * occupied_cut)
            return true;
    }
    //  std::cout << num_pts.first << " " << num_pts.second << " " << asy << std::endl;

    // judge if there

    return false;
}

std::vector<std::vector<WCPointCloud<double>::WCPoint>> PR3DCluster::get_extreme_wcps()
{

    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    Calc_PCA();
    WCPointCloud<double>::WCPoint wcps[8];
    for (int i = 0; i != 8; i++)
    {
        wcps[i] = cloud.pts[0];
    }
    Vector main_axis = get_PCA_axis(0);
    if (main_axis.y < 0)
    {
        main_axis.x = -main_axis.x;
        main_axis.y = -main_axis.y;
        main_axis.z = -main_axis.z;
    }
    double high_value = wcps[0].x * main_axis.x + wcps[0].y * main_axis.y + wcps[0].z * main_axis.z;
    double low_value = wcps[1].x * main_axis.x + wcps[1].y * main_axis.y + wcps[1].z * main_axis.z;

    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        double value = cloud.pts[i].x * main_axis.x + cloud.pts[i].y * main_axis.y + cloud.pts[i].z * main_axis.z;
        if (value > high_value)
        {
            wcps[0] = cloud.pts[i];
            high_value = value;
        }
        if (value < low_value)
        {
            wcps[1] = cloud.pts[i];
            low_value = value;
        }
        // top down
        if (cloud.pts[i].y > wcps[2].y)
            wcps[2] = cloud.pts[i];
        if (cloud.pts[i].y < wcps[3].y)
            wcps[3] = cloud.pts[i];

        // front back
        if (cloud.pts[i].z > wcps[4].z)
            wcps[4] = cloud.pts[i];
        if (cloud.pts[i].z < wcps[5].z)
            wcps[5] = cloud.pts[i];

        // early late
        if (cloud.pts[i].x > wcps[6].x)
            wcps[6] = cloud.pts[i];
        if (cloud.pts[i].x < wcps[7].x)
            wcps[7] = cloud.pts[i];
    }

    std::vector<std::vector<WCPointCloud<double>::WCPoint>> out_vec_wcps;

    {
        // first extreme along the main axis
        std::vector<WCPointCloud<double>::WCPoint> saved_wcps;
        saved_wcps.push_back(wcps[0]);
        out_vec_wcps.push_back(saved_wcps);
    }

    {
        // second extreme along the main axis
        std::vector<WCPointCloud<double>::WCPoint> saved_wcps;
        saved_wcps.push_back(wcps[1]);
        out_vec_wcps.push_back(saved_wcps);
    }

    // std::cout << std::endl;
    for (int i = 2; i != 8; i++)
    {
        // if (cluster_id==16)
        // std::cout << i << " C " << wcps[i].x/units::cm << " " << wcps[i].y/units::cm << " " << wcps[i].z/units::cm << std::endl;

        bool flag_save = true;
        for (size_t j = 0; j != out_vec_wcps.size(); j++)
        {
            double dis = sqrt(pow(out_vec_wcps[j].at(0).x - wcps[i].x, 2) + pow(out_vec_wcps[j].at(0).y - wcps[i].y, 2) + pow(out_vec_wcps[j].at(0).z - wcps[i].z, 2));
            if (dis < 5 * units::cm)
            {
                out_vec_wcps.at(j).push_back(wcps[i]);
                flag_save = false;
                break;
            }
        }

        if (flag_save)
        {
            std::vector<WCPointCloud<double>::WCPoint> saved_wcps;
            saved_wcps.push_back(wcps[i]);
            out_vec_wcps.push_back(saved_wcps);
        }
    }

    return out_vec_wcps;

    // WCPointCloud<double>::WCPoint max_wcps = saved_wcps.at(0);
    // int max_count = counters.at(0);
    // WCPointCloud<double>::WCPoint min_wcps;
    // double value = -1e9;

    // for (size_t i=1;i!=saved_wcps.size();i++){
    //   if (counters.at(i)>max_count){
    //     max_wcps = saved_wcps.at(i);
    //     max_count = counters.at(i);
    //   }
    // }

    // //  TVector3 m_pca(main_axis.x, main_axis.y, main_axis.z);
    // Point p1(max_wcps.x,max_wcps.y,max_wcps.z);
    // TVector3 m_dir = VHoughTrans(p1,30*units::cm);

    // for (size_t i=0;i!=saved_wcps.size();i++){
    //   TVector3 dir(saved_wcps.at(i).x-max_wcps.x,
    // 		 saved_wcps.at(i).y-max_wcps.y,
    // 		 saved_wcps.at(i).z-max_wcps.z);
    //   double l1 = fabs(dir.Dot(m_dir)/m_dir.Mag());
    //   TVector3 dir1 = dir.Cross(m_dir);
    //   double l2 = dir1.Mag()/m_dir.Mag();
    //   if (l1-l2 > value){
    //     value = l1-l2;
    //     min_wcps = saved_wcps.at(i);
    //   }
    //   //  std::cout << i << " " << saved_wcps.at(i).x/units::cm << " " << saved_wcps.at(i).y/units::cm << " " << saved_wcps.at(i).z/units::cm << " " << l1/units::cm << " " << l2/units::cm << std::endl;

    // }

    // return std::make_pair(max_wcps,min_wcps);

    //  std::cout << max_wcps.x/units::cm << " " << max_wcps.y/units::cm << " " << max_wcps.z/units::cm << " " << max_count << std::endl;

    //  return std::make_pair(wcps[0],wcps[1]);
}

std::pair<WCPointCloud<double>::WCPoint, WCPointCloud<double>::WCPoint> PR3DCluster::get_main_axis_wcps()
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    Calc_PCA();
    WCPointCloud<double>::WCPoint highest_wcp = cloud.pts[0];
    WCPointCloud<double>::WCPoint lowest_wcp = cloud.pts[0];
    Vector main_axis = get_PCA_axis(0);
    if (main_axis.y < 0)
    {
        main_axis.x = -main_axis.x;
        main_axis.y = -main_axis.y;
        main_axis.z = -main_axis.z;
    }

    double high_value = highest_wcp.x * main_axis.x + highest_wcp.y * main_axis.y + highest_wcp.z * main_axis.z;
    double low_value = lowest_wcp.x * main_axis.x + lowest_wcp.y * main_axis.y + lowest_wcp.z * main_axis.z;

    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        double value = cloud.pts[i].x * main_axis.x + cloud.pts[i].y * main_axis.y + cloud.pts[i].z * main_axis.z;
        if (value > high_value)
        {
            highest_wcp = cloud.pts[i];
            high_value = value;
        }

        if (value < low_value)
        {
            lowest_wcp = cloud.pts[i];
            low_value = value;
        }
    }
    return std::make_pair(highest_wcp, lowest_wcp);
}

std::pair<WCPointCloud<double>::WCPoint, WCPointCloud<double>::WCPoint> PR3DCluster::get_highest_lowest_wcps()
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCPointCloud<double>::WCPoint highest_wcp = cloud.pts[0];
    WCPointCloud<double>::WCPoint lowest_wcp = cloud.pts[0];
    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        if (cloud.pts[i].y > highest_wcp.y)
            highest_wcp = cloud.pts[i];
        if (cloud.pts[i].y < lowest_wcp.y)
            lowest_wcp = cloud.pts[i];
    }
    return std::make_pair(highest_wcp, lowest_wcp);
}

std::pair<WCPointCloud<double>::WCPoint, WCPointCloud<double>::WCPoint> PR3DCluster::get_front_back_wcps()
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCPointCloud<double>::WCPoint highest_wcp = cloud.pts[0];
    WCPointCloud<double>::WCPoint lowest_wcp = cloud.pts[0];
    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        if (cloud.pts[i].z > highest_wcp.z)
            highest_wcp = cloud.pts[i];
        if (cloud.pts[i].z < lowest_wcp.z)
            lowest_wcp = cloud.pts[i];
    }
    return std::make_pair(highest_wcp, lowest_wcp);
}

std::pair<WCPointCloud<double>::WCPoint, WCPointCloud<double>::WCPoint> PR3DCluster::get_earliest_latest_wcps()
{
    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCPointCloud<double>::WCPoint highest_wcp = cloud.pts[0];
    WCPointCloud<double>::WCPoint lowest_wcp = cloud.pts[0];
    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        if (cloud.pts[i].x > highest_wcp.x)
            highest_wcp = cloud.pts[i];
        if (cloud.pts[i].x < lowest_wcp.x)
            lowest_wcp = cloud.pts[i];
    }
    return std::make_pair(lowest_wcp, highest_wcp);
}

Point PR3DCluster::calc_ave_pos(Point &p, int N)
{
    std::map<WCP::SlimMergeGeomCell *, Point> pts = point_cloud->get_closest_mcell(p, N);
    Point pt(0, 0, 0);
    double charge = 0;
    // std::cout << pts.size() << std::endl;
    for (auto it = pts.begin(); it != pts.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it).first;
        Point pc = mcell->center();
        double q = mcell->get_q();
        pt.x += pc.x * q;
        pt.y += pc.y * q;
        pt.z += pc.z * q;
        charge += q;
        // std::cout << pc.x/units::cm << " " << pc.y/units::cm << " " << pc.z/units::cm << " " << q << " " <<
        //  sqrt(pow(pc.x-p.x,2)+pow(pc.y-p.y,2)+pow(pc.z-p.z,2))/units::cm << std::endl;
    }
    if (charge != 0)
    {
        pt.x /= charge;
        pt.y /= charge;
        pt.z /= charge;
    }
    return pt;
}

Point PR3DCluster::calc_ave_pos(Point &p, double dis)
{
    std::map<WCP::SlimMergeGeomCell *, Point> pts = point_cloud->get_closest_mcell(p, dis);
    Point pt(0, 0, 0);
    double charge = 0;

    // hack ...
    //  Point temp_p(2135.94, 707.718, 1531);
    // temp_p = temp_p - p;
    // double dis1 = sqrt(pow(temp_p.x,2) + pow(temp_p.y,2) + pow(temp_p.z,2));

    // std::cout << pts.size() << std::endl;
    for (auto it = pts.begin(); it != pts.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it).first;
        Point pc = mcell->center();
        double q = mcell->get_q();
        pt.x += pc.x * q;
        pt.y += pc.y * q;
        pt.z += pc.z * q;
        charge += q;

        // hack
        // if (dis1 < 1*units::mm){
        //  std::cout << p << " (" << pc.x/units::cm << " " << pc.y/units::cm << " " << pc.z/units::cm << ") " << q << " " << pts.size() << std::endl;
        //}
        //  sqrt(pow(pc.x-p.x,2)+pow(pc.y-p.y,2)+pow(pc.z-p.z,2))/units::cm << std::endl;
    }
    if (charge != 0)
    {
        pt.x /= charge;
        pt.y /= charge;
        pt.z /= charge;
    }
    return pt;
}

std::pair<Point, double> PR3DCluster::get_closest_point_along_vec(Point &p_test1, TVector3 dir, double test_dis, double dis_step, double angle_cut, double dis_cut)
{
    bool flag = false;
    Point p_test;
    double min_dis = 1e9;
    double min_dis1 = 1e9;
    Point min_point = p_test1;
    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;

        std::pair<SlimMergeGeomCell *, Point> pts = get_closest_point_mcell(p_test);
        double dis = sqrt(pow(p_test.x - pts.second.x, 2) + pow(p_test.y - pts.second.y, 2) + pow(p_test.z - pts.second.z, 2));
        double dis1 = sqrt(pow(p_test1.x - pts.second.x, 2) + pow(p_test1.y - pts.second.y, 2) + pow(p_test1.z - pts.second.z, 2));
        if (dis < std::min(dis1 * tan(angle_cut / 180. * 3.1415926), dis_cut))
        {
            if (dis < min_dis)
            {
                min_dis = dis;
                min_point = pts.second;
                min_dis1 = dis1;
            }
            if (dis < 3 * units::cm)
                return std::make_pair(pts.second, dis1);
        }
    }

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

std::pair<SlimMergeGeomCell *, Point> PR3DCluster::get_closest_point_mcell(Point &p_test)
{
    std::map<SlimMergeGeomCell *, Point> pts = point_cloud->get_closest_mcell(p_test, 1);
    return std::make_pair((*pts.begin()).first, (*pts.begin()).second);
}

int PR3DCluster::get_num_points(Point &p_test, double dis)
{
    return point_cloud->get_closest_points(p_test, dis).size();
}

TVector3 PR3DCluster::calc_PCA_dir(Point &p, PointVector &ps)
{
    Point center1 = p;

    TMatrixD cov_matrix(3, 3);
    for (int i = 0; i != 3; i++)
    {
        for (int j = i; j != 3; j++)
        {
            cov_matrix(i, j) = 0;

            for (int k = 0; k != ps.size(); k++)
            {
                if (i == 0 && j == 0)
                {
                    cov_matrix(i, j) += (ps.at(k).x - center1.x) * (ps.at(k).x - center1.x); //*q*q/ps.size()/ps.size();
                }
                else if (i == 0 && j == 1)
                {
                    cov_matrix(i, j) += (ps.at(k).x - center1.x) * (ps.at(k).y - center1.y); //*q*q/ps.size()/ps.size();
                }
                else if (i == 0 && j == 2)
                {
                    cov_matrix(i, j) += (ps.at(k).x - center1.x) * (ps.at(k).z - center1.z); //*q*q/ps.size()/ps.size();
                }
                else if (i == 1 && j == 1)
                {
                    cov_matrix(i, j) += (ps.at(k).y - center1.y) * (ps.at(k).y - center1.y); //*q*q/ps.size()/ps.size();
                }
                else if (i == 1 && j == 2)
                {
                    cov_matrix(i, j) += (ps.at(k).y - center1.y) * (ps.at(k).z - center1.z); //*q*q/ps.size()/ps.size();
                }
                else if (i == 2 && j == 2)
                {
                    cov_matrix(i, j) += (ps.at(k).z - center1.z) * (ps.at(k).z - center1.z); //*q*q/ps.size()/ps.size();
                }
            }
        }
    }

    cov_matrix(1, 0) = cov_matrix(0, 1);
    cov_matrix(2, 0) = cov_matrix(0, 2);
    cov_matrix(2, 1) = cov_matrix(1, 2);

    TMatrixDEigen eigen(cov_matrix);
    TMatrixD eigen_values = eigen.GetEigenValues();
    TMatrixD eigen_vectors = eigen.GetEigenVectors();
    TVector3 dir(eigen_vectors(0, 0) / sqrt(eigen_vectors(0, 0) * eigen_vectors(0, 0) + eigen_vectors(1, 0) * eigen_vectors(1, 0) + eigen_vectors(2, 0) * eigen_vectors(2, 0)),
                 eigen_vectors(1, 0) / sqrt(eigen_vectors(0, 0) * eigen_vectors(0, 0) + eigen_vectors(1, 0) * eigen_vectors(1, 0) + eigen_vectors(2, 0) * eigen_vectors(2, 0)),
                 eigen_vectors(2, 0) / sqrt(eigen_vectors(0, 0) * eigen_vectors(0, 0) + eigen_vectors(1, 0) * eigen_vectors(1, 0) + eigen_vectors(2, 0) * eigen_vectors(2, 0)));
    return dir;
}

TVector3 PR3DCluster::calc_PCA_dir(Point &p, double dis)
{
    std::map<WCP::SlimMergeGeomCell *, Point> pts = point_cloud->get_closest_mcell(p, dis);
    Point center1(0, 0, 0);
    // double charge=0;
    // for (auto it = pts.begin(); it!= pts.end(); it++){
    //   SlimMergeGeomCell *mcell = (*it).first;
    //   Point point = mcell->center();
    //   double q = mcell->get_q();
    //   center1.x += point.x * q;
    //   center1.y += point.y * q;
    //   center1.z += point.z * q;
    //   charge+=q;
    // }
    // center1.x/=charge;
    // center1.y/=charge;
    // center1.z/=charge;
    center1.x = p.x;
    center1.y = p.y;
    center1.z = p.z;

    TMatrixD cov_matrix(3, 3);
    for (int i = 0; i != 3; i++)
    {
        for (int j = i; j != 3; j++)
        {
            cov_matrix(i, j) = 0;
            for (auto it = pts.begin(); it != pts.end(); it++)
            {
                SlimMergeGeomCell *mcell = (*it).first;
                double q = mcell->get_q();
                PointVector ps = mcell->get_sampling_points();
                for (int k = 0; k != ps.size(); k++)
                {
                    if (i == 0 && j == 0)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center1.x) * (ps.at(k).x - center1.x); //*q*q/ps.size()/ps.size();
                    }
                    else if (i == 0 && j == 1)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center1.x) * (ps.at(k).y - center1.y); //*q*q/ps.size()/ps.size();
                    }
                    else if (i == 0 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center1.x) * (ps.at(k).z - center1.z); //*q*q/ps.size()/ps.size();
                    }
                    else if (i == 1 && j == 1)
                    {
                        cov_matrix(i, j) += (ps.at(k).y - center1.y) * (ps.at(k).y - center1.y); //*q*q/ps.size()/ps.size();
                    }
                    else if (i == 1 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).y - center1.y) * (ps.at(k).z - center1.z); //*q*q/ps.size()/ps.size();
                    }
                    else if (i == 2 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).z - center1.z) * (ps.at(k).z - center1.z); //*q*q/ps.size()/ps.size();
                    }
                }
            }
        }
    }

    cov_matrix(1, 0) = cov_matrix(0, 1);
    cov_matrix(2, 0) = cov_matrix(0, 2);
    cov_matrix(2, 1) = cov_matrix(1, 2);

    TMatrixDEigen eigen(cov_matrix);
    TMatrixD eigen_values = eigen.GetEigenValues();
    TMatrixD eigen_vectors = eigen.GetEigenVectors();
    TVector3 dir(eigen_vectors(0, 0) / sqrt(eigen_vectors(0, 0) * eigen_vectors(0, 0) + eigen_vectors(1, 0) * eigen_vectors(1, 0) + eigen_vectors(2, 0) * eigen_vectors(2, 0)),
                 eigen_vectors(1, 0) / sqrt(eigen_vectors(0, 0) * eigen_vectors(0, 0) + eigen_vectors(1, 0) * eigen_vectors(1, 0) + eigen_vectors(2, 0) * eigen_vectors(2, 0)),
                 eigen_vectors(2, 0) / sqrt(eigen_vectors(0, 0) * eigen_vectors(0, 0) + eigen_vectors(1, 0) * eigen_vectors(1, 0) + eigen_vectors(2, 0) * eigen_vectors(2, 0)));
    return dir;
}

TVector3 PR3DCluster::calc_dir(Point &p_test, Point &p, double dis)
{
    std::map<WCP::SlimMergeGeomCell *, Point> pts = point_cloud->get_closest_mcell(p, dis);
    TVector3 dir(0, 0, 0);
    for (auto it = pts.begin(); it != pts.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it).first;
        Point point = mcell->center();
        double q = mcell->get_q();
        TVector3 dir1(point.x - p_test.x, point.y - p_test.y, point.z - p_test.z);
        dir += dir1 * q;
    }
    if (dir.Mag() != 0)
        dir.SetMag(1);
    return dir;
}

TVector3 PR3DCluster::VHoughTrans(Point &p, double dis, ToyPointCloud *point_cloud1)
{
    double theta, phi;
    std::pair<double, double> angles_1 = HoughTrans(p, dis, point_cloud1);
    theta = angles_1.first;
    phi = angles_1.second;
    TVector3 temp(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
    return temp;
}

std::pair<double, double> PR3DCluster::HoughTrans(Point &p, double dis, ToyPointCloud *point_cloud1)
{
    double theta, phi;
    TH2F *hough = new TH2F("", "", 180, 0., 3.1415926, 360, -3.1415926, 3.1415926);
    double x0 = p.x;
    double y0 = p.y;
    double z0 = p.z;

    std::vector<std::pair<WCP::SlimMergeGeomCell *, Point>> pts = point_cloud1->get_closest_points(p, dis);

    // std::cout << "Num " <<  pts.size() << std::endl;

    double x, y, z, q;
    for (size_t i = 0; i != pts.size(); i++)
    {
        x = pts.at(i).second.x;
        y = pts.at(i).second.y;
        z = pts.at(i).second.z;
        q = pts.at(i).first->get_q() / pts.at(i).first->get_sampling_points().size();
        if (q <= 0)
            continue;

        //  for (int i1=0; i1!=5; i1++){
        //  for (int j1=0; j1!=5; j1++){
        //	for (int k1=0; k1!=5; k1++){
        TVector3 vec(x - x0, y - y0, z - z0);
        hough->Fill(vec.Theta(), vec.Phi(), q);
    }
    int maxbin = hough->GetMaximumBin();
    int a, b, c;
    hough->GetBinXYZ(maxbin, a, b, c);
    theta = hough->GetXaxis()->GetBinCenter(a);
    phi = hough->GetYaxis()->GetBinCenter(b);

    // std::cout << hough->GetSum() << " " << hough->GetBinContent(a,b)<< std::endl;

    delete hough;
    return std::make_pair(theta, phi);
}

TVector3 PR3DCluster::VHoughTrans(Point &p, double dis)
{
    double theta, phi;
    std::pair<double, double> angles_1 = HoughTrans(p, dis);
    theta = angles_1.first;
    phi = angles_1.second;
    TVector3 temp(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
    return temp;
}

std::pair<double, double> PR3DCluster::HoughTrans(Point &p, double dis)
{
    double theta, phi;
    TH2F *hough = new TH2F("", "", 180, 0., 3.1415926, 360, -3.1415926, 3.1415926);
    double x0 = p.x;
    double y0 = p.y;
    double z0 = p.z;

    std::vector<std::pair<WCP::SlimMergeGeomCell *, Point>> pts = point_cloud->get_closest_points(p, dis);

    // std::cout << "Num " <<  pts.size() << std::endl;

    double x, y, z, q;
    for (size_t i = 0; i != pts.size(); i++)
    {
        x = pts.at(i).second.x;
        y = pts.at(i).second.y;
        z = pts.at(i).second.z;
        q = pts.at(i).first->get_q() / pts.at(i).first->get_sampling_points().size();
        if (q <= 0)
            continue;

        //  for (int i1=0; i1!=5; i1++){
        //  for (int j1=0; j1!=5; j1++){
        //	for (int k1=0; k1!=5; k1++){
        TVector3 vec(x - x0, y - y0, z - z0);
        hough->Fill(vec.Theta(), vec.Phi(), q);
    }
    int maxbin = hough->GetMaximumBin();
    int a, b, c;
    hough->GetBinXYZ(maxbin, a, b, c);
    theta = hough->GetXaxis()->GetBinCenter(a);
    phi = hough->GetYaxis()->GetBinCenter(b);

    // std::cout << hough->GetSum() << " " << hough->GetBinContent(a,b)<< std::endl;

    delete hough;
    return std::make_pair(theta, phi);
}

void PR3DCluster::Calc_PCA(PointVector &points)
{

    center.x = 0;
    center.y = 0;
    center.z = 0;
    int nsum = 0;
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        for (int k = 0; k != points.size(); k++)
        {
            center.x += points.at(k).x;
            center.y += points.at(k).y;
            center.z += points.at(k).z;
            nsum++;
        }
    }

    for (int i = 0; i != 3; i++)
    {
        PCA_axis[i].x = 0;
        PCA_axis[i].y = 0;
        PCA_axis[i].z = 0;
    }

    if (nsum >= 3)
    {
        center.x /= nsum;
        center.y /= nsum;
        center.z /= nsum;
    }
    else
    {
        return;
    }
    TMatrixD cov_matrix(3, 3);

    for (int i = 0; i != 3; i++)
    {
        for (int j = i; j != 3; j++)
        {
            cov_matrix(i, j) = 0;
            for (auto it = mcells.begin(); it != mcells.end(); it++)
            {
                PointVector &ps = points;
                for (int k = 0; k != ps.size(); k++)
                {
                    if (i == 0 && j == 0)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center.x) * (ps.at(k).x - center.x);
                    }
                    else if (i == 0 && j == 1)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center.x) * (ps.at(k).y - center.y);
                    }
                    else if (i == 0 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center.x) * (ps.at(k).z - center.z);
                    }
                    else if (i == 1 && j == 1)
                    {
                        cov_matrix(i, j) += (ps.at(k).y - center.y) * (ps.at(k).y - center.y);
                    }
                    else if (i == 1 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).y - center.y) * (ps.at(k).z - center.z);
                    }
                    else if (i == 2 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).z - center.z) * (ps.at(k).z - center.z);
                    }
                }
            }
        }
    }
    cov_matrix(1, 0) = cov_matrix(0, 1);
    cov_matrix(2, 0) = cov_matrix(0, 2);
    cov_matrix(2, 1) = cov_matrix(1, 2);

    TMatrixDEigen eigen(cov_matrix);
    TMatrixD eigen_values = eigen.GetEigenValues();
    TMatrixD eigen_vectors = eigen.GetEigenVectors();

    PCA_values[0] = eigen_values(0, 0);
    PCA_values[1] = eigen_values(1, 1);
    PCA_values[2] = eigen_values(2, 2);

    // std::cout << eigen_values(0,0) << " " << eigen_values(1,1) << " " << eigen_values(2,2) << std::endl;
    // std::cout << eigen_vectors(0,0) << " " << eigen_vectors(0,1) << " " << eigen_vectors(0,2) << std::endl;
    for (int i = 0; i != 3; i++)
    {
        PCA_axis[i].x = eigen_vectors(0, i) / sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) + eigen_vectors(2, i) * eigen_vectors(2, i));
        PCA_axis[i].y = eigen_vectors(1, i) / sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) + eigen_vectors(2, i) * eigen_vectors(2, i));
        PCA_axis[i].z = eigen_vectors(2, i) / sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) + eigen_vectors(2, i) * eigen_vectors(2, i));
    }
}

bool PR3DCluster::Construct_skeleton(WCP::ToyCTPointCloud &ct_point_cloud)
{
    if (path_wcps.size() > 0)
        return false;
    Calc_PCA();

    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCPointCloud<double>::WCPoint highest_wcp = cloud.pts[0];
    WCPointCloud<double>::WCPoint lowest_wcp = cloud.pts[0];

    TVector3 main_dir(PCA_axis[0].x, PCA_axis[0].y, PCA_axis[0].z);
    main_dir.SetMag(1);
    TVector3 temp_pt(highest_wcp.x - center.x, highest_wcp.y - center.y, highest_wcp.z - center.z);
    double highest_value = temp_pt.Dot(main_dir);
    double lowest_value = highest_value;

    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        temp_pt.SetXYZ(cloud.pts[i].x - center.x, cloud.pts[i].y - center.y, cloud.pts[i].z - center.z);
        double value = temp_pt.Dot(main_dir);
        if (value > highest_value)
        {
            highest_value = value;
            highest_wcp = cloud.pts[i];
        }
        else if (value < lowest_value)
        {
            lowest_value = value;
            lowest_wcp = cloud.pts[i];
        }
    }

    dijkstra_shortest_paths(highest_wcp, ct_point_cloud);
    cal_shortest_path(lowest_wcp);

    // std::cout << main_dir.X() << " " << main_dir.Y() << " " << main_dir.Z() << " "
    //   	    << lowest_wcp.x/units::cm << " " << lowest_wcp.y/units::cm << " " << lowest_wcp.z/units::cm << " "
    // 	    << highest_wcp.x/units::cm << " " << highest_wcp.y/units::cm << " " << highest_wcp.z/units::cm << " "
    // 	    << std::endl;

    return true;
}

bool PR3DCluster::Construct_skeleton()
{
    if (path_wcps.size() > 0)
        return false;
    Calc_PCA();

    WCP::WCPointCloud<double> &cloud = point_cloud->get_cloud();
    WCPointCloud<double>::WCPoint highest_wcp = cloud.pts[0];
    WCPointCloud<double>::WCPoint lowest_wcp = cloud.pts[0];

    TVector3 main_dir(PCA_axis[0].x, PCA_axis[0].y, PCA_axis[0].z);
    main_dir.SetMag(1);
    TVector3 temp_pt(highest_wcp.x - center.x, highest_wcp.y - center.y, highest_wcp.z - center.z);
    double highest_value = temp_pt.Dot(main_dir);
    double lowest_value = highest_value;

    for (size_t i = 1; i < cloud.pts.size(); i++)
    {
        temp_pt.SetXYZ(cloud.pts[i].x - center.x, cloud.pts[i].y - center.y, cloud.pts[i].z - center.z);
        double value = temp_pt.Dot(main_dir);
        if (value > highest_value)
        {
            highest_value = value;
            highest_wcp = cloud.pts[i];
        }
        else if (value < lowest_value)
        {
            lowest_value = value;
            lowest_wcp = cloud.pts[i];
        }
    }

    dijkstra_shortest_paths(highest_wcp);
    cal_shortest_path(lowest_wcp);

    // std::cout << main_dir.X() << " " << main_dir.Y() << " " << main_dir.Z() << " "
    //   	    << lowest_wcp.x/units::cm << " " << lowest_wcp.y/units::cm << " " << lowest_wcp.z/units::cm << " "
    // 	    << highest_wcp.x/units::cm << " " << highest_wcp.y/units::cm << " " << highest_wcp.z/units::cm << " "
    // 	    << std::endl;

    return true;
}

void PR3DCluster::Calc_PCA()
{
    if (flag_PCA)
        return;
    flag_PCA = true;

    center.x = 0;
    center.y = 0;
    center.z = 0;
    int nsum = 0;
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        PointVector ps = (*it)->get_sampling_points();
        for (int k = 0; k != ps.size(); k++)
        {
            center.x += ps.at(k).x;
            center.y += ps.at(k).y;
            center.z += ps.at(k).z;
            nsum++;
        }
    }

    for (int i = 0; i != 3; i++)
    {
        PCA_axis[i].x = 0;
        PCA_axis[i].y = 0;
        PCA_axis[i].z = 0;
    }

    if (nsum >= 3)
    {
        center.x /= nsum;
        center.y /= nsum;
        center.z /= nsum;
    }
    else
    {
        return;
    }
    TMatrixD cov_matrix(3, 3);

    for (int i = 0; i != 3; i++)
    {
        for (int j = i; j != 3; j++)
        {
            cov_matrix(i, j) = 0;
            for (auto it = mcells.begin(); it != mcells.end(); it++)
            {
                PointVector ps = (*it)->get_sampling_points();
                for (int k = 0; k != ps.size(); k++)
                {
                    if (i == 0 && j == 0)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center.x) * (ps.at(k).x - center.x);
                    }
                    else if (i == 0 && j == 1)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center.x) * (ps.at(k).y - center.y);
                    }
                    else if (i == 0 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).x - center.x) * (ps.at(k).z - center.z);
                    }
                    else if (i == 1 && j == 1)
                    {
                        cov_matrix(i, j) += (ps.at(k).y - center.y) * (ps.at(k).y - center.y);
                    }
                    else if (i == 1 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).y - center.y) * (ps.at(k).z - center.z);
                    }
                    else if (i == 2 && j == 2)
                    {
                        cov_matrix(i, j) += (ps.at(k).z - center.z) * (ps.at(k).z - center.z);
                    }
                }
            }
        }
    }
    cov_matrix(1, 0) = cov_matrix(0, 1);
    cov_matrix(2, 0) = cov_matrix(0, 2);
    cov_matrix(2, 1) = cov_matrix(1, 2);

    TMatrixDEigen eigen(cov_matrix);
    TMatrixD eigen_values = eigen.GetEigenValues();
    TMatrixD eigen_vectors = eigen.GetEigenVectors();

    PCA_values[0] = eigen_values(0, 0);
    PCA_values[1] = eigen_values(1, 1);
    PCA_values[2] = eigen_values(2, 2);

    // std::cout << eigen_values(0,0) << " " << eigen_values(1,1) << " " << eigen_values(2,2) << std::endl;
    // std::cout << eigen_vectors(0,0) << " " << eigen_vectors(0,1) << " " << eigen_vectors(0,2) << std::endl;
    for (int i = 0; i != 3; i++)
    {
        PCA_axis[i].x = eigen_vectors(0, i) / sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) + eigen_vectors(2, i) * eigen_vectors(2, i));
        PCA_axis[i].y = eigen_vectors(1, i) / sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) + eigen_vectors(2, i) * eigen_vectors(2, i));
        PCA_axis[i].z = eigen_vectors(2, i) / sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) + eigen_vectors(2, i) * eigen_vectors(2, i));
    }

    // std::cout << mean_x << " " << mean_y << " " << mean_z << std::endl;
}

void PR3DCluster::AddCell(SlimMergeGeomCell *mcell, int time_slice)
{
    if (cell_times_set_map.find(mcell) == cell_times_set_map.end())
    {
        std::set<int> times;
        times.insert(time_slice);
        cell_times_set_map[mcell] = times;
        mcells.push_back(mcell);
    }
    else
    {
        std::set<int> &times = cell_times_set_map[mcell];
        // if (find(times.begin(),times.end(),time_slice)==times.end()){
        times.insert(time_slice);
        // }
    }

    if (time_cells_set_map.find(time_slice) == time_cells_set_map.end())
    {
        SMGCSet mcells_1;
        mcells_1.insert(mcell);
        time_cells_set_map[time_slice] = mcells_1;
    }
    else
    {
        SMGCSet &mcells_1 = time_cells_set_map[time_slice];
        // if (find(mcells_1.begin(),mcells_1.end(), mcell) == mcells_1.end()){
        mcells_1.insert(mcell);
        //}
    }
}

void PR3DCluster::Remove_duplicated_mcells()
{
    SMGCSelection to_be_removed;
    SMGCSelection to_be_saved;
    std::map<SlimMergeGeomCell *, SlimMergeGeomCell *> mcell_map;
    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = (*it);
        bool save = true;
        for (auto it1 = to_be_saved.begin(); it1 != to_be_saved.end(); it1++)
        {
            SlimMergeGeomCell *mcell1 = *it1;
            if (mcell->IsSame(mcell1))
            {
                to_be_removed.push_back(mcell);
                mcell_map[mcell] = mcell1;
                save = false;
                break;
            }
        }
        if (save)
            to_be_saved.push_back(mcell);
    }
    // Now merge ...
    for (auto it = to_be_removed.begin(); it != to_be_removed.end(); it++)
    {
        SlimMergeGeomCell *mcell = *it;               // to be removed
        SlimMergeGeomCell *mcell1 = mcell_map[mcell]; // repalced by this one

        // save time
        std::set<int> times = cell_times_set_map[mcell];
        for (auto it1 = times.begin(); it1 != times.end(); it1++)
        {
            cell_times_set_map[mcell1].insert(*it1);
            time_cells_set_map[*it1].insert(mcell1);
            time_cells_set_map[*it1].erase(mcell);
        }

        // Now remove ...
        cell_times_set_map.erase(mcell);
        auto it2 = find(mcells.begin(), mcells.end(), mcell);
        mcells.erase(it2);
        delete mcell;
    }
    to_be_removed.clear();

    // std::cout << mcells.size() << " " << to_be_saved.size() << " " << to_be_removed.size() << std::endl;
}

SMGCSelection PR3DCluster::Is_Connected(PR3DCluster *cluster1, int offset)
{
    SMGCSelection temp_mcells;

    for (auto it = mcells.begin(); it != mcells.end(); it++)
    {
        SlimMergeGeomCell *mcell = *it;
        int time_slice = mcell->GetTimeSlice();
        std::map<int, SMGCSet> &time_mcell_map = cluster1->get_time_cells_set_map();
        // std::cout << time_slice << " " << time_mcell_map.size() << std::endl;
        // std::vector<int> times;
        // if (time_cells_set_map.find(time_slice)!=time_cells_set_map.end())
        //   times.push_back(time_slice);
        // if (time_cells_set_map.find(time_slice-1)!=time_cells_set_map.end())
        //   times.push_back(time_slice-1);
        // if (time_cells_set_map.find(time_slice+1)!=time_cells_set_map.end())
        //   times.push_back(time_slice+1);

        bool flag = false;
        // for (int i=0;i!=times.size();i++){
        if (time_mcell_map.find(time_slice) != time_mcell_map.end())
        {
            SMGCSet &mcells1 = time_mcell_map[time_slice];
            for (auto it1 = mcells1.begin(); it1 != mcells1.end(); it1++)
            {
                SlimMergeGeomCell *mcell1 = (SlimMergeGeomCell *)(*it1);
                if (mcell->Overlap_fast(mcell1, offset))
                {
                    flag = true;
                    break;
                }
            }
            //     //  if (flag) break;
        }
        if (flag)
            temp_mcells.push_back(mcell);
    }

    return temp_mcells;
}
