
#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] -f[1:MC,2:data]" << std::endl;
    return 1;
  }
  
  TString reco_filename = "tracking_0_0_0.root"; 
  TString reco_treename = "T_rec_charge";
  TString proj_treename = "T_proj_data";
  
  TString truth_filename = "mcs-tracks.root"; 
  TString truth_treename = "T";
  TString out_filename = "track_com.root";

  int file_type = 1; // 1 for MC and 2 for data ...

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

  if (file_type==1){
    std::cout << truth_filename << " " << reco_filename << " " << truth_treename << " " << reco_treename << std::endl;
  }else{
    std::cout << reco_filename << " " << reco_treename << std::endl;
  }

  TFile *file1 = new TFile(reco_filename);
  TTree *T_bad_ch = (TTree*)file1->Get("T_bad_ch");
   
  Double_t dQdx_scale = 1.;//1.2;
  Double_t dQdx_offset = 0;
  TTree *Trun = (TTree*)file1->Get("Trun");
  if (Trun!=0){
    if (Trun->GetBranch("dQdx_scale")){
      Trun->SetBranchAddress("dQdx_scale",&dQdx_scale);
      Trun->SetBranchAddress("dQdx_offset",&dQdx_offset);
      Trun->GetEntry(0);
    }
  }
  //std::cout << dQdx_scale << " " << dQdx_offset << std::endl;
  
  TFile *file = new TFile(out_filename,"RECREATE");
  if (T_bad_ch!=0){
    T_bad_ch->CloneTree()->Write();
  }

  std::vector<std::vector<double> > *vx = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *vy = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *vz = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *vQ = new std::vector<std::vector<double> >;  
  std::vector<int> *vN = new std::vector<int>;

  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;
  
  if (file_type==1){ // MC data ... 
    TChain *T_true = new TChain(truth_treename,truth_treename);
    T_true->Add(truth_filename);
    TTree *t2 = new TTree("T_true","T_true");
    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);
    vN->push_back(N);
    vx->push_back(*x);
    vy->push_back(*y);
    vz->push_back(*z);
    vQ->push_back(*Q);

    for (size_t i=0;i!=vx->at(0).size();i++){
      vx->at(0).at(i) = (vx->at(0).at(i)+0.6)/1.098*1.1009999-0.1101;
    }
    
    t2->Branch("N",&vN);
    t2->Branch("x",&vx);
    t2->Branch("y",&vy);
    t2->Branch("z",&vz);
    t2->Branch("Q",&vQ);
    t2->Fill();
  }

  
  
  // 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;
  
  if (file_type==1){ // MC ... 
    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));

      x->at(i) = (x->at(i)+0.6)/1.098*1.1009999-0.1101;
      //      x1 = (x1+0.1101)/1.1009999*1.098-0.6;//+ 4*0.1101; 
      
      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);
  TChain *T_proj_data = new TChain(proj_treename,proj_treename);
  TChain *T_proj = new TChain("T_proj","T_proj");
  
  T_rec->Add(reco_filename);
  T_proj_data->Add(reco_filename);
  T_proj->Add(reco_filename);
  Double_t x1,y1,z1;
  Double_t dQ1,dx1,ndf;
  Double_t pu, pv, pw, pt;

  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);
  T_rec->SetBranchAddress("ndf",&ndf);
  T_rec->SetBranchAddress("pu",&pu);
  T_rec->SetBranchAddress("pv",&pv);
  T_rec->SetBranchAddress("pw",&pw);
  T_rec->SetBranchAddress("pt",&pt);

  Double_t reduced_chi2;
  if (T_rec->GetBranch("reduced_chi2")){
    T_rec->SetBranchAddress("reduced_chi2",&reduced_chi2);
  }
  Int_t flag_vertex;
  Int_t sub_cluster_id;
  if (T_rec->GetBranch("flag_vertex")){
    T_rec->SetBranchAddress("flag_vertex",&flag_vertex);
    T_rec->SetBranchAddress("sub_cluster_id",&sub_cluster_id);
  }
  
  
  TTree *t1 = new TTree("T_rec","T_rec");
  t1->SetDirectory(file);
  std::vector<double> *max_dis = new std::vector<double>; // maximum distance along the track 
  std::vector<double> *beg_dis = new std::vector<double>; // distance at the beginning ... 
  std::vector<double> *end_dis = new std::vector<double>; // distance at the end ...
  std::vector<double> *total_dis2 = new std::vector<double>; // total distance^2
  std::vector<double> *total_L = new std::vector<double>;
  std::vector<int> *Npoints = new std::vector<int>;
  std::vector<double> *total_dtheta = new std::vector<double>;
  std::vector<double> *max_dtheta = new std::vector<double>;
  if (file_type==1){
    t1->Branch("stat_max_dis",&max_dis);
    t1->Branch("stat_beg_dis",&beg_dis);
    t1->Branch("stat_end_dis",&end_dis);
    t1->Branch("stat_total_dis2",&total_dis2);
    t1->Branch("stat_total_dtheta",&total_dtheta);
    t1->Branch("stat_max_dtheta",&max_dtheta);
  }
  t1->Branch("stat_total_L",&total_L);
  t1->Branch("stat_N",&Npoints);
  
  
  std::vector<std::vector<double> > *x2 = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *y2 = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *z2 = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *dQ_rec = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *dQ_tru = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *dx = new std::vector<std::vector<double> >;
  std::vector<int> *cluster_id = new std::vector<int>;
  std::vector<std::vector<double> > *rec_pu = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *rec_pv = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *rec_pw = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *rec_pt = new std::vector<std::vector<double> >;
  
  std::vector<std::vector<double> > *x2_pair = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *y2_pair = new std::vector<std::vector<double> >;
  std::vector<std::vector<double> > *z2_pair = new std::vector<std::vector<double> >;

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

  std::vector<std::vector<double> > *Vreduced_chi2 = new std::vector<std::vector<double> > ;
  std::vector<std::vector<int> > *Vflag_vertex = new std::vector<std::vector<int> >;
  std::vector<std::vector<int> > *Vsub_cluster_id = new std::vector<std::vector<int> >;
  

  
  t1->Branch("rec_x",&x2);
  t1->Branch("rec_y",&y2);
  t1->Branch("rec_z",&z2);
  t1->Branch("rec_dQ",&dQ_rec);
  t1->Branch("rec_dx",&dx);
  t1->Branch("rec_L",&L);
  t1->Branch("rec_cluster_id",&cluster_id);
  t1->Branch("rec_u",&rec_pu);
  t1->Branch("rec_v",&rec_pv);
  t1->Branch("rec_w",&rec_pw);
  t1->Branch("rec_t",&rec_pt);
  if (T_rec->GetBranch("reduced_chi2")){
    t1->Branch("reduced_chi2",&Vreduced_chi2);
  }
  if (T_rec->GetBranch("flag_vertex")){
    t1->Branch("flag_vertex",&Vflag_vertex);
    t1->Branch("sub_cluster_id",&Vsub_cluster_id);
  }
  
  if (file_type==1){
    t1->Branch("true_dQ",&dQ_tru);
    t1->Branch("true_x",&x2_pair);
    t1->Branch("true_y",&y2_pair);
    t1->Branch("true_z",&z2_pair);
    
    t1->Branch("com_dis",&dis);
    t1->Branch("com_dtheta",&dtheta);
  }
  
  double temp_L = 0;
  double prev_x1, prev_y1, prev_z1;
  int prev_cluster_id = -1;

  // Npoints = T_rec->GetEntries();

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

  //  std::cout << T_rec->GetEntries() << std::endl;
  
  for (int i=0;i!=T_rec->GetEntries();i++){
    T_rec->GetEntry(i);
    if (std::round(ndf)!=prev_cluster_id){
      Npoints->push_back(0);
      total_L->push_back(0);

      std::vector<double> temp_dQ_tru;
      dQ_tru->push_back(temp_dQ_tru);
      std::vector<double> temp_dis;
      dis->push_back(temp_dis);
      std::vector<double> temp_x2_pair;
      x2_pair->push_back(temp_x2_pair);
      std::vector<double> temp_y2_pair;
      y2_pair->push_back(temp_y2_pair);
      std::vector<double> temp_z2_pair;
      z2_pair->push_back(temp_z2_pair);

      std::vector<double> temp_x2;
      x2->push_back(temp_x2);
      std::vector<double> temp_y2;
      y2->push_back(temp_y2);
      std::vector<double> temp_z2;
      z2->push_back(temp_z2);
      std::vector<double> temp_rec_pu;
      rec_pu->push_back(temp_rec_pu);
      std::vector<double> temp_rec_pv;
      rec_pv->push_back(temp_rec_pv);
      std::vector<double> temp_rec_pw;
      rec_pw->push_back(temp_rec_pw);
      std::vector<double> temp_rec_pt;
      rec_pt->push_back(temp_rec_pt);
      cluster_id->push_back(std::round(ndf));
      std::vector<double> temp_dQ_rec;
      dQ_rec->push_back(temp_dQ_rec);
      std::vector<double> temp_dx;
      dx->push_back(temp_dx);
      std::vector<double> temp_L;
      L->push_back(temp_L);

      if (T_rec->GetBranch("reduced_chi2")){
	std::vector<double> temp_reduced_chi2;
	Vreduced_chi2->push_back(temp_reduced_chi2);
      }
      if (T_rec->GetBranch("flag_vertex")){
	std::vector<int> temp_flag_vertex;
	std::vector<int> temp_sub_cluster_id;
	Vflag_vertex->push_back(temp_flag_vertex);
	Vsub_cluster_id->push_back(temp_sub_cluster_id);
      }
      
      max_dis->push_back(0);
      total_dis2->push_back(0);
    }
    Npoints->back()++;
    
    if (Npoints->back()!=1){
      total_L->back() += 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; 
    Point p(x1*units::cm, y1*units::cm, z1*units::cm);
    ps.push_back(p);

   
    
    if (file_type==1){
      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))] = std::make_pair(x2->size()-1,x2->back().size());
      dQ_tru->back().push_back(0);
      dis->back().push_back(point_pair.first/units::cm);
      

      x2_pair->back().push_back(point_pair.second.x/units::cm);
      y2_pair->back().push_back(point_pair.second.y/units::cm);
      z2_pair->back().push_back(point_pair.second.z/units::cm);
        
      if (max_dis->back() < point_pair.first/units::cm)
     	max_dis->back() = point_pair.first/units::cm;
      total_dis2->back() += pow(point_pair.first/units::cm,2);

      if (Npoints->back()==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);
      
    // 	//std::cout << sqrt(dis1)/units::cm << " " << sqrt(dis2)/units::cm << std::endl;
     	if (dis1 < dis2){
     	  beg_dis->push_back(sqrt(dis1));
     	}else{
	  beg_dis->push_back(sqrt(dis2));
     	}
	end_dis->push_back(0);
      }
    //   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->back() = sqrt(dis1);
     	}else{
     	  end_dis->back() = sqrt(dis2);
     	}
      }
    }

    x2->back().push_back(x1);
    y2->back().push_back(y1);
    z2->back().push_back(z1);
    rec_pu->back().push_back(pu);
    rec_pv->back().push_back(pv);
    rec_pw->back().push_back(pw);
    rec_pt->back().push_back(pt);
    dQ_rec->back().push_back((dQ1-dQdx_offset)/dQdx_scale); // hack to match the color scale
    dx->back().push_back(dx1);
    L->back().push_back(total_L->back());
    
    if (T_rec->GetBranch("reduced_chi2")){
      Vreduced_chi2->back().push_back(reduced_chi2);
    }
    if (T_rec->GetBranch("flag_vertex")){
      Vflag_vertex->back().push_back(flag_vertex);
      Vsub_cluster_id->back().push_back(sub_cluster_id);
    }
  

    prev_x1 = x1;
    prev_y1 = y1;
    prev_z1 = z1;
    prev_cluster_id = std::round(ndf);

    //std::cout << prev_cluster_id << std::endl;
  } // loop over i ...
  
  
  pcloud1.AddPoints(ps);
  pcloud1.build_kdtree_index();




  //std::cout << "haha " << std::endl;
  if (file_type==1){
    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)))].second;
      int index1 = 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)))].first;

      // std::cout << index << " " << dQ_tru->back().size() << " " << dQ_tru->front().size() << std::endl;
      if (index <dQ_tru->at(index1).size())
	dQ_tru->at(index1).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));
    }

    //    std::cout << x2->back().size() << std::endl;

    for (size_t k=0;k!=x2->size();k++){
      if (x2->at(k).size()>1){
	std::vector<double> temp_dtheta;
	dtheta->push_back(temp_dtheta);
	max_dtheta->push_back(0);
	total_dtheta->push_back(0);
	
	for (size_t i=0;i!=x2->at(k).size();i++){
	  if (i==0){
	    TVector3 dir1(x2->at(k).at(1)-x2->at(k).at(0),
			  y2->at(k).at(1)-y2->at(k).at(0),
			  z2->at(k).at(1)-z2->at(k).at(0));
	    TVector3 dir2(x2_pair->at(k).at(1) - x2_pair->at(k).at(0),
			  y2_pair->at(k).at(1) - y2_pair->at(k).at(0),
			  z2_pair->at(k).at(1) - z2_pair->at(k).at(0));
	    dtheta->at(k).push_back(dir1.Angle(dir2));
	  }else if(i==x2->at(k).size()-1){
	    TVector3 dir1(x2->at(k).at(x2->at(k).size()-1) - x2->at(k).at(x2->at(k).size()-2),
			  y2->at(k).at(x2->at(k).size()-1) - y2->at(k).at(x2->at(k).size()-2),
			  z2->at(k).at(x2->at(k).size()-1) - z2->at(k).at(x2->at(k).size()-2));
	    TVector3 dir2(x2_pair->at(k).at(x2->at(k).size()-1) - x2_pair->at(k).at(x2->at(k).size()-2),
			  y2_pair->at(k).at(x2->at(k).size()-1) - y2_pair->at(k).at(x2->at(k).size()-2),
			  z2_pair->at(k).at(x2->at(k).size()-1) - z2_pair->at(k).at(x2->at(k).size()-2));
	    dtheta->at(k).push_back(dir1.Angle(dir2));
	  }else{
	    TVector3 dir1(x2->at(k).at(i+1)-x2->at(k).at(i),
			  y2->at(k).at(i+1)-y2->at(k).at(i),
			  z2->at(k).at(i+1)-z2->at(k).at(i));
	    TVector3 dir2(x2_pair->at(k).at(i+1) - x2_pair->at(k).at(i),
			  y2_pair->at(k).at(i+1) - y2_pair->at(k).at(i),
			  z2_pair->at(k).at(i+1) - z2_pair->at(k).at(i));
	    
	    TVector3 dir3(x2->at(k).at(i-1)-x2->at(k).at(i),
			  y2->at(k).at(i-1)-y2->at(k).at(i),
			  z2->at(k).at(i-1)-z2->at(k).at(i));
	    TVector3 dir4(x2_pair->at(k).at(i-1) - x2_pair->at(k).at(i),
			  y2_pair->at(k).at(i-1) - y2_pair->at(k).at(i),
			  z2_pair->at(k).at(i-1) - z2_pair->at(k).at(i));
	    dtheta->at(k).push_back((dir1.Angle(dir2)+dir3.Angle(dir4))/2.);
	  }
	  if (dtheta->at(k).back() > max_dtheta->at(k))
	    max_dtheta->at(k) = dtheta->at(k).back();
	  total_dtheta->at(k) += dtheta->at(k).back();
	}
      }else{
	dtheta->at(k).push_back(0);
      }
    }
  }
  t1->Fill();
  
  T_proj_data->CloneTree(-1,"fast");
  T_proj->CloneTree(-1,"fast");
  
  file->Write();
  file->Close();
  
  
}
