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

#include "TMatrixDEigen.h"
#include "TH2F.h"
#include "TVector3.h"

#include <boost/graph/connected_components.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>

using namespace WCP;

#include "PR3DCluster_dQ_dx_fit.h"
#include "PR3DCluster_trajectory_fit.h"

void PR3DCluster::Connect_graph()
{
    WCP::WCPointCloud<double>& cloud = point_cloud->get_cloud();
    WCP::WC2DPointCloud<double>& cloud_u = point_cloud->get_cloud_u();
    WCP::WC2DPointCloud<double>& cloud_v = point_cloud->get_cloud_v();
    WCP::WC2DPointCloud<double>& cloud_w = point_cloud->get_cloud_w();

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

    if (num > 1) {
        // for separated kd tree to find the closest points between disconnected components,
        std::vector<ToyPointCloud*> pt_clouds;
        for (int j = 0; j != num; j++) {
            ToyPointCloud* pt_cloud = new ToyPointCloud();
            pt_clouds.push_back(pt_cloud);
        }

        std::vector<int>::size_type i;
        for (i = 0; i != component.size(); ++i) {
            pt_clouds.at(component[i])->AddPoint(cloud.pts[i], cloud_u.pts[i], cloud_v.pts[i], cloud_w.pts[i]);
            //   std::cout << "Vertex " << i << " " << cloud.pts[i].x << " " << cloud.pts[i].y << " " << cloud.pts[i].z
            //   << " " << cloud.pts[i].index_u << " " << cloud.pts[i].index_v << " " << cloud.pts[i].index_w << " " <<
            //   cloud.pts[i].mcell << " " << cloud.pts[i].mcell->GetTimeSlice()  << " is in component " << component[i]
            //   << std::endl;
        }
        for (int j = 0; j != num; j++) {
            pt_clouds.at(j)->build_kdtree_index();
        }

        // std::cout << "Xin: " << num << std::endl;
        //  connect these graphs according to closest distance some how ...

        // std::tuple<int,int,double> index_index_dis[num][num];
        // std::tuple<int,int,double> index_index_dis_mst[num][num];
        // std::tuple<int,int,double> index_index_dis_dir1[num][num];
        // std::tuple<int,int,double> index_index_dis_dir2[num][num];

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis(
            num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_mst(
            num, std::vector<std::tuple<int, int, double>>(num));

        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir1(
            num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir2(
            num, std::vector<std::tuple<int, int, double>>(num));
        std::vector<std::vector<std::tuple<int, int, double>>> index_index_dis_dir_mst(
            num, std::vector<std::tuple<int, int, double>>(num));

        for (int j = 0; j != num; j++) {
            for (int k = 0; k != num; k++) {
                index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_mst[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                index_index_dis_dir_mst[j][k] = std::make_tuple(-1, -1, 1e9);
            }
        }

        // MST ...
        const int N = num;
        boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS, boost::no_property,
                              boost::property<boost::edge_weight_t, double>>
            temp_graph(N);

        for (int j = 0; j != num; j++) {
            for (int k = j + 1; k != num; k++) {
                index_index_dis[j][k] = pt_clouds.at(j)->get_closest_points(pt_clouds.at(k));
                int index1 = j;
                int index2 = k;
                auto edge = add_edge(index1, index2, std::get<2>(index_index_dis[j][k]), temp_graph);
            }
        }

        {
            std::vector<int> possible_root_vertex;
            std::vector<int> component(num_vertices(temp_graph));
            const int num1 = connected_components(temp_graph, &component[0]);
            possible_root_vertex.resize(num1);
            std::vector<int>::size_type i;
            for (i = 0; i != component.size(); ++i) {
                possible_root_vertex.at(component[i]) = i;
            }

            for (size_t i = 0; i != possible_root_vertex.size(); i++) {
                std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(num_vertices(temp_graph));
                prim_minimum_spanning_tree(temp_graph, &predecessors[0],
                                           boost::root_vertex(possible_root_vertex.at(i)));
                for (size_t j = 0; j != predecessors.size(); ++j) {
                    if (predecessors[j] != j) {
                        if (j < predecessors[j]) {
                            index_index_dis_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                        }
                        else {
                            index_index_dis_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                        }
                        // std::cout << j << " " << predecessors[j] << " " << std::endl;
                    }
                    else {
                        // std::cout << j << " " << std::endl;
                    }
                }
            }
        }
        // end of mst ...

        // short distance part
        for (int j = 0; j != num; j++) {
            for (int k = j + 1; k != num; k++) {
                // closest distance one ...
                if (std::get<2>(index_index_dis[j][k]) < 3 * units::cm) {
                    index_index_dis_mst[j][k] = index_index_dis[j][k];
                }

                if (num < 100)
                    if (pt_clouds.at(j)->get_num_points() > 100 && pt_clouds.at(k)->get_num_points() > 100 &&
                        (pt_clouds.at(j)->get_num_points() + pt_clouds.at(k)->get_num_points()) > 400) {
                        WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis[j][k]));
                        WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis[j][k]));
                        Point p1(wp1.x, wp1.y, wp1.z);
                        Point p2(wp2.x, wp2.y, wp2.z);

                        TVector3 dir1 = VHoughTrans(p1, 30 * units::cm, pt_clouds.at(j));
                        TVector3 dir2 = VHoughTrans(p2, 30 * units::cm, pt_clouds.at(k));
                        dir1 *= -1;
                        dir2 *= -1;

                        std::pair<int, double> result1 = pt_clouds.at(k)->get_closest_point_along_vec(
                            p1, dir1, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);

                        if (result1.first >= 0) {
                            // Point
                            // test_p1(cloud.pts.at(std::get<0>(index_index_dis[j][k])).x,cloud.pts.at(std::get<0>(index_index_dis[j][k])).y,cloud.pts.at(std::get<0>(index_index_dis[j][k])).z);
                            // Point
                            // test_p2(cloud.pts.at(result1.first).x,cloud.pts.at(result1.first).y,cloud.pts.at(result1.first).z);
                            // double dis =
                            // sqrt(pow(test_p2.x-test_p1.x,2)+pow(test_p2.y-test_p1.y,2)+pow(test_p2.z-test_p1.z,2));
                            // int num_points = dis/(1.5*units::cm)+1;
                            // int num_cut_points = 0;
                            // for (size_t k1=0; k1!=num_points-1; k1++){
                            // 	Point test_p3(test_p1.x + (test_p2.x-test_p1.x) * (k1+1)/num_points ,
                            // 		      test_p1.y + (test_p2.y-test_p1.y) * (k1+1)/num_points ,
                            // 		      test_p1.z + (test_p2.z-test_p1.z) * (k1+1)/num_points );
                            // 	double dis1 = point_cloud->get_closest_dis(test_p3);
                            // 	if (dis1 < 1*units::cm)
                            // 	  num_cut_points ++;
                            // }
                            // if (num_cut_points <=8 && num_cut_points< 0.25 * num_points + 2 && dis > 5*units::cm)
                            index_index_dis_dir1[j][k] =
                                std::make_tuple(std::get<0>(index_index_dis[j][k]), result1.first, result1.second);
                        }

                        std::pair<int, double> result2 = pt_clouds.at(j)->get_closest_point_along_vec(
                            p2, dir2, 80 * units::cm, 5 * units::cm, 7.5, 3 * units::cm);

                        if (result2.first >= 0) {
                            // Point
                            // test_p1(cloud.pts.at(std::get<1>(index_index_dis[j][k])).x,cloud.pts.at(std::get<1>(index_index_dis[j][k])).y,cloud.pts.at(std::get<1>(index_index_dis[j][k])).z);
                            // Point
                            // test_p2(cloud.pts.at(result2.first).x,cloud.pts.at(result2.first).y,cloud.pts.at(result2.first).z);
                            // double dis =
                            // sqrt(pow(test_p2.x-test_p1.x,2)+pow(test_p2.y-test_p1.y,2)+pow(test_p2.z-test_p1.z,2));
                            // int num_points = dis/(1.5*units::cm)+1;
                            // int num_cut_points = 0;
                            // for (size_t k1=0; k1!=num_points-1; k1++){
                            // 	Point test_p3(test_p1.x + (test_p2.x-test_p1.x) * (k1+1)/num_points ,
                            // 		      test_p1.y + (test_p2.y-test_p1.y) * (k1+1)/num_points ,
                            // 		      test_p1.z + (test_p2.z-test_p1.z) * (k1+1)/num_points );
                            // 	double dis1 = point_cloud->get_closest_dis(test_p3);
                            // 	if ( dis1 < 1*units::cm)
                            // 	  num_cut_points ++;
                            // }

                            // if (num_cut_points <=8 && num_cut_points < 0.25 * num_points + 2 && dis > 5*units::cm)
                            index_index_dis_dir2[j][k] =
                                std::make_tuple(result2.first, std::get<1>(index_index_dis[j][k]), result2.second);
                        }
                    }
            }
        }

        // MST for the directionality ...
        {
            const int N = num;
            boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS, boost::no_property,
                                  boost::property<boost::edge_weight_t, double>>
                temp_graph(N);

            for (int j = 0; j != num; j++) {
                for (int k = j + 1; k != num; k++) {
                    int index1 = j;
                    int index2 = k;
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0 || std::get<0>(index_index_dis_dir2[j][k]) >= 0)
                        auto edge = add_edge(
                            index1, index2,
                            std::min(std::get<2>(index_index_dis_dir1[j][k]), std::get<2>(index_index_dis_dir2[j][k])),
                            temp_graph);
                }
            }

            {
                std::vector<int> possible_root_vertex;
                std::vector<int> component(num_vertices(temp_graph));
                const int num1 = connected_components(temp_graph, &component[0]);
                possible_root_vertex.resize(num1);
                std::vector<int>::size_type i;
                for (i = 0; i != component.size(); ++i) {
                    possible_root_vertex.at(component[i]) = i;
                }
                for (size_t i = 0; i != possible_root_vertex.size(); i++) {
                    std::vector<boost::graph_traits<MCUGraph>::vertex_descriptor> predecessors(
                        num_vertices(temp_graph));
                    prim_minimum_spanning_tree(temp_graph, &predecessors[0],
                                               boost::root_vertex(possible_root_vertex.at(i)));
                    for (size_t j = 0; j != predecessors.size(); ++j) {
                        if (predecessors[j] != j) {
                            if (j < predecessors[j]) {
                                index_index_dis_dir_mst[j][predecessors[j]] = index_index_dis[j][predecessors[j]];
                            }
                            else {
                                index_index_dis_dir_mst[predecessors[j]][j] = index_index_dis[predecessors[j]][j];
                            }
                            // std::cout << j << " " << predecessors[j] << " " << std::endl;
                        }
                        else {
                            // std::cout << j << " " << std::endl;
                        }
                    }
                }
            }
        }

        // now complete graph according to the direction
        // according to direction ...
        for (int j = 0; j != num; j++) {
            for (int k = j + 1; k != num; k++) {
                if (std::get<0>(index_index_dis_mst[j][k]) >= 0) {
                    auto edge = add_edge(std::get<0>(index_index_dis_mst[j][k]), std::get<1>(index_index_dis_mst[j][k]),
                                         *graph);
                    if (edge.second) {
                        if (std::get<2>(index_index_dis_mst[j][k]) > 5 * units::cm) {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                        else {
                            (*graph)[edge.first].dist = std::get<2>(index_index_dis_mst[j][k]);
                        }
                    }
                }

                if (std::get<0>(index_index_dis_dir_mst[j][k]) >= 0) {
                    if (std::get<0>(index_index_dis_dir1[j][k]) >= 0) {
                        auto edge = add_edge(std::get<0>(index_index_dis_dir1[j][k]),
                                             std::get<1>(index_index_dis_dir1[j][k]), *graph);
                        if (edge.second) {
                            if (std::get<2>(index_index_dis_dir1[j][k]) > 5 * units::cm) {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]) * 1.2;
                                // }else if (std::get<2>(index_index_dis_dir1[j][k])>2*units::cm){
                                // 	(*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k])*1.1;
                            }
                            else {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir1[j][k]);
                            }
                        }
                    }
                    if (std::get<0>(index_index_dis_dir2[j][k]) >= 0) {
                        auto edge = add_edge(std::get<0>(index_index_dis_dir2[j][k]),
                                             std::get<1>(index_index_dis_dir2[j][k]), *graph);
                        if (edge.second) {
                            if (std::get<2>(index_index_dis_dir2[j][k]) > 5 * units::cm) {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]) * 1.2;
                                // }else if(std::get<2>(index_index_dis_dir2[j][k])>2*units::cm){
                                // 	(*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k])*1.1;
                            }
                            else {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]);
                            }
                        }
                    }
                }
            }
        }

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