// MyProcessor.cc
//

#include <iostream>
#include <vector>
#include <string>

#include "MyProcessor.h"
#include "FDC/DFDCHit_factory.h"
#include "FDC/DFDCPseudo_factory.h"
#include "FDC/DFDCSegment_factory.h"
#include "TRACKING/DMCTrackHit.h"
#include "TRACKING/DMCThrown_factory.h"
#include "TRACKING/DTrackLinker_factory.h"
// #include "TRACKING/DKalmanFilter_factory.h"
#include "DANA/DApplication.h"
#include "TRACKING/DRiemannFit.h"


// root include files
#include "TFile.h"
#include "TH1.h"
#include "TH1F.h"
#include "TH2F.h"
#include "TH3F.h"
#include "TF1.h"
#include "TSpectrum.h"
#include "TMath.h"
#include <DMatrix.h>

// Root file 
TFile *hfile;
// Histograms
TH1F *xlocal;
TH2F *x_vs_z;
TH2F *dp1_theta;
TH2F *dp2_theta;
TH2F *dp3_theta;
TH2F *dp4_theta;
TH2F *dp1_p;
TH2F *dp2_p;
TH2F *dp3_p;
TH2F *dp4_p;
TH2F *dtanl1_p;
TH2F *dtanl2_p;
TH2F *dtanl3_p;
TH2F *dtanl4_p;
TH2F *xresiduals;
TH2F *yresiduals;
TH2F *xrestrue,*yrestrue;
TH2F *vrestrue,*urestrue;
TH2F *dp1_r;
TH1F *nlinks;
TH1F *fit_chi2;
TH2F *vres_vs_theta;
TH2F *vres_vs_kappa;
TH1F *lrbad;
TH2F *vres_vs_status;
TH2F *vres_vs_doca;
TH2F *Charge;
TH1F *vertex,*vertexd;

static int mycount=0;
static int pack4=0;
static int link2=0;
static int link3=0;
static int link4=0;

ofstream *filehandle; 

//------------------------------------------------------------------
// MyProcessor 
//------------------------------------------------------------------
MyProcessor::MyProcessor()
{
}

//------------------------------------------------------------------
// ~MyProcessor 
//------------------------------------------------------------------
MyProcessor::~MyProcessor()
{
  filehandle->close();
}

