void WCPPID::PR3DCluster::fill_data_map_trajectory(
    std::vector<int> indices, std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DU_set,
    std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DV_set,
    std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DW_set,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_ut_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_vt_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_wt_charge)
{
    proj_data_u_map.clear();
    proj_data_v_map.clear();
    proj_data_w_map.clear();

    if (indices.size() == 0) {
        for (auto it = map_2D_ut_charge.begin(); it != map_2D_ut_charge.end(); it++) {
            // good_channels_set.insert(it->first.first);
            proj_data_u_map[it->first] = std::make_tuple(std::get<0>(it->second), std::get<1>(it->second), 0);
        }
        for (auto it = map_2D_vt_charge.begin(); it != map_2D_vt_charge.end(); it++) {
            // good_channels_set.insert(it->first.first+2400);
            proj_data_v_map[std::make_pair(it->first.first + 2400, it->first.second)] =
                std::make_tuple(std::get<0>(it->second), std::get<1>(it->second), 0);
        }
        for (auto it = map_2D_wt_charge.begin(); it != map_2D_wt_charge.end(); it++) {
            // good_channels_set.insert(it->first.first+4800);
            proj_data_w_map[std::make_pair(it->first.first + 4800, it->first.second)] =
                std::make_tuple(std::get<0>(it->second), std::get<1>(it->second), 0);
        }
    }
    else {
        for (auto it = indices.begin(); it != indices.end(); it++) {
            //      std::cout << pu.at(*it) << " " << pv.at(*it) << " " << pw.at(*it) << " " << pt.at(*it) << std::endl;
            for (auto it1 = map_3D_2DU_set[*it].first.begin(); it1 != map_3D_2DU_set[*it].first.end(); it1++) {
                proj_data_u_map[*it1] =
                    std::make_tuple(std::get<0>(map_2D_ut_charge[*it1]), std::get<1>(map_2D_ut_charge[*it1]), 0);
                // std::cout << "U: " << it1->first << " " << it1->second << std::endl;
            }
            for (auto it1 = map_3D_2DV_set[*it].first.begin(); it1 != map_3D_2DV_set[*it].first.end(); it1++) {
                proj_data_v_map[std::make_pair(it1->first + 2400, it1->second)] =
                    std::make_tuple(std::get<0>(map_2D_vt_charge[*it1]), std::get<1>(map_2D_vt_charge[*it1]), 0);
                // std::cout << "V: " << it1->first+2400 << " " << it1->second << std::endl;
            }
            for (auto it1 = map_3D_2DW_set[*it].first.begin(); it1 != map_3D_2DW_set[*it].first.end(); it1++) {
                proj_data_w_map[std::make_pair(it1->first + 4800, it1->second)] =
                    std::make_tuple(std::get<0>(map_2D_wt_charge[*it1]), std::get<1>(map_2D_wt_charge[*it1]), 0);
                // std::cout << "W: " << it1->first+4800 << " " << it1->second << std::endl;
            }
        }
    }
}

