void WCPPID::PR3DCluster::Del_graph()
{
    if (graph != (WCPPID::MCUGraph*) 0) {
        delete graph;
        graph = 0;
        //    std::cout << "Del graph! " << cluster_id << std::endl;
    }
}

void WCPPID::PR3DCluster::remove_same_mcell_steiner_edges(int flag)
{
    if (flag == 1) {
        // std::cout << num_edges(*graph) << std::endl;
        for (auto it = same_mcell_steiner_edges.begin(); it != same_mcell_steiner_edges.end(); it++) {
            remove_edge(*it, *graph);
        }
        same_mcell_steiner_edges.clear();
        //  std::cout << num_edges(*graph) << std::endl;
    }
    else if (flag == 2) {
        // clean up added edges for the steiner tree graph ...
        for (auto it = same_mcell_steiner_edges.begin(); it != same_mcell_steiner_edges.end(); it++) {
            remove_edge(*it, *graph_steiner);
        }
        same_mcell_steiner_edges.clear();
    }
}

void WCPPID::PR3DCluster::establish_same_mcell_steiner_edges(WCP::GeomDataSource& gds, bool disable_dead_mix_cell,
                                                             int flag)
{
    if (flag == 1) {
        if (graph == (MCUGraph*) 0) Create_graph();

        WCP::WCPointCloud<double>& cloud = point_cloud->get_cloud();
        std::map<SlimMergeGeomCell*, std::set<int>> map_mcell_all_indices;

        // std::map<SlimMergeGeomCell*, std::set<int> > map_mcell_steiner_indices;

        for (size_t i = 0; i != cloud.pts.size(); i++) {
            if (map_mcell_all_indices.find(cloud.pts.at(i).mcell) == map_mcell_all_indices.end()) {
                std::set<int> temp_vec;
                temp_vec.insert(i);
                map_mcell_all_indices[cloud.pts.at(i).mcell] = temp_vec;
            }
            else {
                map_mcell_all_indices[cloud.pts.at(i).mcell].insert(i);
            }
        }

        find_steiner_terminals(gds, disable_dead_mix_cell);
        /* for (auto it = steiner_terminal_indices.begin(); it!=steiner_terminal_indices.end(); it++){ */
        /*   if (map_mcell_steiner_indices.find(cloud.pts[*it].mcell)==map_mcell_steiner_indices.end()){ */
        /*     std::set<int> temp_vec; */
        /*     temp_vec.insert(*it); */
        /*     map_mcell_steiner_indices[cloud.pts[*it].mcell] = temp_vec; */
        /*   }else{ */
        /*     map_mcell_steiner_indices[cloud.pts[*it].mcell].insert(*it); */
        /*   } */
        /* } */

        for (auto it = map_mcell_all_indices.begin(); it != map_mcell_all_indices.end(); it++) {
            std::set<int>& temp_vec = it->second;
            for (auto it1 = temp_vec.begin(); it1 != temp_vec.end(); it1++) {
                int index1 = *it1;
                WCPointCloud<double>::WCPoint& wcp1 = cloud.pts[index1];
                bool flag_index1 = steiner_terminal_indices.find(index1) != steiner_terminal_indices.end();
                for (auto it2 = it1; it2 != temp_vec.end(); it2++) {
                    if (it2 == it1) continue;
                    int index2 = *it2;
                    bool flag_index2 = steiner_terminal_indices.find(index2) != steiner_terminal_indices.end();
                    WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[index2];

                    if (flag_index1 && flag_index2) {
                        auto edge = add_edge(index1, index2,
                                             WCPPID::EdgeProp(sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) +
                                                                   pow(wcp1.z - wcp2.z, 2)) *
                                                              0.8),
                                             *graph);
                        if (edge.second) same_mcell_steiner_edges.push_back(edge.first);
                    }
                    else if (flag_index1 || flag_index2) {
                        auto edge = add_edge(index1, index2,
                                             WCPPID::EdgeProp(sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) +
                                                                   pow(wcp1.z - wcp2.z, 2)) *
                                                              0.9),
                                             *graph);
                        if (edge.second) same_mcell_steiner_edges.push_back(edge.first);
                    }
                }
            }
        }
    }
    else if (flag == 2) {
        WCP::WCPointCloud<double>& cloud = point_cloud_steiner->get_cloud();
        std::map<SlimMergeGeomCell*, std::set<int>> map_mcell_all_indices;

        for (size_t i = 0; i != cloud.pts.size(); i++) {
            if (cloud.pts.at(i).mcell == 0) continue;

            if (map_mcell_all_indices.find(cloud.pts.at(i).mcell) == map_mcell_all_indices.end()) {
                std::set<int> temp_vec;
                temp_vec.insert(i);
                map_mcell_all_indices[cloud.pts.at(i).mcell] = temp_vec;
            }
            else {
                map_mcell_all_indices[cloud.pts.at(i).mcell].insert(i);
            }
        }

        for (auto it = map_mcell_all_indices.begin(); it != map_mcell_all_indices.end(); it++) {
            std::set<int>& temp_vec = it->second;
            for (auto it1 = temp_vec.begin(); it1 != temp_vec.end(); it1++) {
                int index1 = *it1;
                WCPointCloud<double>::WCPoint& wcp1 = cloud.pts[index1];
                bool flag_index1 = flag_steiner_terminal[index1];
                for (auto it2 = it1; it2 != temp_vec.end(); it2++) {
                    if (it2 == it1) continue;
                    int index2 = *it2;
                    bool flag_index2 = flag_steiner_terminal[index2];
                    WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[index2];

                    if (flag_index1 && flag_index2) {
                        auto edge = add_edge(index1, index2,
                                             WCPPID::EdgeProp(sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) +
                                                                   pow(wcp1.z - wcp2.z, 2))),
                                             *graph_steiner);
                        if (edge.second) same_mcell_steiner_edges.push_back(edge.first);
                    }
                    else if (flag_index1 || flag_index2) {
                        auto edge = add_edge(index1, index2,
                                             WCPPID::EdgeProp(sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) +
                                                                   pow(wcp1.z - wcp2.z, 2))),
                                             *graph_steiner);
                        if (edge.second) same_mcell_steiner_edges.push_back(edge.first);
                    }
                }
            }
        }
    }
}

