//#include "Minuit2/FunctionMinimum.h"
//#include "Minuit2/MnUserParameterState.h"
//#include "Minuit2/MnMigrad.h"
//#include "WCPData/Line.h"

#include "TMatrixDEigen.h"

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

bool WCPPID::NeutrinoID::fit_vertex(WCPPID::ProtoVertex *vtx, WCPPID::ProtoSegmentSet &sg_set,
                                    WCPPID::PR3DCluster *temp_cluster)
{
    // allow to move 1.5 cm ...
    WCPPID::MyFCN fcn(vtx, true, 0.43 * units::cm, 1.5 * units::cm, 0.9 * units::cm, 6 * units::cm);
    for (auto it = sg_set.begin(); it != sg_set.end(); it++) {
        fcn.AddSegment(*it);
    }
    if (vtx == main_vertex) fcn.set_enforce_two_track_fit(true);

    std::pair<bool, Point> results = fcn.FitVertex();

    double old_charge = ct_point_cloud->get_ave_3d_charge(vtx->get_fit_pt());
    double new_charge = ct_point_cloud->get_ave_3d_charge(results.second);

    // std::cout << old_charge << " " << new_charge << " " << results.second << std::endl;
    if (new_charge < 5000 && new_charge < 0.4 * old_charge) {
        results.second = vtx->get_fit_pt();
    }
    else if (new_charge < 8000 && new_charge < 0.6 * old_charge) {
        // reduce the strength ...
        //    fcn.set_vtx_constraint_range(0.11*units::cm);
        // results = fcn.FitVertex();
        results.second = vtx->get_fit_pt();
        new_charge = ct_point_cloud->get_ave_3d_charge(results.second);
        // std::cout << old_charge << " " << new_charge << " " << results.second << std::endl;
    }

    if (results.first) fcn.UpdateInfo(results.second, temp_cluster);

    return results.first;
}