void WCPPID::PR3DCluster::trajectory_fit(
    WCP::PointVector& ps_vec, std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DU_set,
    std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DV_set,
    std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DW_set,
    std::map<std::pair<int, int>, std::set<int>>& map_2DU_3D_set,
    std::map<std::pair<int, int>, std::set<int>>& map_2DV_3D_set,
    std::map<std::pair<int, int>, std::set<int>>& map_2DW_3D_set,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_ut_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_vt_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_wt_charge, int charge_div_method,
    double div_sigma)
{
    TPCParams& mp = Singleton<TPCParams>::Instance();
    double pitch_u = mp.get_pitch_u();
    double pitch_v = mp.get_pitch_v();
    double pitch_w = mp.get_pitch_w();
    double angle_u = mp.get_angle_u();
    double angle_v = mp.get_angle_v();
    double angle_w = mp.get_angle_w();
    double time_slice_width = mp.get_ts_width();
    double first_u_dis = mp.get_first_u_dis();
    double first_v_dis = mp.get_first_v_dis();
    double first_w_dis = mp.get_first_w_dis();

    /* for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){ */
    /*   std::cout  << " " << it->mcell << std::endl; */
    /* } */

    double slope_x = 1. / time_slice_width;
    // mcell
    double first_t_dis =
        point_cloud->get_cloud().pts[0].mcell->GetTimeSlice() * time_slice_width - point_cloud->get_cloud().pts[0].x;
    //  double first_t_dis = path_wcps.front().mcell->GetTimeSlice()*time_slice_width - path_wcps.front().x;
    double offset_t = first_t_dis / time_slice_width;

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

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

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

    // 2D pixel and then the 3D index ...
    std::map<std::tuple<int, int, int>, double> map_Udiv_fac;
    std::map<std::tuple<int, int, int>, double> map_Vdiv_fac;
    std::map<std::tuple<int, int, int>, double> map_Wdiv_fac;

    // charge division method ...
    if (charge_div_method == 1) {
        // equal division
        for (auto it = map_2DU_3D_set.begin(); it != map_2DU_3D_set.end(); it++) {
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                map_Udiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = 1. / it->second.size();
                // std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
            }
        }
        for (auto it = map_2DV_3D_set.begin(); it != map_2DV_3D_set.end(); it++) {
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                map_Vdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = 1. / it->second.size();
                // std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
            }
        }
        for (auto it = map_2DW_3D_set.begin(); it != map_2DW_3D_set.end(); it++) {
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                map_Wdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = 1. / it->second.size();
                //	std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
            }
        }
    }
    else if (charge_div_method == 2) {
        // use div_sigma ...
        for (auto it = map_2DU_3D_set.begin(); it != map_2DU_3D_set.end(); it++) {
            double sum = 0;
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                double central_t = slope_x * ps_vec.at(*it1).x + offset_t;
                double central_ch = slope_yu * ps_vec.at(*it1).y + slope_zu * ps_vec.at(*it1).z + offset_u;
                double factor = exp(-0.5 *
                                    (pow((central_t - it->first.second) * time_slice_width, 2) +
                                     pow((central_ch - it->first.first) * pitch_u, 2)) /
                                    pow(div_sigma, 2));
                map_Udiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = factor;
                sum += factor;
                // std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
            }
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                map_Udiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] /= sum;
            }
        }

        for (auto it = map_2DV_3D_set.begin(); it != map_2DV_3D_set.end(); it++) {
            double sum = 0;
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                double central_t = slope_x * ps_vec.at(*it1).x + offset_t;
                double central_ch = slope_yv * ps_vec.at(*it1).y + slope_zv * ps_vec.at(*it1).z + offset_v;
                double factor = exp(-0.5 *
                                    (pow((central_t - it->first.second) * time_slice_width, 2) +
                                     pow((central_ch - it->first.first) * pitch_v, 2)) /
                                    pow(div_sigma, 2));
                map_Vdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = factor;
                sum += factor;
                //	if ( it->first.first == 2520 &&  it->first.second ==1)
                //  std::cout << it->first.first << " " << it->first.second << " " << central_t << " " << central_ch <<
                //  " " << factor << std::endl;
            }

            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                map_Vdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] /= sum;
            }
        }

        for (auto it = map_2DW_3D_set.begin(); it != map_2DW_3D_set.end(); it++) {
            double sum = 0;
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                double central_t = slope_x * ps_vec.at(*it1).x + offset_t;
                double central_ch = slope_yw * ps_vec.at(*it1).y + slope_zw * ps_vec.at(*it1).z + offset_w;
                double factor = exp(-0.5 *
                                    (pow((central_t - it->first.second) * time_slice_width, 2) +
                                     pow((central_ch - it->first.first) * pitch_w, 2)) /
                                    pow(div_sigma, 2));
                map_Wdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = factor;
                sum += factor;
                // std::cout << it->first.first << " " << it->first.second << " " << *it1 << " " << factor << std::endl;
            }
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                map_Wdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] /= sum;
            }
        }
    }

    Eigen::VectorXd pos_3D(3 * ps_vec.size());

    for (size_t i = 0; i != ps_vec.size(); i++) {
        int n_2D_u = 2 * map_3D_2DU_set[i].first.size();
        int n_2D_v = 2 * map_3D_2DV_set[i].first.size();
        int n_2D_w = 2 * map_3D_2DW_set[i].first.size();
        Eigen::VectorXd temp_pos_3D(3), data_u_2D(n_2D_u), data_v_2D(n_2D_v), data_w_2D(n_2D_w);
        Eigen::VectorXd temp_pos_3D_init(3);
        Eigen::SparseMatrix<double> RU(n_2D_u, 3);
        Eigen::SparseMatrix<double> RV(n_2D_v, 3);
        Eigen::SparseMatrix<double> RW(n_2D_w, 3);
        // initialization
        temp_pos_3D_init(0) = ps_vec.at(i).x;
        temp_pos_3D_init(1) = ps_vec.at(i).y;
        temp_pos_3D_init(2) = ps_vec.at(i).z;

        for (size_t i = 0; i != n_2D_u; i++) {
            data_u_2D(i) = 0;
        }
        for (size_t i = 0; i != n_2D_v; i++) {
            data_v_2D(i) = 0;
        }
        for (size_t i = 0; i != n_2D_w; i++) {
            data_w_2D(i) = 0;
        }

        int index = 0;
        for (auto it = map_3D_2DU_set[i].first.begin(); it != map_3D_2DU_set[i].first.end(); it++) {
            double charge, charge_err;
            if (map_2D_ut_charge.find(*it) != map_2D_ut_charge.end()) {
                charge = std::get<0>(map_2D_ut_charge[*it]);
                charge_err = std::get<1>(map_2D_ut_charge[*it]);
            }
            else {
                charge = 100;  // serve as regularization ...
                charge_err = 1000;
            }
            if (charge < 100) {
                charge = 100;
                charge_err = 1000;
            }
            // induction plane error is large, so they have less weights in the fit to decide X position
            double scaling = charge / charge_err * map_Udiv_fac[std::make_tuple(it->first, it->second, i)];

            if (map_3D_2DU_set[i].second < 0.5) {
                if (map_3D_2DU_set[i].second != 0)
                    scaling *= pow(map_3D_2DU_set[i].second / 0.5, 1);
                else
                    scaling *= 0.05;
            }

            if (scaling != 0) {
                data_u_2D(2 * index) = scaling * (it->first - offset_u);
                data_u_2D(2 * index + 1) = scaling * (it->second - offset_t);

                RU.insert(2 * index, 1) = scaling * slope_yu;     // Y--> U
                RU.insert(2 * index, 2) = scaling * slope_zu;     // Z--> U
                RU.insert(2 * index + 1, 0) = scaling * slope_x;  // X --> T
            }
            index++;
        }

        index = 0;
        for (auto it = map_3D_2DV_set[i].first.begin(); it != map_3D_2DV_set[i].first.end(); it++) {
            double charge, charge_err;
            if (map_2D_vt_charge.find(*it) != map_2D_vt_charge.end()) {
                charge = std::get<0>(map_2D_vt_charge[*it]);
                charge_err = std::get<1>(map_2D_vt_charge[*it]);
            }
            else {
                charge = 100;  // serve as regularization ...
                charge_err = 1000;
            }
            if (charge < 100) {
                charge = 100;
                charge_err = 1000;
            }
            // induction plane error is large, so they have less weights in the fit to decide X position
            double scaling = charge / charge_err * map_Vdiv_fac[std::make_tuple(it->first, it->second, i)];
            if (map_3D_2DV_set[i].second < 0.5) {
                if (map_3D_2DV_set[i].second != 0)
                    scaling *= pow(map_3D_2DV_set[i].second / 0.5, 1);
                else
                    scaling *= 0.05;
            }

            if (scaling != 0) {
                data_v_2D(2 * index) = scaling * (it->first - offset_v);
                data_v_2D(2 * index + 1) = scaling * (it->second - offset_t);

                RV.insert(2 * index, 1) = scaling * slope_yv;     // Y--> V
                RV.insert(2 * index, 2) = scaling * slope_zv;     // Z--> V
                RV.insert(2 * index + 1, 0) = scaling * slope_x;  // X --> T
            }
            index++;
        }

        index = 0;
        for (auto it = map_3D_2DW_set[i].first.begin(); it != map_3D_2DW_set[i].first.end(); it++) {
            double charge, charge_err;
            if (map_2D_wt_charge.find(*it) != map_2D_wt_charge.end()) {
                charge = std::get<0>(map_2D_wt_charge[*it]);
                charge_err = std::get<1>(map_2D_wt_charge[*it]);
            }
            else {
                charge = 100;  // serve as regularization ...
                charge_err = 1000;
            }
            if (charge < 100) {
                charge = 100;
                charge_err = 1000;
            }
            // induction plane error is large, so they have less weights in the fit to decide X position
            double scaling = charge / charge_err * map_Wdiv_fac[std::make_tuple(it->first, it->second, i)];

            if (map_3D_2DW_set[i].second < 0.5) {
                if (map_3D_2DW_set[i].second != 0)
                    scaling *= pow(map_3D_2DW_set[i].second / 0.5, 1);
                else
                    scaling *= 0.05;
            }
            if (scaling != 0) {
                data_w_2D(2 * index) = scaling * (it->first - offset_w);
                data_w_2D(2 * index + 1) = scaling * (it->second - offset_t);

                RW.insert(2 * index, 2) = scaling * slope_zw;     // Z--> W
                RW.insert(2 * index + 1, 0) = scaling * slope_x;  // X --> T
            }
            index++;
        }

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

        Eigen::BiCGSTAB<Eigen::SparseMatrix<double>> solver;
        Eigen::VectorXd b = RUT * data_u_2D + RVT * data_v_2D + RWT * data_w_2D;  // + PMatrixT * pos_3D_init ;

        Eigen::SparseMatrix<double> A = RUT * RU + RVT * RV + RWT * RW;  // + PMatrixT*PMatrix;
        solver.compute(A);

        temp_pos_3D = solver.solveWithGuess(b, temp_pos_3D_init);

        //  std::cout << i << " " << temp_pos_3D(0) << " " << temp_pos_3D(1) << " " << temp_pos_3D(2) << " " <<
        //  temp_pos_3D_init(0) << " " << temp_pos_3D_init(1) << " " << temp_pos_3D_init(2) << std::endl;

        if (std::isnan(solver.error())) {
            pos_3D(3 * i) = temp_pos_3D_init(0);
            pos_3D(3 * i + 1) = temp_pos_3D_init(1);
            pos_3D(3 * i + 2) = temp_pos_3D_init(2);
        }
        else {
            pos_3D(3 * i) = temp_pos_3D(0);
            pos_3D(3 * i + 1) = temp_pos_3D(1);
            pos_3D(3 * i + 2) = temp_pos_3D(2);
        }
    }

    fine_tracking_path.clear();
    pu.clear();
    pv.clear();
    pw.clear();
    pt.clear();

    WCP::PointVector temp_fine_tracking_path;

    int skip_count = 0;

    for (size_t i = 0; i != ps_vec.size(); i++) {
        Point p;
        p.x = pos_3D(3 * i);
        p.y = pos_3D(3 * i + 1);
        p.z = pos_3D(3 * i + 2);

        bool flag_skip =
            skip_trajectory_point(p, i, i, ps_vec, map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set, map_2D_ut_charge,
                                  map_2D_vt_charge, map_2D_wt_charge, fine_tracking_path, offset_t, slope_x, offset_u,
                                  slope_yu, slope_zu, offset_v, slope_yv, slope_zv, offset_w, slope_yw, slope_zw);

        // protection ...
        if (flag_skip) {
            skip_count++;
            if (skip_count <= 3)
                continue;
            else
                skip_count = 0;
        }

        temp_fine_tracking_path.push_back(ps_vec.at(i));

        fine_tracking_path.push_back(p);
        // fine_tracking_path.push_back(ps_vec.at(i));

        //	std::cout << ps_vec.at(i) << " " << p << " " <<
        //sqrt(pow(ps_vec.at(i).x-p.x,2)+pow(ps_vec.at(i).y-p.y,2)+pow(ps_vec.at(i).z-p.z,2))/units::mm <<std::endl;
    }

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

    for (size_t i = 0; i != fine_tracking_path.size(); i++) {
        bool flag_replace = false;

        if (i != 0 && i + 1 != fine_tracking_path.size()) {
            // -1 vs. +1
            double a = sqrt(pow(fine_tracking_path.at(i - 1).x - fine_tracking_path.at(i).x, 2) +
                            pow(fine_tracking_path.at(i - 1).y - fine_tracking_path.at(i).y, 2) +
                            pow(fine_tracking_path.at(i - 1).z - fine_tracking_path.at(i).z, 2));
            double b = sqrt(pow(fine_tracking_path.at(i + 1).x - fine_tracking_path.at(i).x, 2) +
                            pow(fine_tracking_path.at(i + 1).y - fine_tracking_path.at(i).y, 2) +
                            pow(fine_tracking_path.at(i + 1).z - fine_tracking_path.at(i).z, 2));
            double c = sqrt(pow(fine_tracking_path.at(i - 1).x - fine_tracking_path.at(i + 1).x, 2) +
                            pow(fine_tracking_path.at(i - 1).y - fine_tracking_path.at(i + 1).y, 2) +
                            pow(fine_tracking_path.at(i - 1).z - fine_tracking_path.at(i + 1).z, 2));
            double s = (a + b + c) / 2.;
            double area1 = sqrt(s * (s - a) * (s - b) * (s - c));

            a = sqrt(pow(fine_tracking_path.at(i - 1).x - temp_fine_tracking_path.at(i).x, 2) +
                     pow(fine_tracking_path.at(i - 1).y - temp_fine_tracking_path.at(i).y, 2) +
                     pow(fine_tracking_path.at(i - 1).z - temp_fine_tracking_path.at(i).z, 2));
            b = sqrt(pow(fine_tracking_path.at(i + 1).x - temp_fine_tracking_path.at(i).x, 2) +
                     pow(fine_tracking_path.at(i + 1).y - temp_fine_tracking_path.at(i).y, 2) +
                     pow(fine_tracking_path.at(i + 1).z - temp_fine_tracking_path.at(i).z, 2));
            s = (a + b + c) / 2.;
            double area2 = sqrt(s * (s - a) * (s - b) * (s - c));

            //      std::cout << i << " A " << area1/c << " " << area2/c  << std::endl;

            if (area1 > 1.8 * units::mm * c && area1 > 1.7 * area2) flag_replace = true;
        }

        // -2, +1
        if ((!flag_replace) && i >= 2 && i + 1 != fine_tracking_path.size()) {
            double a = sqrt(pow(fine_tracking_path.at(i - 2).x - fine_tracking_path.at(i).x, 2) +
                            pow(fine_tracking_path.at(i - 2).y - fine_tracking_path.at(i).y, 2) +
                            pow(fine_tracking_path.at(i - 2).z - fine_tracking_path.at(i).z, 2));
            double b = sqrt(pow(fine_tracking_path.at(i + 1).x - fine_tracking_path.at(i).x, 2) +
                            pow(fine_tracking_path.at(i + 1).y - fine_tracking_path.at(i).y, 2) +
                            pow(fine_tracking_path.at(i + 1).z - fine_tracking_path.at(i).z, 2));
            double c = sqrt(pow(fine_tracking_path.at(i - 2).x - fine_tracking_path.at(i + 1).x, 2) +
                            pow(fine_tracking_path.at(i - 2).y - fine_tracking_path.at(i + 1).y, 2) +
                            pow(fine_tracking_path.at(i - 2).z - fine_tracking_path.at(i + 1).z, 2));
            double s = (a + b + c) / 2.;
            double area1 = sqrt(s * (s - a) * (s - b) * (s - c));

            a = sqrt(pow(fine_tracking_path.at(i - 2).x - temp_fine_tracking_path.at(i).x, 2) +
                     pow(fine_tracking_path.at(i - 2).y - temp_fine_tracking_path.at(i).y, 2) +
                     pow(fine_tracking_path.at(i - 2).z - temp_fine_tracking_path.at(i).z, 2));
            b = sqrt(pow(fine_tracking_path.at(i + 1).x - temp_fine_tracking_path.at(i).x, 2) +
                     pow(fine_tracking_path.at(i + 1).y - temp_fine_tracking_path.at(i).y, 2) +
                     pow(fine_tracking_path.at(i + 1).z - temp_fine_tracking_path.at(i).z, 2));
            s = (a + b + c) / 2.;
            double area2 = sqrt(s * (s - a) * (s - b) * (s - c));
            // std::cout << i << " B " << area1/c << " " << area2/c  << std::endl;
            if (area1 > 1.8 * units::mm * c && area1 > 1.7 * area2) flag_replace = true;
        }

        // -1, +2
        if ((!flag_replace) && i > 0 && i + 2 < fine_tracking_path.size()) {
            double a = sqrt(pow(fine_tracking_path.at(i - 1).x - fine_tracking_path.at(i).x, 2) +
                            pow(fine_tracking_path.at(i - 1).y - fine_tracking_path.at(i).y, 2) +
                            pow(fine_tracking_path.at(i - 1).z - fine_tracking_path.at(i).z, 2));
            double b = sqrt(pow(fine_tracking_path.at(i + 2).x - fine_tracking_path.at(i).x, 2) +
                            pow(fine_tracking_path.at(i + 2).y - fine_tracking_path.at(i).y, 2) +
                            pow(fine_tracking_path.at(i + 2).z - fine_tracking_path.at(i).z, 2));
            double c = sqrt(pow(fine_tracking_path.at(i - 1).x - fine_tracking_path.at(i + 2).x, 2) +
                            pow(fine_tracking_path.at(i - 1).y - fine_tracking_path.at(i + 2).y, 2) +
                            pow(fine_tracking_path.at(i - 1).z - fine_tracking_path.at(i + 2).z, 2));
            double s = (a + b + c) / 2.;
            double area1 = sqrt(s * (s - a) * (s - b) * (s - c));

            a = sqrt(pow(fine_tracking_path.at(i - 1).x - temp_fine_tracking_path.at(i).x, 2) +
                     pow(fine_tracking_path.at(i - 1).y - temp_fine_tracking_path.at(i).y, 2) +
                     pow(fine_tracking_path.at(i - 1).z - temp_fine_tracking_path.at(i).z, 2));
            b = sqrt(pow(fine_tracking_path.at(i + 2).x - temp_fine_tracking_path.at(i).x, 2) +
                     pow(fine_tracking_path.at(i + 2).y - temp_fine_tracking_path.at(i).y, 2) +
                     pow(fine_tracking_path.at(i + 2).z - temp_fine_tracking_path.at(i).z, 2));
            s = (a + b + c) / 2.;
            double area2 = sqrt(s * (s - a) * (s - b) * (s - c));

            //      std::cout << i << " C " << area1/c << " " << area2/c  << std::endl;

            if (area1 > 1.8 * units::mm * c && area1 > 1.7 * area2) flag_replace = true;
        }

        if (flag_replace) {
            fine_tracking_path.at(i) = temp_fine_tracking_path.at(i);
            //      std::cout << i << std::endl;
        }
        // if (area1 > area2 && area1 > 1.0*units::mm * c)
        // std::cout << i << " " << area1/c << " " << area2/c << std::endl;
    }

    for (size_t i = 0; i != fine_tracking_path.size(); i++) {
        WCP::Point p = fine_tracking_path.at(i);
        pu.push_back(offset_u + 0.5 + (slope_yu * p.y + slope_zu * p.z));
        pv.push_back(offset_v + 0.5 + (slope_yv * p.y + slope_zv * p.z) + 2400);
        pw.push_back(offset_w + 0.5 + (slope_yw * p.y + slope_zw * p.z) + 4800);
        pt.push_back(offset_t + 0.5 + slope_x * p.x);
    }

    ps_vec = fine_tracking_path;
}