void WCPPID::PR3DCluster::Connect_graph(WCP::ToyCTPointCloud& ct_point_cloud, WCP::ToyPointCloud* ref_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();

    TVector3 drift_dir(1, 0, 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) {
            if (cloud.pts[i].mcell != 0) {
                // if (cloud.pts[i].mcell->Estimate_total_charge() > 0)
                if (cloud.pts[i].mcell->IsPointGood(cloud.pts[i].index_u, cloud.pts[i].index_v, cloud.pts[i].index_w,
                                                    2)) {
                    double temp_min_dis = 0;
                    if (ref_point_cloud != 0) {
                        Point temp_p(cloud.pts[i].x, cloud.pts[i].y, cloud.pts[i].z);
                        temp_min_dis = ref_point_cloud->get_closest_dis(temp_p);
                    }
                    if (temp_min_dis < 1.0 * units::cm)
                        pt_clouds.at(component[i])
                            ->AddPoint(cloud.pts[i], cloud_u.pts[i], cloud_v.pts[i], cloud_w.pts[i]);
                    else
                        excluded_points.insert(i);
                }
                else {
                    excluded_points.insert(i);
                }
            }
            else {
                excluded_points.insert(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++) {
            //      std::cout << j << " " << pt_clouds.at(j)->get_cloud().pts.size() << std::endl;
            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++) {
                if (pt_clouds.at(j)->get_cloud().pts.size() == 0 || pt_clouds.at(k)->get_cloud().pts.size() == 0)
                    continue;

                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));
                    dir1 *= -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 && fabs(dir1.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.) {
                        if (fabs(dir1.Angle(drift_dir) / 3.1415926 * 180. - 90) < 5.)
                            dir1 = VHoughTrans(p1, 80 * units::cm, pt_clouds.at(j));
                        else if (fabs(dir1.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.)
                            dir1 = VHoughTrans(p1, 50 * units::cm, pt_clouds.at(j));
                        dir1 *= -1;
                        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);
                    }

                    // search for iso connection ...
                    /* if (result1.first < 0 && fabs(dir1.Angle(drift_dir)/3.1415926*180.-90)<10. ){ */
                    /*   if (fabs(dir1.Angle(drift_dir)/3.1415926*180.-90)<5.) dir1 = VHoughTrans(p1,
                     * 80*units::cm,pt_clouds.at(j)); */
                    /*   else  dir1 = VHoughTrans(p1, 50*units::cm,pt_clouds.at(j)); */
                    /*   dir1 *= -1; */
                    /*   search_for_connection_isochronous(result1, p1, dir1, pt_clouds.at(j), pt_clouds.at(k),
                     * 80*units::cm, 10, 1.5*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;

                    TVector3 dir2 = VHoughTrans(p2, 30 * units::cm, pt_clouds.at(k));
                    dir2 *= -1;
                    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 && fabs(dir2.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.) {
                        if (fabs(dir2.Angle(drift_dir) / 3.1415926 * 180. - 90) < 5.)
                            dir2 = VHoughTrans(p2, 80 * units::cm, pt_clouds.at(k));
                        else if (fabs(dir2.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.)
                            dir2 = VHoughTrans(p2, 50 * units::cm, pt_clouds.at(k));
                        dir2 *= -1;
                        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;
                    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) {
                    if (std::get<2>(index_index_dis_mst[j][k]) > 5 * units::cm) {
                        auto edge =
                            add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]),
                                     std::get<2>(index_index_dis_mst[j][k]), *graph);
                    }
                    else {
                        auto edge =
                            add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]),
                                     std::get<2>(index_index_dis_mst[j][k]), *graph);
                    }
                    //	  std::cout << "Xin1: " << std::get<0>(index_index_dis_mst[j][k]) << " " <<
                    //std::get<1>(index_index_dis_mst[j][k]) << " " << std::get<2>(index_index_dis_mst[j][k]) <<
                    //std::endl;
                }

                // 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) {
                        if (std::get<2>(index_index_dis_dir1[j][k]) > 5 * units::cm) {
                            auto edge = add_edge(std::get<0>(index_index_dis_dir1[j][k]),
                                                 std::get<1>(index_index_dis_dir1[j][k]),
                                                 std::get<2>(index_index_dis_dir1[j][k]) * 1.1, *graph);
                        }
                        else {
                            auto edge = add_edge(std::get<0>(index_index_dis_dir1[j][k]),
                                                 std::get<1>(index_index_dis_dir1[j][k]),
                                                 std::get<2>(index_index_dis_dir1[j][k]), *graph);
                        }
                        //	    std::cout << "Xin2: " << std::get<0>(index_index_dis_dir1[j][k]) << " " <<
                        //std::get<1>(index_index_dis_dir1[j][k]) << " " << std::get<2>(index_index_dis_dir1[j][k]) <<
                        //std::endl;
                    }
                    if (std::get<0>(index_index_dis_dir2[j][k]) >= 0) {
                        if (std::get<2>(index_index_dis_dir2[j][k]) > 5 * units::cm) {
                            auto edge = add_edge(std::get<0>(index_index_dis_dir2[j][k]),
                                                 std::get<1>(index_index_dis_dir2[j][k]),
                                                 std::get<2>(index_index_dis_dir2[j][k]) * 1.1, *graph);
                        }
                        else {
                            auto edge = add_edge(std::get<0>(index_index_dis_dir2[j][k]),
                                                 std::get<1>(index_index_dis_dir2[j][k]),
                                                 std::get<2>(index_index_dis_dir2[j][k]), *graph);
                        }
                        //	    std::cout << "Xin3: " << std::get<0>(index_index_dis_dir2[j][k]) << " " <<
                        //std::get<1>(index_index_dis_dir2[j][k]) << " " << std::get<2>(index_index_dis_dir2[j][k]) <<
                        //std::endl;
                    }
                }

            }  // k
        }      // j

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

void WCPPID::PR3DCluster::Create_graph(WCP::ToyCTPointCloud& ct_point_cloud, WCP::ToyPointCloud* ref_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);

    //  std::cout << "test1 " << std::endl;
    Establish_close_connected_graph();
    Connect_graph(ct_point_cloud, ref_point_cloud);
    Connect_graph(ref_point_cloud);

    // std::cout <<"Create Graph! " << cluster_id  << " " << N << " " << num_edges(*graph) << std::endl;
}

void WCPPID::PR3DCluster::Create_graph(WCP::ToyPointCloud* ref_point_cloud)
{
    if (graph != (WCPPID::MCUGraph*) 0) return;

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

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

    // std::cout << "test " << std::endl;
    Establish_close_connected_graph();
    Connect_graph(ref_point_cloud);
    // std::cout <<"Create Graph! " << cluster_id  << " " << N << std::endl;
    // Connect_graph(ref_point_cloud);
    // calc_num_components();
}

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

    //  bool flag_reduce_memory = true;
    const int max_num_edges = 12;
    const int max_num_nodes = 5;
    double ref_dir_y[max_num_edges];  // = {0,-1./sqrt(2.),-1, -1./sqrt(2.),  0,  1./sqrt(2.), 1, 1./sqrt(2.)};
    double ref_dir_z[max_num_edges];  // = {1, 1./sqrt(2.), 0, -1./sqrt(2.), -1, -1./sqrt(2.), 0, 1./sqrt(2.)};
    for (size_t i = 0; i != max_num_edges; i++) {
        ref_dir_z[i] = cos(2 * 3.1415926 / max_num_edges * i);
        ref_dir_y[i] = sin(2 * 3.1415926 / max_num_edges * i);
    }

    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;
            {
                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;
                //	if (common_set.size() <= max_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;
                        // add edge ...
                        auto edge = add_edge(index1, index2,
                                             WCPPID::EdgeProp(sqrt(pow(wcp1.x - wcp2.x, 2) + pow(wcp1.y - wcp2.y, 2) +
                                                                   pow(wcp1.z - wcp2.z, 2))),
                                             *graph);
                        //	    std::cout << index1 << " " << index2 << " " << edge.second << std::endl;
                        if (edge.second) {
                            num_edges++;
                        }
                    }
                }
                /* }else{ */
                /*   std::vector<int> temp_saved_indices_min; */
                /*   std::vector<int> temp_saved_dis_min; */
                /*   //std::vector<int> temp_saved_indices_max; */
                /*   //std::vector<int> temp_saved_dis_max; */
                /*   temp_saved_indices_min.resize(max_num_edges,-1); */
                /*   temp_saved_dis_min.resize(max_num_edges,1e9); */
                /*   //temp_saved_indices_max.resize(max_num_edges,-1); */
                /*   //temp_saved_dis_max.resize(max_num_edges,0); */
                /*   for (auto it4 = common_set.begin(); it4!=common_set.end(); it4++){ */
                /*     WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[*it4]; */
                /*     if (wcp2.index != wcp1.index){ */
                /*       for (int qx = 0;qx!=max_num_edges; qx++){ */
                /* 	if (sqrt(pow(wcp2.y-wcp1.y,2)+pow(wcp2.z-wcp1.z,2))==0) continue; */
                /* 	double dis = acos(((wcp2.y-wcp1.y) * ref_dir_y[qx] +
                 * (wcp2.z-wcp1.z)*ref_dir_z[qx])/sqrt(pow(wcp2.y-wcp1.y,2)+pow(wcp2.z-wcp1.z,2))); */
                /* 	if (dis < temp_saved_dis_min.at(qx)){ */
                /* 	  temp_saved_dis_min[qx] = dis; */
                /* 	  temp_saved_indices_min[qx] = (*it4); */
                /* 	} */
                /* 	/\* if (dis > 0 && dis > temp_saved_dis_max.at(qx)){ *\/ */
                /* 	/\*   temp_saved_dis_max[qx] = dis; *\/ */
                /* 	/\*   temp_saved_indices_max[qx] = (*it4); *\/ */
                /* 	/\* } *\/ */
                /*       } */
                /*     } */
                /*   } */
                /*   for (int qx = 0; qx!=max_num_edges;qx++){ */
                /*     if (temp_saved_indices_min[qx] >=0){ */
                /*       WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[temp_saved_indices_min[qx]]; */
                /*       int index2 = wcp2.index; */
                /*       // add edge ... */
                /*       auto edge =
                 * add_edge(index1,index2,WCPPID::EdgeProp(sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2))),*graph);
                 */
                /*       if (edge.second){ */
                /* 	num_edges ++; */
                /*       } */
                /*     } */
                /*     /\* if (temp_saved_indices_max[qx] >=0){ *\/ */
                /*     /\*   WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[temp_saved_indices_max[qx]]; *\/ */
                /*     /\*   int index2 = wcp2.index; *\/ */
                /*     /\*   // add edge ... *\/ */
                /*     /\*   auto edge =
                 * add_edge(index1,index2,WCPPID::EdgeProp(sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2))),*graph);
                 * *\/ */
                /*     /\*   if (edge.second){ *\/ */
                /*     /\* 	num_edges ++; *\/ */
                /*     /\*   } *\/ */
                /*     /\* } *\/ */
                /*   } */
                /* } */
            }
        }
    }

    //  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)) {
                        //	    std::cout << mcell1->get_sampling_points().size() << " " <<
                        //mcell2->get_sampling_points().size() << std::endl;
                        // flag = true; // correct???
                        connected_mcells.push_back(std::make_pair(mcell1, mcell2));
                    }
                }
            }
        }
    }

    // establish edge ...
    std::map<std::pair<int, int>, std::set<std::pair<double, int>>> 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());
            }

            {
                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()) {
                            std::set<std::pair<double, int>> temp_sets;
                            temp_sets.insert(std::make_pair(dis, index2));
                            closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())] =
                                temp_sets;  // std::make_pair(index2,dis);
                        }
                        else {
                            closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].insert(
                                std::make_pair(dis, index2));
                            if (closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].size() >
                                max_num_nodes) {
                                auto it5 = closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].begin();
                                for (int qx = 0; qx != max_num_nodes; qx++) {
                                    it5++;
                                }
                                closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].erase(
                                    it5, closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].end());
                            }
                            // 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);
                        }
                    }
                }

                //	if (common_set.size() <= max_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,WCPPID::EdgeProp(sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2))),*graph);
                 */
                /*     if (edge.second){ */
                /* 	num_edges ++; */
                /*     } */
                /*   } */
                /* } */
                /* }else{ */
                /*   std::vector<int> temp_saved_indices; */
                /*   std::vector<int> temp_saved_dis; */
                /*   temp_saved_indices.resize(max_num_edges,-1); */
                /*   temp_saved_dis.resize(max_num_edges,1e9); */
                /*   for (auto it4 = common_set.begin(); it4!=common_set.end(); it4++){ */
                /*     WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[*it4]; */
                /*     if (wcp2.index != wcp1.index){ */
                /*       for (int qx = 0;qx!=max_num_edges; qx++){ */
                /* 	double dis = (wcp2.y-wcp1.y) * ref_dir_y[qx] + (wcp2.z-wcp1.z)*ref_dir_z[qx]; */
                /* 	if (dis > 0 && dis < temp_saved_dis.at(qx)){ */
                /* 	  temp_saved_dis[qx] = dis; */
                /* 	  temp_saved_indices[qx] = (*it4); */
                /* 	} */
                /*       } */
                /*     } */
                /*   } */
                /*   for (int qx = 0; qx!=max_num_edges;qx++){ */
                /*     if (temp_saved_indices[qx] >=0){ */
                /*       WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[temp_saved_indices[qx]]; */
                /*       int index2 = wcp2.index; */
                /*       // add edge ... */
                /*       auto edge =
                 * add_edge(index1,index2,WCPPID::EdgeProp(sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2))),*graph);
                 */
                /*       if (edge.second){ */
                /* 	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());
            }

            {
                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()) {
                            std::set<std::pair<double, int>> temp_sets;
                            temp_sets.insert(std::make_pair(dis, index2));
                            closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())] =
                                temp_sets;  // std::make_pair(index2,dis);
                        }
                        else {
                            closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].insert(
                                std::make_pair(dis, index2));
                            if (closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].size() >
                                max_num_nodes) {
                                auto it5 = closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].begin();
                                for (int qx = 0; qx != max_num_nodes; qx++) {
                                    it5++;
                                }
                                closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].erase(
                                    it5, closest_index[std::make_pair(index1, wcp2.mcell->GetTimeSlice())].end());
                                // 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);
                            }
                        }
                    }
                }

                // if (common_set.size() <= max_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,WCPPID::EdgeProp(sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2))),*graph);
                 */
                /*     if (edge.second){ */
                /* 	num_edges ++; */
                /*     } */
                /*   } */
                /* } */
                /* }else{ */
                /*   std::vector<int> temp_saved_indices; */
                /*   std::vector<int> temp_saved_dis; */
                /*   temp_saved_indices.resize(max_num_edges,-1); */
                /*   temp_saved_dis.resize(max_num_edges,1e9); */
                /*   for (auto it4 = common_set.begin(); it4!=common_set.end(); it4++){ */
                /*     WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[*it4]; */
                /*     if (wcp2.index != wcp1.index){ */
                /*       for (int qx = 0;qx!=max_num_edges; qx++){ */
                /* 	double dis = (wcp2.y-wcp1.y) * ref_dir_y[qx] + (wcp2.z-wcp1.z)*ref_dir_z[qx]; */
                /* 	if (dis > 0 && dis < temp_saved_dis.at(qx)){ */
                /* 	  temp_saved_dis[qx] = dis; */
                /* 	  temp_saved_indices[qx] = (*it4); */
                /* 	} */
                /*       } */
                /*     } */
                /*   } */
                /*   for (int qx = 0; qx!=max_num_edges;qx++){ */
                /*     if (temp_saved_indices[qx] >=0){ */
                /*       WCPointCloud<double>::WCPoint& wcp2 = cloud.pts[temp_saved_indices[qx]]; */
                /*       int index2 = wcp2.index; */
                /*       // add edge ... */
                /*       auto edge =
                 * add_edge(index1,index2,WCPPID::EdgeProp(sqrt(pow(wcp1.x-wcp2.x,2)+pow(wcp1.y-wcp2.y,2)+pow(wcp1.z-wcp2.z,2))),*graph);
                 */
                /*       if (edge.second){ */
                /* 	num_edges ++; */
                /*       } */
                /*     } */
                /*   } */
                /* } */
            }
        }
    }

    for (auto it4 = closest_index.begin(); it4 != closest_index.end(); it4++) {
        int index1 = it4->first.first;
        // std::cout << it4->second.size() << std::endl;

        for (auto it5 = it4->second.begin(); it5 != it4->second.end(); it5++) {
            int index2 = (*it5).second;
            double dis = (*it5).first;
            auto edge = add_edge(index1, index2, WCPPID::EdgeProp(dis), *graph);
            if (edge.second) {
                //      (*graph)[edge.first].dist = dis;
                num_edges++;
            }
            // protect against dead cells ...
            if (it5 == it4->second.begin() && dis > 0.25 * units::cm) break;
        }
    }
    // end of copying ...
}

