#include "WCPPaal/graph_metrics.h"
#include "WCPPaal/steiner_tree_greedy.h"
#include "WCPPID/ImprovePR3DCluster.h"
#include "WCP2dToy/WCPHolder.h"
#include "WCPPID/CalcPoints.h"

void WCPPID::PR3DCluster::create_steiner_graph(WCP::ToyCTPointCloud& ct_point_cloud, WCPSst::GeomDataSource& gds,
                                               int nrebin, int frame_length, double unit_dis)
{
    if (graph_steiner == (MCUGraph*) 0) {
        WCP2dToy::WCPHolder* temp_holder = new WCP2dToy::WCPHolder();
        WCPPID::PR3DCluster* new_cluster =
            WCPPID::Improve_PR3DCluster_2(this, ct_point_cloud, gds, temp_holder, nrebin, frame_length, unit_dis);

        WCPPID::calc_sampling_points(gds, new_cluster, nrebin, frame_length, unit_dis, false);

        new_cluster->Create_point_cloud();
        new_cluster->Create_graph(ct_point_cloud, point_cloud);

        /* { */
        /*   MCUGraph *graph = new_cluster->get_graph(); */
        /*   std::cout << num_vertices(*graph) << " " << num_edges(*graph) << " " <<
         * new_cluster->get_point_cloud()->get_num_points() << std::endl; */
        /* } */

        new_cluster->establish_same_mcell_steiner_edges(gds, false);
        // find the shortest path
        std::pair<WCPointCloud<double>::WCPoint, WCPointCloud<double>::WCPoint> wcps =
            new_cluster->get_two_boundary_wcps();
        new_cluster->dijkstra_shortest_paths(wcps.first);
        new_cluster->cal_shortest_path(wcps.second);
        new_cluster->remove_same_mcell_steiner_edges();

        point_cloud_steiner = new ToyPointCloud();

        // steiner tree with some basic cuts ...
        graph_steiner =
            new_cluster->Create_steiner_tree(point_cloud_steiner, flag_steiner_terminal, gds, mcells, true, false);
        establish_same_mcell_steiner_edges(gds, true, 2);

        new_cluster->Del_graph();
        new_cluster->Del_point_cloud();

        delete new_cluster;
        delete temp_holder;

        point_cloud_steiner_terminal = new ToyPointCloud();
        WCP::WCPointCloud<double>& cloud = point_cloud_steiner->get_cloud();
        WCP::WC2DPointCloud<double>& cloud_u = point_cloud_steiner->get_cloud_u();
        WCP::WC2DPointCloud<double>& cloud_v = point_cloud_steiner->get_cloud_v();
        WCP::WC2DPointCloud<double>& cloud_w = point_cloud_steiner->get_cloud_w();
        for (size_t i = 0; i != flag_steiner_terminal.size(); i++) {
            if (flag_steiner_terminal[i]) {
                point_cloud_steiner_terminal->AddPoint(cloud.pts[i], cloud_u.pts[i], cloud_v.pts[i], cloud_w.pts[i]);
            }
        }

        // examine steiner tree terminals

        // return new_cluster;
    }
}

void WCPPID::PR3DCluster::recover_steiner_graph()
{
    // std::cout << "haha " << graph_steiner << std::endl;
    // holder for more sophisticated algorithm later ...
    if (graph_steiner != (MCUGraph*) 0) {
        steiner_graph_terminal_indices.clear();
        std::vector<int> terminals;
        for (size_t i = 0; i != flag_steiner_terminal.size(); i++) {
            if (flag_steiner_terminal[i]) {
                terminals.push_back(i);
                steiner_graph_terminal_indices.insert(i);
            }
        }
        const int N = point_cloud_steiner->get_num_points();
        using Vertex = typename boost::graph_traits<WCPPID::MCUGraph>::vertex_descriptor;
        using Edge = typename boost::graph_traits<WCPPID::MCUGraph>::edge_descriptor;
        using Base = typename boost::property<edge_base_t, Edge>;
        using EdgeWeightMap = typename boost::property_map<WCPPID::MCUGraph, boost::edge_weight_t>::type;
        using Weight = typename boost::property_traits<EdgeWeightMap>::value_type;
        using WeightProperty = typename boost::property<boost::edge_weight_t, Weight, Base>;
        using TerminalGraph =
            boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, WeightProperty>;
        using EdgeTerminal = typename boost::graph_traits<TerminalGraph>::edge_descriptor;

        EdgeWeightMap edge_weight = boost::choose_pmap(
            boost::get_param(boost::no_named_parameters(), boost::edge_weight), *graph_steiner, boost::edge_weight);

        // distance array used in the dijkstra runs
        std::vector<Weight> distance(N);

        std::vector<Vertex> nearest_terminal(num_vertices(*graph_steiner));
        auto index = get(boost::vertex_index, *graph_steiner);
        auto nearest_terminal_map =
            boost::make_iterator_property_map(nearest_terminal.begin(), get(boost::vertex_index, *graph_steiner));
        for (auto terminal : terminals) {
            nearest_terminal_map[terminal] = terminal;
        }
        // compute voronoi diagram each vertex get nearest terminal and last edge on
        // path to nearest terminal
        auto distance_map = make_iterator_property_map(distance.begin(), index);
        std::vector<Edge> vpred(N);
        auto last_edge = boost::make_iterator_property_map(vpred.begin(), get(boost::vertex_index, *graph_steiner));
        boost::dijkstra_shortest_paths(*graph_steiner, terminals.begin(), terminals.end(), boost::dummy_property_map(),
                                       distance_map, edge_weight, index, paal::utils::less(),
                                       boost::closed_plus<Weight>(), std::numeric_limits<Weight>::max(), 0,
                                       boost::make_dijkstra_visitor(paal::detail::make_nearest_recorder(
                                           nearest_terminal_map, last_edge, boost::on_edge_relaxed{})));

        // computing distances between terminals
        // creating terminal_graph
        TerminalGraph terminal_graph(N);
        std::map<std::pair<int, int>, std::pair<Weight, Edge>> map_saved_edge;

        for (auto w : boost::as_array(edges(*graph_steiner))) {
            auto const& nearest_to_source = nearest_terminal_map[source(w, *graph_steiner)];
            auto const& nearest_to_target = nearest_terminal_map[target(w, *graph_steiner)];
            if (nearest_to_source != nearest_to_target) {
                Weight temp_weight =
                    distance[source(w, *graph_steiner)] + distance[target(w, *graph_steiner)] + edge_weight[w];
                if (map_saved_edge.find(std::make_pair(nearest_to_source, nearest_to_target)) != map_saved_edge.end()) {
                    if (temp_weight < map_saved_edge[std::make_pair(nearest_to_source, nearest_to_target)].first)
                        map_saved_edge[std::make_pair(nearest_to_source, nearest_to_target)] =
                            std::make_pair(temp_weight, w);
                }
                else if (map_saved_edge.find(std::make_pair(nearest_to_target, nearest_to_source)) !=
                         map_saved_edge.end()) {
                    if (temp_weight < map_saved_edge[std::make_pair(nearest_to_target, nearest_to_source)].first)
                        map_saved_edge[std::make_pair(nearest_to_target, nearest_to_source)] =
                            std::make_pair(temp_weight, w);
                }
                else {
                    map_saved_edge[std::make_pair(nearest_to_source, nearest_to_target)] =
                        std::make_pair(temp_weight, w);
                }
            }
        }

        std::vector<Edge> terminal_edge;
        for (auto it = map_saved_edge.begin(); it != map_saved_edge.end(); it++) {
            std::pair<Edge, bool> p =
                add_edge(it->first.first, it->first.second, WeightProperty(it->second.first, Base(it->second.second)),
                         terminal_graph);
            //      terminal_edge.push_back(p.first);
        }
        // minimal spanning tree ...
        boost::kruskal_minimum_spanning_tree(terminal_graph, std::back_inserter(terminal_edge));

        // computing result
        std::vector<Edge> tree_edges;
        for (auto edge : terminal_edge) {
            auto base = get(edge_base, terminal_graph, edge);
            tree_edges.push_back(base);
            for (auto pom : {source(base, *graph), target(base, *graph)}) {
                while (nearest_terminal_map[pom] != pom) {
                    tree_edges.push_back(vpred[pom]);
                    pom = source(vpred[pom], *graph);
                }
            }
        }
        boost::sort(tree_edges);
        auto unique_edges = boost::unique(tree_edges);

        steiner_graph_selected_terminal_indices.clear();
        for (auto e : unique_edges) {
            steiner_graph_selected_terminal_indices.insert(index[source(e, *graph)]);
            steiner_graph_selected_terminal_indices.insert(index[target(e, *graph)]);
        }
    }
}