bool WCPPID::PR3DCluster::skip_trajectory_point(
    WCP::Point& p, int i, int index, WCP::PointVector& ps_vec,
    std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DU_set,
    std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DV_set,
    std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DW_set,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_ut_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_vt_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_wt_charge,
    WCP::PointVector& fine_tracking_path, double offset_t, double slope_x, double offset_u, double slope_yu,
    double slope_zu, double offset_v, double slope_yv, double slope_zv, double offset_w, double slope_yw,
    double slope_zw)
{
    // examine the calculation
    int t1 = std::floor(offset_t + 0.5 + slope_x * p.x);
    int u1 = std::floor(offset_u + 0.5 + (slope_yu * p.y + slope_zu * p.z));
    int v1 = std::floor(offset_v + 0.5 + (slope_yv * p.y + slope_zv * p.z));
    int w1 = std::floor(offset_w + 0.5 + (slope_yw * p.y + slope_zw * p.z));

    double c1_u = 0;
    {
        auto it = map_2D_ut_charge.find(std::make_pair(u1, t1));
        if (it != map_2D_ut_charge.end()) {
            if (std::get<2>(it->second) != 0) {
                c1_u += std::get<0>(it->second);
                // if (i==9) std::cout << std::get<0>(it->second) << " " ;
            }
        }
        auto it1 = map_2D_ut_charge.find(std::make_pair(u1, t1 + 1));
        if (it1 != map_2D_ut_charge.end()) {
            if (std::get<2>(it1->second) != 0) {
                c1_u += std::get<0>(it1->second);
                // if (i==9) std::cout << std::get<0>(it->second) << " " ;
            }
        }
        auto it2 = map_2D_ut_charge.find(std::make_pair(u1, t1 - 1));
        if (it2 != map_2D_ut_charge.end()) {
            if (std::get<2>(it2->second) != 0) {
                c1_u += std::get<0>(it2->second);
                //	if (i==9) std::cout << std::get<0>(it->second) << " " ;
            }
        }
        auto it3 = map_2D_ut_charge.find(std::make_pair(u1 + 1, t1));
        if (it3 != map_2D_ut_charge.end()) {
            if (std::get<2>(it3->second) != 0) c1_u += std::get<0>(it3->second);
        }
        auto it4 = map_2D_ut_charge.find(std::make_pair(u1 - 1, t1));
        if (it4 != map_2D_ut_charge.end()) {
            if (std::get<2>(it4->second) != 0) c1_u += std::get<0>(it4->second);
        }
    }

    double c1_v = 0;
    {
        auto it = map_2D_vt_charge.find(std::make_pair(v1, t1));
        if (it != map_2D_vt_charge.end()) {
            if (std::get<2>(it->second) != 0) {
                c1_v += std::get<0>(it->second);
                //		if (i==9) std::cout << std::get<0>(it->second) << " " ;
            }
        }
        auto it1 = map_2D_vt_charge.find(std::make_pair(v1, t1 + 1));
        if (it1 != map_2D_vt_charge.end()) {
            if (std::get<2>(it1->second) != 0) {
                c1_v += std::get<0>(it1->second);
                // if (i==9) std::cout << std::get<0>(it1->second) << " " ;
            }
        }
        auto it2 = map_2D_vt_charge.find(std::make_pair(v1, t1 - 1));
        if (it2 != map_2D_vt_charge.end()) {
            if (std::get<2>(it2->second) != 0) {
                c1_v += std::get<0>(it2->second);
                // if (i==9) std::cout << std::get<0>(it2->second) << " " ;
            }
        }
        auto it3 = map_2D_vt_charge.find(std::make_pair(v1 + 1, t1));
        if (it3 != map_2D_vt_charge.end()) {
            if (std::get<2>(it3->second) != 0) {
                c1_v += std::get<0>(it3->second);
                //	if (i==9) std::cout << std::get<0>(it3->second) << " " ;
            }
        }
        auto it4 = map_2D_vt_charge.find(std::make_pair(v1 - 1, t1));
        if (it4 != map_2D_vt_charge.end()) {
            if (std::get<2>(it4->second) != 0) {
                c1_v += std::get<0>(it4->second);
                // if (i==9) std::cout << std::get<0>(it4->second) << " " ;
            }
        }
        //	    if (i==9) std::cout << std::endl;
    }

    double c1_w = 0;
    {
        auto it = map_2D_wt_charge.find(std::make_pair(w1, t1));
        if (it != map_2D_wt_charge.end()) {
            if (std::get<2>(it->second) != 0) c1_w += std::get<0>(it->second);
        }
        auto it1 = map_2D_wt_charge.find(std::make_pair(w1, t1 + 1));
        if (it1 != map_2D_wt_charge.end()) {
            if (std::get<2>(it1->second) != 0) c1_w += std::get<0>(it1->second);
        }
        auto it2 = map_2D_wt_charge.find(std::make_pair(w1, t1 - 1));
        if (it2 != map_2D_wt_charge.end()) {
            if (std::get<2>(it2->second) != 0) c1_w += std::get<0>(it2->second);
        }
        auto it3 = map_2D_wt_charge.find(std::make_pair(w1 + 1, t1));
        if (it3 != map_2D_wt_charge.end()) {
            if (std::get<2>(it3->second) != 0) c1_w += std::get<0>(it3->second);
        }
        auto it4 = map_2D_wt_charge.find(std::make_pair(w1 - 1, t1));
        if (it4 != map_2D_wt_charge.end()) {
            if (std::get<2>(it4->second) != 0) c1_w += std::get<0>(it4->second);
        }
    }

    int t2 = std::floor(offset_t + 0.5 + slope_x * ps_vec.at(i).x);
    int u2 = std::floor(offset_u + 0.5 + (slope_yu * ps_vec.at(i).y + slope_zu * ps_vec.at(i).z));
    int v2 = std::floor(offset_v + 0.5 + (slope_yv * ps_vec.at(i).y + slope_zv * ps_vec.at(i).z));
    int w2 = std::floor(offset_w + 0.5 + (slope_yw * ps_vec.at(i).y + slope_zw * ps_vec.at(i).z));

    double c2_u = 0;
    {
        auto it = map_2D_ut_charge.find(std::make_pair(u2, t2));
        if (it != map_2D_ut_charge.end()) {
            if (std::get<2>(it->second) != 0) c2_u += std::get<0>(it->second);
        }
        auto it1 = map_2D_ut_charge.find(std::make_pair(u2, t2 + 1));
        if (it1 != map_2D_ut_charge.end()) {
            if (std::get<2>(it1->second) != 0) c2_u += std::get<0>(it1->second);
        }
        auto it2 = map_2D_ut_charge.find(std::make_pair(u2, t2 - 1));
        if (it2 != map_2D_ut_charge.end()) {
            if (std::get<2>(it2->second) != 0) c2_u += std::get<0>(it2->second);
        }
        auto it3 = map_2D_ut_charge.find(std::make_pair(u2 + 1, t2));
        if (it3 != map_2D_ut_charge.end()) {
            if (std::get<2>(it3->second) != 0) c2_u += std::get<0>(it3->second);
        }
        auto it4 = map_2D_ut_charge.find(std::make_pair(u2 - 1, t2));
        if (it4 != map_2D_ut_charge.end()) {
            if (std::get<2>(it4->second) != 0) c2_u += std::get<0>(it4->second);
        }
    }

    double c2_v = 0;
    {
        auto it = map_2D_vt_charge.find(std::make_pair(v2, t2));
        if (it != map_2D_vt_charge.end()) {
            if (std::get<2>(it->second) != 0) {
                c2_v += std::get<0>(it->second);
                //		if (i==9) std::cout << std::get<0>(it->second) << " ";
            }
        }
        auto it1 = map_2D_vt_charge.find(std::make_pair(v2, t2 + 1));
        if (it1 != map_2D_vt_charge.end()) {
            if (std::get<2>(it1->second) != 0) {
                c2_v += std::get<0>(it1->second);
                // if (i==9) std::cout << std::get<0>(it1->second) << " ";
            }
        }
        auto it2 = map_2D_vt_charge.find(std::make_pair(v2, t2 - 1));
        if (it2 != map_2D_vt_charge.end()) {
            if (std::get<2>(it2->second) != 0) {
                c2_v += std::get<0>(it2->second);
                // if (i==9) std::cout << std::get<0>(it2->second) << " ";
            }
        }
        auto it3 = map_2D_vt_charge.find(std::make_pair(v2 + 1, t2));
        if (it3 != map_2D_vt_charge.end()) {
            if (std::get<2>(it3->second) != 0) {
                c2_v += std::get<0>(it3->second);
                // if (i==9) std::cout << std::get<0>(it3->second) << " ";
            }
        }
        auto it4 = map_2D_vt_charge.find(std::make_pair(v2 - 1, t2));
        if (it4 != map_2D_vt_charge.end()) {
            if (std::get<2>(it4->second) != 0) {
                c2_v += std::get<0>(it4->second);
                // if (i==9) std::cout << std::get<0>(it4->second) << " ";
            }
        }
        //	    if (i==9) std::cout << std::endl;
    }

    double c2_w = 0;
    {
        auto it = map_2D_wt_charge.find(std::make_pair(w2, t2));
        if (it != map_2D_wt_charge.end()) {
            if (std::get<2>(it->second) != 0) c2_w += std::get<0>(it->second);
        }
        auto it1 = map_2D_wt_charge.find(std::make_pair(w2, t2 + 1));
        if (it1 != map_2D_wt_charge.end()) {
            if (std::get<2>(it1->second) != 0) c2_w += std::get<0>(it1->second);
        }
        auto it2 = map_2D_wt_charge.find(std::make_pair(w2, t2 - 1));
        if (it2 != map_2D_wt_charge.end()) {
            if (std::get<2>(it2->second) != 0) c2_w += std::get<0>(it2->second);
        }
        auto it3 = map_2D_wt_charge.find(std::make_pair(w2 + 1, t2));
        if (it3 != map_2D_wt_charge.end()) {
            if (std::get<2>(it3->second) != 0) c2_w += std::get<0>(it3->second);
        }
        auto it4 = map_2D_wt_charge.find(std::make_pair(w2 - 1, t2));
        if (it4 != map_2D_wt_charge.end()) {
            if (std::get<2>(it4->second) != 0) c2_w += std::get<0>(it4->second);
        }
    }
    /* if (i==10) */
    /*   std::cout << u1 << " " << v1 << " " << w1 << " " << t1 << " " */
    /* 	      << u2 << " " << v2 << " " << w2 << " " << t2 << " " */
    /* 	      <<  offset_v + (slope_yv * p.y + slope_zv * p.z) << " " << offset_v + (slope_yv * ps_vec.at(i).y +
     * slope_zv * ps_vec.at(i).z) << std::endl; */

    double ratio = 0;
    double ratio_1 = 1;
    if (c2_u != 0) {
        ratio += c1_u / c2_u;
        if (c1_u != 0)
            ratio_1 *= c1_u / c2_u;
        else
            ratio_1 *= 0.25;
        //    std::cout << i << " " << c1_u << " " << c2_u << std::endl;
    }
    else {
        ratio += 1;
    }
    if (c2_v != 0) {
        ratio += c1_v / c2_v;
        if (c1_v != 0)
            ratio_1 *= c1_v / c2_v;
        else
            ratio_1 *= 0.25;
        // std::cout << i << " " << c1_v << " " << c2_v << std::endl;
    }
    else {
        ratio += 1;
    }
    if (c2_w != 0) {
        ratio += c1_w / c2_w;
        if (c1_w != 0)
            ratio_1 *= c1_w / c2_w;
        else
            ratio_1 *= 0.25;
        //    std::cout << i << " " << c1_w << " " << c2_w << std::endl;
    }
    else {
        ratio += 1;
    }

    /* if (ratio/3. >= 0.97 && ratio_1 < 1) */
    //  std::cout << i << " " << ratio/3. << " " << ratio_1 << std::endl;
    //      if (i==88) std::cout << ratio/3. << std::endl;

    if (ratio / 3. < 0.97 || ratio_1 < 0.75) {
        // if (c1_u + c1_v + c1_w < (c2_u + c2_v + c2_w)) {
        /* std::cout << i << " " << ratio/3. << " " << (c1_u+c1_v+c1_w)*1./(c2_u + c2_v + c2_w) << " " << c1_u
         * * 1.0/c2_u << " " << c1_v*1.0/c2_v << " " << c1_w*1.0/c2_w << " " << sqrt(pow(p.x-ps_vec.at(i).x,2) +
         * pow(p.y-ps_vec.at(i).y,2) + pow(p.z-ps_vec.at(i).z,2)) << " " */
        /* 	      << u1 << " " << v1 << " " << w1 << " " << t1 << " " << u2 << " " << v2 << " " << w2 << " " << t2
         * << std::endl;// */
        p = ps_vec.at(i);
    }
    /* if (i==10) */
    /*   std::cout << c1_u + c1_v + c1_w << " " << c2_u + c2_v + c2_w << " " << ratio << " " << ratio_1 << std::endl; */

    // examine angle ...
    if (fine_tracking_path.size() >= 2) {
        TVector3 v1(fine_tracking_path.at(fine_tracking_path.size() - 1).x -
                        fine_tracking_path.at(fine_tracking_path.size() - 2).x,
                    fine_tracking_path.at(fine_tracking_path.size() - 1).y -
                        fine_tracking_path.at(fine_tracking_path.size() - 2).y,
                    fine_tracking_path.at(fine_tracking_path.size() - 1).z -
                        fine_tracking_path.at(fine_tracking_path.size() - 2).z);
        TVector3 v2(p.x - fine_tracking_path.at(fine_tracking_path.size() - 1).x,
                    p.y - fine_tracking_path.at(fine_tracking_path.size() - 1).y,
                    p.z - fine_tracking_path.at(fine_tracking_path.size() - 1).z);

        //      if (i==56)

        TVector3 v3(ps_vec.at(i - 1).x - ps_vec.at(i - 2).x, ps_vec.at(i - 1).y - ps_vec.at(i - 2).y,
                    ps_vec.at(i - 1).z - ps_vec.at(i - 2).z);
        TVector3 v4(ps_vec.at(i).x - ps_vec.at(i - 1).x, ps_vec.at(i).y - ps_vec.at(i - 1).y,
                    ps_vec.at(i).z - ps_vec.at(i - 1).z);

        double angle1 = v3.Angle(v4) / 3.1415926 * 180.;

        double angle = v1.Angle(v2) / 3.1415926 * 180.;

        /* if (i==10) */
        /*   std::cout << i << " " << angle << " " << angle1 << " " << ps_vec.size() << " " << v2.Mag() << " " <<
         * map_3D_2DU_set[index].second << " " << map_3D_2DV_set[index].second << " " << map_3D_2DW_set[index].second <<
         * std::endl; */

        // related to the dead channels
        if (angle > 45 && ((map_3D_2DU_set[index].second == 0 && map_3D_2DV_set[index].second == 0) ||
                           (map_3D_2DU_set[index].second == 0 && map_3D_2DW_set[index].second == 0) ||
                           (map_3D_2DV_set[index].second == 0 && map_3D_2DW_set[index].second == 0)))
            return true;

        if (angle > 160 || angle > angle1 + 90) {
            // completely fold back ...
            return true;
        }
        // protected against the last point ...
        if (i + 1 == ps_vec.size() && angle > 45 && v2.Mag() < 0.5 * units::cm) return true;

        // short distance ...
        /* if (v2.Mag() < 0.45*units::cm && angle > 60) */
        /* 	continue; */
        // std::cout << i << " " << angle << " " << v2.Mag()/units::cm << " " << map_3D_2DU_set[i].second << " " <<
        // map_3D_2DV_set[i].second << " " << map_3D_2DW_set[i].second << " " << offset_t + 0.5 + slope_x * p.x << " "
        // << offset_u + 0.5 + (slope_yu * p.y + slope_zu * p.z) << " " << offset_v + 0.5 + (slope_yv * p.y + slope_zv *
        // p.z)+2400 << " " << offset_w + 0.5 + (slope_yw * p.y + slope_zw * p.z)+4800 << std::endl;
    }

    return false;
}