void WCPPID::PR3DCluster::Connect_graph(WCP::ToyPointCloud* ref_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();

    TVector3 drift_dir(1, 0, 0);

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

    if (num > 1) {
        // std::cout << cluster_id << " " << num << std::endl;
        // 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) {
            if (cloud.pts[i].mcell != 0) {
                //	if (cloud.pts[i].mcell->Estimate_total_charge() > 0)
                if (cloud.pts[i].mcell->IsPointGood(cloud.pts[i].index_u, cloud.pts[i].index_v, cloud.pts[i].index_w,
                                                    2)) {
                    double temp_min_dis = 0;
                    if (ref_point_cloud != 0) {
                        Point temp_p(cloud.pts[i].x, cloud.pts[i].y, cloud.pts[i].z);
                        temp_min_dis = ref_point_cloud->get_closest_dis(temp_p);
                    }
                    if (temp_min_dis < 1.0 * units::cm)
                        pt_clouds.at(component[i])
                            ->AddPoint(cloud.pts[i], cloud_u.pts[i], cloud_v.pts[i], cloud_w.pts[i]);
                    else
                        excluded_points.insert(i);
                }
                else {
                    excluded_points.insert(i);
                }
            }
            else {
                excluded_points.insert(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++) {
            // std::cout << j << " " << pt_clouds.at(j)->get_num_points() << std::endl;
            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++) {
                if (pt_clouds.at(j)->get_cloud().pts.size() == 0 || pt_clouds.at(k)->get_cloud().pts.size() == 0)
                    continue;
                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<WCPPID::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++) {
                if (pt_clouds.at(j)->get_cloud().pts.size() == 0 || pt_clouds.at(k)->get_cloud().pts.size() == 0)
                    continue;
                // 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));
                        dir1 *= -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 && fabs(dir1.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.) {
                            if (fabs(dir1.Angle(drift_dir) / 3.1415926 * 180. - 90) < 5.)
                                dir1 = VHoughTrans(p1, 80 * units::cm, pt_clouds.at(j));
                            else if (fabs(dir1.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.)
                                dir1 = VHoughTrans(p1, 50 * units::cm, pt_clouds.at(j));
                            dir1 *= -1;
                            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);
                        }

                        TVector3 dir2 = VHoughTrans(p2, 30 * units::cm, pt_clouds.at(k));
                        dir2 *= -1;
                        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 && fabs(dir2.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.) {
                            if (fabs(dir2.Angle(drift_dir) / 3.1415926 * 180. - 90) < 5.)
                                dir2 = VHoughTrans(p2, 80 * units::cm, pt_clouds.at(k));
                            else if (fabs(dir2.Angle(drift_dir) / 3.1415926 * 180. - 90) < 10.)
                                dir2 = VHoughTrans(p2, 50 * units::cm, pt_clouds.at(k));
                            dir2 *= -1;
                            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);
                        }
                    }
            }
        }

        // test ...
        /* for (int j=0;j!=num;j++){ */
        /*   for (int k=j+1;k!=num;k++){ */
        /* 	if (std::get<0>(index_index_dis_dir1[j][k])>=0){ */
        /* 	  std::cout << "Xin5_1: " << j << " " << k << " " << std::get<0>(index_index_dis_dir1[j][k]) << " " <<
         * std::get<1>(index_index_dis_dir1[j][k]) << " " << std::get<2>(index_index_dis_dir1[j][k]) << std::endl; */
        /* 	} */
        /* 	if (std::get<0>(index_index_dis_dir2[j][k])>=0){ */
        /* 	  std::cout << "Xin6_1: " << std::get<0>(index_index_dis_dir2[j][k]) << " " <<
         * std::get<1>(index_index_dis_dir2[j][k]) << " " << std::get<2>(index_index_dis_dir2[j][k]) << std::endl; */
        /* 	} */
        /*   } */
        /* } */

        // 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<WCPPID::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) {
                    if (std::get<2>(index_index_dis_mst[j][k]) > 5 * units::cm) {
                        auto edge =
                            add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]),
                                     WCPPID::EdgeProp(std::get<2>(index_index_dis_mst[j][k])), *graph);
                    }
                    else {
                        auto edge =
                            add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]),
                                     WCPPID::EdgeProp(std::get<2>(index_index_dis_mst[j][k])), *graph);
                    }
                    // std::cout << "Xin4: " << std::get<0>(index_index_dis_mst[j][k]) << " " <<
                    // std::get<1>(index_index_dis_mst[j][k]) << " " << std::get<2>(index_index_dis_mst[j][k]) <<
                    // std::endl;
                }

                if (std::get<0>(index_index_dis_dir_mst[j][k]) >= 0) {
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0) {
                        if (std::get<2>(index_index_dis_dir1[j][k]) > 5 * units::cm) {
                            auto edge = add_edge(
                                std::get<0>(index_index_dis_dir1[j][k]), std::get<1>(index_index_dis_dir1[j][k]),
                                WCPPID::EdgeProp(std::get<2>(index_index_dis_dir1[j][k]) * 1.2), *graph);
                        }
                        else {
                            auto edge = add_edge(std::get<0>(index_index_dis_dir1[j][k]),
                                                 std::get<1>(index_index_dis_dir1[j][k]),
                                                 WCPPID::EdgeProp(std::get<2>(index_index_dis_dir1[j][k])), *graph);
                        }
                        // std::cout << "Xin5: " << std::get<0>(index_index_dis_dir1[j][k]) << " " <<
                        // std::get<1>(index_index_dis_dir1[j][k]) << " " <<
                        // std::get<2>(index_index_dis_dir1[j][k])/units::cm << " " <<
                        // cloud.pts[std::get<0>(index_index_dis_dir1[j][k])].x << " " <<
                        // cloud.pts[std::get<0>(index_index_dis_dir1[j][k])].y << " " <<
                        // cloud.pts[std::get<0>(index_index_dis_dir1[j][k])].z << " -> " <<
                        // cloud.pts[std::get<1>(index_index_dis_dir1[j][k])].x << " " <<
                        // cloud.pts[std::get<1>(index_index_dis_dir1[j][k])].y << " " <<
                        // cloud.pts[std::get<1>(index_index_dis_dir1[j][k])].z <<std::endl;
                    }
                    if (std::get<0>(index_index_dis_dir2[j][k]) >= 0) {
                        if (std::get<2>(index_index_dis_dir2[j][k]) > 5 * units::cm) {
                            auto edge = add_edge(
                                std::get<0>(index_index_dis_dir2[j][k]), std::get<1>(index_index_dis_dir2[j][k]),
                                WCPPID::EdgeProp(std::get<2>(index_index_dis_dir2[j][k]) * 1.2), *graph);
                        }
                        else {
                            auto edge = add_edge(std::get<0>(index_index_dis_dir2[j][k]),
                                                 std::get<1>(index_index_dis_dir2[j][k]),
                                                 WCPPID::EdgeProp(std::get<2>(index_index_dis_dir2[j][k])), *graph);
                        }
                        //	    std::cout << "Xin6: " << std::get<0>(index_index_dis_dir2[j][k]) << " " <<
                        //std::get<1>(index_index_dis_dir2[j][k]) << " " << std::get<2>(index_index_dis_dir2[j][k]) <<
                        //std::endl;
                    }
                }
            }
        }

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