void WCPPID::NeutrinoID::improve_vertex(WCPPID::PR3DCluster *temp_cluster, bool flag_search_vertex_activity,
                                        bool flag_final_vertex)
{
    std::set<WCPPID::ProtoVertex *> fitted_vertices;
    std::set<WCPPID::ProtoSegment *> existing_segments;
    // if all showers, no need to fit vertex with only two legs ...
    bool flag_skip_two_legs = false;
    {
        int ntracks = 0;
        for (auto it = map_segment_vertices.begin(); it != map_segment_vertices.end(); it++) {
            WCPPID::ProtoSegment *sg = it->first;
            if (sg->get_cluster_id() != temp_cluster->get_cluster_id()) continue;
            existing_segments.insert(sg);
            if (!sg->get_flag_shower()) ntracks++;
        }
        if (ntracks == 0) flag_skip_two_legs = true;  // also all showers ...
    }

    bool flag_found_vertex_activities = false;

    if (flag_search_vertex_activity) {
        // search for vertex activities ...
        if (examine_structure_4(main_vertex, temp_cluster, flag_final_vertex)) {
            flag_found_vertex_activities = true;
            temp_cluster->do_multi_tracking(map_vertex_segments, map_segment_vertices, *ct_point_cloud, global_wc_map,
                                            flash_time * units::microsecond, true, true, true);
        }
    }

    bool flag_update_fit = false;
    // find the vertex
    for (auto it = map_vertex_segments.begin(); it != map_vertex_segments.end(); it++) {
        WCPPID::ProtoVertex *vtx = it->first;
        // hack for now ...
        if (vtx->get_cluster_id() != temp_cluster->get_cluster_id() || it->second.size() <= 2 && (vtx != main_vertex))
            continue;
        int ntracks = 0, nshowers = 0;
        int n_long_muons = 0;
        for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
            if ((*it1)->get_flag_shower())
                nshowers++;
            else
                ntracks++;
            if (segments_in_long_muon.find(*it1) != segments_in_long_muon.end()) n_long_muons++;
            //      std::cout << vtx->get_cluster_id() << " " << vtx->get_id() << " " << (*it1)->get_cluster_id() << " "
            //      << (*it1)->get_id() << " " << ntracks << " " << flag_skip_two_legs << " " << it->second.size() <<
            //      std::endl;
        }
        //    std::cout << n_long_muons << std::endl;
        //    if (n_long_muons >1 && (vtx != main_vertex)) continue; // long muon ...
        if (ntracks == 0 && (vtx != main_vertex)) continue;
        if (flag_skip_two_legs && it->second.size() <= 2) continue;

        auto wcp_save = vtx->get_wcpt();

        bool flag_update = fit_vertex(vtx, it->second, temp_cluster);
        if (flag_update) fitted_vertices.insert(vtx);
        if (flag_update) {
            flag_update_fit = true;

            double tmp_dis = sqrt(pow(wcp_save.x - vtx->get_wcpt().x, 2) + pow(wcp_save.y - vtx->get_wcpt().y, 2) +
                                  pow(wcp_save.z - vtx->get_wcpt().z, 2));

            if (tmp_dis > 0.5 * units::cm) {  // if the vertex is moving far, refit ...
                temp_cluster->do_multi_tracking(map_vertex_segments, map_segment_vertices, *ct_point_cloud,
                                                global_wc_map, flash_time * units::microsecond, true, true, true);
                fit_vertex(vtx, map_vertex_segments[vtx], temp_cluster);
            }

            //    for (auto it1 = map_vertex_segments[vtx].begin(); it1 != map_vertex_segments[vtx].end(); it1++){
            //	std::cout << (*it1)->get_wcpt_vec().size() << " ";
            //}
            // std::cout << std::endl;
        }
    }

    if (flag_update_fit) {
        // do the overall fit again
        temp_cluster->do_multi_tracking(map_vertex_segments, map_segment_vertices, *ct_point_cloud, global_wc_map,
                                        flash_time * units::microsecond, true, true, true);

        bool flag_keep_main_vertex = false;
        Point main_vtx_pt;
        if (main_vertex != 0) {
            main_vtx_pt = main_vertex->get_fit_pt();
            flag_keep_main_vertex = true;
        }
        examine_vertices(temp_cluster);

        if (flag_keep_main_vertex)
            if (map_vertex_segments.find(main_vertex) == map_vertex_segments.end()) {
                double min_dis = 1e9;
                for (auto it = map_vertex_segments.begin(); it != map_vertex_segments.end(); it++) {
                    if (it->first->get_cluster_id() != temp_cluster->get_cluster_id()) continue;
                    double dis = sqrt(pow(it->first->get_fit_pt().x - main_vtx_pt.x, 2) +
                                      pow(it->first->get_fit_pt().y - main_vtx_pt.y, 2) +
                                      pow(it->first->get_fit_pt().z - main_vtx_pt.z, 2));
                    if (dis < min_dis) {
                        min_dis = dis;
                        main_vertex = it->first;
                    }
                }
            }
    }

    std::vector<WCPPID::ProtoVertex *> refit_vertices;
    flag_update_fit = false;
    if (flag_search_vertex_activity) {
        // search for vertex activities ...
        if (!flag_found_vertex_activities)
            if (examine_structure_4(main_vertex, temp_cluster, flag_final_vertex)) {
                flag_found_vertex_activities = true;
                if (map_vertex_segments[main_vertex].size() == 3) refit_vertices.push_back(main_vertex);
                flag_update_fit = true;
            }

        for (auto it = map_vertex_segments.begin(); it != map_vertex_segments.end(); it++) {
            WCPPID::ProtoVertex *vtx = it->first;
            // hack for now ...
            if (vtx->get_cluster_id() != temp_cluster->get_cluster_id() ||
                it->second.size() <= 2 && (vtx != main_vertex))
                continue;

            int ntracks = 0, nshowers = 0;
            for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++) {
                if ((*it1)->get_flag_shower())
                    nshowers++;
                else
                    ntracks++;
            }

            if (ntracks == 0 && vtx != main_vertex) continue;
            if (vertices_in_long_muon.find(vtx) != vertices_in_long_muon.end()) continue;
            if (vtx == main_vertex && flag_found_vertex_activities) continue;
            if (flag_skip_two_legs && it->second.size() <= 2) continue;

            double search_range = 1.5 * units::cm;
            if (it->second.size() == 1) search_range = 3.0 * units::cm;

            bool flag_update = search_for_vertex_activities(vtx, it->second, temp_cluster, search_range);

            if (flag_update) {
                // if there are only two tracks starting, the fit is not precise, plan for refit
                if (map_vertex_segments[vtx].size() == 3) refit_vertices.push_back(vtx);
                flag_update_fit = true;
            }
        }

        //  std::cout << flag_update_fit << std::endl;
        if (flag_update_fit) {
            // do the overall fit again
            temp_cluster->do_multi_tracking(map_vertex_segments, map_segment_vertices, *ct_point_cloud, global_wc_map,
                                            flash_time * units::microsecond, true, true, true);
            flag_update_fit = false;
            // redo the fit ...
            for (auto it = refit_vertices.begin(); it != refit_vertices.end(); it++) {
                bool flag_update = fit_vertex(*it, map_vertex_segments[*it], temp_cluster);
                if (flag_update) fitted_vertices.insert(*it);
                if (flag_update) flag_update_fit = true;
            }
            if (flag_update_fit)
                temp_cluster->do_multi_tracking(map_vertex_segments, map_segment_vertices, *ct_point_cloud,
                                                global_wc_map, flash_time * units::microsecond, true, true, true);
        }

        // eliminate the short tracks ...
        if (eliminate_short_vertex_activities(temp_cluster, existing_segments))
            temp_cluster->do_multi_tracking(map_vertex_segments, map_segment_vertices, *ct_point_cloud, global_wc_map,
                                            flash_time * units::microsecond, true, true, true);

        for (auto it = map_segment_vertices.begin(); it != map_segment_vertices.end(); it++) {
            WCPPID::ProtoSegment *sg1 = it->first;
            if (sg1->get_cluster_id() != temp_cluster->get_cluster_id()) continue;
            if (sg1->get_particle_type() == 0) {
                sg1->is_shower_topology();

                WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
                if ((*it->second.begin())->get_wcpt().index == sg1->get_wcpt_vec().front().index) {
                    start_v = (*it->second.begin());
                    end_v = (*it->second.rbegin());
                }
                else {
                    end_v = (*it->second.begin());
                    start_v = (*it->second.rbegin());
                }
                //	for (auto it = map_segment_vertices[sg1].begin(); it!=map_segment_vertices[sg1].end(); it++){
                //  if ((*it)->get_wcpt().index == sg1->get_wcpt_vec().front().index) start_v = *it;
                // if ((*it)->get_wcpt().index == sg1->get_wcpt_vec().back().index) end_v = *it;
                //}

                if (sg1->get_flag_shower_trajectory()) {
                    // trajectory shower
                    sg1->determine_dir_shower_trajectory(map_vertex_segments[start_v].size(),
                                                         map_vertex_segments[end_v].size(), false);
                }
                else {
                    sg1->determine_dir_track(map_vertex_segments[start_v].size(), map_vertex_segments[end_v].size(),
                                             false);
                }
            }
        }
    }
    else {  // flag vertex activities ...
        for (auto it = fitted_vertices.begin(); it != fitted_vertices.end(); it++) {
            WCPPID::ProtoVertex *vtx = *it;
            for (auto it1 = map_vertex_segments[vtx].begin(); it1 != map_vertex_segments[vtx].end(); it1++) {
                WCPPID::ProtoSegment *sg = *it1;
                if (sg->get_particle_type() == 0) sg->is_shower_topology();
                WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
                for (auto it = map_segment_vertices[sg].begin(); it != map_segment_vertices[sg].end(); it++) {
                    if ((*it)->get_wcpt().index == sg->get_wcpt_vec().front().index) start_v = *it;
                    if ((*it)->get_wcpt().index == sg->get_wcpt_vec().back().index) end_v = *it;
                }
                bool flag_print = false;
                if (!sg->get_flag_shower()) {
                    // track ...
                    sg->determine_dir_track(map_vertex_segments[start_v].size(), map_vertex_segments[end_v].size(),
                                            flag_print);
                    // std::cout << sg->get_id() << " " << sg->get_flag_dir() << std::endl;
                }
            }
        }
    }

    // if a track connecting to the vertex is trajectory shower, recheck, probably issue with vertex location ...
    if (main_vertex != 0 && main_vertex->get_cluster_id() == temp_cluster->get_cluster_id()) {
        for (auto it = map_vertex_segments[main_vertex].begin(); it != map_vertex_segments[main_vertex].end(); it++) {
            WCPPID::ProtoSegment *sg = (*it);
            std::pair<int, double> pair_result = calculate_num_daughter_showers(main_vertex, sg, false);

            //  std::cout << sg->get_medium_dQ_dx()/(43e3/units::cm) << " " << pair_result.first << " " <<
            //  sg->get_flag_shower_trajectory() << std::endl;
            if ((pair_result.first <= 2 ||
                 sg->get_medium_dQ_dx() / (43e3 / units::cm) > 1.6 && pair_result.first <= 3) &&
                sg->get_flag_shower_trajectory()) {
                if (!sg->is_shower_trajectory()) {
                    WCPPID::ProtoVertex *start_v = 0, *end_v = 0;

                    if ((*map_segment_vertices[sg].begin())->get_wcpt().index == sg->get_wcpt_vec().front().index) {
                        start_v = (*map_segment_vertices[sg].begin());
                        end_v = (*map_segment_vertices[sg].rbegin());
                    }
                    else {
                        end_v = (*map_segment_vertices[sg].begin());
                        start_v = (*map_segment_vertices[sg].rbegin());
                    }

                    /* for (auto it = map_segment_vertices[sg].begin(); it!=map_segment_vertices[sg].end(); it++){ */
                    /*   if ((*it)->get_wcpt().index == sg->get_wcpt_vec().front().index) start_v = *it; */
                    /*   if ((*it)->get_wcpt().index == sg->get_wcpt_vec().back().index) end_v = *it; */
                    /* } */
                    sg->determine_dir_track(map_vertex_segments[start_v].size(), map_vertex_segments[end_v].size(),
                                            false);
                }
                //	std::cout << sg->get_id() << " " << sg->is_shower_trajectory() << std::endl;
            }

            //  std::cout << sg->get_id() << " " << sg->is_shower_topology() << " " << pair_result.first << std::endl;
            // examine topology case ...
            if (pair_result.first == 1 && sg->get_flag_shower_topology()) {
                int dir_save = sg->get_flag_dir();
                WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
                if ((*map_segment_vertices[sg].begin())->get_wcpt().index == sg->get_wcpt_vec().front().index) {
                    start_v = (*map_segment_vertices[sg].begin());
                    end_v = (*map_segment_vertices[sg].rbegin());
                }
                else {
                    end_v = (*map_segment_vertices[sg].begin());
                    start_v = (*map_segment_vertices[sg].rbegin());
                }
                // for (auto it = map_segment_vertices[sg].begin(); it!=map_segment_vertices[sg].end(); it++){
                //	  if ((*it)->get_wcpt().index == sg->get_wcpt_vec().front().index) start_v = *it;
                //	  if ((*it)->get_wcpt().index == sg->get_wcpt_vec().back().index) end_v = *it;
                //}
                sg->set_flag_shower_topology(false);
                sg->determine_dir_track(map_vertex_segments[start_v].size(), map_vertex_segments[end_v].size(), false);
                //	std::cout << sg->get_id() << " " << sg->get_particle_type() << " " << sg->get_particle_score()
                //<< std::endl;
                if (sg->get_particle_type() == 2212 && sg->get_particle_score() < 0.09 ||
                    sg->get_particle_type() == 13 && sg->get_particle_score() < 0.06) {
                    sg->set_flag_shower_topology(false);
                }
                else {
                    sg->set_flag_shower_topology(true);
                    sg->set_particle_type(11);
                    sg->set_particle_score(100);
                    sg->set_flag_dir(dir_save);
                    TPCParams &mp = Singleton<TPCParams>::Instance();
                    sg->set_particle_mass(mp.get_mass_electron());
                }
            }

            if (flag_skip_two_legs && existing_segments.find(sg) == existing_segments.end()) {
                WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
                if ((*map_segment_vertices[sg].begin())->get_wcpt().index == sg->get_wcpt_vec().front().index) {
                    start_v = (*map_segment_vertices[sg].begin());
                    end_v = (*map_segment_vertices[sg].rbegin());
                }
                else {
                    end_v = (*map_segment_vertices[sg].begin());
                    start_v = (*map_segment_vertices[sg].rbegin());
                }
                sg->determine_dir_track(map_vertex_segments[start_v].size(), map_vertex_segments[end_v].size(), false);
                if ((sg->get_particle_type() == 0 || sg->is_dir_weak()) &&
                    sg->get_medium_dQ_dx() / (43e3 / units::cm) < 1.3) {
                    sg->set_particle_type(11);
                    sg->set_particle_score(100);
                    TPCParams &mp = Singleton<TPCParams>::Instance();
                    sg->set_particle_mass(mp.get_mass_electron());
                }
                //	std::cout << sg->get_id() << " " << sg->get_particle_type() << " " << sg->get_particle_score()
                //<< " " << sg->get_medium_dQ_dx()/(43e3/units::cm) << std::endl;
            }
        }
    }
}