std::vector<float> WCPPID::PR3DCluster::examine_point_association(
    std::vector<int>& temp_results, std::set<std::pair<int, int>>& temp_2dut, std::set<std::pair<int, int>>& temp_2dvt,
    std::set<std::pair<int, int>>& temp_2dwt,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_ut_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_vt_charge,
    std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_wt_charge, bool flag_end_point,
    double charge_cut)
{
    std::set<int> temp_types_u;
    std::set<int> temp_types_v;
    std::set<int> temp_types_w;

    std::set<std::pair<int, int>> saved_2dut;
    std::set<std::pair<int, int>> saved_2dvt;
    std::set<std::pair<int, int>> saved_2dwt;

    std::vector<float> results;
    results.resize(3, 0);

    for (auto it = temp_2dut.begin(); it != temp_2dut.end(); it++) {
        auto it1 = map_2D_ut_charge.find(*it);
        if (it1 != map_2D_ut_charge.end() && std::get<0>(it1->second) > charge_cut) {
            temp_types_u.insert(std::get<2>(it1->second));
            if (std::get<2>(it1->second) == 0) results.at(0)++;
            saved_2dut.insert(*it);
        }
    }

    for (auto it = temp_2dvt.begin(); it != temp_2dvt.end(); it++) {
        auto it1 = map_2D_vt_charge.find(*it);
        if (it1 != map_2D_vt_charge.end() && std::get<0>(it1->second) > charge_cut) {
            temp_types_v.insert(std::get<2>(it1->second));
            if (std::get<2>(it1->second) == 0) results.at(1)++;
            saved_2dvt.insert(*it);
        }
    }

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

    for (auto it = temp_2dwt.begin(); it != temp_2dwt.end(); it++) {
        auto it1 = map_2D_wt_charge.find(*it);
        if (it1 != map_2D_wt_charge.end() && std::get<0>(it1->second) > charge_cut) {
            temp_types_w.insert(std::get<2>(it1->second));
            if (std::get<2>(it1->second) == 0) results.at(2)++;
            saved_2dwt.insert(*it);
        }
    }
    if (temp_2dut.size() != 0)
        results.at(0) = (saved_2dut.size() - results.at(0) * 1.0) / temp_2dut.size();
    else
        results.at(0) = 0;

    if (temp_2dvt.size() != 0)
        results.at(1) = (saved_2dvt.size() - results.at(1) * 1.0) / temp_2dvt.size();
    else
        results.at(1) = 0;

    if (temp_2dwt.size() != 0)
        results.at(2) = (saved_2dwt.size() - results.at(2) * 1.0) / temp_2dwt.size();
    else
        results.at(2) = 0;

    if (temp_types_u.find(0) != temp_types_u.end() && temp_types_u.size() == 1) {
        saved_2dut.clear();
        results.at(0) = 0;
    }
    if (temp_types_v.find(0) != temp_types_v.end() && temp_types_v.size() == 1) {
        saved_2dvt.clear();
        results.at(1) = 0;
    }
    if (temp_types_w.find(0) != temp_types_w.end() && temp_types_w.size() == 1) {
        saved_2dwt.clear();
        results.at(2) = 0;
    }

    //  std::cout << saved_2dut.size() << " " << saved_2dvt.size() << " " << saved_2dwt.size() << std::endl;

    // U and V planes are dead ...
    if (saved_2dut.size() == 0 && saved_2dvt.size() == 0 && saved_2dwt.size() != 0) {
        //    temp_2dwt = saved_2dwt;
        saved_2dut.insert(std::make_pair(temp_results.at(1), temp_results.at(0)));
        saved_2dvt.insert(std::make_pair(temp_results.at(2), temp_results.at(0)));
        // std::cout << "haha1 " << temp_results.at(0) << " " << temp_results.at(1) << " " << temp_results.at(2) << " "
        // << temp_results.at(3) << std::endl;
        // W plane check
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dwt.begin(); it1 != saved_2dwt.end(); it1++) {
                auto it2 = map_2D_wt_charge.find(*it1);
                if (it2 != map_2D_wt_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dwt.begin(); it1 != saved_2dwt.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dwt.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dwt.size() <= 5 && saved_2dwt.size() < 0.2 * temp_2dwt.size()) {
                saved_2dwt.clear();
                saved_2dwt.insert(std::make_pair(temp_results.at(3), temp_results.at(0)));
                results.at(2) = 0;
                /* std::cout << sqrt(pow(ave_pos.first - temp_results.at(3),2) + pow(ave_pos.second -
                 * temp_results.at(0),2)) << " " << rms << " " << saved_2dwt.size() << " " << temp_2dwt.size() <<
                 * std::endl; */
            }
        }
    }
    else if (saved_2dut.size() == 0 && saved_2dwt.size() == 0 && saved_2dvt.size() != 0) {
        // U and W planes are dead ...
        // temp_2dvt = saved_2dvt;
        saved_2dut.insert(std::make_pair(temp_results.at(1), temp_results.at(0)));
        saved_2dwt.insert(std::make_pair(temp_results.at(3), temp_results.at(0)));
        // std::cout << "haha2 " << temp_results.at(0) << " " << temp_results.at(1) << " " << temp_results.at(2) << " "
        // << temp_results.at(3) << std::endl;
        // V plane check
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dvt.begin(); it1 != saved_2dvt.end(); it1++) {
                auto it2 = map_2D_vt_charge.find(*it1);
                if (it2 != map_2D_vt_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dvt.begin(); it1 != saved_2dvt.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dvt.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dvt.size() <= 5 && saved_2dvt.size() < 0.2 * temp_2dvt.size()) {
                saved_2dvt.clear();
                saved_2dvt.insert(std::make_pair(temp_results.at(2), temp_results.at(0)));
                results.at(1) = 0;
            }
        }
    }
    else if (saved_2dvt.size() == 0 && saved_2dwt.size() == 0 && saved_2dut.size() != 0) {
        // V and W planes are dead ...
        // temp_2dut = saved_2dut;
        saved_2dvt.insert(std::make_pair(temp_results.at(2), temp_results.at(0)));
        saved_2dwt.insert(std::make_pair(temp_results.at(3), temp_results.at(0)));
        // std::cout << "haha3 " << temp_results.at(0) << " " << temp_results.at(1) << " " << temp_results.at(2) << " "
        // << temp_results.at(3) << std::endl;
        // U plane check
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dut.begin(); it1 != saved_2dut.end(); it1++) {
                auto it2 = map_2D_ut_charge.find(*it1);
                if (it2 != map_2D_ut_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dut.begin(); it1 != saved_2dut.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dut.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dut.size() <= 5 && saved_2dut.size() < 0.2 * temp_2dut.size()) {
                saved_2dut.clear();
                saved_2dut.insert(std::make_pair(temp_results.at(1), temp_results.at(0)));
                results.at(0) = 0;
            }
        }
    }
    else if (saved_2dut.size() == 0 && saved_2dwt.size() != 0 && saved_2dvt.size() != 0) {
        // only U plane is dead ...  one of the other plane is problematic ...

        // W plane check
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dwt.begin(); it1 != saved_2dwt.end(); it1++) {
                auto it2 = map_2D_wt_charge.find(*it1);
                if (it2 != map_2D_wt_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dwt.begin(); it1 != saved_2dwt.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dwt.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dwt.size() <= 5 && saved_2dwt.size() < 0.2 * temp_2dwt.size()) {
                saved_2dwt.clear();
                saved_2dwt.insert(std::make_pair(temp_results.at(3), temp_results.at(0)));
                results.at(2) = 0;
                //	std::cout << "haha1 " << std::endl;
                /* std::cout << sqrt(pow(ave_pos.first - temp_results.at(3),2) + pow(ave_pos.second -
                 * temp_results.at(0),2)) << " " << rms << " " << saved_2dwt.size() << " " << temp_2dwt.size() <<
                 * std::endl; */
            }
        }

        // V plane
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dvt.begin(); it1 != saved_2dvt.end(); it1++) {
                auto it2 = map_2D_vt_charge.find(*it1);
                if (it2 != map_2D_vt_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dvt.begin(); it1 != saved_2dvt.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dvt.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dvt.size() <= 5 && saved_2dvt.size() < 0.2 * temp_2dvt.size()) {
                saved_2dvt.clear();
                saved_2dvt.insert(std::make_pair(temp_results.at(2), temp_results.at(0)));
                results.at(1) = 0;
                //	std::cout << "haha2 " << std::endl;
            }
        }
    }
    else if (saved_2dvt.size() == 0 && saved_2dut.size() != 0 && saved_2dwt.size() != 0) {
        // only V plane is dead ... one of the other plane is problematic ...

        // U plane
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dut.begin(); it1 != saved_2dut.end(); it1++) {
                auto it2 = map_2D_ut_charge.find(*it1);
                if (it2 != map_2D_ut_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dut.begin(); it1 != saved_2dut.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dut.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dut.size() <= 5 && saved_2dut.size() < 0.2 * temp_2dut.size()) {
                saved_2dut.clear();
                saved_2dut.insert(std::make_pair(temp_results.at(1), temp_results.at(0)));
                results.at(0) = 0;
            }
        }

        // W plane
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dwt.begin(); it1 != saved_2dwt.end(); it1++) {
                auto it2 = map_2D_wt_charge.find(*it1);
                if (it2 != map_2D_wt_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dwt.begin(); it1 != saved_2dwt.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dwt.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dwt.size() <= 5 && saved_2dwt.size() < 0.2 * temp_2dwt.size()) {
                saved_2dwt.clear();
                saved_2dwt.insert(std::make_pair(temp_results.at(3), temp_results.at(0)));
                results.at(2) = 0;
                /* std::cout << sqrt(pow(ave_pos.first - temp_results.at(3),2) + pow(ave_pos.second -
                 * temp_results.at(0),2)) << " " << rms << " " << saved_2dwt.size() << " " << temp_2dwt.size() <<
                 * std::endl; */
            }
        }
    }
    else if (saved_2dwt.size() == 0 && saved_2dut.size() != 0 && saved_2dvt.size() != 0) {
        // only W plane is dead ... one of the other plane is problematic ...

        // U plane
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dut.begin(); it1 != saved_2dut.end(); it1++) {
                auto it2 = map_2D_ut_charge.find(*it1);
                if (it2 != map_2D_ut_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dut.begin(); it1 != saved_2dut.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dut.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dut.size() <= 5 && saved_2dut.size() < 0.2 * temp_2dut.size()) {
                saved_2dut.clear();
                saved_2dut.insert(std::make_pair(temp_results.at(1), temp_results.at(0)));
                results.at(0) = 0;
            }
        }

        // V plane
        if (!flag_end_point) {
            std::pair<double, double> ave_pos = std::make_pair(0, 0);  // average positon
            double total_charge = 0;
            for (auto it1 = saved_2dvt.begin(); it1 != saved_2dvt.end(); it1++) {
                auto it2 = map_2D_vt_charge.find(*it1);
                if (it2 != map_2D_vt_charge.end()) {
                    ave_pos.first += it1->first * (std::get<0>(it2->second));
                    ave_pos.second += it1->second * (std::get<0>(it2->second));
                    total_charge += std::get<0>(it2->second);
                }
            }
            if (total_charge != 0) {
                ave_pos.first /= total_charge;
                ave_pos.second /= total_charge;
            }
            double rms = 0;
            for (auto it1 = saved_2dvt.begin(); it1 != saved_2dvt.end(); it1++) {
                rms += pow(it1->first - ave_pos.first, 2) + pow(it1->second - ave_pos.second, 2);
            }
            rms = sqrt(rms / saved_2dvt.size());

            if (sqrt(pow(ave_pos.first - temp_results.at(3), 2) + pow(ave_pos.second - temp_results.at(0), 2)) >
                    0.75 * rms &&
                saved_2dvt.size() <= 5 && saved_2dvt.size() < 0.2 * temp_2dvt.size()) {
                saved_2dvt.clear();
                saved_2dvt.insert(std::make_pair(temp_results.at(2), temp_results.at(0)));
                results.at(1) = 0;
            }
        }
    }
    else if (saved_2dwt.size() == 0 && saved_2dut.size() == 0 && saved_2dvt.size() == 0) {
        saved_2dut.insert(std::make_pair(temp_results.at(1), temp_results.at(0)));
        saved_2dvt.insert(std::make_pair(temp_results.at(2), temp_results.at(0)));
        saved_2dwt.insert(std::make_pair(temp_results.at(3), temp_results.at(0)));
    }

    // std::cout << " " << saved_2dut.size() << " " << saved_2dvt.size() << " " << saved_2dwt.size() << " " <<
    // temp_2dut.size() << " " << temp_2dvt.size() << " " << temp_2dwt.size() << std::endl;

    temp_2dut = saved_2dut;
    temp_2dvt = saved_2dvt;
    temp_2dwt = saved_2dwt;

    return results;
}