WCPPID::MCUGraph* WCPPID::PR3DCluster::Create_steiner_tree(WCP::ToyPointCloud* point_cloud_steiner,
                                                           std::vector<bool>& flag_steiner_terminal,
                                                           WCP::GeomDataSource& gds, WCP::SMGCSelection& old_mcells,
                                                           bool flag_path, bool disable_dead_mix_cell)
{
    Create_graph();

    // find all the steiner terminal indices ...
    find_steiner_terminals(gds, disable_dead_mix_cell);

    // form point cloud
    WCP::ToyPointCloud temp_pcloud;
    if (flag_path && path_wcps.size() > 0) {
        WCP::Point prev_p((*(path_wcps.begin())).x, (*(path_wcps.begin())).y, (*(path_wcps.begin())).z);
        std::tuple<int, int, int> prev_wire_index = std::make_tuple(
            (*(path_wcps.begin())).index_u, (*(path_wcps.begin())).index_v, (*(path_wcps.begin())).index_w);

        for (auto it = path_wcps.begin(); it != path_wcps.end(); it++) {
            if (temp_pcloud.get_num_points() == 0) {
                WCP::Point p((*it).x, (*it).y, (*it).z);
                std::tuple<int, int, int> wire_index = std::make_tuple((*it).index_u, (*it).index_v, (*it).index_w);
                temp_pcloud.AddPoint(p, wire_index, 0);
            }
            else {
                WCP::Point p((*it).x, (*it).y, (*it).z);
                std::tuple<int, int, int> wire_index = std::make_tuple((*it).index_u, (*it).index_v, (*it).index_w);

                float step_dis = 0.6 * units::cm;
                float dis = sqrt(pow(p.x - prev_p.x, 2) + pow(p.y - prev_p.y, 2) + pow(p.z - prev_p.z, 2));

                if (dis <= step_dis) {
                    temp_pcloud.AddPoint(p, wire_index, 0);
                    prev_p = p;
                    prev_wire_index = wire_index;
                }
                else {
                    int num_steps = dis / step_dis;
                    for (int qx = 0; qx != num_steps; qx++) {
                        WCP::Point temp_p(prev_p.x + (p.x - prev_p.x) / num_steps * (qx + 1),
                                          prev_p.y + (p.y - prev_p.y) / num_steps * (qx + 1),
                                          prev_p.z + (p.z - prev_p.z) / num_steps * (qx + 1));
                        std::tuple<int, int, int> temp_wire_index = std::make_tuple(
                            int(std::get<0>(prev_wire_index) +
                                (std::get<0>(wire_index) - std::get<0>(prev_wire_index)) * (qx + 1.0) / num_steps),
                            int(std::get<1>(prev_wire_index) +
                                (std::get<1>(wire_index) - std::get<1>(prev_wire_index)) * (qx + 1.0) / num_steps),
                            int(std::get<2>(prev_wire_index) +
                                (std::get<2>(wire_index) - std::get<2>(prev_wire_index)) * (qx + 1.0) / num_steps));
                        temp_pcloud.AddPoint(temp_p, temp_wire_index, 0);
                    }
                    temp_pcloud.AddPoint(p, wire_index, 0);
                    prev_p = p;
                    prev_wire_index = wire_index;
                }
            }
        }
        temp_pcloud.build_kdtree_index();
    }
    // organize mcells
    std::map<int, SMGCSelection> old_time_mcells_map;
    for (auto it = old_mcells.begin(); it != old_mcells.end(); it++) {
        SlimMergeGeomCell* mcell = (*it);
        int time_slice = mcell->GetTimeSlice();
        if (old_time_mcells_map.find(time_slice) == old_time_mcells_map.end()) {
            SMGCSelection mcells;
            mcells.push_back(mcell);
            old_time_mcells_map[time_slice] = mcells;
        }
        else {
            old_time_mcells_map[time_slice].push_back(mcell);
        }
    }

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

    std::set<int> indices_to_be_removal;

    for (auto it = steiner_terminal_indices.begin(); it != steiner_terminal_indices.end(); it++) {
        // examine the steiner terminals according to the mcells, inside known mcells
        int time_slice = cloud.pts[*it].mcell->GetTimeSlice();
        bool flag_remove = true;

        //    if (cloud.pts[*it].y/units::cm >-100 && cloud.pts[*it].y/units::cm<-50)
        // std::cout << cloud.pts[*it].x/units::cm << " " <<
        //	cloud.pts[*it].y/units::cm << " " <<
        //	cloud.pts[*it].z/units::cm << " " << std::endl;

        if (old_time_mcells_map.find(time_slice) != old_time_mcells_map.end()) {
            for (auto it1 = old_time_mcells_map[time_slice].begin(); it1 != old_time_mcells_map[time_slice].end();
                 it1++) {
                SlimMergeGeomCell* mcell = *it1;
                int u1_low_index = mcell->get_uwires().front()->index();
                int u1_high_index = mcell->get_uwires().back()->index();

                int v1_low_index = mcell->get_vwires().front()->index();
                int v1_high_index = mcell->get_vwires().back()->index();

                int w1_low_index = mcell->get_wwires().front()->index();
                int w1_high_index = mcell->get_wwires().back()->index();

                /* if (cloud.pts[*it].y/units::cm <-100) */
                /*   std::cout << cloud.pts[*it].index_u << " " << u1_low_index << " " << u1_high_index << ", " */
                /* 	      << cloud.pts[*it].index_v << " " << v1_low_index << " " << v1_high_index << ", " */
                /* 	      << cloud.pts[*it].index_w << " " << w1_low_index << " " << w1_high_index << ", " <<
                 * std::endl; */

                if (cloud.pts[*it].index_u <= u1_high_index + 1 && cloud.pts[*it].index_u >= u1_low_index - 1 &&
                    cloud.pts[*it].index_v <= v1_high_index + 1 && cloud.pts[*it].index_v >= v1_low_index - 1 &&
                    cloud.pts[*it].index_w <= w1_high_index + 1 && cloud.pts[*it].index_w >= w1_low_index - 1) {
                    flag_remove = false;
                    break;
                }
            }
        }

        if (flag_remove) {
            //+1 time slice
            if (old_time_mcells_map.find(time_slice + 1) != old_time_mcells_map.end()) {
                for (auto it1 = old_time_mcells_map[time_slice + 1].begin();
                     it1 != old_time_mcells_map[time_slice + 1].end(); it1++) {
                    SlimMergeGeomCell* mcell = *it1;
                    int u1_low_index = mcell->get_uwires().front()->index();
                    int u1_high_index = mcell->get_uwires().back()->index();

                    int v1_low_index = mcell->get_vwires().front()->index();
                    int v1_high_index = mcell->get_vwires().back()->index();

                    int w1_low_index = mcell->get_wwires().front()->index();
                    int w1_high_index = mcell->get_wwires().back()->index();
                    if (cloud.pts[*it].index_u <= u1_high_index + 1 && cloud.pts[*it].index_u >= u1_low_index - 1 &&
                        cloud.pts[*it].index_v <= v1_high_index + 1 && cloud.pts[*it].index_v >= v1_low_index - 1 &&
                        cloud.pts[*it].index_w <= w1_high_index + 1 && cloud.pts[*it].index_w >= w1_low_index - 1) {
                        flag_remove = false;
                        break;
                    }
                }
            }
        }

        if (flag_remove) {
            //-1
            if (old_time_mcells_map.find(time_slice - 1) != old_time_mcells_map.end()) {
                for (auto it1 = old_time_mcells_map[time_slice - 1].begin();
                     it1 != old_time_mcells_map[time_slice - 1].end(); it1++) {
                    SlimMergeGeomCell* mcell = *it1;
                    int u1_low_index = mcell->get_uwires().front()->index();
                    int u1_high_index = mcell->get_uwires().back()->index();

                    int v1_low_index = mcell->get_vwires().front()->index();
                    int v1_high_index = mcell->get_vwires().back()->index();

                    int w1_low_index = mcell->get_wwires().front()->index();
                    int w1_high_index = mcell->get_wwires().back()->index();
                    if (cloud.pts[*it].index_u <= u1_high_index + 1 && cloud.pts[*it].index_u >= u1_low_index - 1 &&
                        cloud.pts[*it].index_v <= v1_high_index + 1 && cloud.pts[*it].index_v >= v1_low_index - 1 &&
                        cloud.pts[*it].index_w <= w1_high_index + 1 && cloud.pts[*it].index_w >= w1_low_index - 1) {
                        flag_remove = false;
                        break;
                    }
                }
            }
        }

        // within a mcell check path ...
        if ((!flag_remove) && flag_path) {
            // examine the steiner terminals according to the path,

            // if within cerntain distance in projection, but far away in 3D, remove ...
            WCP::Point p(cloud.pts[*it].x, cloud.pts[*it].y, cloud.pts[*it].z);
            double dis_3d = temp_pcloud.get_closest_dis(p);
            double dis_2du = (temp_pcloud.get_closest_2d_dis(p, 0)).second;
            double dis_2dv = (temp_pcloud.get_closest_2d_dis(p, 1)).second;
            double dis_2dw = (temp_pcloud.get_closest_2d_dis(p, 2)).second;

            /* if(cluster_id == 2){ */

            //}

            if ((dis_2du < 1.8 * units::cm && dis_2dv < 1.8 * units::cm ||
                 dis_2du < 1.8 * units::cm && dis_2dw < 1.8 * units::cm ||
                 dis_2dv < 1.8 * units::cm && dis_2dw < 1.8 * units::cm) &&
                dis_3d > 6 * units::cm)
                flag_remove = true;
        }

        /* if (flag_remove) */
        /*   std::cout << cloud.pts[*it].x/units::cm << " " <<cloud.pts[*it].y/units::cm << " " <<
         * cloud.pts[*it].z/units::cm<< " " << flag_remove << " " << old_time_mcells_map[time_slice].size() <<
         * std::endl; */

        if (flag_remove) {
            indices_to_be_removal.insert(*it);
        }
    }

    // std::cout << indices_to_be_removal.size() << std::endl;

    for (auto it = indices_to_be_removal.begin(); it != indices_to_be_removal.end(); it++) {
        steiner_terminal_indices.erase(*it);
    }

    // figure out the extreme points ...
    std::vector<std::vector<WCPointCloud<double>::WCPoint>> extreme_wcps = get_extreme_wcps(1, &old_time_mcells_map);
    for (size_t i = 0; i != extreme_wcps.size(); i++) {
        for (size_t j = 0; j != extreme_wcps.at(i).size(); j++) {
            if (steiner_terminal_indices.find(extreme_wcps.at(i).at(j).index) == steiner_terminal_indices.end()) {
                steiner_terminal_indices.insert(extreme_wcps.at(i).at(j).index);
                // terminals.push_back(extreme_wcps.at(i).at(j).index);
                /* std::cout << extreme_wcps.at(i).at(j).x/units::cm << " " << */
                /*   extreme_wcps.at(i).at(j).y/units::cm << " " << */
                /*   extreme_wcps.at(i).at(j).z/units::cm << " " << std::endl; */
            }
        }
    }

    // form the tree ...
    std::vector<int> terminals(steiner_terminal_indices.begin(), steiner_terminal_indices.end());

    const int N = point_cloud->get_num_points();
    /* std::vector<int> nonterminals; */
    /* for (int i=0;i!=N;i++){ */
    /*   if (steiner_terminal_indices.find(i)==steiner_terminal_indices.end()) */
    /*     nonterminals.push_back(i); */
    /* } */
    // std::cout << N << " " << terminals.size() + nonterminals.size() << std::endl;

    // Now try to manually write the Steiner Tree Greedy algorithm ...

    using Vertex = typename boost::graph_traits<WCPPID::MCUGraph>::vertex_descriptor;
    using Edge = typename boost::graph_traits<WCPPID::MCUGraph>::edge_descriptor;
    using Base = typename boost::property<edge_base_t, Edge>;
    // using EdgeWeightMap = typename boost::choose_const_pmap(boost::get_param(boost::no_named_parameters(),
    // boost::edge_weight), *graph, boost::edge_weight);
    using EdgeWeightMap = typename boost::property_map<WCPPID::MCUGraph, boost::edge_weight_t>::type;
    using Weight = typename boost::property_traits<EdgeWeightMap>::value_type;
    using WeightProperty = typename boost::property<boost::edge_weight_t, Weight, Base>;
    using TerminalGraph =
        boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, WeightProperty>;
    using EdgeTerminal = typename boost::graph_traits<TerminalGraph>::edge_descriptor;

    EdgeWeightMap edge_weight = boost::choose_pmap(boost::get_param(boost::no_named_parameters(), boost::edge_weight),
                                                   *graph, boost::edge_weight);

    // distance array used in the dijkstra runs
    std::vector<Weight> distance(N);

    std::vector<Vertex> nearest_terminal(num_vertices(*graph));
    auto index = get(boost::vertex_index, *graph);
    auto nearest_terminal_map =
        boost::make_iterator_property_map(nearest_terminal.begin(), get(boost::vertex_index, *graph));
    for (auto terminal : terminals) {
        nearest_terminal_map[terminal] = terminal;
    }
    // compute voronoi diagram each vertex get nearest terminal and last edge on
    // path to nearest terminal
    auto distance_map = make_iterator_property_map(distance.begin(), index);
    std::vector<Edge> vpred(N);
    auto last_edge = boost::make_iterator_property_map(vpred.begin(), get(boost::vertex_index, *graph));
    boost::dijkstra_shortest_paths(*graph, terminals.begin(), terminals.end(), boost::dummy_property_map(),
                                   distance_map, edge_weight, index, paal::utils::less(), boost::closed_plus<Weight>(),
                                   std::numeric_limits<Weight>::max(), 0,
                                   boost::make_dijkstra_visitor(paal::detail::make_nearest_recorder(
                                       nearest_terminal_map, last_edge, boost::on_edge_relaxed{})));

    // computing distances between terminals
    // creating terminal_graph
    TerminalGraph terminal_graph(N);
    std::map<std::pair<int, int>, std::pair<Weight, Edge>> map_saved_edge;

    for (auto w : boost::as_array(edges(*graph))) {
        auto const& nearest_to_source = nearest_terminal_map[source(w, *graph)];
        auto const& nearest_to_target = nearest_terminal_map[target(w, *graph)];
        if (nearest_to_source != nearest_to_target) {
            Weight temp_weight = distance[source(w, *graph)] + distance[target(w, *graph)] + edge_weight[w];
            if (map_saved_edge.find(std::make_pair(nearest_to_source, nearest_to_target)) != map_saved_edge.end()) {
                if (temp_weight < map_saved_edge[std::make_pair(nearest_to_source, nearest_to_target)].first)
                    map_saved_edge[std::make_pair(nearest_to_source, nearest_to_target)] =
                        std::make_pair(temp_weight, w);
            }
            else if (map_saved_edge.find(std::make_pair(nearest_to_target, nearest_to_source)) !=
                     map_saved_edge.end()) {
                if (temp_weight < map_saved_edge[std::make_pair(nearest_to_target, nearest_to_source)].first)
                    map_saved_edge[std::make_pair(nearest_to_target, nearest_to_source)] =
                        std::make_pair(temp_weight, w);
            }
            else {
                map_saved_edge[std::make_pair(nearest_to_source, nearest_to_target)] = std::make_pair(temp_weight, w);
            }
        }
    }

    std::vector<Edge> terminal_edge;
    for (auto it = map_saved_edge.begin(); it != map_saved_edge.end(); it++) {
        std::pair<Edge, bool> p = add_edge(it->first.first, it->first.second,
                                           WeightProperty(it->second.first, Base(it->second.second)), terminal_graph);
        terminal_edge.push_back(p.first);
    }
    // minimal spanning tree ...
    //  boost::kruskal_minimum_spanning_tree(terminal_graph,
    //				       std::back_inserter(terminal_edge));

    // computing result
    std::vector<Edge> tree_edges;
    for (auto edge : terminal_edge) {
        auto base = get(edge_base, terminal_graph, edge);
        tree_edges.push_back(base);
        for (auto pom : {source(base, *graph), target(base, *graph)}) {
            while (nearest_terminal_map[pom] != pom) {
                tree_edges.push_back(vpred[pom]);
                pom = source(vpred[pom], *graph);
            }
        }
    }
    boost::sort(tree_edges);
    auto unique_edges = boost::unique(tree_edges);

    selected_terminal_indices.clear();
    std::map<int, float> map_index_charge;
    for (auto e : unique_edges) {
        if (map_index_charge.find(index[source(e, *graph)]) == map_index_charge.end()) {
            selected_terminal_indices.insert(index[source(e, *graph)]);
            WCPointCloud<double>::WCPoint& wcp = cloud.pts[index[source(e, *graph)]];
            std::pair<bool, double> temp_charge = calc_charge_wcp(wcp, gds, disable_dead_mix_cell);
            map_index_charge[index[source(e, *graph)]] = temp_charge.second;
            // std::cout << index[source(e,*graph)] << " " << temp_charge.second << std::endl;
        }
        if (map_index_charge.find(index[target(e, *graph)]) == map_index_charge.end()) {
            selected_terminal_indices.insert(index[target(e, *graph)]);
            WCPointCloud<double>::WCPoint& wcp = cloud.pts[index[target(e, *graph)]];
            std::pair<bool, double> temp_charge = calc_charge_wcp(wcp, gds, disable_dead_mix_cell);
            map_index_charge[index[target(e, *graph)]] = temp_charge.second;
            // std::cout << index[target(e,*graph)] << " " << temp_charge.second << std::endl;
        }
    }

    // STG try ...

    // fill the data for point_cloud_steiner, also the flag
    // terminal first, and then non-terminals ...

    std::map<int, int> map_old_new_indices;
    std::map<int, int> map_new_old_indices;

    for (auto it = selected_terminal_indices.begin(); it != selected_terminal_indices.end(); it++) {
        int index = *it;
        WCP::SlimMergeGeomCell* mcell = 0;

        int time_slice = cloud.pts[index].mcell->GetTimeSlice();
        if (old_time_mcells_map.find(time_slice) != old_time_mcells_map.end()) {
            for (auto it1 = old_time_mcells_map[time_slice].begin(); it1 != old_time_mcells_map[time_slice].end();
                 it1++) {
                SlimMergeGeomCell* mcell1 = *it1;
                int u1_low_index = mcell1->get_uwires().front()->index();
                int u1_high_index = mcell1->get_uwires().back()->index();

                int v1_low_index = mcell1->get_vwires().front()->index();
                int v1_high_index = mcell1->get_vwires().back()->index();

                int w1_low_index = mcell1->get_wwires().front()->index();
                int w1_high_index = mcell1->get_wwires().back()->index();
                if (cloud.pts[index].index_u <= u1_high_index && cloud.pts[index].index_u >= u1_low_index &&
                    cloud.pts[index].index_v <= v1_high_index && cloud.pts[index].index_v >= v1_low_index &&
                    cloud.pts[index].index_w <= w1_high_index && cloud.pts[index].index_w >= w1_low_index) {
                    mcell = mcell1;
                    break;
                }
            }
        }

        Point p(cloud.pts[index].x, cloud.pts[index].y, cloud.pts[index].z);
        /* if (p.z>30*units::cm) */
        /*   std::cout << p.x/units::cm << " " << p.y/units::cm << " " << p.z/units::cm << " " << mcell <<  std::endl;
         */

        std::tuple<int, int, int> temp_indices =
            std::make_tuple(cloud.pts[index].index_u, cloud.pts[index].index_v, cloud.pts[index].index_w);
        point_cloud_steiner->AddPoint(p, temp_indices, mcell);
        // std::cout << index << " " << cloud.pts[*it].index << std::endl;
        map_old_new_indices[index] = flag_steiner_terminal.size();
        map_new_old_indices[flag_steiner_terminal.size()] = index;

        if (steiner_terminal_indices.find(index) == steiner_terminal_indices.end()) {
            flag_steiner_terminal.push_back(false);
        }
        else {
            flag_steiner_terminal.push_back(true);
        }
    }
    point_cloud_steiner->build_kdtree_index();
    // std::cout << point_cloud_steiner->get_num_points() << " " << flag_steiner_terminal.size() << std::endl;

    // fill the graph ...
    MCUGraph* graph_steiner = new MCUGraph(flag_steiner_terminal.size());
    for (auto e : unique_edges) {
        int index1 = map_old_new_indices[index[source(e, *graph)]];
        int index2 = map_old_new_indices[index[target(e, *graph)]];
        float dis = get(boost::edge_weight_t(), *graph, e);

        float Q0 = 10000;  // constant term ...
        float Qs = map_index_charge[index[source(e, *graph)]];
        float Qt = map_index_charge[index[target(e, *graph)]];
        float factor1 = 0.8, factor2 = 0.4;

        /* int nsteiner = 0; */
        /* if (steiner_terminal_indices.find(index[source(e,*graph)])!=steiner_terminal_indices.end()) */
        /*   nsteiner ++; */
        /* if (steiner_terminal_indices.find(index[target(e,*graph)])!=steiner_terminal_indices.end()) */
        /*   nsteiner ++; */
        /* if (nsteiner==1){ */
        /*   factor1 = 0.5; */
        /*   factor2 = 0.5; */
        /* }else if (nsteiner==2){ */
        /*   factor1 = 0.25; */
        /*   factor2 = 0.75; */
        /* } */

        float temp_dis = dis * (factor1 + factor2 * (0.5 * Q0 / (Qs + Q0) + 0.5 * Q0 / (Qt + Q0)));
        //    if (Qs>0 || Qt>0)
        //  std::cout << Qs << " " << Qt << std::endl;

        // std::cout << dis << std::endl;
        auto edge = add_edge(index1, index2, temp_dis, *graph_steiner);
    }

    return graph_steiner;

    /* auto index = get(boost::vertex_index, *graph); */
    /* typedef boost::graph_traits<WCPPID::MCUGraph>::edge_descriptor Edge;  */
    /* std::set<Edge> steinerEdges;  */
    /* std::vector<int> color(terminals.size()+nonterminals.size()); */
    /* { */
    /*   auto c = &color[0]; */
    /*   for (size_t i=0;i!=terminals.size();i++){ */
    /*     put(c, terminals.at(i),paal::Terminals::TERMINAL); */
    /*   } */
    /*   for (size_t i=0;i!=nonterminals.size();i++){ */
    /*     put(c, nonterminals.at(i), paal::Terminals::NONTERMINAL); */
    /*   } */
    /* } */

    /* paal::steiner_tree_greedy(*graph, std::inserter(steinerEdges, steinerEdges.begin()), */
    /* 			    boost::vertex_color_map(boost::make_iterator_property_map(color.begin(),index))); */
    /* auto weight = get(boost::edge_weight, *graph); */
    /* auto sum = 0; */
    /* selected_terminal_indices.clear(); */
    /* for (auto e : steinerEdges){ */
    /*   //sum += get(weight,e); */
    /*   selected_terminal_indices.insert(index[source(e,*graph)]); */
    /*   selected_terminal_indices.insert(index[target(e,*graph)]); */
    /*   //std::cout << index[source(e,*graph)] << " " << index[target(e,*graph)] << std::endl; */
    /* } */

    //  std::cout << terminals.size() << " " << selected_terminal_indices.size() << std::endl;
    //  std::cout << "result " << sum/units::cm << std::endl;

    // too slow ...
    /* using GraphMT = paal::data_structures::graph_metric<WCPPID::MCUGraph, float,
     * paal::data_structures::graph_type::sparse_tag>; */
    /* auto metric = GraphMT(*graph); */
    // solve it
    /* paal::ir::steiner_tree_iterative_rounding(metric, terminals, */
    /* 					    nonterminals, std::back_inserter(selected_nonterminals)); */
}