bool WCPPID::NeutrinoID::eliminate_short_vertex_activities(WCPPID::PR3DCluster *temp_cluster,
                                                           std::set<WCPPID::ProtoSegment *> &existing_segments)
{
    bool flag_updated = false;
    bool flag_continue = true;

    while (flag_continue) {
        flag_continue = false;
        std::set<WCPPID::ProtoSegment *> to_be_removed_segments;
        std::set<WCPPID::ProtoVertex *> to_be_removed_vertices;

        for (auto it = map_segment_vertices.begin(); it != map_segment_vertices.end(); it++) {
            WCPPID::ProtoSegment *sg = it->first;
            if (sg->get_cluster_id() != temp_cluster->get_cluster_id()) continue;
            if (existing_segments.find(sg) != existing_segments.end()) continue;

            WCPPID::ProtoVertex *v1 = *it->second.begin();
            WCPPID::ProtoVertex *v2 = *it->second.rbegin();

            double length = sg->get_direct_length();
            //      std::cout << sg->get_id() << " " << length/units::cm << std::endl;
            if (map_vertex_segments[v1].size() == 1 && map_vertex_segments[v2].size() >= 3) {
                if (length < 0.36 * units::cm) {
                    to_be_removed_segments.insert(sg);
                    to_be_removed_vertices.insert(v1);
                    flag_continue = true;
                    break;
                }
                else if (length < 0.5 * units::cm && map_vertex_segments[v2].size() > 3) {
                    to_be_removed_segments.insert(sg);
                    to_be_removed_vertices.insert(v1);
                    flag_continue = true;
                    break;
                }
            }
            else if (map_vertex_segments[v1].size() >= 3 && map_vertex_segments[v2].size() == 1) {
                //	std::cout << sg->get_id() << " " << length/units::cm << std::endl;
                if (length < 0.36 * units::cm) {
                    to_be_removed_segments.insert(sg);
                    to_be_removed_vertices.insert(v2);
                    flag_continue = true;
                    break;
                }
                else if (length < 0.5 * units::cm && map_vertex_segments[v1].size() > 3) {
                    to_be_removed_segments.insert(sg);
                    to_be_removed_vertices.insert(v2);
                    flag_continue = true;
                    break;
                }
            }
            else if (v1 == main_vertex && map_vertex_segments[v1].size() > 1 ||
                     v2 == main_vertex && map_vertex_segments[v2].size() > 1) {
                if (length < 0.1 * units::cm) {
                    to_be_removed_segments.insert(sg);
                    to_be_removed_vertices.insert(v2);
                    flag_continue = true;
                    break;
                }
            }

            //      std::cout << sg->get_id() << " " << sg->get_length()/units::cm << " " << flag_continue << std::endl;
            if (!flag_continue)
                if (map_vertex_segments[v1].size() == 1 && map_vertex_segments[v2].size() > 1) {
                    for (auto it1 = map_vertex_segments[v2].begin(); it1 != map_vertex_segments[v2].end(); it1++) {
                        WCPPID::ProtoSegment *sg1 = *it1;
                        if (sg1 == sg) continue;
                        double dis = sg1->get_closest_point(v1->get_fit_pt()).first;
                        //	    std::cout << sg->get_id() << " " << sg->get_length()/units::cm << " " <<
                        //dis/units::cm << std::endl;
                        if (dis < 0.36 * units::cm) {
                            to_be_removed_segments.insert(sg);
                            to_be_removed_vertices.insert(v1);
                            flag_continue = true;
                            break;
                        }
                        else if (v2 == main_vertex && dis < 0.45 * units::cm && sg->get_length() < 0.45 * units::cm) {
                            to_be_removed_segments.insert(sg);
                            to_be_removed_vertices.insert(v1);
                            flag_continue = true;
                            break;
                        }
                    }
                }
                else if (map_vertex_segments[v2].size() == 1 && map_vertex_segments[v1].size() > 1) {
                    for (auto it1 = map_vertex_segments[v1].begin(); it1 != map_vertex_segments[v1].end(); it1++) {
                        WCPPID::ProtoSegment *sg1 = *it1;
                        if (sg1 == sg) continue;
                        double dis = sg1->get_closest_point(v2->get_fit_pt()).first;
                        // std::cout << sg->get_id() << " " << sg->get_length()/units::cm << " " << dis/units::cm <<
                        // std::endl;
                        if (dis < 0.36 * units::cm) {
                            to_be_removed_segments.insert(sg);
                            to_be_removed_vertices.insert(v2);
                            flag_continue = true;
                            break;
                        }
                        else if (v1 == main_vertex && dis < 0.45 * units::cm && sg->get_length() < 0.45 * units::cm) {
                            to_be_removed_segments.insert(sg);
                            to_be_removed_vertices.insert(v2);
                            flag_continue = true;
                            break;
                        }
                    }
                }

            // std::cout << sg->get_id() << " " << sg->get_length()/units::cm << " " << flag_continue << std::endl;

            if (!flag_continue && existing_segments.find(sg) == existing_segments.end() && length > 0.45 * units::cm) {
                int n_good = 0;
                PointVector &pts = sg->get_point_vec();
                for (size_t i = 0; i != pts.size(); i++) {
                    double dis_u = 1e9, dis_v = 1e9, dis_w = 1e9;
                    for (auto it1 = existing_segments.begin(); it1 != existing_segments.end(); it1++) {
                        if (map_segment_vertices.find(*it1) == map_segment_vertices.end()) continue;
                        auto tuple_results = (*it1)->get_closest_2d_dis(pts.at(i));
                        if (std::get<0>(tuple_results) < dis_u) dis_u = std::get<0>(tuple_results);
                        if (std::get<1>(tuple_results) < dis_v) dis_v = std::get<1>(tuple_results);
                        if (std::get<2>(tuple_results) < dis_w) dis_w = std::get<2>(tuple_results);
                    }
                    // std::cout << i << " " << dis_u/units::cm << " " << dis_v/units::cm << " " << dis_w/units::cm << "
                    // " << length/units::cm << std::endl;
                    if ((dis_u > 0.45 * units::cm || dis_v > 0.45 * units::cm || dis_w > 0.45 * units::cm)) n_good++;
                }
                if (n_good == 0) {
                    //	  std::cout << sg->get_id() << " " << sg->get_length()/units::cm << " " << flag_continue <<
                    //std::endl;
                    to_be_removed_segments.insert(sg);
                    if (map_vertex_segments[v1].size() == 1) to_be_removed_vertices.insert(v1);
                    if (map_vertex_segments[v2].size() == 1) to_be_removed_vertices.insert(v2);
                }
            }

            if (flag_continue) break;
        }  // for loop of segment

        for (auto it = to_be_removed_segments.begin(); it != to_be_removed_segments.end(); it++) {
            flag_updated = true;
            //      std::cout << (*it)->get_id() << " " << (*it)->get_length()/units::cm << std::endl;
            del_proto_segment(*it);
        }
        for (auto it = to_be_removed_vertices.begin(); it != to_be_removed_vertices.end(); it++) {
            del_proto_vertex(*it);
        }
    }

    return flag_updated;
}

WCPPID::MyFCN::MyFCN(WCPPID::ProtoVertex *vtx, bool flag_vtx_constraint, double vtx_constraint_range,
                     double vertex_protect_dis, double vertex_protect_dis_short_track, double fit_dis)
  : vtx(vtx)
  , enforce_two_track_fit(false)
  , flag_vtx_constraint(flag_vtx_constraint)
  , vtx_constraint_range(vtx_constraint_range)
  , vertex_protect_dis(vertex_protect_dis)
  , vertex_protect_dis_short_track(vertex_protect_dis_short_track)
  , fit_dis(fit_dis)
{
    segments.clear();
    vec_points.clear();
}

WCPPID::MyFCN::~MyFCN() {}