//------------------------------------------------------------------
// init 
//------------------------------------------------------------------
jerror_t MyProcessor::init(void)
{
  // hist file
  hfile = new TFile("fdc_mc_analysis.root","RECREATE","fdc MC histograms");
  
  xlocal= new TH1F("xlocal","Local x coordinate from strips matched to wires",
		  6000,-600.0,600.0);
  xlocal->SetXTitle("Local X position (mm)");

  x_vs_z=new TH2F("x_vs_z","x_vs_z",2,200,410,2,-60,60);
  
  dp1_theta = new TH2F("dp1_theta","Momentum resolution using only package 1",
		  100,0,20,100,-1,1);
  dp1_theta->SetXTitle("#theta(degrees)");
  dp1_theta->SetYTitle("#deltapT/pT");

  dp2_theta = new TH2F("dp2_theta","Momentum resolution using only package 2",
		  100,0,20,100,-1,1);
  dp2_theta->SetXTitle("#theta(degrees)");
  dp2_theta->SetYTitle("#deltapT/pT");

  dp3_theta = new TH2F("dp3_theta","Momentum resolution using only package 3",
		  100,0,20,100,-1,1);
  dp3_theta->SetXTitle("#theta(degrees)");
  dp3_theta->SetYTitle("#deltapT/pT");
  dp4_theta = new TH2F("dp4_theta","Momentum resolution using only package 4",
		  100,0,20,100,-1,1);
  dp4_theta->SetXTitle("#theta(degrees)");
  dp4_theta->SetYTitle("#deltapT/pT");

  dp1_p = new TH2F("dp1_p","Momentum resolution using only package 1",
		  10,0.25,5.25,100,-1,1);
  dp1_p->SetXTitle("p (GeV/c)");
  dp1_p->SetYTitle("#deltapT/pT");

  dp2_p = new TH2F("dp2_p","Momentum resolution using only package 2",
		  10,0.25,5.25,100,-1,1); 
  dp2_p->SetXTitle("p (GeV/c)");
  dp2_p->SetYTitle("#deltapT/pT");
  dp3_p = new TH2F("dp3_p","Momentum resolution using only package 3",
		  10,0.25,5.25,100,-1,1);
  dp3_p->SetXTitle("p (GeV/c)");
  dp3_p->SetYTitle("#deltapT/pT");
  dp4_p = new TH2F("dp4_p","Momentum resolution using only package 4",
		  10,0.25,5.25,100,-1,1);
  dp4_p->SetXTitle("p (GeV/c)");
  dp4_p->SetYTitle("#deltapT/pT");
  dtanl1_p= new TH2F("dtanl1_p","Dip angle resolution with package 1",
		    10,0.25,5.25,100,-0.05,0.05);
  dtanl1_p->SetXTitle("p (GeV/c)");
  dtanl1_p->SetYTitle("#delta#lambda");
  dtanl2_p= new TH2F("dtanl2_p","Dip angle resolution with package 2",
		    10,0.25,5.25,100,-0.05,0.05);
  dtanl2_p->SetXTitle("p (GeV/c)");
  dtanl2_p->SetYTitle("#delta#lambda");
  dtanl3_p= new TH2F("dtanl3_p","Dip angle resolution with package 3",
		    10,0.25,5.25,100,-0.05,0.05);
  dtanl3_p->SetXTitle("p (GeV/c)");
  dtanl3_p->SetYTitle("#delta#lambda");
  dtanl4_p= new TH2F("dtanl4_p","Dip angle resolution with package 4",
		    10,0.25,5.25,100,-0.05,0.05);
  dtanl4_p->SetXTitle("p (GeV/c)");
  dtanl4_p->SetYTitle("#delta#lambda");
  xresiduals=new TH2F("xresiduals","xresiduals",24,0.5,24.5,100,-0.5,0.5);
  xresiduals->SetYTitle("Residual (cm)");
  yresiduals=new TH2F("yresiduals","yresiduals",24,0.5,24.5,100,-0.5,0.5); 
  yresiduals->SetYTitle("Residual (cm)");

  xrestrue=new TH2F("xrestrue","x(measure)-x(true)",24,0.5,24.5,101,-0.5,0.5);
  xrestrue->SetYTitle("Residual (cm)");
  yrestrue=new TH2F("yrestrue","y(measured)-y(true)",24,0.5,24.5,101,-0.5,0.5); 
  yrestrue->SetYTitle("Residual (cm)"); 

  urestrue=new TH2F("urestrue","u(measure)-u(true) transverse to wire",24,0.5,24.5,301,-0.75,0.75);
  urestrue->SetYTitle("Residual (cm)");
  vrestrue=new TH2F("vrestrue","v(measured)-v(true) along wire",24,0.5,24.5,301,-0.75,0.75); 
  vrestrue->SetYTitle("Residual (cm)"); 

  vres_vs_theta=new TH2F("vres_vs_theta","vres_vs_theta",40,0,20,301,-0.75,0.75);
 vres_vs_kappa=new TH2F("vres_vs_kappa","vres_vs_kappa",100,0,0.3,301,-0.75,0.75);
  nlinks= new TH1F("nlinks","Number of linked tracks",20,-0.5,19.5);
  
  dp1_r=new TH2F("dp1_r","dp1_r",120,0,60,100,-1,1);
  fit_chi2=new TH1F("fit_chi2","fit_chi2",1000,0,1000);

  lrbad=new TH1F("lrbad","lrbad",7,-0.5,6.5);
  vres_vs_status = new TH2F("vres_vs_status","vres_vs_status",10,-0.5,9.5,
			    301,-0.75,0.75);
  vres_vs_doca = new TH2F("vres_vs_doca","vres_vs_doca",100,-1.,1.,
			   301,-0.75,0.75);
  Charge = new TH2F("Charge","Charge",4,-0.5,3.5,2,-1.5,1.5);

  vertex = new TH1F("vertex","vertex z position",100,40,90);
  vertexd= new TH1F("vertexd","DOCA to origin",100,0,1.);

  filehandle = new ofstream("fdc.txt");
  
  return NOERROR;
}

//------------------------------------------------------------------
// brun 
//------------------------------------------------------------------
jerror_t MyProcessor::brun(JEventLoop *eventLoop, int runnumber)
{
DApplication* dapp=dynamic_cast<DApplication*>(eventLoop->GetJApplication());
  bfield = dapp->GetBfield();
  JFactory_base *base = eventLoop->GetFactory("DFDCSegment");
	segment_factory = dynamic_cast<DFDCSegment_factory*>(base);
	return NOERROR;
}