void WCPPID::PR3DCluster::search_for_connection_isochronous(std::pair<int, double>& result1, WCP::Point& p1,
                                                            TVector3& dir1, ToyPointCloud* pcloud1,
                                                            ToyPointCloud* pcloud2, double search_distance,
                                                            double angle_cut, double tran_dis_cut)
{
    TVector3 drift_dir(1, 0, 0);
    if (result1.first < 0 && fabs(drift_dir.Angle(dir1) / 3.1415926 * 180. - 90.) < angle_cut) {
        // isochrounous track situation ...
        dir1 = dir1.Unit();
        TVector3 dir2 = dir1.Cross(drift_dir);
        dir2 = dir2.Unit();  // in plane
        TVector3 dir3 = dir1.Cross(dir2);

        // calculate RMS values
        double rms[3] = {0, 0, 0};
        double mean[3] = {0, 0, 0};
        std::vector<std::pair<WCP::SlimMergeGeomCell*, Point>> pts = pcloud1->get_closest_points(p1, 30 * units::cm);

        if (pts.size() <= 1) return;

        for (size_t i = 0; i != pts.size(); i++) {
            TVector3 dir(pts.at(i).second.x - p1.x, pts.at(i).second.y - p1.y, pts.at(i).second.z - p1.z);
            double tmp1 = dir.Dot(dir1);
            double tmp2 = dir.Dot(dir2);
            double tmp3 = dir.Dot(dir3);

            mean[0] += tmp1;
            rms[0] += pow(tmp1, 2);

            mean[1] += tmp2;
            rms[1] += pow(tmp2, 2);

            mean[2] += tmp3;
            rms[2] += pow(tmp3, 2);
        }
        rms[0] = sqrt((rms[0] - pow(mean[0], 2) / pts.size()) / (pts.size() - 1.));
        rms[1] = sqrt((rms[1] - pow(mean[1], 2) / pts.size()) / (pts.size() - 1.));
        rms[2] = sqrt((rms[2] - pow(mean[2], 2) / pts.size()) / (pts.size() - 1.));
        mean[0] /= pts.size();
        mean[1] /= pts.size();
        mean[2] /= pts.size();
        double orig_dis = sqrt(pow(mean[0], 2) + pow(mean[1], 2) + pow(mean[2], 2));
        //    std::cout << rms[0]/units::cm << " " << rms[1]/units::cm << " " << rms[2]/units::cm << std::endl;
        /* 	      std::cout << dir1.X() << " " << dir1.Y() << " " << dir1.Z() << " " << dir1_2.X() << " " <<
         * dir1_2.Y() << " " << dir1_2.Z() << " " << dir1_3.X() << " " << dir1_3.Y() << " " << dir1_3.Z() << std::endl;
         */
        WCP::WCPointCloud<double>& tmp_cloud = pcloud2->get_cloud();
        double min_dis = 1e9;
        int min_index = -1;
        double min_dis1, min_dis2, min_dis3;
        for (size_t qx = 0; qx != tmp_cloud.pts.size(); qx++) {
            TVector3 tmp_dir(tmp_cloud.pts.at(qx).x - p1.x, tmp_cloud.pts.at(qx).y - p1.y,
                             tmp_cloud.pts.at(qx).z - p1.z);
            double tmp_dis = tmp_dir.Mag();
            double tmp_dis1 = tmp_dir.Dot(dir1);
            double tmp_dis2 = tmp_dir.Dot(dir2);
            double tmp_dis3 = tmp_dir.Dot(dir3);
            if (fabs(tmp_dis3) < tran_dis_cut && fabs(tmp_dis3) < rms[2] / orig_dis * tmp_dis ||
                fabs(tmp_dis3) < 0.9 * units::cm) {
                double value = pow(2 * tmp_dis1 / search_distance, 2) +
                               pow(tmp_dis2 * orig_dis, 2) / (pow(rms[0] * tmp_dis, 2) + pow(rms[1] * tmp_dis, 2)) +
                               pow(tmp_dis3 * orig_dis, 2) / pow(rms[2] * tmp_dis, 2);
                if (value < min_dis) {
                    min_dis = value;
                    min_index = qx;
                    min_dis1 = tmp_dis1;
                    min_dis2 = tmp_dis2;
                    min_dis3 = tmp_dis3;
                }
            }
        }
        if (min_index >= 0) {
            result1.first = tmp_cloud.pts[min_index].index;
            result1.second =
                sqrt(pow(tmp_cloud.pts[min_index].x - p1.x, 2) + pow(tmp_cloud.pts[min_index].y - p1.y, 2) +
                     pow(tmp_cloud.pts[min_index].z - p1.z, 2));
            std::cout << p1 << " " << min_dis1 / units::cm << " " << min_dis2 / units::cm << " " << min_dis3 / units::cm
                      << " " << sqrt(pow(min_dis1, 2) + pow(min_dis2, 2) + pow(min_dis3, 2)) / units::cm << " "
                      << tmp_cloud.pts[min_index].x << " " << tmp_cloud.pts[min_index].y << " "
                      << tmp_cloud.pts[min_index].z << std::endl;
        }
    }
}