void WCPPID::MyFCN::print_points()
{
    for (size_t i = 0; i != vec_points.size(); i++) {
        for (size_t j = 0; j != vec_points.at(i).size(); j++) {
            std::cout << i << " " << j << " " << vec_points.at(i).at(j).x / units::cm << " "
                      << vec_points.at(i).at(j).y / units::cm << " " << vec_points.at(i).at(j).z / units::cm
                      << std::endl;
        }
    }
}

void WCPPID::MyFCN::AddSegment(ProtoSegment *sg)
{
    // push in ...
    segments.push_back(sg);
    {
        WCP::PointVector pts;
        vec_points.push_back(pts);
    }

    WCP::Point center;
    double min_dis = 1e9;
    WCP::PointVector &pts = sg->get_point_vec();
    double length = sqrt(pow(pts.front().x - pts.back().x, 2) + pow(pts.front().y - pts.back().y, 2) +
                         pow(pts.front().z - pts.back().z, 2));

    //  std::cout << length/units::cm << std::endl;

    for (size_t i = 0; i != pts.size(); i++) {
        double dis_to_vertex =
            sqrt(pow(pts.at(i).x - vtx->get_fit_pt().x, 2) + pow(pts.at(i).y - vtx->get_fit_pt().y, 2) +
                 pow(pts.at(i).z - vtx->get_fit_pt().z, 2));
        if (length > 3.0 * units::cm) {
            if (dis_to_vertex < vertex_protect_dis || dis_to_vertex > fit_dis) continue;
        }
        else {
            if (dis_to_vertex < vertex_protect_dis_short_track || dis_to_vertex > fit_dis) continue;
        }
        //  if (sg->get_closest_point(pts.at(i)).first > point_track_dis) continue;
        vec_points.back().push_back(pts.at(i));
        if (dis_to_vertex < min_dis) {
            center = pts.at(i);
            min_dis = dis_to_vertex;
        }
    }

    // calculate the PCA ...
    if (vec_points.back().size() > 1) {
        int nsum = vec_points.back().size();

        /* // calculate center ... */
        /* center.x = 0; center.y = 0; center.z = 0; */
        /* for (auto it = vec_points.back().begin(); it!=vec_points.back().end();it++){ */
        /*   center.x += (*it).x; */
        /*   center.y += (*it).y; */
        /*   center.z += (*it).z; */
        /* } */
        /* center.x /= nsum; center.y /= nsum; center.z /= nsum;  */

        // Eigen vectors ...
        PointVector PCA_axis(3);
        for (int i = 0; i != 3; i++) {
            PCA_axis[i].x = 0;
            PCA_axis[i].y = 0;
            PCA_axis[i].z = 0;
        }
        double PCA_values[3] = {0, 0, 0};

        TMatrixD cov_matrix(3, 3);
        for (int i = 0; i != 3; i++) {
            for (int j = 0; j != 3; j++) {
                for (size_t k = 0; k != vec_points.back().size(); k++) {
                    if (i == 0 && j == 0) {
                        cov_matrix(i, j) +=
                            (vec_points.back().at(k).x - center.x) * (vec_points.back().at(k).x - center.x);
                    }
                    else if (i == 0 && j == 1) {
                        cov_matrix(i, j) +=
                            (vec_points.back().at(k).x - center.x) * (vec_points.back().at(k).y - center.y);
                    }
                    else if (i == 0 && j == 2) {
                        cov_matrix(i, j) +=
                            (vec_points.back().at(k).x - center.x) * (vec_points.back().at(k).z - center.z);
                    }
                    else if (i == 1 && j == 1) {
                        cov_matrix(i, j) +=
                            (vec_points.back().at(k).y - center.y) * (vec_points.back().at(k).y - center.y);
                    }
                    else if (i == 1 && j == 2) {
                        cov_matrix(i, j) +=
                            (vec_points.back().at(k).y - center.y) * (vec_points.back().at(k).z - center.z);
                    }
                    else if (i == 2 && j == 2) {
                        cov_matrix(i, j) +=
                            (vec_points.back().at(k).z - center.z) * (vec_points.back().at(k).z - center.z);
                    }
                }
            }
        }
        cov_matrix(1, 0) = cov_matrix(0, 1);
        cov_matrix(2, 0) = cov_matrix(0, 2);
        cov_matrix(2, 1) = cov_matrix(1, 2);
        for (int i = 0; i != 3; i++) {
            for (int j = 0; j != 3; j++) {
                cov_matrix(i, j) = cov_matrix(i, j) / nsum;
            }
        }

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

        PCA_values[0] = eigen_values(0, 0) + pow(0.15 * units::cm, 2);
        PCA_values[1] = eigen_values(1, 1) + pow(0.15 * units::cm, 2);
        PCA_values[2] = eigen_values(2, 2) + pow(0.15 * units::cm, 2);

        //    std::cout << sqrt(eigen_values(0,0))/units::cm << " " << sqrt(eigen_values(1,1))/units::cm << " " <<
        //    sqrt(eigen_values(2,2))/units::cm << " " << std::endl;

        for (int i = 0; i != 3; i++) {
            PCA_axis[i].x = eigen_vectors(0, i) /
                            sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) +
                                 eigen_vectors(2, i) * eigen_vectors(2, i));
            PCA_axis[i].y = eigen_vectors(1, i) /
                            sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) +
                                 eigen_vectors(2, i) * eigen_vectors(2, i));
            PCA_axis[i].z = eigen_vectors(2, i) /
                            sqrt(eigen_vectors(0, i) * eigen_vectors(0, i) + eigen_vectors(1, i) * eigen_vectors(1, i) +
                                 eigen_vectors(2, i) * eigen_vectors(2, i));
        }

        // TVector3 v1(vec_points.back().front().x - vec_points.back().back().x, vec_points.back().front().y -
        // vec_points.back().back().y, vec_points.back().front().z - vec_points.back().back().z); TVector3
        // v2(PCA_axis[0].x, PCA_axis[0].y, PCA_axis[0].z); TVector3 v3(PCA_axis[1].x, PCA_axis[1].y, PCA_axis[1].z);
        // TVector3 v4(PCA_axis[2].x, PCA_axis[2].y, PCA_axis[2].z);
        //    std::cout << vec_points.back().size() << " " << sqrt(PCA_values[0])/units::cm << " " <<
        //    sqrt(PCA_values[1])/units::cm << " " << sqrt(PCA_values[2])/units::cm << " " <<
        //    v1.Angle(v2)/3.1415926*180. << " " << v1.Angle(v3)/3.1415926*180. << " " << v1.Angle(v4)/3.1415926*180. <<
        //    " " << v4.Mag() << " " << v2.Mag() << " " << v3.Mag() << std::endl;

        vec_PCA_dirs.push_back(std::make_tuple(PCA_axis[0], PCA_axis[1], PCA_axis[2]));
        vec_PCA_vals.push_back(std::make_tuple(PCA_values[0], PCA_values[1], PCA_values[2]));
        vec_centers.push_back(center);
    }
    else {
        Point a(0, 0, 0);
        vec_PCA_dirs.push_back(std::make_tuple(a, a, a));
        vec_PCA_vals.push_back(std::make_tuple(0, 0, 0));
        if (vec_points.back().size() == 1) {
            vec_centers.push_back(vec_points.back().back());
        }
        else {
            vec_centers.push_back(a);
        }
    }

    //  std::cout << vec_points.size() << " " << vec_points.back().size() << std::endl;
}

void WCPPID::MyFCN::update_fit_range(double tmp_vertex_protect_dis, double tmp_vertex_protect_dis_short_track,
                                     double tmp_fit_dis)
{
    vertex_protect_dis = tmp_vertex_protect_dis;
    vertex_protect_dis_short_track = tmp_vertex_protect_dis_short_track;
    fit_dis = tmp_fit_dis;

    std::vector<WCPPID::ProtoSegment *> tmp_segments = segments;
    segments.clear();
    vec_points.clear();
    for (auto it = tmp_segments.begin(); it != tmp_segments.end(); it++) {
        AddSegment(*it);
    }
}

int WCPPID::MyFCN::get_fittable_tracks()
{
    int ncount = 0;
    for (size_t i = 0; i != vec_points.size(); i++) {
        if (vec_points.at(i).size() > 1) ncount++;
    }
    return ncount;
}

std::pair<WCPPID::ProtoSegment *, int> WCPPID::MyFCN::get_seg_info(int i)
{
    if (i < segments.size()) {
        return std::make_pair(segments.at(i), vec_points.at(i).size());
    }
    return std::make_pair((WCPPID::ProtoSegment *) 0, 0);
}