//------------------------------------------------------------------
// erun 
//------------------------------------------------------------------
jerror_t MyProcessor::erun()
{
     fprintf(stderr,"Number of (1111) events %d linked tracks:  %d %d %d %d\n",
	  pack4,
	  mycount,link2,link3,link4);

 hfile->Write();
 return NOERROR;
}

bool truthsort(const DVector3 a, const DVector3 b){
  return (a(2)>b(2));

}


//------------------------------------------------------------------
// evnt 
//------------------------------------------------------------------
jerror_t MyProcessor::evnt(JEventLoop *eventLoop, int eventnumber)
{
  static int myeventno=1;

  //  cout<<"----------- New Event "<<myeventno++<<" -------------"<<endl;
  vector<const DFDCHit*>fdchits;
  //eventLoop->Get(fdchits);  

  vector<const DFDCPseudo*>pseudopoints;
  eventLoop->Get(pseudopoints);  
  
  vector<const DMCTrackHit*>truepoints;
  eventLoop->Get(truepoints);

  vector<const DMCThrown*>thrown;
  eventLoop->Get(thrown);

  vector<const DTrackLinker*>links;
  eventLoop->Get(links);

  //  vector<const DKalmanFilter*>tracks;
  // eventLoop->Get(tracks);

  float x = 0, y = 0, z = 0, xcorr = 0, ycorr = 0;
  float w_x = 0, w_y = 0, s_x = 0, s_y = 0;
  cout << "number of pseudopoints = " << pseudopoints.size() << endl;
  *filehandle << "event, " << eventnumber << ", " << pseudopoints.size() << endl;
  for (vector<const DFDCPseudo*>::iterator i = pseudopoints.begin();
       i != pseudopoints.end(); i++) {
    x = (*i)->x; y = (*i)->y; z = (*i)->wire->origin(2); xcorr = (*i)->xcorr; ycorr = (*i)->ycorr; w_x = (*i)->w_x; w_y = (*i)->w_y; s_x = (*i)->s_x; s_y = (*i)->s_y;  
    cout << "x = " << x << " y = " << y << " z = " << z << " xcorr = " << xcorr << " ycorr = " << ycorr << " w_x,y = " << w_x << " " << w_y << " s_x,y = " << s_x << " " << s_y << endl;
    *filehandle << x << ", " << y << ", " << z << ", " << xcorr << ", " << ycorr << endl;
  }

  nlinks->Fill(links.size());

  for (unsigned int j=0;j<links.size();j++){
    printf("Track %d:\n",j+1);
     for (unsigned int k=0;k<links[j]->points.size();k++)
	printf("track x %f y %f z %f\n",links[j]->points[k]->x,
	       links[j]->points[k]->y,links[j]->points[k]->wire->origin(2));
     links[j]->S.Print();
     links[j]->cov.Print();
  }

  float mcp,mc_kappa,mc_tanl,theta;
  float mcq,mcpt;
  for (unsigned int i=0;i<thrown.size();i++){
    const DMCThrown *track=thrown[i];
    float p=track->p;
    float phi=track->phi;
    theta=track->theta;
    float pz=p*cos(theta);
    float px=p*sin(theta)*cos(phi);
    float py=p*sin(theta)*sin(phi);
    printf("MC track p= %f\n",p);
    printf("MC track pt= %f\n",sqrt(px*px+py*py));
    printf("MC track tanl = %f\n",pz/sqrt(px*px+py*py));

    printf("mc z %f\n",track->z);
    mcq=track->q;

    mc_tanl=pz/sqrt(px*px+py*py);
    double Bx,By,Bz;
    bfield->GetField(0,0,65.,Bx,By,Bz);
    double B=sqrt(Bx*Bx+By*By+Bz*Bz);
    cout << "B = " << B << endl;
    mc_kappa=0.002998*mcq*B/2./sqrt(px*px+py*py);
    printf("MC kappa = %f tanl= %f\n",mc_kappa,pz/sqrt(px*px+py*py));
    printf("MC phi = %f\n",phi);
    printf("MC xc,yc= %f, %f\n",-sin(phi)/2./mc_kappa,cos(phi)/2./mc_kappa);
    mcp=p;
    mcpt=sqrt(px*px+py*py);
  }
  


  vector<const DFDCSegment*>segments;
  eventLoop->Get(segments);  

  vector<DVector3>mcpackage[4];
  
  DRiemannFit fit;
  for (vector<const DMCTrackHit*>::iterator j=truepoints.begin();
       j!=truepoints.end();j++){
    if ((*j)->system==SYS_FDC){
      float y=((*j)->r)*sin((*j)->phi);
      float x=((*j)->r)*cos((*j)->phi);
      float z=(*j)->z;
      
      fit.AddHitXYZ(x,y,z);



      DVector3 temp;
      temp.SetXYZ(x,y,z);
      if (z<223.1) mcpackage[0].push_back(temp);
      else if (z<283.1) mcpackage[1].push_back(temp);
      else if (z<342.1) mcpackage[2].push_back(temp);
      else mcpackage[3].push_back(temp);
      
    }
  }
  
  fit.FitCircle(0.1,NULL);
  printf("Riemann xc %f yc %f rc %f\n",fit.xc,fit.yc,fit.rc);
  

  for (int i=0;i<4;i++){
    std::sort(mcpackage[i].begin(),mcpackage[i].end(),truthsort);
	
  }


  
  vector<DFDCSegment*>package[4];
  for (unsigned int i=0;i<segments.size();i++){
    const DFDCSegment *segment=segments[i];
    package[(segment->hits[0]->wire->layer-1)/6].push_back((DFDCSegment*)segment);
  }
  /*   
  if (package[0].size()==1 
      //&& package[0][0]->hits.size()==6 
      && package[1].size()==1
      //&& package[1][0]->hits.size()==6 
      && package[2].size()==1  
      //&& package[2][0]->hits.size()==6 
      && package[3].size()==1 
      //&& package[3][0]->hits.size()==6
      )
  */
  if (theta<5.){
  
    for (int i=0;i<4;i++){
      if (mcpackage[i].size()>0 && package[i].size()>0 &&
	  mcpackage[i].size()==package[i][0]->hits.size()
	  && package[i][0]->hits.size()>0
	  // && (package[i][0]->S(0,0)/fabs(package[i][0]->S(0,0))!=mcq)
	  //&& package[i][0]->hits.size()==6
	  ){
	int numbad=0;

	printf("Sizes %d %d\n",package[i][0]->hits.size(),
	       package[i][0]->track.size());
	for (unsigned int m=0;m<package[i][0]->hits.size();m++){ 
	  unsigned int hit_id=package[i][0]->track[m].hit_id;  
	  double cosangle=package[i][0]->hits[m]->wire->udir(1);
	  double sinangle=package[i][0]->hits[m]->wire->udir(0);
	  double mcv=mcpackage[i][m](0)*sinangle+mcpackage[i][m](1)*cosangle;
	  double mcu=mcpackage[i][m](0)*cosangle-mcpackage[i][m](1)*sinangle;
	  double doca=package[i][0]->hits[m]->w-mcu;

	  printf("doca %f\n",doca);

	  printf("truth x %f y %f z %f\n",mcpackage[i][m](0),mcpackage[i][m](1),
	     mcpackage[i][m](2));
	  printf("fdc x %f y %f z %f\n",package[i][0]->hits[m]->x,
		 package[i][0]->hits[m]->y,
		 package[i][0]->hits[m]->wire->origin(2));

	  printf("segment x %f y %f\n",package[i][0]->track[m].dx,
		 package[i][0]->track[m].dy);
	  double helx,hely;
	  segment_factory->GetHelicalTrackPosition(
	    package[i][0]->hits[m]->wire->origin(2),package[i][0],helx,hely);
	  printf("helix x %f y %f\n",helx,hely);
	

	  xrestrue->Fill(package[i][0]->hits[m]->wire->layer,
			 package[i][0]->hits[m]->x-mcpackage[i][m](0));
	  urestrue->Fill(package[i][0]->hits[m]->wire->layer,
			 package[i][0]->hits[m]->w+package[i][0]->hits[m]->dw
			 -mcu);
	  yrestrue->Fill(package[i][0]->hits[m]->wire->layer,
			 package[i][0]->hits[m]->y-mcpackage[i][m](1));
	  float vrestrue_this =
	    package[i][0]->hits[m]->s+package[i][0]->hits[m]->ds - mcv;
	  float layer_this = package[i][0]->hits[m]->wire->layer;
	  vrestrue->Fill(layer_this, vrestrue_this);
	  cout << "vrestrue = " << vrestrue_this << endl;
	  if (abs(vrestrue_this) > 0.05) {
	    cout << "big res: event = " << eventnumber << " layer = " << layer_this << " vrestrue = " << vrestrue_this << endl;  
	  }

	  vres_vs_theta->Fill(theta*180./M_PI, 
		    package[i][0]->hits[m]->s+package[i][0]->hits[m]->ds
			      -mcv); 
	  vres_vs_kappa->Fill(mc_kappa, 
		    package[i][0]->hits[m]->s+package[i][0]->hits[m]->ds
			      -mcv);
	  /* if (fabs(package[i][0]->hits[m]->s+package[i][0]->hits[m]->ds
	     -mcv)>0.008)*/{
	    vres_vs_status->Fill(package[i][0]->hits[m]->status,
				 package[i][0]->hits[m]->s+package[i][0]->hits[m]->ds-mcv);
	   
	    /*if (package[i][0]->hits[m]->wire->layer>12
	      && package[i][0]->hits[m]->wire->layer<19)*/
	    vres_vs_doca->Fill(doca, package[i][0]->hits[m]->s+package[i][0]->hits[m]->ds-mcv);
	  }
	  if (fabs(package[i][0]->hits[m]->s+package[i][0]->hits[m]->ds
		   -mcv)>0.008) numbad++;
	}
	if (package[i][0]->hits.size()==6){
	  lrbad->Fill(numbad);
  
	}
	cout << "num bad " << numbad << " of " << package[i][0]->hits.size()
	     <<endl;
      }
	
    }
      

  pack4++;
  if (segments.size()==4){
    if (links.size()==1) mycount++;
    
    if (links.size()==2) link2++;
    
    if (links.size()==3) link3++; 
    
    if (links.size()==4) link4++;
  }

  //if (segments.size()<=4)
  for (unsigned int k=0;k<segments.size();k++){
    printf("Package %d K %f phi %f D %f tanl %f z0 %f\n",
	   (segments[k]->hits[0]->wire->layer-1)/6,
	   segments[k]->S(0,0),
	   segments[k]->S(1,0),segments[k]->S(2,0),segments[k]->S(3,0),
	   segments[k]->S(4,0));
    printf("xc %f yc %f rc %f\n",segments[k]->xc,segments[k]->yc,
	   segments[k]->rc);


   

    double kappa=segments[k]->S(0,0);
    double tanl=segments[k]->S(3,0);
    double dtheta=M_PI/2.-atan(tanl)-theta;
    printf("chi2 = %f\n",segments[k]->chisq);
    double Bx,By,Bz;
    int middle=segments[k]->hits.size()/2;
   
    bfield->GetField(segments[k]->hits[middle]->x,segments[k]->hits[middle]->y,
		     segments[k]->hits[middle]->wire->origin(2),Bx,By,Bz);
    double B=sqrt(Bx*Bx+By*By+Bz*Bz);
    double pt=0.003*B/2./fabs(kappa);
    
    fit_chi2->Fill(segments[k]->chisq);

    Charge->Fill((segments[k]->hits[0]->wire->layer-1)/6,kappa/fabs(kappa));

    if (fabs(kappa)>0. && kappa/fabs(kappa)==mcq)
      switch((segments[k]->hits[0]->wire->layer-1)/6){
      case 0:
	dp1_p->Fill(mcp,(pt-mcpt)/mcpt);
	dtanl1_p->Fill(mcp,dtheta);
	dp1_theta->Fill(90-180/3.14159*atan(mc_tanl),(pt-mcpt)/mcpt);
	printf("vertex %f\n",segments[k]->S(4,0));
	vertex->Fill(segments[k]->S(4,0));
	vertexd->Fill(segments[k]->S(2,0));
	break;
      case 1:
	dp2_p->Fill(mcp,(pt-mcpt)/mcpt);
	dtanl2_p->Fill(mcp,dtheta);
	dp2_theta->Fill(90-180/3.14159*atan(mc_tanl),(pt-mcpt)/mcpt);
	break;
      case 2:
	dp3_p->Fill(mcp,(pt-mcpt)/mcpt);
	dtanl3_p->Fill(mcp,dtheta);
	dp3_theta->Fill(90-180/3.14159*atan(mc_tanl),(pt-mcpt)/mcpt);
	break;
      case 3:
	dp4_p->Fill(mcp,(pt-mcpt)/mcpt);
	dtanl4_p->Fill(mcp,dtheta);
	dp4_theta->Fill(90-180/3.14159*atan(mc_tanl),(pt-mcpt)/mcpt);
	break;
      }
   
      
    
    for (unsigned int m=0;m<segments[k]->track.size();m++){
      unsigned int hit_id=segments[k]->track[m].hit_id;

      xresiduals->Fill(segments[k]->hits[hit_id]->wire->layer,
		       segments[k]->track[m].dx); 
      yresiduals->Fill(segments[k]->hits[hit_id]->wire->layer,
		       segments[k]->track[m].dy);
		       }
  }
  }
   
  return NOERROR;
 
}