#include "WCPData/Line.h"

using namespace WCP;

void WCP2dToy::Clustering_connect1(WCP::PR3DClusterSelection& live_clusters,
                                   WCP::map_pr3dcluster_double& cluster_length_map,
                                   WCP::DynamicToyPointCloud& global_point_cloud,
                                   std::map<int, std::pair<double, double>>& dead_u_index,
                                   std::map<int, std::pair<double, double>>& dead_v_index,
                                   std::map<int, std::pair<double, double>>& dead_w_index)
{
    // sort the clusters length ...
    {
        std::vector<std::pair<PR3DCluster*, double>> temp_pair_vec;
        for (auto it = cluster_length_map.begin(); it != cluster_length_map.end(); it++) {
            temp_pair_vec.push_back(std::make_pair(it->first, it->second));
        }
        sort(temp_pair_vec.begin(), temp_pair_vec.end(), sortbysec);
        live_clusters.clear();
        for (auto it = temp_pair_vec.begin(); it != temp_pair_vec.end(); it++) {
            live_clusters.push_back(it->first);
        }
    }

    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();

    DynamicToyPointCloud global_skeleton_cloud(angle_u, angle_v, angle_w);

    double extending_dis = 50 * units::cm;
    double angle = 7.5;
    double loose_dis_cut = 7.5 * units::cm;

    std::set<std::pair<PR3DCluster*, PR3DCluster*>> to_be_merged_pairs;

    WCP::WCPointCloud<double>& global_cloud = global_skeleton_cloud.get_cloud();

    std::map<PR3DCluster*, TVector3> map_cluster_dir1;
    std::map<PR3DCluster*, TVector3> map_cluster_dir2;

    TVector3 drift_dir(1, 0, 0);
    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);

    for (size_t i = 0; i != live_clusters.size(); i++) {
        PR3DCluster* cluster = live_clusters.at(i);
        cluster->Create_point_cloud();

        std::pair<Point, Point> extreme_points = cluster->get_two_extreme_points();
        TVector3 main_dir(extreme_points.second.x - extreme_points.first.x,
                          extreme_points.second.y - extreme_points.first.y,
                          extreme_points.second.z - extreme_points.first.z);
        TVector3 dir1, dir2;
        bool flag_para_1 = false;
        bool flag_prol_1 = false;
        bool flag_para_2 = false;
        bool flag_prol_2 = false;

        if (main_dir.Mag() > 10 * units::cm &&
            fabs(main_dir.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180.) {
            dir1 = main_dir;
            dir1 *= -1;
            dir2 = main_dir;
        }
        else if (cluster_length_map[cluster] > 25 * units::cm) {
            dir1 = cluster->VHoughTrans(extreme_points.first, 80 * units::cm);
            if (dir1.Mag() != 0) dir1.SetMag(1);
            if (fabs(dir1.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180.) {
                dir1.SetXYZ(dir1.X(), (extreme_points.second.y - extreme_points.first.y) / main_dir.Mag(),
                            (extreme_points.second.z - extreme_points.first.z) / main_dir.Mag());
                dir1 *= -1;
            }
            dir2 = cluster->VHoughTrans(extreme_points.second, 80 * units::cm);
            if (dir2.Mag() != 0) dir2.SetMag(1);
            if (fabs(dir2.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180.) {
                dir2.SetXYZ(dir2.X(), (extreme_points.second.y - extreme_points.first.y) / main_dir.Mag(),
                            (extreme_points.second.z - extreme_points.first.z) / main_dir.Mag());
            }
            if (dir1.Dot(main_dir) > 0) dir1 *= -1;
            if (dir2.Dot(dir1) > 0) dir2 *= -1;
        }
        else {
            dir1 = global_point_cloud.VHoughTrans(extreme_points.first, extending_dis);
            dir2 = global_point_cloud.VHoughTrans(extreme_points.second, extending_dis);
            if (dir1.Dot(main_dir) > 0) dir1 *= -1;
            if (dir2.Dot(dir1) > 0) dir2 *= -1;
        }

        bool flag_add_dir1 = true;
        bool flag_add_dir2 = true;
        map_cluster_dir1[cluster] = dir1;
        map_cluster_dir2[cluster] = dir2;

        // judge if something is good ...
        /* if (cluster_length_map[cluster] < 15*units::cm && i!=0){ */
        /*   flag_add_dir1 = false; */
        /*   flag_add_dir2 = false; */

        if (fabs(dir1.Angle(drift_dir) - 3.1415926 / 2.) < 7.5 * 3.1415926 / 180.) {
            flag_para_1 = true;
        }
        else {
            TVector3 tempV1(0, dir1.Y(), dir1.Z());
            TVector3 tempV5;
            double angle1 = tempV1.Angle(U_dir);
            tempV5.SetXYZ(fabs(dir1.X()), sqrt(pow(dir1.Y(), 2) + pow(dir1.Z(), 2)) * sin(angle1), 0);
            angle1 = tempV5.Angle(drift_dir);

            if (angle1 < 7.5 / 180. * 3.1415926) {
                flag_prol_1 = true;
            }
            else {
                angle1 = tempV1.Angle(V_dir);
                tempV5.SetXYZ(fabs(dir1.X()), sqrt(pow(dir1.Y(), 2) + pow(dir1.Z(), 2)) * sin(angle1), 0);
                angle1 = tempV5.Angle(drift_dir);

                /* if (extreme_points.first.z<20*units::cm) */
                /*   std::cout << angle1/3.1415926*180. << " " << cluster_length_map[cluster]/units::cm << std::endl; */

                if (angle1 < 7.5 / 180. * 3.1415926) {
                    flag_prol_1 = true;
                }
                else {
                    angle1 = tempV1.Angle(W_dir);
                    tempV5.SetXYZ(fabs(dir1.X()), sqrt(pow(dir1.Y(), 2) + pow(dir1.Z(), 2)) * sin(angle1), 0);
                    angle1 = tempV5.Angle(drift_dir);

                    if (angle1 < 7.5 / 180. * 3.1415926) {
                        flag_prol_1 = true;
                    }
                }
            }
        }

        if (fabs(dir2.Angle(drift_dir) - 3.1415926 / 2.) < 7.5 * 3.1415926 / 180.) {
            flag_para_2 = true;
        }
        else {
            TVector3 tempV2(0, dir2.Y(), dir2.Z());
            TVector3 tempV6;
            double angle2 = tempV2.Angle(U_dir);
            tempV6.SetXYZ(fabs(dir2.X()), sqrt(pow(dir2.Y(), 2) + pow(dir2.Z(), 2)) * sin(angle2), 0);
            angle2 = tempV6.Angle(drift_dir);
            if (angle2 < 7.5 / 180. * 3.1415926) {
                flag_prol_2 = true;
            }
            else {
                angle2 = tempV2.Angle(V_dir);
                tempV6.SetXYZ(fabs(dir2.X()), sqrt(pow(dir2.Y(), 2) + pow(dir2.Z(), 2)) * sin(angle2), 0);
                angle2 = tempV6.Angle(drift_dir);
                if (angle2 < 7.5 / 180. * 3.1415926) {
                    flag_prol_2 = true;
                }
                else {
                    angle2 = tempV2.Angle(W_dir);
                    tempV6.SetXYZ(fabs(dir2.X()), sqrt(pow(dir2.Y(), 2) + pow(dir2.Z(), 2)) * sin(angle2), 0);
                    angle2 = tempV6.Angle(drift_dir);
                    if (angle2 < 7.5 / 180. * 3.1415926) {
                        flag_prol_2 = true;
                    }
                }
            }
        }

        if ((flag_para_1 || flag_prol_1) && cluster_length_map[cluster] < 15 * units::cm) {
            flag_add_dir1 = true;
        }
        else if (cluster_length_map[cluster] >= 15 * units::cm) {
            flag_add_dir1 = true;
        }
        else {
            flag_add_dir1 = false;
        }

        if ((flag_para_2 || flag_prol_2) && cluster_length_map[cluster] < 15 * units::cm) {
            flag_add_dir2 = true;
        }
        else if (cluster_length_map[cluster] >= 15 * units::cm) {
            flag_add_dir2 = true;
        }
        else {
            flag_add_dir2 = false;
        }

        if (fabs(dir1.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180.) {
            flag_para_1 = true;
        }
        else {
            flag_para_1 = false;
        }
        if (fabs(dir2.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180.) {
            flag_para_2 = true;
        }
        else {
            flag_para_2 = false;
        }

        //}

        if (i == 0) {
            if (flag_para_1 || flag_prol_1) {
                global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis * 3, 1.2 * units::cm,
                                                angle);
                dir1 *= -1;
                global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis * 3, 1.2 * units::cm,
                                                angle);
            }
            else {
                global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis, 1.2 * units::cm,
                                                angle);
                dir1 *= -1;
                global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis, 1.2 * units::cm,
                                                angle);
            }

            if (flag_para_2 || flag_prol_2) {
                global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis * 3.0,
                                                1.2 * units::cm, angle);
                dir2 *= -1;
                global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis * 3.0,
                                                1.2 * units::cm, angle);
            }
            else {
                global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis, 1.2 * units::cm,
                                                angle);
                dir2 *= -1;
                global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis, 1.2 * units::cm,
                                                angle);
            }

            /* if (cluster_length_map[cluster] > extending_dis * 0.8){ */
            /* 	live_clusters.at(i)->Construct_skeleton(); */
            /* 	global_skeleton_cloud.AddPoints(live_clusters.at(i),1); */
            /* } */
        }
        else {
            if (cluster_length_map[cluster] < 100 * units::cm ||
                fabs(dir2.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180. &&
                    fabs(dir1.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180. &&
                    cluster_length_map[cluster] < 200 * units::cm) {
                WCP::WCPointCloud<double>& cloud = cluster->get_point_cloud()->get_cloud();
                int num_total_points = cloud.pts.size();  // total number of points
                int num_unique[3] = {0, 0, 0};            // points that are unique (not agree with any other clusters)
                std::map<PR3DCluster*, int> map_cluster_num[3];
                for (size_t j = 0; j != num_total_points; j++) {
                    Point test_point(cloud.pts.at(j).x, cloud.pts.at(j).y, cloud.pts.at(j).z);

                    bool flag_dead = false;
                    if (dead_u_index.find(cloud.pts.at(j).index_u) != dead_u_index.end()) {
                        if (cloud.pts.at(j).x >= dead_u_index[cloud.pts.at(j).index_u].first &&
                            cloud.pts.at(j).x <= dead_u_index[cloud.pts.at(j).index_u].second) {
                            flag_dead = true;
                        }
                    }

                    if (!flag_dead) {
                        std::vector<std::tuple<double, PR3DCluster*, size_t>> results =
                            global_skeleton_cloud.get_2d_points_info(test_point, loose_dis_cut, 0);
                        bool flag_unique = true;
                        if (results.size() > 0) {
                            std::set<PR3DCluster*> temp_clusters;
                            for (size_t k = 0; k != results.size(); k++) {
                                /* if (extreme_points.first.x < 125*units::cm && extreme_points.first.x > 100*units::cm
                                 * && */
                                /*     extreme_points.first.z < 660*units::cm && extreme_points.first.z > 620*units::cm)
                                 */
                                /*   std::cout <<std::get<0>(results.at(k))/units::cm << " " <<
                                 * global_cloud.pts.at(std::get<2>(results.at(k))).index_u/units::cm << std::endl; */

                                if (std::get<0>(results.at(k)) <
                                    global_cloud.pts.at(std::get<2>(results.at(k))).index_u) {
                                    flag_unique = false;
                                    temp_clusters.insert(std::get<1>(results.at(k)));
                                }
                            }
                            for (auto it = temp_clusters.begin(); it != temp_clusters.end(); it++) {
                                if (map_cluster_num[0].find(*it) == map_cluster_num[0].end()) {
                                    map_cluster_num[0][*it] = 1;
                                }
                                else {
                                    map_cluster_num[0][*it]++;
                                }
                            }
                        }
                        if (flag_unique) num_unique[0]++;
                    }
                    else {
                        std::vector<std::tuple<double, PR3DCluster*, size_t>> results =
                            global_skeleton_cloud.get_2d_points_info(test_point, loose_dis_cut, 0);
                        bool flag_unique = true;
                        if (results.size() > 0) {
                            std::set<PR3DCluster*> temp_clusters;
                            for (size_t k = 0; k != results.size(); k++) {
                                if (std::get<0>(results.at(k)) < loose_dis_cut / 3. * 2.) {
                                    flag_unique = false;
                                    temp_clusters.insert(std::get<1>(results.at(k)));
                                }
                            }
                            for (auto it = temp_clusters.begin(); it != temp_clusters.end(); it++) {
                                if (map_cluster_num[0].find(*it) == map_cluster_num[0].end()) {
                                    map_cluster_num[0][*it] = 1;
                                }
                                else {
                                    map_cluster_num[0][*it]++;
                                }
                            }
                        }
                        if (flag_unique) num_unique[0]++;
                    }

                    flag_dead = false;
                    if (dead_v_index.find(cloud.pts.at(j).index_v) != dead_v_index.end()) {
                        if (cloud.pts.at(j).x >= dead_v_index[cloud.pts.at(j).index_v].first &&
                            cloud.pts.at(j).x <= dead_v_index[cloud.pts.at(j).index_v].second) {
                            flag_dead = true;
                        }
                    }

                    if (!flag_dead) {
                        std::vector<std::tuple<double, PR3DCluster*, size_t>> results =
                            global_skeleton_cloud.get_2d_points_info(test_point, loose_dis_cut, 1);
                        bool flag_unique = true;
                        if (results.size() > 0) {
                            std::set<PR3DCluster*> temp_clusters;
                            for (size_t k = 0; k != results.size(); k++) {
                                if (std::get<0>(results.at(k)) <
                                    global_cloud.pts.at(std::get<2>(results.at(k))).index_v) {
                                    flag_unique = false;
                                    temp_clusters.insert(std::get<1>(results.at(k)));
                                }
                            }
                            for (auto it = temp_clusters.begin(); it != temp_clusters.end(); it++) {
                                if (map_cluster_num[1].find(*it) == map_cluster_num[1].end()) {
                                    map_cluster_num[1][*it] = 1;
                                }
                                else {
                                    map_cluster_num[1][*it]++;
                                }
                            }
                        }
                        if (flag_unique) num_unique[1]++;
                    }
                    else {
                        std::vector<std::tuple<double, PR3DCluster*, size_t>> results =
                            global_skeleton_cloud.get_2d_points_info(test_point, loose_dis_cut, 1);
                        bool flag_unique = true;
                        if (results.size() > 0) {
                            std::set<PR3DCluster*> temp_clusters;
                            for (size_t k = 0; k != results.size(); k++) {
                                if (std::get<0>(results.at(k)) < loose_dis_cut / 3. * 2.) {
                                    flag_unique = false;
                                    temp_clusters.insert(std::get<1>(results.at(k)));
                                }
                            }
                            for (auto it = temp_clusters.begin(); it != temp_clusters.end(); it++) {
                                if (map_cluster_num[1].find(*it) == map_cluster_num[1].end()) {
                                    map_cluster_num[1][*it] = 1;
                                }
                                else {
                                    map_cluster_num[1][*it]++;
                                }
                            }
                        }
                        if (flag_unique) num_unique[1]++;
                    }

                    flag_dead = false;
                    if (dead_w_index.find(cloud.pts.at(j).index_w) != dead_w_index.end()) {
                        if (cloud.pts.at(j).x >= dead_w_index[cloud.pts.at(j).index_w].first &&
                            cloud.pts.at(j).x <= dead_w_index[cloud.pts.at(j).index_w].second) {
                            flag_dead = true;
                        }
                    }

                    if (!flag_dead) {
                        std::vector<std::tuple<double, PR3DCluster*, size_t>> results =
                            global_skeleton_cloud.get_2d_points_info(test_point, loose_dis_cut, 2);
                        bool flag_unique = true;
                        if (results.size() > 0) {
                            std::set<PR3DCluster*> temp_clusters;
                            for (size_t k = 0; k != results.size(); k++) {
                                if (std::get<0>(results.at(k)) <
                                    global_cloud.pts.at(std::get<2>(results.at(k))).index_w) {
                                    flag_unique = false;
                                    temp_clusters.insert(std::get<1>(results.at(k)));
                                }
                            }
                            for (auto it = temp_clusters.begin(); it != temp_clusters.end(); it++) {
                                if (map_cluster_num[2].find(*it) == map_cluster_num[2].end()) {
                                    map_cluster_num[2][*it] = 1;
                                }
                                else {
                                    map_cluster_num[2][*it]++;
                                }
                            }
                        }
                        if (flag_unique) num_unique[2]++;
                    }
                    else {
                        std::vector<std::tuple<double, PR3DCluster*, size_t>> results =
                            global_skeleton_cloud.get_2d_points_info(test_point, loose_dis_cut, 2);
                        bool flag_unique = true;
                        if (results.size() > 0) {
                            std::set<PR3DCluster*> temp_clusters;
                            for (size_t k = 0; k != results.size(); k++) {
                                if (std::get<0>(results.at(k)) < loose_dis_cut / 3. * 2.) {
                                    flag_unique = false;
                                    temp_clusters.insert(std::get<1>(results.at(k)));
                                }
                            }
                            for (auto it = temp_clusters.begin(); it != temp_clusters.end(); it++) {
                                if (map_cluster_num[2].find(*it) == map_cluster_num[2].end()) {
                                    map_cluster_num[2][*it] = 1;
                                }
                                else {
                                    map_cluster_num[2][*it]++;
                                }
                            }
                        }
                        if (flag_unique) num_unique[2]++;
                    }
                }
                //      PR3DCluster *curr_cluster = cluster;

                bool flag_merge = false;

                /* if (extreme_points.first.x < 125*units::cm && extreme_points.first.x > 100*units::cm && */
                /*     extreme_points.first.z < 660*units::cm && extreme_points.first.z > 620*units::cm) */
                /* 	std::cout << cluster->get_cluster_id()  << " A " */
                /* 		  << num_unique[0] << " " << num_unique[1] << " " << num_unique[2] << " " <<
                 * num_total_points << " " << extreme_points.first.x/units::cm << " " <<
                 * extreme_points.first.y/units::cm << " " << extreme_points.first.z/units::cm << " " <<
                 * extreme_points.second.x/units::cm << " " << extreme_points.second.y/units::cm << " " <<
                 * extreme_points.second.z/units::cm << " " */
                /* 		  << dir1.X() << " " << dir1.Y() << " " << dir1.Z() << " " << dir2.X() << " " << dir2.Y()
                 * << " " << dir2.Z() << " " << cluster_length_map[cluster] /units::cm << std::endl; */

                {
                    PR3DCluster *max_cluster_u = 0, *max_cluster_v = 0, *max_cluster_w = 0;
                    int max_value_u[3] = {0, 0, 0};
                    int max_value_v[3] = {0, 0, 0};
                    int max_value_w[3] = {0, 0, 0};

                    int max_value[3] = {0, 0, 0};
                    PR3DCluster* max_cluster = 0;

                    for (auto it = map_cluster_num[0].begin(); it != map_cluster_num[0].end(); it++) {
                        if (it->second > max_value_u[0]) {
                            max_value_u[0] = it->second;
                            max_cluster_u = it->first;

                            if (map_cluster_num[1].find(max_cluster_u) != map_cluster_num[1].end()) {
                                max_value_u[1] = map_cluster_num[1][max_cluster_u];
                            }
                            else {
                                max_value_u[1] = 0;
                            }

                            if (map_cluster_num[2].find(max_cluster_u) != map_cluster_num[2].end()) {
                                max_value_u[2] = map_cluster_num[2][max_cluster_u];
                            }
                            else {
                                max_value_u[2] = 0;
                            }
                        }
                    }
                    for (auto it = map_cluster_num[1].begin(); it != map_cluster_num[1].end(); it++) {
                        if (it->second > max_value_v[1]) {
                            max_value_v[1] = it->second;
                            max_cluster_v = it->first;

                            if (map_cluster_num[0].find(max_cluster_v) != map_cluster_num[0].end()) {
                                max_value_v[0] = map_cluster_num[0][max_cluster_v];
                            }
                            else {
                                max_value_v[0] = 0;
                            }
                            if (map_cluster_num[2].find(max_cluster_v) != map_cluster_num[2].end()) {
                                max_value_v[2] = map_cluster_num[2][max_cluster_v];
                            }
                            else {
                                max_value_v[2] = 0;
                            }
                        }
                    }
                    for (auto it = map_cluster_num[2].begin(); it != map_cluster_num[2].end(); it++) {
                        if (it->second > max_value_w[2]) {
                            max_value_w[2] = it->second;
                            max_cluster_w = it->first;

                            if (map_cluster_num[1].find(max_cluster_w) != map_cluster_num[1].end()) {
                                max_value_w[1] = map_cluster_num[1][max_cluster_w];
                            }
                            else {
                                max_value_w[1] = 0;
                            }
                            if (map_cluster_num[0].find(max_cluster_w) != map_cluster_num[0].end()) {
                                max_value_w[0] = map_cluster_num[0][max_cluster_w];
                            }
                            else {
                                max_value_w[0] = 0;
                            }
                        }
                    }

                    /* std::cout << max_value_u[0] << " " << max_value_u[1] << " " << max_value_u[2] << " " */
                    /* 	    << max_value_v[0] << " " << max_value_v[1] << " " << max_value_v[2] << " " */
                    /* 	    << max_value_w[0] << " " << max_value_w[1] << " " << max_value_w[2] << " " << std::endl; */

                    if ((max_value_u[0] > 0.33 * num_total_points || max_value_u[0] > 100) &&
                        (max_value_u[1] > 0.33 * num_total_points || max_value_u[1] > 100) &&
                        (max_value_u[2] > 0.33 * num_total_points || max_value_u[2] > 100)) {
                        if (max_value_u[0] + max_value_u[1] + max_value_u[2] >
                            max_value[0] + max_value[1] + max_value[2]) {
                            max_value[0] = max_value_u[0];
                            max_value[1] = max_value_u[1];
                            max_value[2] = max_value_u[2];
                            max_cluster = max_cluster_u;
                        }
                    }
                    if ((max_value_v[0] > 0.33 * num_total_points || max_value_v[0] > 100) &&
                        (max_value_v[1] > 0.33 * num_total_points || max_value_v[1] > 100) &&
                        (max_value_v[2] > 0.33 * num_total_points || max_value_v[2] > 100)) {
                        if (max_value_v[0] + max_value_v[1] + max_value_v[2] >
                            max_value[0] + max_value[1] + max_value[2]) {
                            max_value[0] = max_value_v[0];
                            max_value[1] = max_value_v[1];
                            max_value[2] = max_value_v[2];
                            max_cluster = max_cluster_v;
                        }
                    }
                    if ((max_value_w[0] > 0.33 * num_total_points || max_value_w[0] > 100) &&
                        (max_value_w[1] > 0.33 * num_total_points || max_value_w[1] > 100) &&
                        (max_value_w[2] > 0.33 * num_total_points || max_value_w[2] > 100)) {
                        if (max_value_w[0] + max_value_w[1] + max_value_w[2] >
                            max_value[0] + max_value[1] + max_value[2]) {
                            max_value[0] = max_value_w[0];
                            max_value[1] = max_value_w[1];
                            max_value[2] = max_value_w[2];
                            max_cluster = max_cluster_w;
                        }
                    }

                    /* if (extreme_points.first.x < 125*units::cm && extreme_points.first.x > 100*units::cm && */
                    /*     extreme_points.first.z < 660*units::cm && extreme_points.first.z > 620*units::cm){ */
                    /*   std::cout << extreme_points.first.x /units::cm << " " << extreme_points.first.y/units::cm << "
                     * " << extreme_points.first.z/units::cm << " " << cluster->get_cluster_id() << " B " <<
                     * max_value[0] << " " << max_value[1] << " " << max_value[2] << " " << num_total_points << " " <<
                     * cluster_length_map[cluster]/units::cm << std::endl; */
                    /* } */

                    // if overlap a lot merge
                    if ((max_value[0] + max_value[1] + max_value[2]) >
                            0.75 * (num_total_points + num_total_points + num_total_points) &&
                        ((num_unique[1] + num_unique[0] + num_unique[2]) < 0.24 * num_total_points ||
                         ((num_unique[1] + num_unique[0] + num_unique[2]) < 0.45 * num_total_points &&
                          (num_unique[1] + num_unique[0] + num_unique[2]) < 25))) {
                        if (fabs(dir1.Angle(map_cluster_dir1[max_cluster]) - 3.1415926 / 2.) >= 70 * 3.1415926 / 180. ||
                            fabs(dir1.Angle(map_cluster_dir2[max_cluster]) - 3.1415926 / 2.) >= 70 * 3.1415926 / 180. ||
                            fabs(dir2.Angle(map_cluster_dir1[max_cluster]) - 3.1415926 / 2.) >= 70 * 3.1415926 / 180. ||
                            fabs(dir2.Angle(map_cluster_dir2[max_cluster]) - 3.1415926 / 2.) >= 70 * 3.1415926 / 180.) {
                            flag_merge = true;
                            to_be_merged_pairs.insert(std::make_pair(cluster, max_cluster));
                            // curr_cluster = max_cluster;
                        }

                        if (fabs(dir1.Angle(map_cluster_dir1[max_cluster]) - 3.1415926 / 2.) < 75 * 3.1415926 / 180. &&
                            fabs(dir1.Angle(map_cluster_dir2[max_cluster]) - 3.1415926 / 2.) < 75 * 3.1415926 / 180.) {
                            flag_add_dir1 = false;
                        }
                        else {
                            flag_add_dir1 = true;
                        }
                        if (fabs(dir2.Angle(map_cluster_dir1[max_cluster]) - 3.1415926 / 2.) < 75 * 3.1415926 / 180. &&
                            fabs(dir2.Angle(map_cluster_dir2[max_cluster]) - 3.1415926 / 2.) < 75 * 3.1415926 / 180.) {
                            flag_add_dir2 = false;
                        }
                        else {
                            flag_add_dir2 = true;
                        }

                        /* max_cluster->Calc_PCA(); */
                        /* TVector3 p2_dir(max_cluster->get_PCA_axis(0).x, max_cluster->get_PCA_axis(0).y,
                         * max_cluster->get_PCA_axis(0).z); */
                        /* if (fabs(p2_dir.Angle(dir1)-3.1415926/2.) < 75 || fabs(p2_dir.Angle(dir2)-3.1415926/2.) < 75)
                         */
                        /*   flag_add = false; */
                        /* if ( flag_merge ) */
                        /*   std::cout << extreme_points.first.x /units::cm << " " << extreme_points.first.y/units::cm
                         * << " " << extreme_points.first.z/units::cm << " " << cluster->get_cluster_id() << " B " <<
                         * max_cluster->get_cluster_id() << " " << cluster_length_map[cluster]/units::cm << " " <<
                         * cluster_length_map[max_cluster]/units::cm << " " << max_value[0] << " " << max_value[1] << "
                         * " << max_value[2] << " " << num_total_points << " " << num_unique[0] << " " << num_unique[1]
                         * << " " << num_unique[2] << std::endl; */
                    }

                    if ((max_value[0] + max_value[1] + max_value[2]) > 300 && !flag_merge) {
                        if (cluster_length_map[cluster] > 25 * units::cm ||
                            cluster_length_map[max_cluster] > 25 * units::cm) {
                            // if overlap significant, compare the PCA
                            cluster->Calc_PCA();
                            Point p1_c = cluster->get_center();
                            TVector3 p1_dir(cluster->get_PCA_axis(0).x, cluster->get_PCA_axis(0).y,
                                            cluster->get_PCA_axis(0).z);
                            max_cluster->Calc_PCA();
                            Point p2_c = max_cluster->get_center();
                            TVector3 p2_dir(max_cluster->get_PCA_axis(0).x, max_cluster->get_PCA_axis(0).y,
                                            max_cluster->get_PCA_axis(0).z);

                            double angle_diff = p1_dir.Angle(p2_dir) / 3.1415926 * 180.;
                            double angle1_drift = p1_dir.Angle(drift_dir) / 3.1415926 * 180.;
                            double angle2_drift = p2_dir.Angle(drift_dir) / 3.1415926 * 180.;
                            Line l1(p1_c, p1_dir);
                            Line l2(p2_c, p2_dir);
                            double dis = l1.closest_dis(l2);
                            double dis1 =
                                sqrt(pow(p1_c.x - p2_c.x, 2) + pow(p1_c.y - p2_c.y, 2) + pow(p1_c.z - p2_c.z, 2));

                            if ((angle_diff < 5 || angle_diff > 175 ||
                                 fabs(angle1_drift - 90) < 5 && fabs(angle2_drift - 90) < 5 &&
                                     fabs(angle1_drift - 90) + fabs(angle2_drift - 90) < 6 &&
                                     (angle_diff < 30 || angle_diff > 150)) &&
                                    dis < 1.5 * units::cm ||
                                (angle_diff < 10 || angle_diff > 170) && dis < 0.9 * units::cm &&
                                    dis1 > (cluster_length_map[cluster] + cluster_length_map[max_cluster]) / 3.) {
                                to_be_merged_pairs.insert(std::make_pair(cluster, max_cluster));
                                // curr_cluster = max_cluster;
                                flag_merge = true;
                            }
                            else if (((angle_diff < 5 || angle_diff > 175) && dis < 2.5 * units::cm ||
                                      (angle_diff < 10 || angle_diff > 170) && dis < 1.2 * units::cm) &&
                                     dis1 > (cluster_length_map[cluster] + cluster_length_map[max_cluster]) / 3.) {
                                to_be_merged_pairs.insert(std::make_pair(cluster, max_cluster));
                                flag_merge = true;
                            }

                            if ((fabs(dir2.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180. &&
                                 fabs(dir1.Angle(drift_dir) - 3.1415926 / 2.) < 5 * 3.1415926 / 180.) &&
                                (max_value[0] + max_value[1] + max_value[2]) >
                                    0.7 * (num_total_points + num_total_points + num_total_points)) {
                                to_be_merged_pairs.insert(std::make_pair(cluster, max_cluster));
                                flag_merge = true;
                            }

                            //  std::cout << extreme_points.first.x /units::cm << " " <<
                            //  extreme_points.first.y/units::cm << " " << extreme_points.first.z/units::cm << " " <<
                            //  cluster->get_cluster_id() << " " << max_cluster->get_cluster_id() << " " <<
                            //  cluster_length_map[cluster]/units::cm << " " <<
                            //  cluster_length_map[max_cluster]/units::cm << " " << angle_diff << " " << angle1_drift <<
                            //  " " << angle2_drift << " " << dis/units::cm << " " << dis1/units::cm << " " <<
                            //  flag_merge << " " << dir2.Angle(drift_dir)  /3.1415928*180. << " " <<
                            //  dir1.Angle(drift_dir) /3.1415926*180. << " " << (max_value[0]+max_value[1]+max_value[2])
                            //  << " " << num_total_points << std::endl;
                        }
                    }

                    /* if (fabs(extreme_points.first.x-228.6*units::cm)< 5*units::cm && */
                    /*     fabs(extreme_points.first.y+5.2*units::cm)< 5*units::cm && */
                    /*     fabs(extreme_points.first.z-448.4*units::cm)< 5*units::cm ){ */
                    /*   // if (max_cluster!=0) */
                    /*   std::cout << cluster->get_cluster_id() << " " << max_cluster << " " << max_value[0] << " " <<
                     * max_value[1] << " " << max_value[2] << " " << num_total_points << std::endl; */
                    /*   std::cout << max_value_u[0] << " " << max_value_u[1] << " " << max_value_u[2] << " " <<
                     * max_cluster_u->get_cluster_id() << " "<< max_cluster_u << " " << max_value_v[0] << " " <<
                     * max_value_v[1] << " " << max_value_v[2] << " " << max_cluster_v->get_cluster_id() << " " <<
                     * max_cluster_v << " " << max_value_w[0] << " " << max_value_w[1] << " " << max_value_w[2] << " "
                     * << max_cluster_w->get_cluster_id() << " " << max_cluster_w << std::endl; */
                    /*   std::cout << map_cluster_num[0][max_cluster_w] << " " << map_cluster_num[1][max_cluster_w] << "
                     * " << map_cluster_num[2][max_cluster_w] << " " << std::endl; */
                    /* } */
                }

                // when added points in
                // if overlap a lot merge
                // if overlap significant, compare the PCA

                // if (max_value[0] + max_value[1] + max_value[2]>0)
                /* if (extreme_points.first.z <=1020*units::cm && extreme_points.first.z >= 960*units::cm && */
                /*     extreme_points.first.y < 0) */
                /*   std::cout << extreme_points.first.x/units::cm << " " << extreme_points.first.y/units::cm << " " <<
                 * extreme_points.first.z/units::cm << " " << max_value[0] << " " << max_value[1] << " " << max_value[2]
                 * << " " << num_total_points << " " << num_unique[0] << " " << num_unique[1] << " " << num_unique[2] <<
                 * " " << flag_merge << " " << flag_add_dir1 << " " << flag_add_dir2 << std::endl; */

            }  // length cut ...

            /* if (extreme_points.first.x < 125*units::cm && extreme_points.first.x > 100*units::cm && */
            /* 	  extreme_points.first.z < 660*units::cm && extreme_points.first.z > 620*units::cm){ */
            /* 	std::cout << flag_add_dir1 << " " << flag_add_dir2 << " " << extreme_points.first.x/units::cm << " " <<
             * extreme_points.first.y/units::cm << " " << extreme_points.first.z/units::cm << " " << dir1.X() << " " <<
             * dir1.Y() << " " << dir1.Z() << " " << extreme_points.second.x/units::cm << " " <<
             * extreme_points.second.y/units::cm << " " << extreme_points.second.z/units::cm << " " << dir2.X() << " "
             * << dir2.Y() << " " << dir2.Z() << " " << cluster_length_map[cluster]/units::cm << std::endl; */

            /* } */

            if (flag_add_dir1) {
                // add extension points in ...
                if (flag_para_1 || flag_prol_1) {
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis * 3,
                                                    1.2 * units::cm, angle);
                    dir1 *= -1;
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis * 3,
                                                    1.2 * units::cm, angle);
                }
                else {
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis, 1.2 * units::cm,
                                                    angle);
                    dir1 *= -1;
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.first, dir1, extending_dis, 1.2 * units::cm,
                                                    angle);
                }
            }

            if (flag_add_dir2) {
                if (flag_para_2 || flag_prol_2) {
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis * 3.0,
                                                    1.2 * units::cm, angle);
                    dir2 *= -1;
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis * 3.0,
                                                    1.2 * units::cm, angle);
                }
                else {
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis,
                                                    1.2 * units::cm, angle);
                    dir2 *= -1;
                    global_skeleton_cloud.AddPoints(cluster, extreme_points.second, dir2, extending_dis,
                                                    1.2 * units::cm, angle);
                }
            }
        }  // not the first cluster ...

        // std::cout << extreme_points.first.x/units::cm << " " << extreme_points.first.y/units::cm << " " <<
        // extreme_points.first.z/units::cm << " " << extreme_points.second.x/units::cm << " " <<
        // extreme_points.second.y/units::cm << " " << extreme_points.second.z/units::cm << std::endl;
    }  // loop over clusters ...

    // to_be_merged_pairs.clear();

    // merge clusters
    std::vector<std::set<PR3DCluster*>> merge_clusters;
    for (auto it = to_be_merged_pairs.begin(); it != to_be_merged_pairs.end(); it++) {
        PR3DCluster* cluster1 = (*it).first;
        PR3DCluster* cluster2 = (*it).second;
        //  std::cout << cluster1 << " " << cluster2 << " " << cluster1->get_cluster_id() << " " <<
        //  cluster2->get_cluster_id() << std::endl;

        bool flag_new = true;
        std::vector<std::set<PR3DCluster*>> temp_set;
        for (auto it1 = merge_clusters.begin(); it1 != merge_clusters.end(); it1++) {
            std::set<PR3DCluster*>& clusters = (*it1);
            if (clusters.find(cluster1) != clusters.end() || clusters.find(cluster2) != clusters.end()) {
                clusters.insert(cluster1);
                clusters.insert(cluster2);
                flag_new = false;
                temp_set.push_back(clusters);
                // break;
            }
        }
        if (flag_new) {
            std::set<PR3DCluster*> clusters;
            clusters.insert(cluster1);
            clusters.insert(cluster2);
            merge_clusters.push_back(clusters);
        }
        if (temp_set.size() > 1) {
            // merge them further ...
            std::set<PR3DCluster*> clusters;
            for (size_t i = 0; i != temp_set.size(); i++) {
                for (auto it1 = temp_set.at(i).begin(); it1 != temp_set.at(i).end(); it1++) {
                    clusters.insert(*it1);
                }
                merge_clusters.erase(find(merge_clusters.begin(), merge_clusters.end(), temp_set.at(i)));
            }
            merge_clusters.push_back(clusters);
        }
    }

    WCP::PR3DClusterSelection new_clusters;

    // merge clusters into new clusters, delete old clusters
    for (auto it = merge_clusters.begin(); it != merge_clusters.end(); it++) {
        std::set<PR3DCluster*>& clusters = (*it);
        PR3DCluster* ncluster = new PR3DCluster((*clusters.begin())->get_cluster_id());
        live_clusters.push_back(ncluster);

        new_clusters.push_back(ncluster);

        for (auto it1 = clusters.begin(); it1 != clusters.end(); it1++) {
            PR3DCluster* ocluster = *(it1);
            // std::cout << ocluster->get_cluster_id() << " ";
            SMGCSelection& mcells = ocluster->get_mcells();
            for (auto it2 = mcells.begin(); it2 != mcells.end(); it2++) {
                SlimMergeGeomCell* mcell = (*it2);
                // std::cout << ocluster->get_cluster_id() << " " << mcell << std::endl;
                int time_slice = mcell->GetTimeSlice();
                ncluster->AddCell(mcell, time_slice);
            }
            live_clusters.erase(find(live_clusters.begin(), live_clusters.end(), ocluster));
            cluster_length_map.erase(ocluster);
            delete ocluster;
        }
        std::vector<int> range_v1 = ncluster->get_uvwt_range();
        double length_1 = sqrt(2. / 3. *
                                   (pow(pitch_u * range_v1.at(0), 2) + pow(pitch_v * range_v1.at(1), 2) +
                                    pow(pitch_w * range_v1.at(2), 2)) +
                               pow(time_slice_width * range_v1.at(3), 2));
        cluster_length_map[ncluster] = length_1;
        // std::cout << std::endl;
    }

    to_be_merged_pairs.clear();
    for (auto it = new_clusters.begin(); it != new_clusters.end(); it++) {
        PR3DCluster* cluster_1 = (*it);
        cluster_1->Calc_PCA();
        Point p1_c = cluster_1->get_center();
        TVector3 p1_dir(cluster_1->get_PCA_axis(0).x, cluster_1->get_PCA_axis(0).y, cluster_1->get_PCA_axis(0).z);
        Line l1(p1_c, p1_dir);
        for (auto it1 = live_clusters.begin(); it1 != live_clusters.end(); it1++) {
            PR3DCluster* cluster_2 = (*it1);
            if (cluster_length_map[cluster_2] < 3 * units::cm) continue;
            if (cluster_2 == cluster_1) continue;

            if (cluster_length_map[cluster_1] > 25 * units::cm || cluster_length_map[cluster_2] > 25 * units::cm ||
                (cluster_length_map[cluster_1] + cluster_length_map[cluster_2]) > 30 * units::cm) {
                cluster_2->Calc_PCA();
                Point p2_c = cluster_2->get_center();
                TVector3 p2_dir(cluster_2->get_PCA_axis(0).x, cluster_2->get_PCA_axis(0).y,
                                cluster_2->get_PCA_axis(0).z);

                TVector3 cc_dir(p2_c.x - p1_c.x, p2_c.y - p1_c.y, p2_c.z - p1_c.z);

                double angle_diff = fabs(p1_dir.Angle(p2_dir) - 3.1415926 / 2.) / 3.1415926 * 180.;
                double angle_diff1 = fabs(cc_dir.Angle(p1_dir) - 3.1415926 / 2.) / 3.1415926 * 180;
                double angle_diff2 = fabs(cc_dir.Angle(p2_dir) - 3.1415926 / 2.) / 3.1415926 * 180;

                Line l2(p2_c, p2_dir);
                double dis = l1.closest_dis(l2);

                double dis1 = sqrt(pow(p1_c.x - p2_c.x, 2) + pow(p1_c.y - p2_c.y, 2) + pow(p1_c.z - p2_c.z, 2));

                if (p1_dir.Mag() != 0) p1_dir.SetMag(1);
                if (p2_dir.Mag() != 0) p2_dir.SetMag(1);

                // if (cluster_2->get_cluster_id()==431 || cluster_1->get_cluster_id()==431)
                /* if ((fabs(p2_c.z/units::cm-420) <20 && fabs(p2_c.x/units::cm-250)<20 || */
                /*      fabs(p1_c.z/units::cm-420) <20 && fabs(p1_c.x/units::cm-250)<20 */
                /*      ) && cluster_length_map[cluster_1]/units::cm> 5 && cluster_length_map[cluster_2]/units::cm > 5)
                 */

                bool flag_merge = false;

                if (((angle_diff > 85) && (angle_diff1 > 90 - 1.5 * (90 - angle_diff)) &&
                         (angle_diff2 > 90 - 1.5 * (90 - angle_diff)) && dis < 2.5 * units::cm ||
                     (angle_diff > 80) && angle_diff1 > 80 && angle_diff2 > 80 && dis < 1.2 * units::cm) &&
                    dis1 > (cluster_length_map[cluster_2] + cluster_length_map[cluster_1]) / 3.) {
                    to_be_merged_pairs.insert(std::make_pair(cluster_1, cluster_2));
                    flag_merge = true;
                }
                else if ((angle_diff > 87) && (angle_diff1 > 90 - 1.5 * (90 - angle_diff)) &&
                         (angle_diff2 > 90 - 1.5 * (90 - angle_diff)) && dis < 4.0 * units::cm &&
                         dis1 > (cluster_length_map[cluster_2] + cluster_length_map[cluster_1]) / 2. &&
                         cluster_length_map[cluster_2] > 15 * units::cm &&
                         cluster_length_map[cluster_1] > 15 * units::cm &&
                         cluster_length_map[cluster_2] + cluster_length_map[cluster_1] > 45 * units::cm) {
                    to_be_merged_pairs.insert(std::make_pair(cluster_1, cluster_2));
                    flag_merge = true;
                }

                /* if (flag_merge) */
                /*   std::cout << cluster_2->get_cluster_id() << " " << cluster_1->get_cluster_id() << " " << angle_diff
                 * << " " << angle_diff1 << " " << dis/units::cm << " " << dis1/units::cm <<  " " <<
                 * cluster_length_map[cluster_1]/units::cm << " " << cluster_length_map[cluster_2]/units::cm << " " <<
                 * p1_dir.X() << " " << p1_dir.Y() << " " <<  p1_dir.Z() << " " << p2_dir.X() << " " << p2_dir.Y() << "
                 * " << p2_dir.Z() << " " << p1_c.x/units::cm << " " << p1_c.y/units::cm << " " << p1_c.z/units::cm << "
                 * " << p2_c.x/units::cm << " " << p2_c.y/units::cm << " " << p2_c.z/units::cm << std::endl;  */
            }
        }
    }

    merge_clusters.clear();
    for (auto it = to_be_merged_pairs.begin(); it != to_be_merged_pairs.end(); it++) {
        PR3DCluster* cluster1 = (*it).first;
        PR3DCluster* cluster2 = (*it).second;
        //  std::cout << cluster1 << " " << cluster2 << " " << cluster1->get_cluster_id() << " " <<
        //  cluster2->get_cluster_id() << std::endl;

        bool flag_new = true;
        std::vector<std::set<PR3DCluster*>> temp_set;
        for (auto it1 = merge_clusters.begin(); it1 != merge_clusters.end(); it1++) {
            std::set<PR3DCluster*>& clusters = (*it1);
            if (clusters.find(cluster1) != clusters.end() || clusters.find(cluster2) != clusters.end()) {
                clusters.insert(cluster1);
                clusters.insert(cluster2);
                flag_new = false;
                temp_set.push_back(clusters);
                // break;
            }
        }
        if (flag_new) {
            std::set<PR3DCluster*> clusters;
            clusters.insert(cluster1);
            clusters.insert(cluster2);
            merge_clusters.push_back(clusters);
        }
        if (temp_set.size() > 1) {
            // merge them further ...
            std::set<PR3DCluster*> clusters;
            for (size_t i = 0; i != temp_set.size(); i++) {
                for (auto it1 = temp_set.at(i).begin(); it1 != temp_set.at(i).end(); it1++) {
                    clusters.insert(*it1);
                }
                merge_clusters.erase(find(merge_clusters.begin(), merge_clusters.end(), temp_set.at(i)));
            }
            merge_clusters.push_back(clusters);
        }
    }

    for (auto it = merge_clusters.begin(); it != merge_clusters.end(); it++) {
        std::set<PR3DCluster*>& clusters = (*it);
        PR3DCluster* ncluster = new PR3DCluster((*clusters.begin())->get_cluster_id());
        live_clusters.push_back(ncluster);
        for (auto it1 = clusters.begin(); it1 != clusters.end(); it1++) {
            PR3DCluster* ocluster = *(it1);
            // std::cout << ocluster->get_cluster_id() << " ";
            SMGCSelection& mcells = ocluster->get_mcells();
            for (auto it2 = mcells.begin(); it2 != mcells.end(); it2++) {
                SlimMergeGeomCell* mcell = (*it2);
                // std::cout << ocluster->get_cluster_id() << " " << mcell << std::endl;
                int time_slice = mcell->GetTimeSlice();
                ncluster->AddCell(mcell, time_slice);
            }
            live_clusters.erase(find(live_clusters.begin(), live_clusters.end(), ocluster));
            cluster_length_map.erase(ocluster);
            delete ocluster;
        }
        std::vector<int> range_v1 = ncluster->get_uvwt_range();
        double length_1 = sqrt(2. / 3. *
                                   (pow(pitch_u * range_v1.at(0), 2) + pow(pitch_v * range_v1.at(1), 2) +
                                    pow(pitch_w * range_v1.at(2), 2)) +
                               pow(time_slice_width * range_v1.at(3), 2));
        cluster_length_map[ncluster] = length_1;
        // std::cout << std::endl;
    }
}
