
#include "WCPData/ToyPointCloud.h"

#include <iostream>
#include <vector>

#include "TTree.h"
#include "TFile.h"
#include "TChain.h"
#include "TString.h"

#include "TGraph.h"
#include "TGraph2D.h"
#include "TVector3.h"

#include <map>

using namespace WCP;

int main(int argc, char *argv[])
{
    if (argc < 2) {
        std::cerr << "usage: wire-cell-track-com -a[truth.root] -b[reco.root] -t[reco_treename] -n[truth_treename] "
                     "-o[out.root]"
                  << std::endl;
        return 1;
    }

    TString truth_filename = "mcs-tracks.root";
    TString reco_filename = "tracking_0_0_0.root";
    TString reco_treename = "T_rec_charge";
    TString truth_treename = "T";
    TString out_filename = "track_com.root";

    for (Int_t i = 1; i != argc; i++) {
        switch (argv[i][1]) {
        case 'a':
            truth_filename = &argv[i][2];
            break;
        case 'b':
            reco_filename = &argv[i][2];
            break;
        case 't':
            reco_treename = &argv[i][2];
            break;
        case 'n':
            truth_treename = &argv[i][2];
            break;
        case 'o':
            out_filename = &argv[i][2];
            break;
        }
    }

    std::cout << truth_filename << " " << reco_filename << " " << truth_treename << " " << reco_treename << std::endl;

    TFile *file = new TFile(out_filename, "RECREATE");

    TChain *T_true = new TChain(truth_treename, truth_treename);
    T_true->Add(truth_filename);
    std::vector<double> *x = new std::vector<double>;
    std::vector<double> *y = new std::vector<double>;
    std::vector<double> *z = new std::vector<double>;
    std::vector<double> *Q = new std::vector<double>;
    Int_t N;
    T_true->SetBranchAddress("N", &N);
    T_true->SetBranchAddress("x", &x);
    T_true->SetBranchAddress("y", &y);
    T_true->SetBranchAddress("z", &z);
    T_true->SetBranchAddress("Q", &Q);
    T_true->GetEntry(0);

    // std::cout << N << std::endl;

    TGraph *g1_xy = new TGraph();
    TGraph *g1_xz = new TGraph();
    TGraph *g1_yz = new TGraph();

    TGraph2D *g1 = new TGraph2D();

    ToyPointCloud pcloud, pcloud1;

    WCP::PointVector ps;

    for (size_t i = 0; i != x->size(); i++) {
        g1_xy->SetPoint(i, x->at(i), y->at(i));
        g1_xz->SetPoint(i, x->at(i), z->at(i));
        g1_yz->SetPoint(i, y->at(i), z->at(i));

        Point p(x->at(i) * units::cm, y->at(i) * units::cm, z->at(i) * units::cm);
        ps.push_back(p);

        g1->SetPoint(i, x->at(i), y->at(i), z->at(i));
    }
    pcloud.AddPoints(ps);
    pcloud.build_kdtree_index();
    ps.clear();

    g1_xy->SetLineColor(1);
    g1_xz->SetLineColor(1);
    g1_yz->SetLineColor(1);

    g1_xy->SetLineWidth(2);
    g1_xz->SetLineWidth(2);
    g1_yz->SetLineWidth(2);

    g1->SetLineColor(1);
    g1->SetLineWidth(2);

    TChain *T_rec = new TChain(reco_treename, reco_treename);
    T_rec->Add(reco_filename);
    Double_t x1, y1, z1;
    Double_t dQ1, dx1;
    T_rec->SetBranchAddress("x", &x1);
    T_rec->SetBranchAddress("y", &y1);
    T_rec->SetBranchAddress("z", &z1);
    T_rec->SetBranchAddress("q", &dQ1);
    T_rec->SetBranchAddress("nq", &dx1);
    TGraph *g2_xy = new TGraph();
    TGraph *g2_xz = new TGraph();
    TGraph *g2_yz = new TGraph();

    TGraph2D *g2 = new TGraph2D();

    TTree *t1 = new TTree("T", "T");
    t1->SetDirectory(file);
    double max_dis = 0;     // maximum distance along the track
    double beg_dis;         // distance at the beginning ...
    double end_dis;         // distance at the end ...
    double total_dis2 = 0;  // total distance^2
    double total_L = 0;
    int Npoints = 0;
    double total_dtheta = 0;
    double max_dtheta = 0;
    t1->Branch("max_dis", &max_dis);
    t1->Branch("beg_dis", &beg_dis);
    t1->Branch("end_dis", &end_dis);
    t1->Branch("total_dis2", &total_dis2);
    t1->Branch("total_L", &total_L);
    t1->Branch("N", &Npoints);
    t1->Branch("total_dtheta", &total_dtheta);
    t1->Branch("max_dtheta", &max_dtheta);

    std::vector<double> *x2 = new std::vector<double>;
    std::vector<double> *y2 = new std::vector<double>;
    std::vector<double> *z2 = new std::vector<double>;
    std::vector<double> *dQ_rec = new std::vector<double>;
    std::vector<double> *dQ_tru = new std::vector<double>;
    std::vector<double> *dx = new std::vector<double>;

    std::vector<double> *x2_pair = new std::vector<double>;
    std::vector<double> *y2_pair = new std::vector<double>;
    std::vector<double> *z2_pair = new std::vector<double>;

    std::vector<double> *dis = new std::vector<double>;
    std::vector<double> *L = new std::vector<double>;
    std::vector<double> *dtheta = new std::vector<double>;

    t1->Branch("x", &x2);
    t1->Branch("y", &y2);
    t1->Branch("z", &z2);
    t1->Branch("dQ_rec", &dQ_rec);
    t1->Branch("dQ_tru", &dQ_tru);
    t1->Branch("dx", &dx);

    t1->Branch("x_pair", &x2_pair);
    t1->Branch("y_pair", &y2_pair);
    t1->Branch("z_pair", &z2_pair);

    t1->Branch("dis", &dis);
    t1->Branch("L", &L);
    t1->Branch("dtheta", &dtheta);

    double temp_L = 0;
    double prev_x1, prev_y1, prev_z1;

    Npoints = T_rec->GetEntries();

    std::map<std::tuple<int, int, int>, int> map_point_index;

    for (int i = 0; i != T_rec->GetEntries(); i++) {
        T_rec->GetEntry(i);

        if (i != 0) {
            temp_L += sqrt(pow(x1 - prev_x1, 2) + pow(y1 - prev_y1, 2) + pow(z1 - prev_z1, 2));
        }

        // binning effect 1 us later from the binned slice effect, rebinned 4 ...
        // speed of imaging 1.101 mm / us
        // speed of simulation 1.098 mm/us
        // there is a potential 1 us offset at SP
        // -0.6 cm is the distance difference between Y and U planes
        x1 = (x1 + 0.1101) / 1.1009999 * 1.098 - 0.6;  //+ 4*0.1101;

        g2_xy->SetPoint(i, x1, y1);
        g2_xz->SetPoint(i, x1, z1);
        g2_yz->SetPoint(i, y1, z1);

        Point p(x1 * units::cm, y1 * units::cm, z1 * units::cm);
        ps.push_back(p);

        std::pair<double, Point> point_pair = pcloud.get_closest_point(p);

        map_point_index[std::make_tuple(p.x / (0.01 * units::mm), p.y / (0.01 * units::mm), p.z / (0.01 * units::mm))] =
            x2->size();

        x2->push_back(x1);
        y2->push_back(y1);
        z2->push_back(z1);
        dQ_rec->push_back(dQ1);
        dQ_tru->push_back(0);
        dx->push_back(dx1);
        dis->push_back(point_pair.first / units::cm);

        x2_pair->push_back(point_pair.second.x / units::cm);
        y2_pair->push_back(point_pair.second.y / units::cm);
        z2_pair->push_back(point_pair.second.z / units::cm);

        L->push_back(temp_L);

        if (max_dis < point_pair.first / units::cm) max_dis = point_pair.first / units::cm;
        total_dis2 += pow(point_pair.first / units::cm, 2);

        if (i == 0) {
            double dis1 = pow(x1 - x->front(), 2) + pow(y1 - y->front(), 2) + pow(z1 - z->front(), 2);
            double dis2 = pow(x1 - x->back(), 2) + pow(y1 - y->back(), 2) + pow(z1 - z->back(), 2);

            // std::cout << sqrt(dis1)/units::cm << " " << sqrt(dis2)/units::cm << std::endl;
            if (dis1 < dis2) {
                beg_dis = sqrt(dis1);
            }
            else {
                beg_dis = sqrt(dis2);
            }
        }
        if (i == T_rec->GetEntries() - 1) {
            double dis1 = pow(x1 - x->front(), 2) + pow(y1 - y->front(), 2) + pow(z1 - z->front(), 2);
            double dis2 = pow(x1 - x->back(), 2) + pow(y1 - y->back(), 2) + pow(z1 - z->back(), 2);
            if (dis1 < dis2) {
                end_dis = sqrt(dis1);
            }
            else {
                end_dis = sqrt(dis2);
            }
        }

        //  std::cout << point_pair.first/units::cm << " " << point_pair.second.x/units::cm << " " <<
        //  point_pair.second.y/units::cm << " " << point_pair.second.z/units::cm << std::endl;

        g2->SetPoint(i, x1, y1, z1);

        prev_x1 = x1;
        prev_y1 = y1;
        prev_z1 = z1;
    }

    pcloud1.AddPoints(ps);
    pcloud1.build_kdtree_index();

    for (size_t i = 0; i != x->size(); i++) {
        Point p(x->at(i) * units::cm, y->at(i) * units::cm, z->at(i) * units::cm);
        std::pair<double, Point> point_pair = pcloud1.get_closest_point(p);
        int index = map_point_index[std::make_tuple(int(point_pair.second.x / (0.01 * units::mm)),
                                                    int(point_pair.second.y / (0.01 * units::mm)),
                                                    int(point_pair.second.z / (0.01 * units::mm)))];
        // std::cout << index << std::endl;
        dQ_tru->at(index) += Q->at(i);

        if (i == 0 || i == x->size() - 1)
            std::cout << p << " "
                      << sqrt(pow(p.x - point_pair.second.x, 2) + pow(p.y - point_pair.second.y, 2) +
                              pow(p.z - point_pair.second.z, 2)) /
                             units::cm
                      << std::endl;

        // g1->SetPoint(i,x->at(i),y->at(i),z->at(i));
    }

    if (x2->size() > 1) {
        for (size_t i = 0; i != x2->size(); i++) {
            if (i == 0) {
                TVector3 dir1(x2->at(1) - x2->at(0), y2->at(1) - y2->at(0), z2->at(1) - z2->at(0));
                TVector3 dir2(x2_pair->at(1) - x2_pair->at(0), y2_pair->at(1) - y2_pair->at(0),
                              z2_pair->at(1) - z2_pair->at(0));
                dtheta->push_back(dir1.Angle(dir2));
            }
            else if (i == x2->size() - 1) {
                TVector3 dir1(x2->at(x2->size() - 1) - x2->at(x2->size() - 2),
                              y2->at(x2->size() - 1) - y2->at(x2->size() - 2),
                              z2->at(x2->size() - 1) - z2->at(x2->size() - 2));
                TVector3 dir2(x2_pair->at(x2->size() - 1) - x2_pair->at(x2->size() - 2),
                              y2_pair->at(x2->size() - 1) - y2_pair->at(x2->size() - 2),
                              z2_pair->at(x2->size() - 1) - z2_pair->at(x2->size() - 2));
                dtheta->push_back(dir1.Angle(dir2));
            }
            else {
                TVector3 dir1(x2->at(i + 1) - x2->at(i), y2->at(i + 1) - y2->at(i), z2->at(i + 1) - z2->at(i));
                TVector3 dir2(x2_pair->at(i + 1) - x2_pair->at(i), y2_pair->at(i + 1) - y2_pair->at(i),
                              z2_pair->at(i + 1) - z2_pair->at(i));

                TVector3 dir3(x2->at(i - 1) - x2->at(i), y2->at(i - 1) - y2->at(i), z2->at(i - 1) - z2->at(i));
                TVector3 dir4(x2_pair->at(i - 1) - x2_pair->at(i), y2_pair->at(i - 1) - y2_pair->at(i),
                              z2_pair->at(i - 1) - z2_pair->at(i));
                dtheta->push_back((dir1.Angle(dir2) + dir3.Angle(dir4)) / 2.);
            }
            if (dtheta->back() > max_dtheta) max_dtheta = dtheta->back();
            total_dtheta += dtheta->back();
        }
    }
    else {
        dtheta->push_back(0);
    }

    total_L = temp_L;

    t1->Fill();

    g2_xy->SetLineColor(2);
    g2_xz->SetLineColor(2);
    g2_yz->SetLineColor(2);

    g2_xy->SetLineWidth(1);
    g2_xz->SetLineWidth(1);
    g2_yz->SetLineWidth(1);

    g2->SetLineColor(2);
    g2->SetLineWidth(2);

    //  g1_xy->GetXaxis()->SetTitle("X (cm)");
    //   g1_xy->GetYaxis()->SetTitle("Y (cm)");

    //    g1_xz->GetXaxis()->SetTitle("X (cm)");
    //   g1_xz->GetYaxis()->SetTitle("Z (cm)");

    // g1_yz->GetXaxis()->SetTitle("Y (cm)");
    //   g1_yz->GetYaxis()->SetTitle("Z (cm)");

    g2_xy->SetMarkerStyle(23);
    g2_xy->SetMarkerColor(2);
    g2_xy->SetMarkerSize(0.6);

    g2_xz->SetMarkerStyle(23);
    g2_xz->SetMarkerColor(2);
    g2_xz->SetMarkerSize(0.6);

    g2_yz->SetMarkerStyle(23);
    g2_yz->SetMarkerColor(2);
    g2_yz->SetMarkerSize(0.6);

    g1_xy->Write("gt_xy");
    g1_xz->Write("gt_xz");
    g1_yz->Write("gt_yz");

    g2_xy->Write("gr_xy");
    g2_xz->Write("gr_xz");
    g2_yz->Write("gr_yz");

    g1->Write("gt_3D");
    g2->Write("gr_3D");
    t1->Write();

    // T_true->CloneTree(-1,"fast");
    // T_rec->CloneTree(-1,"fast");

    file->Write();
    file->Close();
}