void WCPPID::PR3DCluster::find_steiner_terminals(WCP::GeomDataSource& gds, bool disable_dead_mix_cell)
{
    // reset ...
    steiner_terminal_indices.clear();

    // form all the maps ...
    form_cell_points_map();

    // double sum = 0,sum1=0;
    for (size_t i = 0; i != mcells.size(); i++) {
        SMGCSelection temp_mcells;
        temp_mcells.push_back(mcells.at(i));
        std::set<int> indices = find_peak_point_indices(temp_mcells, gds, disable_dead_mix_cell);
        steiner_terminal_indices.insert(indices.begin(), indices.end());
        /* sum += indices.size(); */
        /* sum1 += mcells.at(i)->get_sampling_points().size(); */
    }
    // std::cout << sum << " " << sum/sum1 << std::endl;
    // std::cout << steiner_terminal_indices.size() << std::endl;
}

std::set<int> WCPPID::PR3DCluster::find_peak_point_indices(SMGCSelection mcells, WCP::GeomDataSource& gds,
                                                           bool disable_dead_mix_cell, int nlevel)
{
    std::set<int> all_indices;
    for (auto it = mcells.begin(); it != mcells.end(); it++) {
        SlimMergeGeomCell* mcell = (*it);
        std::set<int>& indices = cell_point_indices_map[mcell];
        all_indices.insert(indices.begin(), indices.end());
    }

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

    // form another set with the actual points according to their charge
    std::map<int, double> map_index_charge;
    std::set<std::pair<double, int>, std::greater<std::pair<double, int>>> candidates_set;
    for (auto it = all_indices.begin(); it != all_indices.end(); it++) {
        // std::cout << all_indices.size() << std::endl;
        WCPointCloud<double>::WCPoint& wcp = cloud.pts[(*it)];
        std::pair<bool, double> temp_charge = calc_charge_wcp(wcp, gds, disable_dead_mix_cell);

        //    bool flag_good_point = wcp.mcell->IsPointGood(wcp.index_u, wcp.index_v, wcp.index_w,2);

        double charge = temp_charge.second;
        map_index_charge[(*it)] = charge;

        /* if ( cloud.pts[(*it)].y<-80*units::cm) */
        /*   std::cout << cloud.pts[(*it)].y/units::cm << " " << cloud.pts[(*it)].z/units::cm << " " << charge << " " <<
         * temp_charge.first << std::endl; */

        if ((charge > 4000) && temp_charge.first) {
            // if ((charge > 4000 || charge == 0 && (!disable_dead_mix_cell) ) && temp_charge.first){
            candidates_set.insert(std::make_pair(charge, *it));
        }
        // std::cout <<  << std::endl;
    }

    // std::cout << candidates_set.size() << std::endl;
    std::set<int> peak_indices;
    std::set<int> non_peak_indices;

    typedef boost::property_map<MCUGraph, boost::vertex_index_t>::type IndexMap;
    IndexMap index = get(boost::vertex_index, *graph);
    typedef boost::graph_traits<MCUGraph>::adjacency_iterator adjacency_iterator;

    for (auto it = candidates_set.begin(); it != candidates_set.end(); it++) {
        int current_index = it->second;
        double current_charge = it->first;

        //    if (fabs(cloud.pts[current_index].x/units::cm -126.175)<0.1)
        // std::cout << cloud.pts[current_index].x/units::cm << " A " << cloud.pts[current_index].y/units::cm << " " <<
        // cloud.pts[current_index].z/units::cm << " " << current_charge << std::endl;

        std::set<int> total_vertices_found;
        total_vertices_found.insert(current_index);
        {
            std::set<int> vertices_to_be_examined;
            vertices_to_be_examined.insert(current_index);
            for (int j = 0; j != nlevel; j++) {
                std::set<int> vertices_saved_for_next;
                for (auto it = vertices_to_be_examined.begin(); it != vertices_to_be_examined.end(); it++) {
                    int temp_current_index = (*it);
                    std::pair<adjacency_iterator, adjacency_iterator> neighbors =
                        boost::adjacent_vertices(vertex(temp_current_index, *graph), *graph);
                    for (; neighbors.first != neighbors.second; ++neighbors.first) {
                        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;
            }
            total_vertices_found.erase(current_index);
        }
        // std::cout << total_vertices_found.size() << std::endl;

        // find the vertices with the point
        // std::pair<adjacency_iterator, adjacency_iterator> neighbors =
        // boost::adjacent_vertices(vertex(current_index,*graph),*graph);

        if (peak_indices.size() == 0) {
            // if the current's charge is the biggest, push into good list
            peak_indices.insert(current_index);
            for (auto it = total_vertices_found.begin(); it != total_vertices_found.end(); it++) {
                //      for (; neighbors.first!=neighbors.second; ++neighbors.first){
                // std::cout << *neighbors.first << " " << *neighbors.second << std::endl;
                if (map_index_charge.find(*it) == map_index_charge.end()) continue;
                // if charge smaller, push into dead list
                if (current_charge > map_index_charge[*it]) non_peak_indices.insert(*it);
            }
        }
        else {
            if (peak_indices.find(current_index) != peak_indices.end() ||
                non_peak_indices.find(current_index) != non_peak_indices.end())
                continue;
            bool flag_insert = true;
            // if charge bigger, push current into dead list
            // loop over the connected vertices (not in the dead or good list)
            //      for (; neighbors.first!=neighbors.second; ++neighbors.first){
            for (auto it = total_vertices_found.begin(); it != total_vertices_found.end(); it++) {
                if (map_index_charge.find(*it) == map_index_charge.end()) continue;
                if (current_charge > map_index_charge[*it]) {
                    non_peak_indices.insert(*it);
                }
                else if (current_charge < map_index_charge[*it]) {
                    flag_insert = false;
                    break;
                }
            }

            /* if (cloud.pts[current_index].y/units::cm < -100) */
            /* std::cout << cloud.pts[current_index].x/units::cm << " " << cloud.pts[current_index].y/units::cm << " "
             * << cloud.pts[current_index].z/units::cm << " " << current_charge << " " << flag_insert << std::endl; */

            if (flag_insert) peak_indices.insert(current_index);
        }
    }

    /*  std::cout << peak_indices.size() << " " << non_peak_indices.size() << " " << candidates_set.size() << " " <<
     * all_indices.size() << std::endl; */
    /* for (auto it = peak_indices.begin(); it!=peak_indices.end(); it++){ */
    /*   int current_index = *it; */
    /*   std::cout << "Peak: " << current_index << " " << map_index_charge[current_index] << std::endl; */
    /*   std::pair<adjacency_iterator, adjacency_iterator> neighbors =
     * boost::adjacent_vertices(vertex(current_index,*graph),*graph); */
    /*   for (; neighbors.first!=neighbors.second; ++neighbors.first){ */
    /*     if (map_index_charge.find(*neighbors.first)==map_index_charge.end()) continue; */
    /*     std::cout << *neighbors.first << " " << map_index_charge[*neighbors.first] << std::endl; */
    /*   } */
    /* } */

    /* for (auto it = peak_indices.begin();  it!= peak_indices.end(); it++){  */
    /*   if (fabs(cloud.pts[(*it)].x/units::cm -126.175)<0.1)  */
    /*     // if (cloud.pts[(*it)].y/units::cm > -90 && cloud.pts[(*it)].y/units::cm < -70 && */
    /*     // 	cloud.pts[(*it)].z/units::cm > 490 && cloud.pts[(*it)].z/units::cm < 510) */
    /*     std::cout <<  cloud.pts[(*it)].x/units::cm << " " << cloud.pts[(*it)].y/units::cm << " " <<
     * cloud.pts[(*it)].z/units::cm  << " " << map_index_charge[*it] << std::endl; */
    /* } */

    // form a graph to find the independent component ...

    if (peak_indices.size() > 1) {
        std::vector<int> vec_peak_indices(peak_indices.begin(), peak_indices.end());
        peak_indices.clear();

        const int N = vec_peak_indices.size();
        boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS, boost::no_property,
                              boost::property<boost::edge_weight_t, double>>
            temp_graph(N);
        for (int j = 0; j != N; j++) {
            for (int k = 0; k != N; k++) {
                int index1 = j;
                int index2 = k;
                if (boost::edge(vertex(vec_peak_indices.at(index1), *graph),
                                vertex(vec_peak_indices.at(index2), *graph), *graph)
                        .second)
                    add_edge(index1, index2, temp_graph);
            }
        }
        std::vector<int> component(num_vertices(temp_graph));
        const int num = connected_components(temp_graph, &component[0]);

        double min_dis[num];
        PointVector points(num);
        int min_index[num];
        int ncounts[num];
        for (int i = 0; i != num; i++) {
            min_dis[i] = 1e9;
            points.at(i).x = 0;
            points.at(i).y = 0;
            points.at(i).z = 0;
            min_index[i] = -1;
            ncounts[i] = 0;
        }

        std::vector<int>::size_type i;
        for (i = 0; i != component.size(); ++i) {
            ncounts[component[i]]++;
            points.at(component[i]).x += cloud.pts[vec_peak_indices.at(i)].x;
            points.at(component[i]).y += cloud.pts[vec_peak_indices.at(i)].y;
            points.at(component[i]).z += cloud.pts[vec_peak_indices.at(i)].z;
        }
        // for each independent component, find the average position, and the closest point, maybe looping is good
        // enough ...
        for (int i = 0; i != num; i++) {
            points.at(i).x /= ncounts[i];
            points.at(i).y /= ncounts[i];
            points.at(i).z /= ncounts[i];
        }

        for (i = 0; i != component.size(); ++i) {
            double dis = pow(points.at(component[i]).x - cloud.pts[vec_peak_indices.at(i)].x, 2) +
                         pow(points.at(component[i]).y - cloud.pts[vec_peak_indices.at(i)].y, 2) +
                         pow(points.at(component[i]).z - cloud.pts[vec_peak_indices.at(i)].z, 2);
            if (dis < min_dis[component[i]]) {
                min_dis[component[i]] = dis;
                min_index[component[i]] = vec_peak_indices.at(i);
            }
        }

        for (int i = 0; i != num; i++) {
            peak_indices.insert(min_index[i]);
        }

        //  std::cout << num << " " << peak_indices.size() << std::endl;
        /* for (auto it = peak_indices.begin();  it!= peak_indices.end(); it++){ */
        /*   auto it1 = it; */
        /*   it1++; */
        /*   if (it1!=peak_indices.end()){ */
        /* 	std::cout << *it << " " << *it1 << " " << boost::edge(vertex(*it,*graph), vertex(*it1,*graph),
         * *graph).second << " " << map_index_charge[*it] << " " << map_index_charge[*it1] */
        /* 		  << " " << cloud.pts[(*it)].x/units::cm << " " << cloud.pts[(*it)].y/units::cm << " " <<
         * cloud.pts[(*it)].z/units::cm << " " */
        /* 		  << " " << cloud.pts[(*it1)].x/units::cm << " " << cloud.pts[(*it1)].y/units::cm << " " <<
         * cloud.pts[(*it1)].z/units::cm << std::endl; */
        /*   } */
        /* } */
    }

    /* for (auto it = peak_indices.begin();  it!= peak_indices.end(); it++){ */
    /*   if (cloud.pts[(*it)].y/units::cm < -100) */
    /*     std::cout <<  cloud.pts[(*it)].x/units::cm << " " << cloud.pts[(*it)].y/units::cm << " " <<
     * cloud.pts[(*it)].z/units::cm  << std::endl; */
    /* } */

    return peak_indices;
}

std::pair<bool, double> WCPPID::PR3DCluster::calc_charge_wcp(WCP::WCPointCloud<double>::WCPoint& wcp,
                                                             WCP::GeomDataSource& gds, bool disable_dead_mix_cell,
                                                             double charge_cut)
{
    double charge = 0;
    double ncharge = 0;
    SlimMergeGeomCell* mcell = wcp.mcell;

    const GeomWire* uwire = gds.by_planeindex(WirePlaneType_t(0), wcp.index_u);
    const GeomWire* vwire = gds.by_planeindex(WirePlaneType_t(1), wcp.index_v);
    const GeomWire* wwire = gds.by_planeindex(WirePlaneType_t(2), wcp.index_w);

    double charge_u = mcell->Get_Wire_Charge(uwire);
    double charge_v = mcell->Get_Wire_Charge(vwire);
    double charge_w = mcell->Get_Wire_Charge(wwire);

    bool flag_charge_u = false;
    bool flag_charge_v = false;
    bool flag_charge_w = false;

    if (charge_u > charge_cut) flag_charge_u = true;
    if (charge_v > charge_cut) flag_charge_v = true;
    if (charge_w > charge_cut) flag_charge_w = true;

    if (disable_dead_mix_cell) {
        charge += charge_u * charge_u;
        ncharge++;
        charge += charge_v * charge_v;
        ncharge++;
        charge += charge_w * charge_w;
        ncharge++;
        // std::cout << charge_u << " " << charge_v << " " << charge_w << std::endl;

        // deal with bad planes ...
        std::vector<WirePlaneType_t> bad_planes = mcell->get_bad_planes();
        for (size_t i = 0; i != bad_planes.size(); i++) {
            if (bad_planes.at(i) == WirePlaneType_t(0)) {
                flag_charge_u = true;
                charge -= charge_u * charge_u;
                ncharge--;
            }
            else if (bad_planes.at(i) == WirePlaneType_t(1)) {
                flag_charge_v = true;
                charge -= charge_v * charge_v;
                ncharge--;
            }
            else if (bad_planes.at(i) == WirePlaneType_t(2)) {
                flag_charge_w = true;
                charge -= charge_w * charge_w;
                ncharge--;
            }
        }
    }
    else {
        if (charge_u == 0) flag_charge_u = true;
        if (charge_v == 0) flag_charge_v = true;
        if (charge_w == 0) flag_charge_w = true;

        if (charge_u != 0) {
            charge += charge_u * charge_u;
            ncharge++;
        }
        if (charge_v != 0) {
            charge += charge_v * charge_v;
            ncharge++;
        }
        if (charge_w != 0) {
            charge += charge_w * charge_w;
            ncharge++;
        }

        /* return std::make_pair(flag_charge_u && flag_charge_v || */
        /* 			flag_charge_v && flag_charge_w || */
        /* 			flag_charge_u && flag_charge_w, charge); */
    }

    // get charge for each indices ...

    // require more than two planes are good
    if (ncharge > 1) {
        charge = sqrt(charge / ncharge);
    }
    else {
        charge = 0;
    }
    return std::make_pair(flag_charge_u && flag_charge_v && flag_charge_w, charge);
}

void WCPPID::PR3DCluster::form_cell_points_map()
{
    cell_point_indices_map.clear();
    WCP::WCPointCloud<double>& cloud = point_cloud->get_cloud();

    for (auto it = mcells.begin(); it != mcells.end(); it++) {
        SlimMergeGeomCell* mcell = (*it);
        std::vector<int>& wcps = point_cloud->get_mcell_indices(mcell);
        if (cell_point_indices_map.find(mcell) == cell_point_indices_map.end()) {
            std::set<int> point_indices;
            cell_point_indices_map[mcell] = point_indices;
        }

        for (auto it1 = wcps.begin(); it1 != wcps.end(); it1++) {
            WCPointCloud<double>::WCPoint& wcp = cloud.pts[*it1];
            cell_point_indices_map[mcell].insert(wcp.index);
        }
    }
}