void WCPPID::PR3DCluster::form_point_association(WCP::Point& p, std::set<std::pair<int, int>>& temp_2dut,
                                                 std::set<std::pair<int, int>>& temp_2dvt,
                                                 std::set<std::pair<int, int>>& temp_2dwt,
                                                 WCP::ToyCTPointCloud& ct_point_cloud, double dis_cut, int nlevel,
                                                 double time_cut)
{
    // global information
    TPCParams& mp = Singleton<TPCParams>::Instance();
    double pitch_u = mp.get_pitch_u();
    double pitch_v = mp.get_pitch_v();
    double pitch_w = mp.get_pitch_w();
    double angle_u = mp.get_angle_u();
    double angle_v = mp.get_angle_v();
    double angle_w = mp.get_angle_w();
    double time_slice_width = mp.get_ts_width();
    double first_u_dis = mp.get_first_u_dis();
    double first_v_dis = mp.get_first_v_dis();
    double first_w_dis = mp.get_first_w_dis();

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

    typedef boost::property_map<MCUGraph, boost::vertex_index_t>::type IndexMap;
    typedef boost::graph_traits<MCUGraph>::adjacency_iterator adjacency_iterator;

    // original point cloud ...
    if (point_cloud != 0 && graph != 0) {
        WCP::WCPointCloud<double>& cloud = point_cloud->get_cloud();
        IndexMap index = get(boost::vertex_index, *graph);
        if (cloud.pts.size() > 0) {
            WCP::WCPointCloud<double>::WCPoint wcp = point_cloud->get_closest_wcpoint(p);
            double temp_dis = sqrt(pow(wcp.x - p.x, 2) + pow(wcp.y - p.y, 2) + pow(wcp.z - p.z, 2));

            // std::cout << temp_dis/units::cm << " " << dis_cut/units::cm << std::endl;
            if (temp_dis < dis_cut) {
                std::set<int> total_vertices_found;
                std::set<int> vertices_to_be_examined;
                std::set<int> vertices_saved_for_next;
                total_vertices_found.insert(wcp.index);
                vertices_to_be_examined.insert(wcp.index);

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

                for (int j = 0; j != nlevel; j++) {
                    for (auto it = vertices_to_be_examined.begin(); it != vertices_to_be_examined.end(); it++) {
                        int temp_current_index = (*it);

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

                        std::pair<adjacency_iterator, adjacency_iterator> neighbors =
                            boost::adjacent_vertices(vertex(temp_current_index, *graph), *graph);
                        for (; neighbors.first != neighbors.second; ++neighbors.first) {
                            // std::cout << *neighbors.first << " " << *neighbors.second << std::endl;
                            if (total_vertices_found.find(index(*neighbors.first)) == total_vertices_found.end()) {
                                total_vertices_found.insert(index(*neighbors.first));
                                vertices_saved_for_next.insert(index(*neighbors.first));
                            }
                        }
                    }
                    vertices_to_be_examined = vertices_saved_for_next;
                }

                SMGCSet nearby_mcells_set;
                for (auto it = total_vertices_found.begin(); it != total_vertices_found.end(); it++) {
                    SlimMergeGeomCell* mcell = cloud.pts[*it].mcell;
                    nearby_mcells_set.insert(mcell);
                }

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

                //	std::cout << "A: " << cur_time_slice << " " << cur_wire_u << " " << cur_wire_v << " " <<
                //cur_wire_w << std::endl;

                double dis_cut_u = dis_cut;
                double dis_cut_v = dis_cut;
                double dis_cut_w = dis_cut;

                double max_time_slice_u = 0;
                double max_time_slice_v = 0;
                double max_time_slice_w = 0;
                for (auto it = nearby_mcells_set.begin(); it != nearby_mcells_set.end(); it++) {
                    SlimMergeGeomCell* mcell = *it;
                    int this_time_slice = mcell->GetTimeSlice();
                    GeomWireSelection uwires = mcell->get_uwires();
                    GeomWireSelection vwires = mcell->get_vwires();
                    GeomWireSelection wwires = mcell->get_wwires();
                    for (auto it1 = uwires.begin(); it1 != uwires.end(); it1++) {
                        if (fabs((*it1)->index() - cur_wire_u) <= 1)
                            if (fabs(this_time_slice - cur_time_slice) > max_time_slice_u)
                                max_time_slice_u = fabs(this_time_slice - cur_time_slice);
                    }
                    for (auto it1 = vwires.begin(); it1 != vwires.end(); it1++) {
                        if (fabs((*it1)->index() - cur_wire_v) <= 1)
                            if (fabs(this_time_slice - cur_time_slice) > max_time_slice_v)
                                max_time_slice_v = fabs(this_time_slice - cur_time_slice);
                    }
                    for (auto it1 = wwires.begin(); it1 != wwires.end(); it1++) {
                        if (fabs((*it1)->index() - cur_wire_w) <= 1)
                            if (fabs(this_time_slice - cur_time_slice) > max_time_slice_w)
                                max_time_slice_w = fabs(this_time_slice - cur_time_slice);
                    }
                }

                if (max_time_slice_u * time_slice_width * 1.2 < dis_cut_u)
                    dis_cut_u = max_time_slice_u * time_slice_width * 1.2;
                if (max_time_slice_v * time_slice_width * 1.2 < dis_cut_v)
                    dis_cut_v = max_time_slice_v * time_slice_width * 1.2;
                if (max_time_slice_w * time_slice_width * 1.2 < dis_cut_w)
                    dis_cut_w = max_time_slice_w * time_slice_width * 1.2;

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

                    double rem_dis_cut_u =
                        pow(dis_cut_u, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    double rem_dis_cut_v =
                        pow(dis_cut_v, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    double rem_dis_cut_w =
                        pow(dis_cut_w, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    if ((rem_dis_cut_u > 0 || rem_dis_cut_v > 0 || rem_dis_cut_w > 0) &&
                        fabs(cur_time_slice - this_time_slice) <= time_cut) {
                        GeomWireSelection uwires = mcell->get_uwires();
                        GeomWireSelection vwires = mcell->get_vwires();
                        GeomWireSelection wwires = mcell->get_wwires();
                        float min_u_dis;
                        if (cur_wire_u < uwires.front()->index()) {
                            min_u_dis = uwires.front()->index() - cur_wire_u;
                        }
                        else if (cur_wire_u >= uwires.front()->index() && cur_wire_u <= uwires.back()->index()) {
                            min_u_dis = 0;
                        }
                        else {
                            min_u_dis = cur_wire_u - uwires.back()->index();
                        }
                        float min_v_dis;
                        if (cur_wire_v < vwires.front()->index()) {
                            min_v_dis = vwires.front()->index() - cur_wire_v;
                        }
                        else if (cur_wire_v >= vwires.front()->index() && cur_wire_v <= vwires.back()->index()) {
                            min_v_dis = 0;
                        }
                        else {
                            min_v_dis = cur_wire_v - vwires.back()->index();
                        }
                        float min_w_dis;
                        if (cur_wire_w < wwires.front()->index()) {
                            min_w_dis = wwires.front()->index() - cur_wire_w;
                        }
                        else if (cur_wire_w >= wwires.front()->index() && cur_wire_w <= wwires.back()->index()) {
                            min_w_dis = 0;
                        }
                        else {
                            min_w_dis = cur_wire_w - wwires.back()->index();
                        }
                        float range_u =
                            rem_dis_cut_u * coef1 - pow(min_v_dis * pitch_v, 2) - coef2 * pow(min_w_dis * pitch_w, 2);
                        float range_v =
                            rem_dis_cut_v * coef1 - pow(min_u_dis * pitch_u, 2) - coef2 * pow(min_w_dis * pitch_w, 2);
                        float range_w =
                            (rem_dis_cut_w * coef1 - pow(min_u_dis * pitch_u, 2) - pow(min_v_dis * pitch_v, 2)) / coef2;

                        if (range_u > 0 && range_v > 0 && range_w > 0) {
                            float low_u_limit = cur_wire_u - sqrt(range_u) / pitch_u;
                            float high_u_limit = cur_wire_u + sqrt(range_u) / pitch_u;
                            float low_v_limit = cur_wire_v - sqrt(range_v) / pitch_v;
                            float high_v_limit = cur_wire_v + sqrt(range_v) / pitch_v;
                            float low_w_limit = cur_wire_w - sqrt(range_w) / pitch_w;
                            float high_w_limit = cur_wire_w + sqrt(range_w) / pitch_w;
                            for (int j = std::round(low_u_limit); j <= std::round(high_u_limit); j++) {
                                // auto it1 = map_2D_ut_charge.find(std::make_pair(j,this_time_slice));
                                // if (it1!=map_2D_ut_charge.end() && std::get<0>(it1->second) > 0 )
                                temp_2dut.insert(std::make_pair(j, this_time_slice));
                            }
                            for (int j = std::round(low_v_limit); j <= std::round(high_v_limit); j++) {
                                // auto it1 = map_2D_vt_charge.find(std::make_pair(j,this_time_slice));
                                // if (it1!=map_2D_vt_charge.end() && std::get<0>(it1->second) > 0 )
                                temp_2dvt.insert(std::make_pair(j, this_time_slice));
                            }
                            for (int j = std::round(low_w_limit); j <= std::round(high_w_limit); j++) {
                                // auto it1 = map_2D_wt_charge.find(std::make_pair(j,this_time_slice));
                                // if (it1!=map_2D_wt_charge.end() && std::get<0>(it1->second) > 0 )
                                temp_2dwt.insert(std::make_pair(j, this_time_slice));
                            }
                        }  // cuts
                    }      // cuts
                }          // loop over all mcells
            }              // find a good closest point?
        }                  // point cloud exist
    }

    // steiner tree point cloud ...
    if (point_cloud_steiner != 0 && graph_steiner != 0) {
        WCP::WCPointCloud<double>& cloud = point_cloud_steiner->get_cloud();
        IndexMap index = get(boost::vertex_index, *graph_steiner);

        if (cloud.pts.size() > 0) {
            WCP::WCPointCloud<double>::WCPoint wcp = point_cloud_steiner->get_closest_wcpoint(p);
            double temp_dis = sqrt(pow(wcp.x - p.x, 2) + pow(wcp.y - p.y, 2) + pow(wcp.z - p.z, 2));

            //      std::cout << temp_dis/units::cm << " " << dis_cut/units::cm << std::endl;
            if (temp_dis < dis_cut) {
                std::set<int> total_vertices_found;
                std::set<int> vertices_to_be_examined;
                std::set<int> vertices_saved_for_next;
                total_vertices_found.insert(wcp.index);
                vertices_to_be_examined.insert(wcp.index);

                for (int j = 0; j != nlevel; j++) {
                    for (auto it = vertices_to_be_examined.begin(); it != vertices_to_be_examined.end(); it++) {
                        int temp_current_index = (*it);
                        std::pair<adjacency_iterator, adjacency_iterator> neighbors =
                            boost::adjacent_vertices(vertex(temp_current_index, *graph_steiner), *graph_steiner);
                        for (; neighbors.first != neighbors.second; ++neighbors.first) {
                            // std::cout << *neighbors.first << " " << *neighbors.second << std::endl;
                            if (total_vertices_found.find(index(*neighbors.first)) == total_vertices_found.end()) {
                                total_vertices_found.insert(index(*neighbors.first));
                                vertices_saved_for_next.insert(index(*neighbors.first));
                            }
                        }
                    }
                    vertices_to_be_examined = vertices_saved_for_next;
                }

                WCP::Point temp_p(wcp.x, wcp.y, wcp.z);
                std::vector<int> temp_results = ct_point_cloud.convert_3Dpoint_time_ch(temp_p);
                // std::cout << cloud.pts[wcp.index].index_u << " "  << temp_results.at(0) << " " << temp_results.at(1)
                // << std::endl;
                //	if (wcp.mcell!=0)
                // std::cout << temp_results.at(0) << " " << wcp.mcell->GetTimeSlice() << std::endl;
                int cur_time_slice = temp_results.at(0);
                int cur_wire_u = cloud.pts[wcp.index].index_u;
                int cur_wire_v = cloud.pts[wcp.index].index_v;
                int cur_wire_w = cloud.pts[wcp.index].index_w;

                //	std::cout << "B: " << cur_time_slice << " " << cur_wire_u << " " << cur_wire_v << " " <<
                //cur_wire_w << std::endl;

                SMGCSet nearby_mcells_set;
                std::vector<int> point_indices;
                std::vector<int> point_timeslices;

                for (auto it = total_vertices_found.begin(); it != total_vertices_found.end(); it++) {
                    SlimMergeGeomCell* mcell = cloud.pts[*it].mcell;
                    if (mcell != 0) {
                        nearby_mcells_set.insert(mcell);
                    }
                    else {
                        temp_p.x = cloud.pts[*it].x;
                        temp_p.y = cloud.pts[*it].y;
                        temp_p.z = cloud.pts[*it].z;
                        temp_results = ct_point_cloud.convert_3Dpoint_time_ch(temp_p);

                        point_indices.push_back(*it);
                        point_timeslices.push_back(temp_results.at(0));
                    }
                }

                double dis_cut_u = dis_cut;
                double dis_cut_v = dis_cut;
                double dis_cut_w = dis_cut;

                double max_time_slice_u = 0;
                double max_time_slice_v = 0;
                double max_time_slice_w = 0;

                for (auto it = nearby_mcells_set.begin(); it != nearby_mcells_set.end(); it++) {
                    SlimMergeGeomCell* mcell = *it;
                    int this_time_slice = mcell->GetTimeSlice();
                    GeomWireSelection uwires = mcell->get_uwires();
                    GeomWireSelection vwires = mcell->get_vwires();
                    GeomWireSelection wwires = mcell->get_wwires();
                    for (auto it1 = uwires.begin(); it1 != uwires.end(); it1++) {
                        if (fabs((*it1)->index() - cur_wire_u) < 1)
                            if (fabs(this_time_slice - cur_time_slice) > max_time_slice_u)
                                max_time_slice_u = fabs(this_time_slice - cur_time_slice);
                    }
                    for (auto it1 = vwires.begin(); it1 != vwires.end(); it1++) {
                        if (fabs((*it1)->index() - cur_wire_v) < 1)
                            if (fabs(this_time_slice - cur_time_slice) > max_time_slice_v)
                                max_time_slice_v = fabs(this_time_slice - cur_time_slice);
                    }
                    for (auto it1 = wwires.begin(); it1 != wwires.end(); it1++) {
                        if (fabs((*it1)->index() - cur_wire_w) < 1)
                            if (fabs(this_time_slice - cur_time_slice) > max_time_slice_w)
                                max_time_slice_w = fabs(this_time_slice - cur_time_slice);
                    }
                }
                for (size_t i = 0; i != point_indices.size(); i++) {
                    int this_time_slice = point_timeslices.at(i);
                    if (fabs(cloud.pts[point_indices.at(i)].index_u - cur_wire_u) <= 1) {
                        if (fabs(this_time_slice - cur_time_slice) > max_time_slice_u)
                            max_time_slice_u = fabs(this_time_slice - cur_time_slice);
                    }
                    if (fabs(cloud.pts[point_indices.at(i)].index_v - cur_wire_v) <= 1) {
                        if (fabs(this_time_slice - cur_time_slice) > max_time_slice_v)
                            max_time_slice_v = fabs(this_time_slice - cur_time_slice);
                    }
                    if (fabs(cloud.pts[point_indices.at(i)].index_w - cur_wire_w) <= 1) {
                        if (fabs(this_time_slice - cur_time_slice) > max_time_slice_w)
                            max_time_slice_w = fabs(this_time_slice - cur_time_slice);
                    }
                }

                if (max_time_slice_u * time_slice_width * 1.2 < dis_cut_u)
                    dis_cut_u = max_time_slice_u * time_slice_width * 1.2;
                if (max_time_slice_v * time_slice_width * 1.2 < dis_cut_v)
                    dis_cut_v = max_time_slice_v * time_slice_width * 1.2;
                if (max_time_slice_w * time_slice_width * 1.2 < dis_cut_w)
                    dis_cut_w = max_time_slice_w * time_slice_width * 1.2;

                // actual cut ...
                for (auto it = nearby_mcells_set.begin(); it != nearby_mcells_set.end(); it++) {
                    SlimMergeGeomCell* mcell = *it;
                    int this_time_slice = mcell->GetTimeSlice();

                    double rem_dis_cut_u =
                        pow(dis_cut_u, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    double rem_dis_cut_v =
                        pow(dis_cut_v, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    double rem_dis_cut_w =
                        pow(dis_cut_w, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    if ((rem_dis_cut_u > 0 || rem_dis_cut_v > 0 || rem_dis_cut_w > 0) &&
                        fabs(cur_time_slice - this_time_slice) <= time_cut) {
                        GeomWireSelection uwires = mcell->get_uwires();
                        GeomWireSelection vwires = mcell->get_vwires();
                        GeomWireSelection wwires = mcell->get_wwires();
                        float min_u_dis;
                        if (cur_wire_u < uwires.front()->index()) {
                            min_u_dis = uwires.front()->index() - cur_wire_u;
                        }
                        else if (cur_wire_u >= uwires.front()->index() && cur_wire_u <= uwires.back()->index()) {
                            min_u_dis = 0;
                        }
                        else {
                            min_u_dis = cur_wire_u - uwires.back()->index();
                        }
                        float min_v_dis;
                        if (cur_wire_v < vwires.front()->index()) {
                            min_v_dis = vwires.front()->index() - cur_wire_v;
                        }
                        else if (cur_wire_v >= vwires.front()->index() && cur_wire_v <= vwires.back()->index()) {
                            min_v_dis = 0;
                        }
                        else {
                            min_v_dis = cur_wire_v - vwires.back()->index();
                        }
                        float min_w_dis;
                        if (cur_wire_w < wwires.front()->index()) {
                            min_w_dis = wwires.front()->index() - cur_wire_w;
                        }
                        else if (cur_wire_w >= wwires.front()->index() && cur_wire_w <= wwires.back()->index()) {
                            min_w_dis = 0;
                        }
                        else {
                            min_w_dis = cur_wire_w - wwires.back()->index();
                        }
                        float range_u =
                            rem_dis_cut_u * coef1 - pow(min_v_dis * pitch_v, 2) - coef2 * pow(min_w_dis * pitch_w, 2);
                        float range_v =
                            rem_dis_cut_v * coef1 - pow(min_u_dis * pitch_u, 2) - coef2 * pow(min_w_dis * pitch_w, 2);
                        float range_w =
                            (rem_dis_cut_w * coef1 - pow(min_u_dis * pitch_u, 2) - pow(min_v_dis * pitch_v, 2)) / coef2;

                        if (range_u > 0 && range_v > 0 && range_w > 0) {
                            float low_u_limit = cur_wire_u - sqrt(range_u) / pitch_u;
                            float high_u_limit = cur_wire_u + sqrt(range_u) / pitch_u;
                            float low_v_limit = cur_wire_v - sqrt(range_v) / pitch_v;
                            float high_v_limit = cur_wire_v + sqrt(range_v) / pitch_v;
                            float low_w_limit = cur_wire_w - sqrt(range_w) / pitch_w;
                            float high_w_limit = cur_wire_w + sqrt(range_w) / pitch_w;
                            for (int j = std::round(low_u_limit); j <= std::round(high_u_limit); j++) {
                                temp_2dut.insert(std::make_pair(j, this_time_slice));
                            }
                            for (int j = std::round(low_v_limit); j <= std::round(high_v_limit); j++) {
                                temp_2dvt.insert(std::make_pair(j, this_time_slice));
                            }
                            for (int j = std::round(low_w_limit); j <= std::round(high_w_limit); j++) {
                                temp_2dwt.insert(std::make_pair(j, this_time_slice));
                            }
                        }  // cuts
                    }      // cuts
                }          // loop over all mcells

                for (size_t i = 0; i != point_indices.size(); i++) {
                    int this_time_slice = point_timeslices.at(i);
                    int this_index_u = cloud.pts[point_indices.at(i)].index_u;
                    int this_index_v = cloud.pts[point_indices.at(i)].index_v;
                    int this_index_w = cloud.pts[point_indices.at(i)].index_w;

                    double rem_dis_cut_u =
                        pow(dis_cut_u, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    double rem_dis_cut_v =
                        pow(dis_cut_v, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    double rem_dis_cut_w =
                        pow(dis_cut_w, 2) - pow((cur_time_slice - this_time_slice) * time_slice_width, 2);
                    if ((rem_dis_cut_u > 0 || rem_dis_cut_v > 0 || rem_dis_cut_w > 0) &&
                        fabs(cur_time_slice - this_time_slice) <= time_cut) {
                        float min_u_dis;
                        if (cur_wire_u < this_index_u - 1) {
                            min_u_dis = this_index_u - 1 - cur_wire_u;
                        }
                        else if (cur_wire_u > this_index_u + 1) {
                            min_u_dis = cur_wire_u - this_index_u - 1;
                        }
                        else {
                            min_u_dis = 0;
                        }
                        float min_v_dis;
                        if (cur_wire_v < this_index_v - 1) {
                            min_v_dis = this_index_v - 1 - cur_wire_v;
                        }
                        else if (cur_wire_v > this_index_v + 1) {
                            min_v_dis = cur_wire_v - this_index_v - 1;
                        }
                        else {
                            min_v_dis = 0;
                        }
                        float min_w_dis;
                        if (cur_wire_w < this_index_w - 1) {
                            min_w_dis = this_index_w - 1 - cur_wire_w;
                        }
                        else if (cur_wire_w > this_index_w + 1) {
                            min_w_dis = cur_wire_w - this_index_w - 1;
                        }
                        else {
                            min_w_dis = 0;
                        }
                        float range_u =
                            rem_dis_cut_u * coef1 - pow(min_v_dis * pitch_v, 2) - coef2 * pow(min_w_dis * pitch_w, 2);
                        float range_v =
                            rem_dis_cut_v * coef1 - pow(min_u_dis * pitch_u, 2) - coef2 * pow(min_w_dis * pitch_w, 2);
                        float range_w =
                            (rem_dis_cut_w * coef1 - pow(min_u_dis * pitch_u, 2) - pow(min_v_dis * pitch_v, 2)) / coef2;

                        if (range_u > 0 && range_v > 0 && range_w > 0) {
                            float low_u_limit = cur_wire_u - sqrt(range_u) / pitch_u;
                            float high_u_limit = cur_wire_u + sqrt(range_u) / pitch_u;
                            float low_v_limit = cur_wire_v - sqrt(range_v) / pitch_v;
                            float high_v_limit = cur_wire_v + sqrt(range_v) / pitch_v;
                            float low_w_limit = cur_wire_w - sqrt(range_w) / pitch_w;
                            float high_w_limit = cur_wire_w + sqrt(range_w) / pitch_w;
                            for (int j = std::round(low_u_limit); j <= std::round(high_u_limit); j++) {
                                temp_2dut.insert(std::make_pair(j, this_time_slice));
                            }
                            for (int j = std::round(low_v_limit); j <= std::round(high_v_limit); j++) {
                                temp_2dvt.insert(std::make_pair(j, this_time_slice));
                            }
                            for (int j = std::round(low_w_limit); j <= std::round(high_w_limit); j++) {
                                temp_2dwt.insert(std::make_pair(j, this_time_slice));
                            }
                        }  // cuts
                    }      // cuts

                }  // loop through points

            }  // distance
        }      // no steiner tree cloud
    }

    // just projection ...
    if (temp_2dut.size() == 0 && temp_2dvt.size() == 0 && temp_2dwt.size() == 0) {
        // std::cout << "haha " << std::endl;
        std::vector<int> temp_results = ct_point_cloud.convert_3Dpoint_time_ch(p);
        int cur_time_slice = temp_results.at(0);
        int cur_index_u = temp_results.at(1);
        int cur_index_v = temp_results.at(2) - 2400;
        int cur_index_w = temp_results.at(3) - 4800;

        // std::cout << cur_time_slice << " " << cur_index_u << " " << cur_index_v << " " << cur_index_w << std::endl;

        for (int i = -time_cut; i != time_cut + 1; i++) {
            for (int j = -time_cut; j != time_cut + 1; j++) {
                if (abs(i) + abs(j) <= time_cut) {
                    temp_2dut.insert(std::make_pair(cur_index_u + i, cur_time_slice + j));
                    temp_2dvt.insert(std::make_pair(cur_index_v + i, cur_time_slice + j));
                    temp_2dwt.insert(std::make_pair(cur_index_w + i, cur_time_slice + j));
                }
            }
        }
    }
}

void WCPPID::PR3DCluster::form_map(WCP::ToyCTPointCloud& ct_point_cloud, WCP::PointVector& pts,
                                   std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_ut_charge,
                                   std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_vt_charge,
                                   std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_wt_charge,
                                   std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DU_set,
                                   std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DV_set,
                                   std::map<int, std::pair<std::set<std::pair<int, int>>, float>>& map_3D_2DW_set,
                                   std::map<std::pair<int, int>, std::set<int>>& map_2DU_3D_set,
                                   std::map<std::pair<int, int>, std::set<int>>& map_2DV_3D_set,
                                   std::map<std::pair<int, int>, std::set<int>>& map_2DW_3D_set,
                                   double end_point_factor, double mid_point_factor, int nlevel, double time_cut,
                                   double charge_cut)
{
    map_3D_2DU_set.clear();
    map_3D_2DV_set.clear();
    map_3D_2DW_set.clear();

    map_2DU_3D_set.clear();
    map_2DV_3D_set.clear();
    map_2DW_3D_set.clear();

    WCP::PointVector saved_pts;
    int count = 0;

    // distance ...
    std::vector<double> distances;
    for (size_t i = 0; i + 1 != pts.size(); i++) {
        distances.push_back(sqrt(pow(pts.at(i + 1).x - pts.at(i).x, 2) + pow(pts.at(i + 1).y - pts.at(i).y, 2) +
                                 pow(pts.at(i + 1).z - pts.at(i).z, 2)));
    }

    // start to loop over the path ...
    for (size_t i = 0; i != pts.size(); i++) {
        double dis_cut;
        if (i == 0) {
            dis_cut = std::min(distances.at(i) * end_point_factor, 4 / 3. * end_point_factor * units::cm);
        }
        else if (i + 1 == pts.size()) {
            dis_cut = std::min(distances.back() * end_point_factor, 4 / 3. * end_point_factor * units::cm);
        }
        else {
            dis_cut = std::min(std::max(distances.at(i - 1) * mid_point_factor, distances.at(i) * mid_point_factor),
                               4 / 3. * mid_point_factor * units::cm);
        }

        std::set<std::pair<int, int>> temp_2dut, temp_2dvt, temp_2dwt;
        form_point_association(pts.at(i), temp_2dut, temp_2dvt, temp_2dwt, ct_point_cloud, dis_cut, nlevel, time_cut);
        // examine ...

        // std::cout << (*temp_2dvt.begin()).first << " " << (*temp_2dvt.end()).second << std::endl;

        std::vector<int> temp_results = ct_point_cloud.convert_3Dpoint_time_ch(pts.at(i));
        temp_results.at(2) -= 2400;
        temp_results.at(3) -= 4800;

        //    std::cout << i << " ";// << pts.at(i) << " " << temp_results.at(0) << " " << temp_results.at(1) << " " <<
        //    temp_results.at(2) << " " << temp_results.at(3) << std::endl;
        std::vector<float> temp_flag;
        if (i == 0 || i == 1 || i + 1 == pts.size() || i + 2 == pts.size()) {
            temp_flag = examine_point_association(temp_results, temp_2dut, temp_2dvt, temp_2dwt, map_2D_ut_charge,
                                                  map_2D_vt_charge, map_2D_wt_charge, true, charge_cut);
        }
        else {
            temp_flag = examine_point_association(temp_results, temp_2dut, temp_2dvt, temp_2dwt, map_2D_ut_charge,
                                                  map_2D_vt_charge, map_2D_wt_charge, false, charge_cut);
        }

        //    std::cout << temp_2dut.size() << " " << temp_2dvt.size() << " " << temp_2dwt.size() << " " <<
        //    temp_flag.at(0) << " " << temp_flag.at(1) << " " << temp_flag.at(2) << std::endl;
        // just projection ...
        // fill the data ...
        //    if (temp_2dut.size() + temp_2dvt.size() + temp_2dwt.size() > 0){
        if (temp_flag.at(0) + temp_flag.at(1) + temp_flag.at(2) > 0) {
            map_3D_2DU_set[count] = std::make_pair(temp_2dut, temp_flag.at(0));
            map_3D_2DV_set[count] = std::make_pair(temp_2dvt, temp_flag.at(1));
            map_3D_2DW_set[count] = std::make_pair(temp_2dwt, temp_flag.at(2));
            for (auto it = temp_2dut.begin(); it != temp_2dut.end(); it++) {
                if (map_2DU_3D_set.find(*it) == map_2DU_3D_set.end()) {
                    std::set<int> temp_set;
                    temp_set.insert(count);
                    map_2DU_3D_set[*it] = temp_set;
                }
                else {
                    map_2DU_3D_set[*it].insert(count);
                }
            }
            for (auto it = temp_2dvt.begin(); it != temp_2dvt.end(); it++) {
                if (map_2DV_3D_set.find(*it) == map_2DV_3D_set.end()) {
                    std::set<int> temp_set;
                    temp_set.insert(count);
                    map_2DV_3D_set[*it] = temp_set;
                }
                else {
                    map_2DV_3D_set[*it].insert(count);
                }
            }
            for (auto it = temp_2dwt.begin(); it != temp_2dwt.end(); it++) {
                if (map_2DW_3D_set.find(*it) == map_2DW_3D_set.end()) {
                    std::set<int> temp_set;
                    temp_set.insert(count);
                    map_2DW_3D_set[*it] = temp_set;
                }
                else {
                    map_2DW_3D_set[*it].insert(count);
                }
            }

            saved_pts.push_back(pts.at(i));
            count++;
        }
    }

    pts = saved_pts;
}

void WCPPID::PR3DCluster::prepare_data(WCP::ToyCTPointCloud& ct_point_cloud,
                                       std::map<int, std::map<const WCP::GeomWire*, WCP::SMGCSelection>>& global_wc_map,
                                       std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_ut_charge,
                                       std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_vt_charge,
                                       std::map<std::pair<int, int>, std::tuple<double, double, int>>& map_2D_wt_charge)
{
    std::vector<int> proj_channel;
    std::vector<int> proj_timeslice;
    std::vector<int> proj_charge;
    std::vector<int> proj_charge_err;
    std::vector<int> proj_flag;
    get_projection(proj_channel, proj_timeslice, proj_charge, proj_charge_err, proj_flag, global_wc_map);

    // enlarge uncertainty for channels next to dead channels ...
    std::map<int, std::pair<int, int>> map_dead_chs = ct_point_cloud.get_all_dead_chs();
    float factor_enlarge_ind = 5;
    float factor_enlarge_col = 2.5;

    for (size_t i = 0; i != proj_channel.size(); i++) {
        int ch = proj_channel.at(i);
        int time = proj_timeslice.at(i);

        if (proj_charge.at(i) <= 0) continue;

        bool flag_cur_dead = false;
        {
            auto it1 = map_dead_chs.find(ch);
            if (it1 != map_dead_chs.end())
                if (time >= it1->second.first && time <= it1->second.second) flag_cur_dead = true;
        }

        bool flag_p_dead = false;
        if (ch != 2400 - 1 && ch != 4800 - 1) {
            auto it1 = map_dead_chs.find(ch + 1);
            if (it1 != map_dead_chs.end())
                if (time >= it1->second.first && time <= it1->second.second) flag_p_dead = true;
        }

        bool flag_n_dead = false;
        if (ch != 2400 && ch != 4800) {
            auto it1 = map_dead_chs.find(ch - 1);
            if (it1 != map_dead_chs.end())
                if (time >= it1->second.first && time <= it1->second.second) flag_n_dead = true;
        }

        if ((!flag_cur_dead) && (flag_p_dead || flag_n_dead)) {
            // std::cout << ch << " " << proj_charge.at(i) << " " << proj_charge_err.at(i) << std::endl;
            if (ch < 4800) {
                proj_charge_err.at(i) *= factor_enlarge_ind;
            }
            else {
                proj_charge_err.at(i) *= factor_enlarge_col;
            }
        }
    }
    //

    int min_time = 1e9;
    int max_time = -1e9;
    int min_uch = 1e9;
    int max_uch = -1e9;
    int min_vch = 1e9;
    int max_vch = -1e9;
    int min_wch = 1e9;
    int max_wch = -1e9;

    // std::cout << proj_charge.size() << " " << proj_flag.size() << std::endl;
    for (size_t i = 0; i != proj_channel.size(); i++) {
        if (min_time > proj_timeslice.at(i)) min_time = proj_timeslice.at(i);
        if (max_time < proj_timeslice.at(i)) max_time = proj_timeslice.at(i);

        if (proj_channel.at(i) < 2400) {
            map_2D_ut_charge[std::make_pair(proj_channel.at(i), proj_timeslice.at(i))] =
                std::make_tuple(proj_charge.at(i), proj_charge_err.at(i), proj_flag.at(i));

            if (min_uch > proj_channel.at(i)) min_uch = proj_channel.at(i);
            if (max_uch < proj_channel.at(i)) max_uch = proj_channel.at(i);
        }
        else if (proj_channel.at(i) < 4800) {
            map_2D_vt_charge[std::make_pair(proj_channel.at(i) - 2400, proj_timeslice.at(i))] =
                std::make_tuple(proj_charge.at(i), proj_charge_err.at(i), proj_flag.at(i));

            if (min_vch > proj_channel.at(i)) min_vch = proj_channel.at(i);
            if (max_vch < proj_channel.at(i)) max_vch = proj_channel.at(i);
        }
        else {
            map_2D_wt_charge[std::make_pair(proj_channel.at(i) - 4800, proj_timeslice.at(i))] =
                std::make_tuple(proj_charge.at(i), proj_charge_err.at(i), proj_flag.at(i));
            if (min_wch > proj_channel.at(i)) min_wch = proj_channel.at(i);
            if (max_wch < proj_channel.at(i)) max_wch = proj_channel.at(i);
        }
    }

    // flag 0 for dead
    // flag 1 for good channels or overlapping channels
    // flag 2 for isolated channels
    // flag 3 for additional channels ...

    // add the rest of live channels within range???
    std::map<std::pair<int, int>, std::pair<double, double>> map_u_tcc =
        ct_point_cloud.get_overlap_good_ch_charge(min_time, max_time, min_uch, max_uch, 0);
    std::map<std::pair<int, int>, std::pair<double, double>> map_v_tcc =
        ct_point_cloud.get_overlap_good_ch_charge(min_time, max_time, min_vch, max_vch, 1);
    std::map<std::pair<int, int>, std::pair<double, double>> map_w_tcc =
        ct_point_cloud.get_overlap_good_ch_charge(min_time, max_time, min_wch, max_wch, 2);

    for (auto it = map_u_tcc.begin(); it != map_u_tcc.end(); it++) {
        if (map_2D_ut_charge.find(std::make_pair(it->first.second, it->first.first)) == map_2D_ut_charge.end()) {
            map_2D_ut_charge[std::make_pair(it->first.second, it->first.first)] =
                std::make_tuple(it->second.first, it->second.second, 3);
            // std::cout << it->first.first << " " << it->first.second << std::endl;
        }
    }

    for (auto it = map_v_tcc.begin(); it != map_v_tcc.end(); it++) {
        if (map_2D_vt_charge.find(std::make_pair(it->first.second - 2400, it->first.first)) == map_2D_vt_charge.end()) {
            map_2D_vt_charge[std::make_pair(it->first.second - 2400, it->first.first)] =
                std::make_tuple(it->second.first, it->second.second, 3);
            // std::cout << it->first.first << " " << it->first.second << std::endl;
        }
    }

    for (auto it = map_w_tcc.begin(); it != map_w_tcc.end(); it++) {
        if (map_2D_wt_charge.find(std::make_pair(it->first.second - 4800, it->first.first)) == map_2D_wt_charge.end()) {
            map_2D_wt_charge[std::make_pair(it->first.second - 4800, it->first.first)] =
                std::make_tuple(it->second.first, it->second.second, 3);
            //      std::cout << it->first.first << " " << it->first.second << std::endl;
        }
        else {
            //      std::cout << it->first.first << " " << it->first.second << std::endl;
        }
    }

    //  for (auto it = map_w_tcc.begin(); it!=map_w_tcc.end(); it++){
    // std::cout << it->first.first << " " << it->first.second << std::endl;
    // }
}

WCP::PointVector WCPPID::PR3DCluster::examine_end_ps_vec(WCP::ToyCTPointCloud& ct_point_cloud, WCP::PointVector& pts,
                                                         bool flag_start, bool flag_end)
{
    std::list<Point> ps_list(pts.begin(), pts.end());

    if (flag_start) {
        // test start
        Point temp_start = ps_list.front();
        while (ps_list.size() > 0) {
            if (ct_point_cloud.is_good_point(ps_list.front(), 0.2 * units::cm, 0, 0)) break;
            temp_start = ps_list.front();
            ps_list.pop_front();
        }

        if (ps_list.size() > 0) {
            double dis_step = 0.2 * units::cm;
            double temp_dis = sqrt(pow(temp_start.x - ps_list.front().x, 2) + pow(temp_start.y - ps_list.front().y, 2) +
                                   pow(temp_start.z - ps_list.front().z, 2));
            int ntest = std::round(temp_dis / dis_step);
            for (size_t i = 1; i < ntest; i++) {
                Point test_p(temp_start.x + (ps_list.front().x - temp_start.x) / ntest * i,
                             temp_start.y + (ps_list.front().y - temp_start.y) / ntest * i,
                             temp_start.z + (ps_list.front().z - temp_start.z) / ntest * i);
                if (ct_point_cloud.is_good_point(test_p, 0.2 * units::cm, 0, 0)) {
                    ps_list.push_front(test_p);
                    break;
                }
            }
        }
        else {
            ps_list.push_front(temp_start);
        }
    }

    if (flag_end) {
        Point temp_end = ps_list.back();
        while (ps_list.size() > 0) {
            if (ct_point_cloud.is_good_point(ps_list.back(), 0.2 * units::cm, 0, 0)) break;
            temp_end = ps_list.back();
            ps_list.pop_back();
        }
        if (ps_list.size() > 0) {
            double dis_step = 0.2 * units::cm;
            double temp_dis = sqrt(pow(temp_end.x - ps_list.back().x, 2) + pow(temp_end.y - ps_list.back().y, 2) +
                                   pow(temp_end.z - ps_list.back().z, 2));
            int ntest = std::round(temp_dis / dis_step);
            for (size_t i = 1; i < ntest; i++) {
                Point test_p(temp_end.x + (ps_list.back().x - temp_end.x) / ntest * i,
                             temp_end.y + (ps_list.back().y - temp_end.y) / ntest * i,
                             temp_end.z + (ps_list.back().z - temp_end.z) / ntest * i);
                if (ct_point_cloud.is_good_point(test_p, 0.2 * units::cm, 0, 0)) {
                    ps_list.push_back(test_p);
                    break;
                }
            }
        }
        else {
            ps_list.push_back(temp_end);
        }
    }

    PointVector tmp_pts(ps_list.begin(), ps_list.end());
    return tmp_pts;
}

void WCPPID::PR3DCluster::organize_ps_path(WCP::ToyCTPointCloud& ct_point_cloud, WCP::PointVector& pts,
                                           double low_dis_limit, double end_point_limit)
{
    WCP::PointVector ps_vec = examine_end_ps_vec(ct_point_cloud, pts, true, true);
    if (ps_vec.size() <= 1) ps_vec = pts;

    pts.clear();
    // fill in the beginning part
    {
        Point p1 = ps_vec.front();
        Point p2 = ps_vec.front();
        double dis1 = 0;
        for (auto it = ps_vec.begin(); it != ps_vec.end(); it++) {
            p2 = *it;
            dis1 = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
            if (dis1 > low_dis_limit) break;
        }
        //    std::cout << ct_point_cloud.is_good_point(p1, 0.2*units::cm,0,0) << " " << p1 << std::endl;
        if (dis1 > low_dis_limit) {
            p1.x += (p1.x - p2.x) / dis1 * end_point_limit;
            p1.y += (p1.y - p2.y) / dis1 * end_point_limit;
            p1.z += (p1.z - p2.z) / dis1 * end_point_limit;
            pts.push_back(p1);
            // std::cout << ct_point_cloud.is_good_point(p1, 0.2*units::cm,0,0) << " " << p1 << std::endl;
        }
    }

    // fill in the middle part
    for (size_t i = 0; i != ps_vec.size(); i++) {
        Point p1 = ps_vec.at(i);
        double dis;
        if (pts.size() != 0) {
            dis = sqrt(pow(p1.x - pts.back().x, 2) + pow(p1.y - pts.back().y, 2) + pow(p1.z - pts.back().z, 2));
        }
        else {
            dis =
                sqrt(pow(p1.x - ps_vec.back().x, 2) + pow(p1.y - ps_vec.back().y, 2) + pow(p1.z - ps_vec.back().z, 2));
        }

        // std::cout << i << " " << p1 << " " << pts.back() << " " << dis/units::cm << " " << low_dis_limit/units::cm <<
        // std::endl;
        if (dis < low_dis_limit * 0.8) {
            continue;
        }
        else if (dis < low_dis_limit * 1.6) {
            pts.push_back(p1);
        }
        else {
            int npoints = std::round(dis / low_dis_limit);
            Point p_save = pts.back();
            for (int j = 0; j != npoints; j++) {
                Point p(p_save.x + (p1.x - p_save.x) / npoints * (j + 1),
                        p_save.y + (p1.y - p_save.y) / npoints * (j + 1),
                        p_save.z + (p1.z - p_save.z) / npoints * (j + 1));
                pts.push_back(p);
            }
        }
    }

    // fill in the end part
    if (end_point_limit != 0) {
        Point p1 = ps_vec.back();
        Point p2 = ps_vec.back();
        double dis1 = 0;
        for (auto it = ps_vec.rbegin(); it != ps_vec.rend(); it++) {
            p2 = *it;
            dis1 = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
            if (dis1 > low_dis_limit) break;
        }
        if (dis1 != 0) {
            p1.x += (p1.x - p2.x) / dis1 * end_point_limit;
            p1.y += (p1.y - p2.y) / dis1 * end_point_limit;
            p1.z += (p1.z - p2.z) / dis1 * end_point_limit;
            pts.push_back(p1);
        }
    }
    else {
        Point p1 = ps_vec.back();
        double dis1 = sqrt(pow(p1.x - pts.back().x, 2) + pow(p1.y - pts.back().y, 2) + pow(p1.z - pts.back().z, 2));
        if (dis1 >= 0.45 * units::cm) pts.push_back(p1);
    }

    if (pts.size() <= 1) pts = ps_vec;
}

WCP::PointVector WCPPID::PR3DCluster::organize_wcps_path(WCP::ToyCTPointCloud& ct_point_cloud,
                                                         std::list<WCPointCloud<double>::WCPoint>& path_wcps_list,
                                                         double low_dis_limit, double end_point_limit)
{
    PointVector pts;

    std::vector<WCPointCloud<double>::WCPoint> temp_wcps_vec(path_wcps_list.begin(), path_wcps_list.end());

    // fill in the beginning point ...
    {
        Point p1(temp_wcps_vec.front().x, temp_wcps_vec.front().y, temp_wcps_vec.front().z);
        Point p2(temp_wcps_vec.front().x, temp_wcps_vec.front().y, temp_wcps_vec.front().z);
        double dis1 = 0;
        for (auto it = temp_wcps_vec.begin(); it != temp_wcps_vec.end(); it++) {
            p2.x = (*it).x;
            p2.y = (*it).y;
            p2.z = (*it).z;
            dis1 = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
            if (dis1 > low_dis_limit) break;
        }
        if (dis1 != 0) {
            p1.x += (p1.x - p2.x) / dis1 * end_point_limit;
            p1.y += (p1.y - p2.y) / dis1 * end_point_limit;
            p1.z += (p1.z - p2.z) / dis1 * end_point_limit;
            pts.push_back(p1);
        }
    }

    // fill in the middle part
    for (size_t i = 0; i != temp_wcps_vec.size(); i++) {
        Point p1(temp_wcps_vec.at(i).x, temp_wcps_vec.at(i).y, temp_wcps_vec.at(i).z);

        double dis = low_dis_limit;
        if (pts.size() > 0)
            dis = sqrt(pow(p1.x - pts.back().x, 2) + pow(p1.y - pts.back().y, 2) + pow(p1.z - pts.back().z, 2));

        if (dis < low_dis_limit * 0.8) {
            continue;
        }
        else if (dis < low_dis_limit * 1.6) {
            pts.push_back(p1);
            // std::cout << p1 << " " <<
            // sqrt(pow(p1.x-pts.back().x,2)+pow(p1.y-pts.back().y,2)+pow(p1.z-pts.back().z,2))/units::cm << std::endl;
        }
        else {
            int npoints = std::round(dis / low_dis_limit);

            Point p_save = pts.back();
            for (int j = 0; j != npoints; j++) {
                Point p(p_save.x + (p1.x - p_save.x) / npoints * (j + 1),
                        p_save.y + (p1.y - p_save.y) / npoints * (j + 1),
                        p_save.z + (p1.z - p_save.z) / npoints * (j + 1));
                pts.push_back(p);
                //	std::cout << p << " " <<
                //sqrt(pow(p.x-pts.back().x,2)+pow(p.y-pts.back().y,2)+pow(p.z-pts.back().z,2))/units::cm << std::endl;
            }
        }
    }

    // fill in the end part
    {
        Point p1(temp_wcps_vec.back().x, temp_wcps_vec.back().y, temp_wcps_vec.back().z);
        Point p2(temp_wcps_vec.back().x, temp_wcps_vec.back().y, temp_wcps_vec.back().z);
        double dis1 = 0;
        for (auto it = temp_wcps_vec.rbegin(); it != temp_wcps_vec.rend(); it++) {
            p2.x = (*it).x;
            p2.y = (*it).y;
            p2.z = (*it).z;
            dis1 = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
            if (dis1 > low_dis_limit) break;
        }
        if (dis1 != 0) {
            p1.x += (p1.x - p2.x) / dis1 * end_point_limit;
            p1.y += (p1.y - p2.y) / dis1 * end_point_limit;
            p1.z += (p1.z - p2.z) / dis1 * end_point_limit;
            pts.push_back(p1);
        }
    }

    return pts;
}