std::pair<bool, WCP::Point> WCPPID::MyFCN::FitVertex()
{
    Point fit_pos = vtx->get_fit_pt();
    bool fit_flag = false;

    int ntracks = get_fittable_tracks();

    int npoints = 0;

    int n_large_angles = 0;
    for (size_t i = 0; i != vec_PCA_vals.size(); i++) {
        TVector3 dir1(std::get<0>(vec_PCA_dirs.at(i)).x, std::get<0>(vec_PCA_dirs.at(i)).y,
                      std::get<0>(vec_PCA_dirs.at(i)).z);
        for (size_t j = i + 1; j < vec_PCA_vals.size(); j++) {
            TVector3 dir2(std::get<0>(vec_PCA_dirs.at(j)).x, std::get<0>(vec_PCA_dirs.at(j)).y,
                          std::get<0>(vec_PCA_dirs.at(j)).z);
            if (dir1.Angle(dir2) / 3.1415926 * 180. > 15) n_large_angles++;
            // std::cout << i << " " << j << " " << dir1.Angle(dir2)/3.1415926*180. << std::endl;
        }
    }
    // std::cout << ntracks << " " << n_large_angles << std::endl;

    if (ntracks > 2 && n_large_angles > 1 || ntracks >= 2 && enforce_two_track_fit && n_large_angles >= 1) {
        // start the fit ...
        Eigen::VectorXd temp_pos_3D_init(3), temp_pos_3D(3);  // to be fitted
        temp_pos_3D_init(0) = fit_pos.x;
        temp_pos_3D_init(1) = fit_pos.y;
        temp_pos_3D_init(2) = fit_pos.z;

        Eigen::Vector3d b(0, 0, 0);
        Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, 3);

        //    double old_chi2 = 0;

        for (size_t i = 0; i != vec_PCA_vals.size(); i++) {
            if (std::get<0>(vec_PCA_vals.at(i)) > 0) {
                npoints += vec_points.at(i).size();

                // fill the matrix ... first row, second column
                Eigen::MatrixXd R(3, 3);
                R(0, 0) = 0;
                R(0, 1) = 0;
                R(0, 2) = 0;
                double val1 = sqrt(std::get<0>(vec_PCA_vals.at(i)) / std::get<1>(vec_PCA_vals.at(i)));
                R(1, 0) = val1 * std::get<1>(vec_PCA_dirs.at(i)).x;
                R(1, 1) = val1 * std::get<1>(vec_PCA_dirs.at(i)).y;
                R(1, 2) = val1 * std::get<1>(vec_PCA_dirs.at(i)).z;
                val1 = sqrt(std::get<0>(vec_PCA_vals.at(i)) / std::get<2>(vec_PCA_vals.at(i)));
                R(2, 0) = val1 * std::get<2>(vec_PCA_dirs.at(i)).x;
                R(2, 1) = val1 * std::get<2>(vec_PCA_dirs.at(i)).y;
                R(2, 2) = val1 * std::get<2>(vec_PCA_dirs.at(i)).z;

                Eigen::Vector3d data(vec_centers.at(i).x, vec_centers.at(i).y, vec_centers.at(i).z);
                data = R * data;
                Eigen::MatrixXd RT = R.transpose();

                b += RT * data;
                A += RT * R;

                //	Eigen::Vector3d temp_data(fit_pos.x, fit_pos.y, fit_pos.z);
                // temp_data = R * temp_data - data;
                //	double temp_dis = sqrt(pow(vec_centers.at(i).x-fit_pos.x,2)
                //+pow(vec_centers.at(i).y-fit_pos.y,2) + pow(vec_centers.at(i).z-fit_pos.z,2)); std::cout <<
                // temp_data(0) << " " << temp_data(1) << " " << temp_data(2) << " " << temp_dis/units::cm << std::endl;

                //	old_chi2 += pow(temp_data(0),2) + pow(temp_data(1),2) + pow(temp_data(2),2);
            }
        }
        // 1.69 is a factor to be tuned ...

        // add constraint ...
        if (flag_vtx_constraint) {
            Eigen::MatrixXd R = Eigen::MatrixXd::Zero(3, 3);
            R(0, 0) = 1. / vtx_constraint_range * sqrt(npoints);
            R(1, 1) = 1. / vtx_constraint_range * sqrt(npoints);
            R(2, 2) = 1. / vtx_constraint_range * sqrt(npoints);

            Eigen::Vector3d data(fit_pos.x / vtx_constraint_range * sqrt(npoints),
                                 fit_pos.y / vtx_constraint_range * sqrt(npoints),
                                 fit_pos.z / vtx_constraint_range * sqrt(npoints));
            Eigen::MatrixXd RT = R.transpose();
            b += RT * data;
            A += RT * R;
        }

        Eigen::BiCGSTAB<Eigen::MatrixXd> solver;
        solver.compute(A);
        temp_pos_3D = solver.solveWithGuess(b, temp_pos_3D_init);

        if (!std::isnan(solver.error())) {
            fit_pos.x = temp_pos_3D(0);
            fit_pos.y = temp_pos_3D(1);
            fit_pos.z = temp_pos_3D(2);
            fit_flag = true;

            //      std::cout << "Fit Vertex: " << sqrt(pow(temp_pos_3D(0) - temp_pos_3D_init(0),2) + pow(temp_pos_3D(1)
            //      - temp_pos_3D_init(1),2) + pow(temp_pos_3D(2) - temp_pos_3D_init(2),2))/units::cm << " " <<
            //      std::endl;
        }
        else {
            std::cout << "Cluster: " << vtx->get_cluster_id() << " Fit Vertex Failed!" << std::endl;
        }

        /* double new_chi2 = 0; */
        /* for (size_t i=0;i!=vec_PCA_vals.size();i++){ */
        /*   if (std::get<0>(vec_PCA_vals.at(i))>0){ */
        /* 		// fill the matrix ... first row, second column */
        /* 	Eigen::MatrixXd R(3,3); */
        /* 	R(0,0) = 0; R(0,1) = 0; R(0,2) = 0; */
        /* 	double val1 = sqrt(std::get<0>(vec_PCA_vals.at(i))/std::get<1>(vec_PCA_vals.at(i))); */
        /* 	R(1,0) = val1 * std::get<1>(vec_PCA_dirs.at(i)).x; */
        /* 	R(1,1) = val1 * std::get<1>(vec_PCA_dirs.at(i)).y; */
        /* 	R(1,2) = val1 * std::get<1>(vec_PCA_dirs.at(i)).z;	 */
        /* 	val1 = sqrt(std::get<0>(vec_PCA_vals.at(i))/std::get<2>(vec_PCA_vals.at(i))); */
        /* 	R(2,0) = val1 * std::get<2>(vec_PCA_dirs.at(i)).x; */
        /* 	R(2,1) = val1 * std::get<2>(vec_PCA_dirs.at(i)).y; */
        /* 	R(2,2) = val1 * std::get<2>(vec_PCA_dirs.at(i)).z; */
        /* 	Eigen::Vector3d data(vec_centers.at(i).x, vec_centers.at(i).y, vec_centers.at(i).z ); */
        /* 	data = R * data; */
        /* 	Eigen::MatrixXd RT = R.transpose(); */
        /* 	b += RT * data; */
        /* 	A += RT * R; */
        /* 	Eigen::Vector3d temp_data = temp_pos_3D; */
        /* 	temp_data = R * temp_data - data; */
        /* 	new_chi2 += pow(temp_data(0),2) + pow(temp_data(1),2) + pow(temp_data(2),2); */
        /*   } */
        /* } */
    }

    return std::make_pair(fit_flag, fit_pos);
}

