#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::ToyCTPointCloud& ct_point_cloud)
{
    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();

    // now form the connected components
    std::vector<int> component(num_vertices(*graph));
    const int num = connected_components(*graph, &component[0]);
    if (num > 1) {
        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::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);
            }
        }

        // check against the closest distance ...
        // no need to have MST ...
        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));

                // if (cluster_id==6)
                //   std::cout << j << " " << k << " " << num << " " << pt_clouds.at(j)->get_num_points() << " " <<
                //   pt_clouds.at(k)->get_num_points() << std::endl;

                if (num < 100 && 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 ||
                    pt_clouds.at(j)->get_num_points() > 500 && pt_clouds.at(k)->get_num_points() > 500) {
                    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)
                    //   result1 = pt_clouds.at(k)->get_closest_point_along_vec(p1, dir1, 6*units::cm, 1*units::cm,
                    //   25, 1.5*units::cm);

                    // if (cluster_id==6)
                    //   std::cout << p1.x/units::cm << " " << p1.y/units::cm << " " << p1.z/units::cm << " " <<
                    //   dir1.X() << " " << dir1.Y() << " " << dir1.Z() << std::endl;

                    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 (cluster_id==6)
                        // //   std::cout << num_cut_points << " " << num_points << " " << dis/units::cm << std::endl;

                        // if (num_cut_points <=8 && num_cut_points< 0.25 * num_points + 2 && dis > 1*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)
                    //   result2 = pt_clouds.at(j)->get_closest_point_along_vec(p2, dir2, 7*units::cm,
                    //   1*units::cm, 12.5, 1.5*units::cm);

                    // if (cluster_id==6)
                    //   std::cout << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << " " <<
                    //   dir2.X() << " " << dir2.Y() << " " << dir2.Z() << std::endl;

                    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 (cluster_id==6)
                        // //   std::cout << num_cut_points << " " << num_points << " " << dis/units::cm << std::endl;

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

                // Now check the path ...
                {
                    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);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    for (int ii = 0; ii != num_steps; ii++) {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point(test_p)) {
                            num_bad++;
                            /* if (cluster->get_cluster_id()==11) */
                            /* 	std::cout << test_p.x/units::cm << " " << test_p.y/units::cm << " " <<
                             * test_p.z/units::cm << std::endl; */
                        }
                    }

                    //  std::cout << cluster->get_cluster_id() << " " << j << " " << k << " " << num_bad << " " <<
                    //  num_steps << std::endl;

                    if (num_bad > 7 || num_bad > 2 && num_bad >= 0.75 * num_steps) {
                        index_index_dis[j][k] = std::make_tuple(-1, -1, 1e9);
                    }

                    // if (cluster_id==13)
                    //   std::cout << cluster_id << " " << p1.x/units::cm << " " << p1.y/units::cm << " " <<
                    //   p1.z/units::cm << " " << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << "
                    //   " << j << " " << k << " " << num_bad << " " << num_steps << std::endl;
                }

                // Now check the path ...
                if (std::get<0>(index_index_dis_dir1[j][k]) >= 0) {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis_dir1[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis_dir1[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    for (int ii = 0; ii != num_steps; ii++) {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point(test_p)) num_bad++;
                    }

                    if (num_bad > 7 || num_bad > 2 && num_bad >= 0.75 * num_steps) {
                        index_index_dis_dir1[j][k] = std::make_tuple(-1, -1, 1e9);
                    }
                    // if (cluster_id==13)
                    //   std::cout << "A: " << p1.x/units::cm << " " << p1.y/units::cm << " " << p1.z/units::cm << " "
                    //   << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << " " << j << " " << k <<
                    //   " " << num_bad << " " << num_steps << std::endl;
                }

                // Now check the path ...
                if (std::get<0>(index_index_dis_dir2[j][k]) >= 0) {
                    WCPointCloud<double>::WCPoint wp1 = cloud.pts.at(std::get<0>(index_index_dis_dir2[j][k]));
                    WCPointCloud<double>::WCPoint wp2 = cloud.pts.at(std::get<1>(index_index_dis_dir2[j][k]));
                    Point p1(wp1.x, wp1.y, wp1.z);
                    Point p2(wp2.x, wp2.y, wp2.z);

                    double dis = sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
                    double step_dis = 1.0 * units::cm;
                    int num_steps = dis / step_dis + 1;
                    int num_bad = 0;
                    for (int ii = 0; ii != num_steps; ii++) {
                        Point test_p;
                        test_p.x = p1.x + (p2.x - p1.x) / num_steps * (ii + 1);
                        test_p.y = p1.y + (p2.y - p1.y) / num_steps * (ii + 1);
                        test_p.z = p1.z + (p2.z - p1.z) / num_steps * (ii + 1);
                        if (!ct_point_cloud.is_good_point(test_p)) num_bad++;
                    }

                    if (num_bad > 7 || num_bad > 2 && num_bad >= 0.75 * num_steps) {
                        index_index_dis_dir2[j][k] = std::make_tuple(-1, -1, 1e9);
                    }
                    // if (cluster_id==13)
                    //   std::cout << "B: " << p1.x/units::cm << " " << p1.y/units::cm << " " << p1.z/units::cm << " "
                    //   << p2.x/units::cm << " " << p2.y/units::cm << " " << p2.z/units::cm << " " << j << " " << k <<
                    //   " " << num_bad << " " << num_steps << std::endl;
                }
            }
        }

        // deal with MST of first type
        {
            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[j][k]) >= 0)
                        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;
                        }
                    }
                }
            }
        }

        // MST of the direction ...
        {
            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;
                        }
                    }
                }
            }
        }

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

                // establish the path ...
                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[j][k])>=0){
                //   auto edge = add_edge(std::get<0>(index_index_dis[j][k]),std::get<1>(index_index_dis[j][k]),*graph);
                //   if (edge.second){
                //     if (std::get<2>(index_index_dis[j][k])>5*units::cm){
                //       (*graph)[edge.first].dist = std::get<2>(index_index_dis[j][k]);
                //     }else{
                //       (*graph)[edge.first].dist = std::get<2>(index_index_dis[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.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.1;
                            }
                            else {
                                (*graph)[edge.first].dist = std::get<2>(index_index_dis_dir2[j][k]);
                            }
                        }
                    }
                }

            }  // k
        }      // j

        // delete newly created point cloud
        for (int i = 0; i != num; i++) {
            delete pt_clouds.at(i);
        }
        pt_clouds.clear();
    }
}