void WCPPID::MyFCN::UpdateInfo(WCP::Point fit_pos, WCPPID::PR3DCluster *temp_cluster, double default_dis_cut)
{
    // convertion to the u, v, w, z ...
    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();
    double slope_x = 1. / time_slice_width;
    // mcell
    double first_t_dis = temp_cluster->get_point_cloud()->get_cloud().pts[0].mcell->GetTimeSlice() * time_slice_width -
                         temp_cluster->get_point_cloud()->get_cloud().pts[0].x;
    double offset_t = first_t_dis / time_slice_width;

    //  convert Z to W ...
    double slope_zw = 1. / pitch_w * cos(angle_w);
    double slope_yw = 1. / pitch_w * sin(angle_w);
    double slope_yu = -1. / pitch_u * sin(angle_u);
    double slope_zu = 1. / pitch_u * cos(angle_u);
    double slope_yv = -1. / pitch_v * sin(angle_v);
    double slope_zv = 1. / pitch_v * cos(angle_v);

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

    if (sqrt(pow(vtx->get_fit_pt().x - fit_pos.x, 2) + pow(vtx->get_fit_pt().y - fit_pos.y, 2) +
             pow(vtx->get_fit_pt().z - fit_pos.z, 2)) > 0.01 * units::cm)
        std::cout << "Cluster: " << vtx->get_cluster_id() << " Update Vertex: ("
                  << offset_u + (slope_yu * fit_pos.y + slope_zu * fit_pos.z) << ", "
                  << offset_v + (slope_yv * fit_pos.y + slope_zv * fit_pos.z) + 2400 << ", "
                  << offset_w + (slope_yw * fit_pos.y + slope_zw * fit_pos.z) + 4800 << ", "
                  << offset_t + slope_x * fit_pos.x << ") <- ("
                  << offset_u + (slope_yu * vtx->get_fit_pt().y + slope_zu * vtx->get_fit_pt().z) << ", "
                  << offset_v + (slope_yv * vtx->get_fit_pt().y + slope_zv * vtx->get_fit_pt().z) + 2400 << ", "
                  << offset_w + (slope_yw * vtx->get_fit_pt().y + slope_zw * vtx->get_fit_pt().z) + 4800 << ", "
                  << offset_t + slope_x * vtx->get_fit_pt().x << ")" << std::endl;

    // find the new wcps point for the vertex ...
    WCP::ToyPointCloud *pcloud = temp_cluster->get_point_cloud_steiner();
    WCP::WCPointCloud<double> &cloud = pcloud->get_cloud();

    WCP::WCPointCloud<double>::WCPoint &vtx_new_wcp = pcloud->get_closest_wcpoint(fit_pos);

    //  for (size_t i=0;i!=segments.size();i++){
    // std::cout << segments.at(i)->get_id() << " " << segments.at(i)->get_wcpt_vec().front().index << " " <<
    // segments.at(i)->get_wcpt_vec().back().index << " " <<  vtx->get_wcpt().index <<  " " << segments.at(i) <<
    // std::endl;
    // }

    // std::cout << sqrt(pow(vtx_new_wcp.x - fit_pos.x,2) + pow(vtx_new_wcp.y - fit_pos.y,2) + pow(vtx_new_wcp.z -
    // fit_pos.z,2))/units::cm << std::endl;

    // quick hack ...
    for (size_t i = 0; i != segments.size(); i++) {
        PointVector &pts = segments.at(i)->get_point_vec();
        std::vector<WCP::WCPointCloud<double>::WCPoint> &vec_wcps = segments.at(i)->get_wcpt_vec();

        // judge direction ...
        bool flag_front = false;
        if (vec_wcps.front().index == vtx->get_wcpt().index) flag_front = true;
        double dis1 = pow(pts.front().x - vtx->get_fit_pt().x, 2) + pow(pts.front().y - vtx->get_fit_pt().y, 2) +
                      pow(pts.front().z - vtx->get_fit_pt().z, 2);
        double dis2 = pow(pts.back().x - vtx->get_fit_pt().x, 2) + pow(pts.back().y - vtx->get_fit_pt().y, 2) +
                      pow(pts.back().z - vtx->get_fit_pt().z, 2);
        //    if (dis1 < dis2) flag_front = true;

        // find the closest point
        WCP::WCPointCloud<double>::WCPoint min_wcp;
        double min_dis = 1e9;
        double max_dis = std::max(
            sqrt(pow(vec_wcps.front().x - vtx->get_fit_pt().x, 2) + pow(vec_wcps.front().y - vtx->get_fit_pt().y, 2) +
                 pow(vec_wcps.front().z - vtx->get_fit_pt().z, 2)),
            sqrt(pow(vec_wcps.back().x - vtx->get_fit_pt().x, 2) + pow(vec_wcps.back().y - vtx->get_fit_pt().y, 2) +
                 pow(vec_wcps.back().z - vtx->get_fit_pt().z, 2)));
        double dis_cut = 0;
        if (max_dis > 2 * default_dis_cut) dis_cut = default_dis_cut;

        for (size_t j = 0; j != vec_wcps.size(); j++) {
            double dis =
                sqrt(pow(vec_centers.at(i).x - vec_wcps.at(j).x, 2) + pow(vec_centers.at(i).y - vec_wcps.at(j).y, 2) +
                     pow(vec_centers.at(i).z - vec_wcps.at(j).z, 2));
            double dis1 =
                sqrt(pow(vec_wcps.at(j).x - vtx->get_fit_pt().x, 2) + pow(vec_wcps.at(j).y - vtx->get_fit_pt().y, 2) +
                     pow(vec_wcps.at(j).z - vtx->get_fit_pt().z, 2));
            if (dis < min_dis && dis1 > dis_cut) {
                min_wcp = vec_wcps.at(j);
                min_dis = dis;
            }
        }

        //    std::cout << i << " (" << offset_u + (slope_yu * min_wcp.y + slope_zu * min_wcp.z) << ", " << offset_v +
        //    (slope_yv * min_wcp.y + slope_zv * min_wcp.z)+2400 << ", " << offset_w + (slope_yw * min_wcp.y + slope_zw
        //    * min_wcp.z)+4800 <<", " << offset_t + slope_x * min_wcp.x << ")" << " " <<
        //    segments.at(i)->get_length()/units::cm << " " << sqrt(pow(vtx->get_fit_pt().x - min_wcp.x,2) +
        //    pow(vtx->get_fit_pt().y - min_wcp.y,2) + pow(vtx->get_fit_pt().z - min_wcp.z,2))/units::cm << std::endl;

        // establish the shortest path ...
        std::list<WCP::WCPointCloud<double>::WCPoint> new_list;

        new_list.push_back(vtx_new_wcp);
        {
            double dis_step = 2.0 * units::cm;
            int ncount = std::round(sqrt(pow(vtx_new_wcp.x - min_wcp.x, 2) + pow(vtx_new_wcp.y - min_wcp.y, 2) +
                                         pow(vtx_new_wcp.z - min_wcp.z, 2)) /
                                    dis_step);
            if (ncount < 2) ncount = 2;

            for (int qx = 1; qx < ncount; qx++) {
                Point tmp_p(vtx_new_wcp.x + (min_wcp.x - vtx_new_wcp.x) / ncount * qx,
                            vtx_new_wcp.y + (min_wcp.y - vtx_new_wcp.y) / ncount * qx,
                            vtx_new_wcp.z + (min_wcp.z - vtx_new_wcp.z) / ncount * qx);
                WCP::WCPointCloud<double>::WCPoint &tmp_wcp = pcloud->get_closest_wcpoint(tmp_p);
                // std::cout << qx << " " << sqrt(pow(tmp_wcp.x - tmp_p.x,2) + pow(tmp_wcp.y - tmp_p.y,2) +
                // pow(tmp_wcp.z - tmp_p.z,2))/units::cm << std::endl;
                if (sqrt(pow(tmp_wcp.x - tmp_p.x, 2) + pow(tmp_wcp.y - tmp_p.y, 2) + pow(tmp_wcp.z - tmp_p.z, 2)) >
                    0.3 * units::cm)
                    continue;  // too large ...
                if (tmp_wcp.index != new_list.back().index && tmp_wcp.index != min_wcp.index)
                    new_list.push_back(tmp_wcp);
            }
        }
        new_list.push_back(min_wcp);

        std::list<WCP::WCPointCloud<double>::WCPoint> old_list;
        std::copy(vec_wcps.begin(), vec_wcps.end(), std::back_inserter(old_list));
        // append the list back ...
        // std::cout << min_wcp.index << " " << temp_cluster->get_path_wcps().size() << std::endl;
        if (flag_front) {
            // std::cout << "f1 " << old_list.size() << std::endl;

            while (old_list.front().index != min_wcp.index && old_list.size() > 0) {
                old_list.pop_front();
            }
            old_list.pop_front();
            //      std::cout << "f2 " << old_list.size() << std::endl;
            for (auto it = new_list.rbegin(); it != new_list.rend(); it++) {
                old_list.push_front(*it);
            }
            //      std::cout << "f3 " << old_list.size() << std::endl;
        }
        else {
            //      std::cout << "b1 " << old_list.size() << std::endl;
            while (old_list.back().index != min_wcp.index && old_list.size() > 0) {
                old_list.pop_back();
            }
            old_list.pop_back();

            //      std::cout << "b2 " << old_list.size() << std::endl;
            for (auto it = new_list.rbegin(); it != new_list.rend(); it++) {
                old_list.push_back(*it);
            }
            //      std::cout << "b3 " << old_list.size() << std::endl;
        }
        vec_wcps.clear();
        vec_wcps.reserve(old_list.size());
        std::copy(std::begin(old_list), std::end(old_list), std::back_inserter(vec_wcps));

        /* std::cout << vec_wcps.front().index << " " << vec_wcps.back().index << " " << min_wcp.index << " " <<
         * vtx_new_wcp.index << std::endl; */

        /* for (size_t i=0;i+1!=vec_wcps.size();i++){ */
        /*   std::cout << i << " " << sqrt(pow(vec_wcps.at(i).x - vec_wcps.at(i+1).x,2)+pow(vec_wcps.at(i).y -
         * vec_wcps.at(i+1).y,2) + pow(vec_wcps.at(i).z - vec_wcps.at(i+1).z,2))/units::cm << std::endl; */
        /* } */

        segments.at(i)->clear_fit();

        /* std::vector<double>& pu = (*it)->get_pu_vec(); */
        /* std::vector<double>& pv = (*it)->get_pv_vec(); */
        /* std::vector<double>& pw = (*it)->get_pw_vec(); */
        /* std::vector<double>& pt = (*it)->get_pt_vec(); */
        /* if (dis1 < dis2){ */
        /*   pts.front() = fit_pos; */
        /*   pu.front() = offset_u + 0.5 + (slope_yu * fit_pos.y + slope_zu * fit_pos.z); */
        /*   pv.front() = offset_v + 0.5 + (slope_yv * fit_pos.y + slope_zv * fit_pos.z)+2400; */
        /*   pw.front() = offset_w + 0.5 + (slope_yw * fit_pos.y + slope_zw * fit_pos.z)+4800; */
        /*   pt.front() = offset_t + 0.5 + slope_x * fit_pos.x; */
        /* }else{ */
        /*   pts.back() = fit_pos; */
        /*   pu.back() = offset_u + 0.5 + (slope_yu * fit_pos.y + slope_zu * fit_pos.z); */
        /*   pv.back() = offset_v + 0.5 + (slope_yv * fit_pos.y + slope_zv * fit_pos.z)+2400; */
        /*   pw.back() = offset_w + 0.5 + (slope_yw * fit_pos.y + slope_zw * fit_pos.z)+4800; */
        /*   pt.back() = offset_t + 0.5 + slope_x * fit_pos.x; */
        /* } */
    }

    // check segments
    // for (size_t i=0;i!=segments.size();i++){
    //  std::cout << segments.at(i)->get_wcpt_vec().front().index << " " << segments.at(i)->get_wcpt_vec().back().index
    //  << " " <<  vtx_new_wcp.index << std::endl;
    //}

    vtx->set_fit(fit_pos, vtx->get_dQ(), vtx->get_dx(), offset_u + 0.5 + (slope_yu * fit_pos.y + slope_zu * fit_pos.z),
                 offset_v + 0.5 + (slope_yv * fit_pos.y + slope_zv * fit_pos.z) + 2400,
                 offset_w + 0.5 + (slope_yw * fit_pos.y + slope_zw * fit_pos.z) + 4800,
                 offset_t + 0.5 + slope_x * fit_pos.x, vtx->get_reduced_chi2());
    vtx->set_wcpt(vtx_new_wcp);
    vtx->set_flag_fit_fix(true);
}

// double WCPPID::MyFCN::operator() (const std::vector<double> & xx) const{
//  return get_chi2(xx);
//}

/*
double WCPPID::MyFCN::get_chi2(const std::vector<double> & xx) const{
  double chi2 = 0;
  const int ntracks = segments.size();
  Point p(vtx->get_fit_pt().x + xx[0] ,vtx->get_fit_pt().y + xx[1] , vtx->get_fit_pt().z + xx[2] );

  std::vector<TVector3 > dir(ntracks);
  for (size_t i=0;i!=ntracks;i++){
    double theta = xx[3+2*i];
    double phi = xx[3+2*i+1];
    dir.at(i).SetXYZ(sin(theta)*cos(phi), sin(theta)*sin(phi), cos(theta));
  }


  double fit_res = 0.1*units::cm;
  for (size_t i=0;i!=ntracks;i++){
    WCP::Line l1(p, dir.at(i));
    //    double tmp_chi2 = 0;
    for (size_t j=0;j!=vec_points.at(i).size();j++){
      WCP::Point p1 =vec_points[i].at(j);
      chi2 += pow(l1.closest_dis(p1),2)/pow(fit_res,2);
    }
    // if (vec_points.at(i).size()>0)
    // chi2 += tmp_chi2/vec_points.at(i).size();

  }

  return chi2;
}
*/

TVector3 WCPPID::NeutrinoID::get_dir(WCPPID::ProtoVertex *vtx, WCPPID::ProtoSegment *sg, double dis)
{
    TVector3 results(0, 0, 0);
    if (map_vertex_segments[vtx].find(sg) != map_vertex_segments[vtx].end()) {
        std::vector<WCP::Point> &pts = sg->get_point_vec();
        double min_dis = 1e9;
        WCP::Point min_point;
        for (size_t i = 0; i != pts.size(); i++) {
            double tmp_dis =
                sqrt(pow(pts.at(i).x - vtx->get_fit_pt().x, 2) + pow(pts.at(i).y - vtx->get_fit_pt().y, 2) +
                     pow(pts.at(i).z - vtx->get_fit_pt().z, 2));
            if (fabs(tmp_dis - dis) < min_dis) {
                min_dis = fabs(tmp_dis - dis);
                min_point = pts.at(i);
            }
        }
        results.SetXYZ(min_point.x - vtx->get_fit_pt().x, min_point.y - vtx->get_fit_pt().y,
                       min_point.z - vtx->get_fit_pt().z);
        results = results.Unit();
    }
    return results;
}

bool WCPPID::NeutrinoID::search_for_vertex_activities(WCPPID::ProtoVertex *vtx, WCPPID::ProtoSegmentSet &sg_set,
                                                      WCPPID::PR3DCluster *temp_cluster, double search_range)
{
    ToyPointCloud *pcloud = temp_cluster->get_point_cloud_steiner();
    std::vector<bool> &flag_terminals = temp_cluster->get_flag_steiner_terminal();

    std::vector<TVector3> saved_dirs;
    int nshowers = 0;
    for (auto it = sg_set.begin(); it != sg_set.end(); it++) {
        TVector3 dir = get_dir(vtx, *it);
        if (dir.Mag() != 0) saved_dirs.push_back(dir);
    }
    //  std::cout << saved_dirs.size() << std::endl;

    std::vector<WCP::WCPointCloud<double>::WCPoint> candidate_wcps =
        pcloud->get_closest_wcpoints(vtx->get_fit_pt(), search_range);
    double max_dis = 0;
    WCP::WCPointCloud<double>::WCPoint max_wcp;

    for (size_t i = 0; i != candidate_wcps.size(); i++) {
        if (flag_terminals.at(candidate_wcps.at(i).index)) {
            if (candidate_wcps.at(i).index == vtx->get_wcpt().index) continue;

            double dis = sqrt(pow(candidate_wcps.at(i).x - vtx->get_fit_pt().x, 2) +
                              pow(candidate_wcps.at(i).y - vtx->get_fit_pt().y, 2) +
                              pow(candidate_wcps.at(i).z - vtx->get_fit_pt().z, 2));
            double min_dis = 1e9;
            double min_dis_u = 1e9;
            double min_dis_v = 1e9;
            double min_dis_w = 1e9;
            Point test_p(candidate_wcps.at(i).x, candidate_wcps.at(i).y, candidate_wcps.at(i).z);

            //    for (auto it = sg_set.begin(); it != sg_set.end(); it++){
            for (auto it = map_segment_vertices.begin(); it != map_segment_vertices.end(); it++) {
                WCPPID::ProtoSegment *sg = it->first;
                //	std::cout << sg->get_id() << std::endl;
                std::pair<double, WCP::Point> results = sg->get_closest_point(test_p);
                if (results.first < min_dis) min_dis = results.first;

                std::tuple<double, double, double> results_2d = sg->get_closest_2d_dis(test_p);
                if (std::get<0>(results_2d) < min_dis_u) min_dis_u = std::get<0>(results_2d);
                if (std::get<1>(results_2d) < min_dis_v) min_dis_v = std::get<1>(results_2d);
                if (std::get<2>(results_2d) < min_dis_w) min_dis_w = std::get<2>(results_2d);
            }

            // std::cout << min_dis/units::cm << " " << flag_terminals.at(candidate_wcps.at(i).index) << " " <<
            // (min_dis_u + min_dis_v + min_dis_w)/units::cm << std::endl;

            if (min_dis > 0.6 * units::cm && min_dis_u + min_dis_v + min_dis_w > 1.2 * units::cm) {
                TVector3 dir(test_p.x - vtx->get_fit_pt().x, test_p.y - vtx->get_fit_pt().y,
                             test_p.z - vtx->get_fit_pt().z);
                double sum_angle = 0;
                double min_angle = 1e9;
                for (size_t j = 0; j != saved_dirs.size(); j++) {
                    //	std::cout << dir.Angle(saved_dirs.at(j))/3.1415926*180. << std::endl;
                    double angle = dir.Angle(saved_dirs.at(j)) / 3.1415926 * 180.;
                    sum_angle += angle;
                    if (angle < min_angle) min_angle = angle;
                }
                //	std::cout << min_angle << std::endl;

                double sum_charge = 0;
                int ncount = 0;
                if (!ct_point_cloud->get_closest_dead_chs(test_p, 0)) {
                    sum_charge += ct_point_cloud->get_ave_charge(test_p, 0.3 * units::cm, 0);
                    ncount++;
                }
                if (!ct_point_cloud->get_closest_dead_chs(test_p, 1)) {
                    sum_charge += ct_point_cloud->get_ave_charge(test_p, 0.3 * units::cm, 1);
                    ncount++;
                }
                if (!ct_point_cloud->get_closest_dead_chs(test_p, 2)) {
                    sum_charge += ct_point_cloud->get_ave_charge(test_p, 0.3 * units::cm, 2);
                    ncount++;
                }
                if (ncount != 0) sum_charge /= ncount;

                //	std::cout << i << " " << dis/units::cm << " " << min_dis/units::cm << " " <<
                //flag_terminals.at(candidate_wcps.at(i).index)  << min_angle << " " << sum_angle << " " << sum_charge
                //<< " " << (sum_angle)  * (sum_charge+1e-9) << " " << min_dis_u << " " << min_dis_v << " " << min_dis_w
                //<< " " << min_dis_u + min_dis_v + min_dis_w << std::endl;

                if ((sum_angle) * (sum_charge + 1e-9) > max_dis) {
                    max_dis = (sum_angle) * (sum_charge + 1e-9);
                    max_wcp = candidate_wcps.at(i);
                }
            }
        }
    }

    // std::cout << max_dis/units::cm << std::endl;

    // 2nd round vertex finding ...
    if (max_dis == 0) {
        for (size_t i = 0; i != candidate_wcps.size(); i++) {
            if (flag_terminals.at(candidate_wcps.at(i).index)) {
                if (candidate_wcps.at(i).index == vtx->get_wcpt().index) continue;
                double dis = sqrt(pow(candidate_wcps.at(i).x - vtx->get_fit_pt().x, 2) +
                                  pow(candidate_wcps.at(i).y - vtx->get_fit_pt().y, 2) +
                                  pow(candidate_wcps.at(i).z - vtx->get_fit_pt().z, 2));
                double min_dis = 1e9;
                double min_dis_u = 1e9;
                double min_dis_v = 1e9;
                double min_dis_w = 1e9;
                Point test_p(candidate_wcps.at(i).x, candidate_wcps.at(i).y, candidate_wcps.at(i).z);

                //    for (auto it = sg_set.begin(); it != sg_set.end(); it++){
                for (auto it = map_segment_vertices.begin(); it != map_segment_vertices.end(); it++) {
                    WCPPID::ProtoSegment *sg = it->first;
                    std::pair<double, WCP::Point> results = sg->get_closest_point(test_p);
                    if (results.first < min_dis) min_dis = results.first;

                    std::tuple<double, double, double> results_2d = sg->get_closest_2d_dis(test_p);
                    if (std::get<0>(results_2d) < min_dis_u) min_dis_u = std::get<0>(results_2d);
                    if (std::get<1>(results_2d) < min_dis_v) min_dis_v = std::get<1>(results_2d);
                    if (std::get<2>(results_2d) < min_dis_w) min_dis_w = std::get<2>(results_2d);
                }

                if (min_dis > 0.36 * units::cm && min_dis_u + min_dis_v + min_dis_w > 0.8 * units::cm) {
                    double sum_charge = 0;
                    int ncount = 0;
                    if (!ct_point_cloud->get_closest_dead_chs(test_p, 0)) {
                        sum_charge += ct_point_cloud->get_ave_charge(test_p, 0.3 * units::cm, 0);
                        ncount++;
                    }
                    if (!ct_point_cloud->get_closest_dead_chs(test_p, 1)) {
                        sum_charge += ct_point_cloud->get_ave_charge(test_p, 0.3 * units::cm, 1);
                        ncount++;
                    }
                    if (!ct_point_cloud->get_closest_dead_chs(test_p, 2)) {
                        sum_charge += ct_point_cloud->get_ave_charge(test_p, 0.3 * units::cm, 2);
                        ncount++;
                    }
                    if (ncount != 0) sum_charge /= ncount;

                    // std::cout << min_dis/units::cm << " " << (min_dis_u + min_dis_v + min_dis_w)/units::cm << " " <<
                    // sum_charge << std::endl;

                    if (min_dis + (min_dis_u + min_dis_v + min_dis_w) / sqrt(3.) > max_dis && sum_charge > 20000) {
                        max_dis = min_dis + (min_dis_u + min_dis_v + min_dis_w) / sqrt(3.);
                        max_wcp = candidate_wcps.at(i);
                    }
                }
            }
        }
    }

    if (max_dis != 0) {
        // double tmp_dis = sqrt(pow(vtx->get_wcpt().x-max_wcp.x,2) + pow(vtx->get_wcpt().y-max_wcp.y,2) +
        // pow(vtx->get_wcpt().z-max_wcp.z,2));
        /* double tmp_dis = sqrt(pow(vtx->get_wcpt().x-vtx->get_fit_pt().x,2) +
         * pow(vtx->get_wcpt().y-vtx->get_fit_pt().y,2) + pow(vtx->get_wcpt().z-vtx->get_fit_pt().z,2)); */
        //    std::cout << tmp_dis/units::cm << " " << max_wcp.index << " " << vtx->get_wcpt().index << " " <<
        //    std::endl;

        WCPPID::ProtoVertex *v1 = new WCPPID::ProtoVertex(acc_vertex_id, max_wcp, temp_cluster->get_cluster_id());
        acc_vertex_id++;
        std::list<WCP::WCPointCloud<double>::WCPoint> wcp_list;
        wcp_list.push_back(vtx->get_wcpt());
        Point tmp_p((vtx->get_wcpt().x + max_wcp.x) / 2., (vtx->get_wcpt().y + max_wcp.y) / 2.,
                    (vtx->get_wcpt().z + max_wcp.z) / 2.);
        WCP::WCPointCloud<double>::WCPoint &tmp_wcp =
            temp_cluster->get_point_cloud_steiner()->get_closest_wcpoint(tmp_p);
        if (tmp_wcp.index != wcp_list.back().index) wcp_list.push_back(tmp_wcp);

        if (max_wcp.index != wcp_list.back().index) wcp_list.push_back(max_wcp);

        if (wcp_list.size() > 1) {
            std::cout << "Cluster: " << temp_cluster->get_cluster_id() << " Vertex Activity Found"
                      << " " << tmp_p << " " << acc_segment_id << " " << wcp_list.size() << " " << v1->get_wcpt().index
                      << " " << vtx->get_wcpt().index << std::endl;
            WCPPID::ProtoSegment *sg1 =
                new WCPPID::ProtoSegment(acc_segment_id, wcp_list, temp_cluster->get_cluster_id());
            acc_segment_id++;
            add_proto_connection(v1, sg1, temp_cluster);
            add_proto_connection(vtx, sg1, temp_cluster);

            return true;
        }
    }

    return false;
}
