#define Align_3_cxx
#include "Align_3.h"
#include <TH1.h>
#include <TH2.h>
#include <TGraph.h>
#include <TStyle.h>
#include <TCanvas.h>
#include <TVector3.h>
#include <TRotation.h>
#include <TMatrix.h>

#include <TFumili.h>

#include "TVirtualFitter.h"
#include "TMath.h"
#include "TStopwatch.h"
#include "TRandom.h"
#include "TMinuit.h"

const Double_t PhiBound[16]={0.196350,0.589049,0.981749,1.374449,1.767148,2.159848,2.552547,2.945247,3.337946, 3.730645,4.123345,4.516045,4.908744,5.301444,5.694143,6.086843};
const  Double_t Rsec[16]={4949.,4550.,4949.,4550.,4949.,4550.,4949.,4550.,4949.,4550.,4949.,4550.,4949.,4550.,4949.,4550.};
const  Double_t RSurfsec[16]={4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.,4250.}; //This is where the surface for the parameters at MS spectrometer "entrance" seem to be defined.
const Double_t EggShift[16]={-3.50,-1.89,0.,6.47,7.00,6.47,0.,-1.89,-3.50,-4.57,0.,0.,0.,0.,0.,-4.57}; //These are expected sector-by-sector shifts for the Egg geometry.

Int_t Sector;
//Int_t ChooseSector;
const Int_t NPAR   = 5;
const Int_t MaxMu = 100000; //100000
const Int_t Nsec = 16;
const Int_t MinSector=1; //counting from one
const Int_t MaxSector=16;
const bool SectorList[16]={0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0};
const Double_t MinZ=-20.;
const Double_t MaxZ=20.;
const Double_t MinCt=-0.8;
const Double_t MaxCt=0.8;
const Double_t twoMinZ=2*MinZ; //histogram limits
const Double_t twoMaxZ=2*MaxZ;
const Double_t Mmu=105.7;
const Double_t degrees=180/TMath::Pi();

Int_t NM;
Int_t count[16]={16*0};
Int_t NoCutCount[16]={16*0};
Int_t jj;
Int_t EvtCnt=0;
Int_t Nsector;
Int_t ILoop=0;
Double_t MSd0[MaxMu];
Double_t MSphi[MaxMu];
Double_t MSz0[MaxMu];
Double_t MStheta[MaxMu];
Double_t MSqOp[MaxMu];
Double_t MScovZZ[MaxMu];
Double_t MScovTT[MaxMu];
Double_t MScovZT[MaxMu];
Double_t IDd0[MaxMu];
Double_t IDphi[MaxMu];
Double_t IDz0[MaxMu];
Double_t IDtheta[MaxMu];
Double_t IDqOp[MaxMu];

Double_t Exz0[MaxMu];

Double_t dS,dSerr,dT,dTerr,dZ,dZerr;
Double_t dTh,dTherr,dPh,dPherr;
Double_t Dt[16],Ds[16],Dz[16],Dth[16],Dph[16];
Double_t Dterr[16],Dserr[16],Dzerr[16],Dtherr[16],Dpherr[16];
Double_t  rorgCtmean2[16],rorgmean2[16];

char      name[200];
char      title[200];

FILE *tout;

void Align_3::Loop()
 { //Start
//   In a ROOT session, you can do:
//      Root > .L Align_3.C
//      Root > Align_3 t
//      Root > t.GetEntry(12); // Fill t data members with entry number 12
//      Root > t.Show();       // Show values of entry 12
//      Root > t.Show(16);     // Read and show values of entry 16
//      Root > t.Loop();       // Loop on all entries
//

//     This is the loop skeleton where:
//    jentry is the global entry number in the chain
//    ientry is the entry number in the current Tree
//  Note that the argument to GetEntry must be:
//    jentry for TChain::GetEntry
//    ientry for TTree::GetEntry and TBranch::GetEntry
//
//       To read only selected branches, Insert statements like:
// METHOD1:
//    fChain->SetBranchStatus("*",0);  // disable all branches
//    fChain->SetBranchStatus("branchname",1);  // activate branchname
// METHOD2: replace line
//    fChain->GetEntry(jentry);       //read all branches
//by  b_branchname->GetEntry(ientry); //read only this branch

   if (fChain == 0) return;
   tout = fopen("AARes.txt","w");

   for(int histno=0; histno<16; histno++) {
     sprintf(name,"CotvsDz%i",histno+1);
     sprintf(title,"Cot vs. Dz Sector %i",histno+1);
     CotvsDzprof[histno] = new TProfile(name,title,10,-1.,1.,-100,100);

     sprintf(name,"DzvsZmu%i",histno+1);
     sprintf(title,"Dz vs. Zmu Sector %i",histno+1);
     DzvsZmuprof[histno] = new TProfile(name,title,10,-10.,10.,-100,100);

     sprintf(name,"AlDzvsZmu%i",histno+1);
     sprintf(title,"Aligned Dz vs. Zmu Sector %i",histno+1);
     AlDzvsZmuprof[histno] = new TProfile(name,title,10,-10.,10.,-100,100);

     sprintf(name,"ALCotvsDz%i",histno+1);
     sprintf(title,"Aligned Cot vs. Dz Sector %i",histno+1);
     AlCotvsDzprof[histno] = new TProfile(name,title,10,-1.,1.,-100,100);

     sprintf(name,"DelZ%i",histno+1);
     sprintf(title,"delta Z0");
     DelZ[histno]= new TH1F(name,title,100,twoMinZ,twoMaxZ);

     sprintf(name,"DelZPull%i",histno+1);
     sprintf(title,"delta Z0 Pull");
     DelZPull[histno]= new TH1F(name,title,100,-5,5);

     sprintf(name,"AlDelZ%i",histno+1);
     sprintf(title,"Aligned delta Z0");
     AlDelZ[histno]= new TH1F(name,title,100,twoMinZ,twoMaxZ);

     sprintf(name,"DelTh%i",histno+1);
     sprintf(title,"delta Theta");
     DelCt[histno]= new TH1F(name,title,200,-0.1,0.1);

     sprintf(name,"DelThPull%i",histno+1);
     sprintf(title,"delta Theta Pull");
     DelThPull[histno]= new TH1F(name,title,100,-5,5);

     sprintf(name,"AlDelTh%i",histno+1);
     sprintf(title,"Aligned delta Theta");
     AlDelCt[histno]= new TH1F(name,title,200,-0.1,0.1);

     sprintf(name,"CotvscovZZ%i",histno+1);
     sprintf(title,"Cotan vs. ZZ covariance Sector %i",histno+1);
     CotcovZZ[histno]= new TH2F(name,title,200,-.8,.8,200,0,1000.);

     sprintf(name,"CotvscovTT%i",histno+1);
     sprintf(title,"Cotan vs. TT covariance Sector %i",histno+1);
     CotcovTT[histno]= new TH2F(name,title,200,-.8,.8,200,0,.0001);
   }

   TH1F* dIDz0MCz0 = new TH1F("dIDz0MCz0","ID minus Truth z0",100,-4.,4.);
   TH1F* dIDCtMCCt = new TH1F("dIDCtMCCt","ID minus Truth CoTan",100,-50.,50.);
   TH1F* ZZ13cov = new TH1F("ZZ13cov","ZZ covariance Sect. 13",200,0,100.);
   TH1F* TrkPt = new TH1F("TrkPt","Pt of tracks used",100,0,200.);
   TH1F *h1 = new TH1F("h1","CoTan MS",100,-1.,1.);
   TH1F *h2 = new TH1F("h2","CoTan ID",100,-1.,1.);
   TH1F *h3 = new TH1F("h3","TestDz",100,MinZ,MaxZ);
   TH1F *h4 = new TH1F("h4","MSz0",100,-600,600);
   TH1F *h5 = new TH1F("h5","MSZ0divCot - ExtrZ0",100,-50,50.);
   TH1F *h6 = new TH1F("h6","MSZ0divCot - ExtrZ0",100,-50,50.);

   TH1F *h100 = new TH1F("h100","Delta Theta MS - MC",100,-0.1,0.1);
   TProfile *h101 = new TProfile("h101","Cot Theta MS vs. Z MS Sector 16",100,-.5,.5,-900,900);
   TProfile *h102 = new TProfile("h102","Cot Theta MS vs. Z MS Sector 1",100,-.5,.5,-900,900);
   TProfile *h103 = new TProfile("h103","Cot Theta MS vs. Z MS Sector 2",100,-.5,.5,-900,900);

   TH2F* DzvsPhi = new TH2F("DzvsPhi","Delta Z0 vs. Local Phi", 100,MinZ,MaxZ,50, 0.,0.3927);
   TH2F* AlDzvsPhi = new TH2F("AlDzvsPhi","Aligned Delta Z0 vs. ID Phi", 200, -500, 500, 50, 0.,6.26);
   TH1F* MuMuMass = new TH1F("MuMuMass","Di muon invariant mass",100,0.,200.);
   TH1F* DeltaDY = new TH1F("DeltaDY","Y Measure-Expected Shifts (mm)",20,-5.,5.);
   TH1F* DeltaDZ = new TH1F("DeltaDZ","Z Measure-Expected Shifts (mm)",20,-5.,5.);
   TH1F* DeltaDPhi = new TH1F("DeltaDPhi","Phi Measure-Expected Shifts (mm)",20,-.00001,.00001);
   TH1F* DeltaDTheta = new TH1F("DeltaDTheta","Theta Measure-Expected Shifts (mm)",20,-.002,.002);

   Long64_t nentries = fChain->GetEntriesFast();
   Long64_t nbytes = 0, nb = 0;

Double_t XMSd0[MaxMu][Nsec];
Double_t XMSphi[MaxMu][Nsec];
Double_t XMSz0[MaxMu][Nsec];
Double_t XMStheta[MaxMu][Nsec];
Double_t XMSqOp[MaxMu][Nsec];
 Double_t XMScovZZ[MaxMu][Nsec];
 Double_t XMScovTT[MaxMu][Nsec];
 Double_t XMScovZT[MaxMu][Nsec];
Double_t XIDd0[MaxMu][Nsec];
Double_t XIDphi[MaxMu][Nsec];
Double_t XIDz0[MaxMu][Nsec];
Double_t XIDtheta[MaxMu][Nsec];
Double_t XIDqOp[MaxMu][Nsec]; 

Double_t ExMSz0[MaxMu][Nsec];

   NM=MaxMu;
   if(nentries<MaxMu) NM=nentries;

   for (Long64_t jentry=0; jentry<NM; jentry++) {
      Long64_t ientry = LoadTree(jentry);
      if (ientry < 0) break;
      nb = fChain->GetEntry(jentry);   nbytes += nb;
      EvtCnt++;
      ILoop=0;
      if(MSTrk_z0->size()>0 && IDTrk_z0->size()>0) {
	 ILoop=min(MSTrk_z0->size(),IDTrk_z0->size());
      }
      if(ILoop>=2 && mcMuonPx->size()>1  && (fabs(mcMuonEta->at(0))<1 || fabs(mcMuonEta->at(1))<1)) {
	Float_t pxt=(mcMuonPx->at(0)+mcMuonPx->at(1))/1000.;
	Float_t pyt=(mcMuonPy->at(0)+mcMuonPy->at(1))/1000.;
	Float_t pzt=(mcMuonPz->at(0)+mcMuonPz->at(1))/1000.;
	Float_t E1=sqrt(mcMuonPx->at(0)*mcMuonPx->at(0)+mcMuonPy->at(0)*mcMuonPy->at(0)+mcMuonPz->at(0)*mcMuonPz->at(0)+Mmu*Mmu)/1000.;
	Float_t E2=sqrt(mcMuonPx->at(1)*mcMuonPx->at(1)+mcMuonPy->at(1)*mcMuonPy->at(1)+mcMuonPz->at(1)*mcMuonPz->at(1)+Mmu*Mmu)/1000.;
	Float_t ET=E1+E2;
	Float_t Mmumu=ET*ET-pxt*pxt-pyt*pyt-pzt*pzt;
	if(Mmumu>0) {
	  Mmumu=sqrt(Mmumu);
	  MuMuMass->Fill(Mmumu);
	}
      }
      for(int ik=0; ik<ILoop; ik++) {//ILoop
	//	if(jentry<NM && MSTrk_z0->size()>0 && ppvtxZ->size()>0 && mcMuonPx->size()>0  &&fabs(mcMuonEta->at(ik))<=1.) {//size()>0
	if(jentry<NM && MSTrk_z0->size()>0 && ppvtxZ->size()>0 && fabs(MSTrk_z0->at(ik))<5000.) {//size()>0
	Double_t PhiTr=IDTrk_phi->at(ik);
	//Double_t PhiTr=atan2(mcMuonPy->at(ik),mcMuonPx->at(ik));
	if(PhiTr<0) PhiTr+=6.28319256;

	//Find the sector
	Sector=-1;

	if(PhiTr>=PhiBound[15] || PhiTr<PhiBound[0]) Sector=0;
	for(int kk=0; kk<15; kk++){
	  if(PhiTr>=PhiBound[kk] && PhiTr<PhiBound[kk+1]) Sector=kk+1;
	}
	if(Sector>=0) NoCutCount[Sector]++;

 	Double_t CtTrk=99999.;
	Double_t ThTrk=999.;

	if(mcMuonPy->size()>0){
	  Double_t MuPt=sqrt(mcMuonPy->at(ik)*mcMuonPy->at(ik)+mcMuonPx->at(ik)*mcMuonPx->at(ik));
	  ThTrk=IDTrk_theta->at(ik);
	  if(ThTrk!=0) CtTrk=1/tan(ThTrk);
	  TrkPt->Fill(MuPt/1000.);
	}
	
	Double_t TestDz=ExtrTrk_z0->at(ik)-IDTrk_z0->at(ik);
	Double_t TestCt= -999.;

	TestCt=CtTrk; //Changed from above to make this a cut on geometry instead of MS-ID match in Cotan.
	//	DzvsPhi->Fill(TestDz,PhiTr);
  
	 h1->Fill(1/tan(MSTrk_theta->at(ik)));
	 h2->Fill(CtTrk);
	 if(fabs(CtTrk)<0.01) h3->Fill(TestDz);

	 h100->Fill(MSTrk_theta->at(ik)-ThTrk);
	 if(Sector==15)h101->Fill(1./tan(MSTrk_theta->at(ik)),MSTrk_z0->at(ik));
	 if(Sector==0)h102->Fill(1./tan(MSTrk_theta->at(ik)),MSTrk_z0->at(ik));
	 if(Sector==1)h103->Fill(1./tan(MSTrk_theta->at(ik)),MSTrk_z0->at(ik));
	  CotcovZZ[Sector]->Fill(CtTrk,ExtrTrk_cov22->at(ik));
	  CotcovTT[Sector]->Fill(CtTrk,ExtrTrk_cov44->at(ik));

	  //	  if((SectorList[Sector]) && (TestDz>MinZ && TestDz<MaxZ) && (TestCt>MinCt && TestCt<MaxCt) && (ExtrTrk_cov22->at(ik)<=50.) && (ExtrTrk_cov44->at(ik)<=3.e-6)){ //Fill Arrays
	  if(SectorList[Sector] && (CtTrk>MinCt && CtTrk<MaxCt)&& (TestDz>MinZ && TestDz<MaxZ)) {
	  //if(SectorList[Sector] && (TestDz>MinZ && TestDz<MaxZ) ) { //Fill Arrays
	  //if(SectorList[Sector] && (TestDz>MinZ && TestDz<MaxZ) && (TestCt>MinCt && TestCt<MaxCt)) { //Fill Arrays
	  //if((SectorList[Sector]) && (TestCt>MinCt && TestCt<MaxCt) && (ExtrTrk_cov22->at(ik)<=400.) && (ExtrTrk_cov44->at(ik)<=3.e-5)){ //Fill Arrays

	  XMSd0[count[Sector]][Sector]=MSTrk_d0->at(ik);
	  XMSphi[count[Sector]][Sector]=MSTrk_phi->at(ik);
	  XMSz0[count[Sector]][Sector]=MSTrk_z0->at(ik);
	  XMStheta[count[Sector]][Sector]=MSTrk_theta->at(ik);
	  XMSqOp[count[Sector]][Sector]=MSTrk_qOverP->at(ik);
	  XMScovZZ[count[Sector]][Sector]=ExtrTrk_cov22->at(ik);
	  XMScovTT[count[Sector]][Sector]=ExtrTrk_cov44->at(ik);
	  XMScovZT[count[Sector]][Sector]=ExtrTrk_cov24->at(ik);

	  XIDd0[count[Sector]][Sector]=IDTrk_d0->at(ik);
	  XIDphi[count[Sector]][Sector]=IDTrk_phi->at(ik);
	  XIDz0[count[Sector]][Sector]=IDTrk_z0->at(ik);
	  XIDtheta[count[Sector]][Sector]=IDTrk_theta->at(ik);
	  XIDqOp[count[Sector]][Sector]=IDTrk_qOverP->at(ik);

	  ExMSz0[count[Sector]][Sector]=ExtrTrk_z0->at(ik);

	  Double_t deltaZ=ExtrTrk_z0->at(ik)-IDTrk_z0->at(ik);
	  DelZ[Sector]->Fill(deltaZ);
	  DelZPull[Sector]->Fill(deltaZ/sqrt(ExtrTrk_cov22->at(ik)));
	  CotvsDzprof[Sector]->Fill(CtTrk,deltaZ);
	  Double_t Zmu=MSTrk_z0->at(ik);
	  DzvsZmuprof[Sector]->Fill(deltaZ,Zmu);//deltaZ is at perigee, Zmu at MS
	 
	  DelCt[Sector]->Fill(MSTrk_theta->at(ik)-ThTrk);  //Currently filling with DelTheta
	  DelThPull[Sector]->Fill((MSTrk_theta->at(ik)-ThTrk)/sqrt(ExtrTrk_cov44->at(ik)));

	  count[Sector]++;       
	}
      }  
    }
   }
   Double_t mean[16];
   Double_t orgmean[16];
   Double_t AlDiffZ[16],AlDiffCt[16];
   Double_t DCtmean[16];
   Double_t DorgCtmean[16];
   Double_t rCtmean2[16];
   Double_t rmean2[16];
   Double_t bin[16],xerr[16];
   for(int ii=MinSector-1; ii<MaxSector; ii++) {
     xerr[ii]=0;
     if(SectorList[ii]){
      for(int ll=0; ll<count[ii]; ll++){//ll<count[ii]
       MSd0[ll]=XMSd0[ll][ii];
       MSphi[ll]=XMSphi[ll][ii];
       MSz0[ll]=XMSz0[ll][ii];
       MStheta[ll]=XMStheta[ll][ii];
       MSqOp[ll]=XMSqOp[ll][ii];
       MScovZZ[ll]=XMScovZZ[ll][ii];
       MScovTT[ll]=XMScovTT[ll][ii];
       MScovZT[ll]=XMScovZT[ll][ii];

       Exz0[ll]=ExMSz0[ll][ii];

       if(ii==12) ZZ13cov->Fill(MScovZZ[ll]);

       IDd0[ll]=XIDd0[ll][ii];
       IDphi[ll]=XIDphi[ll][ii];
       IDz0[ll]=XIDz0[ll][ii];
       IDtheta[ll]=XIDtheta[ll][ii];
       IDqOp[ll]=XIDqOp[ll][ii];
 
       h5->Fill(MSz0[ll]-RSurfsec[ii]/tan(MStheta[ll])-IDz0[ll]);
       //       std::cout<<"Event, MSz0, MStheta, IDz0, IDphi"<<ll<<", "<<MSz0[ll]<<", "<<MStheta[ll]<<", "<<IDz0[ll]<<", "<<IDphi[ll]<<std::endl;
       //       std::cout<<"dZ0 "<<MSz0[ll]-RSurfsec[ii]/tan(MStheta[ll])-IDz0[ll] <<std::endl;
      }

      jj=count[ii];
      rorgmean2[ii]=DelZ[ii]->GetRMS();  //rms/sqrt(N)
      rorgCtmean2[ii]=DelCt[ii]->GetRMS(); //removed: /sqrt(count[ii])
      Nsector=ii;
      //      cout<<"Sector "<< ii+1<<"Count= "<<count[ii]<<" dZRMS= "<<rorgmean2[ii]<<" dCtRMS= "<<rorgCtmean2[ii]<<endl;
      cout<<"******SECTOR "<<ii+1<<" ***********"<<endl;
      cout<<"****Count[Sector] "<<count[ii]<<" ******"<<endl;

      minexam();

      Ds[ii]=dS;
      Dserr[ii]=dSerr;
      Dz[ii]=dZ;
      Dzerr[ii]=dZerr;
      Dt[ii]=dT;
      Dterr[ii]=dTerr;
      Dth[ii]=dTh;
      Dtherr[ii]=dTherr;
      Dph[ii]=dPh;
      Dpherr[ii]=dPherr;
      //      cout<<"Sector= "<<ii<<" Dy= "<<dY<<" Dz= "<<dZ<<" Dth= "<<dTh<<endl;

      Double_t MeanSectorPhi=Nsector*(TMath::Pi()/8);
      //      Double_t MeanSectorPhi=PhiBound[ii]-0.19635;

      //Make a histogram of the delZ values after the alignment
      Double_t Zmu=-1.;
      for(int ll=0; ll<count[ii];ll++){

	Double_t LocPhi=-999.;
	  if(MStheta[ll]!=0 && IDtheta[ll]!=0){

	    LocPhi=IDphi[ll];
	    if(LocPhi<0) LocPhi+=6.28319256;

	    if(LocPhi>=PhiBound[15]) {
	      LocPhi=6.28319256-LocPhi;
	    }
	    else{
	      LocPhi=MeanSectorPhi-LocPhi;
	    }
	    Double_t Rmu;
	    if(cos(LocPhi)!=0){
	      Rmu=RSurfsec[Nsector]/cos(LocPhi); //Distance in x-y plane to muon
	    }
	    else{
	      std::cout<<"BAD LOCPHI in main routine!!!"<<std::endl;
	      Rmu=RSurfsec[Nsector];
	    }
	    TVector3 T,S,V;  //S,T are the AMDB sector coords, V is the muon vector
	    TVector3 MuVec,Zaxis,P; //Mu unit vec, Zaxis vector and perpendicular
	    Double_t X,Y,Z; //Vector from ATLAS global origin to muon track as MS entrance
	    Double_t Xmu,Ymu,Zmu; //Unit vec in direction of mu track, origin at track at MS entrance
	    Double_t VecArray[3];
	    T.SetMagThetaPhi(Rsec[Nsector],TMath::Pi()/2,MeanSectorPhi); //In the ATLAS x-y plane
	    S.SetMagThetaPhi(Rsec[Nsector],TMath::Pi()/2,MeanSectorPhi); //In the ATLAS x-y plane
	    S.RotateZ(TMath::Pi()/2); //S_sector direction
	    //Unit vector (V) from ATLAS origin to muon track at MS
	    X=Rmu*cos(IDphi[ll]);
	    Y=Rmu*sin(IDphi[ll]);
	    Z=MSz0[ll];
	    V.SetXYZ(X,Y,Z); //exit point of muon gets transformed
	    //Muon unit vector
	    Xmu=cos(IDphi[ll])*sin(MStheta[ll]);
	    Ymu=sin(IDphi[ll])*sin(MStheta[ll]);
	    Zmu=cos(MStheta[ll]);
	    MuVec.SetXYZ(Xmu,Ymu,Zmu);
	    Zaxis.SetXYZ(0,0,1);
	    //	    std::cout<<"X,Y,Z "<<X<<", "<<Y<<", "<<Z<<std::endl;
	    //	    std::cout<<"MStheta, LocPhi "<<MStheta[ll]<<", "<<LocPhi<<std::endl;
	    V.Rotate(dTh,S); //Rotation by x[3] around S
	    V.RotateZ(dPh); //Rotation by x[4] around Z

	    //Same for MuVec
	    MuVec.Rotate(dTh,S);
	    MuVec.RotateZ(dPh);

	    V.GetXYZ(VecArray);
	    //Note that T & S have remained fixed at their nominal orientations. 
	    //Now translations..First along T (radial) direction

	    X = VecArray[0] + dT*cos(T.Phi());
	    Y = VecArray[1] + dT*sin(T.Phi());
	    //Next along z
	    Z = VecArray[2] + dZ;
	    //And now along S
	    X += dS*cos(S.Phi());
	    Y += dS*sin(S.Phi());
	    V.SetXYZ(X,Y,Z);
	    //	    std::cout<<"dS,dT,dZ,dTh,dPh "<<dS<<", "<<dT<<", "<<dZ<<", "<<dTh<<", "<<dPh<<std::endl;
	    //Find Z0
	    P=MuVec.Cross(Zaxis);
	    P=P/P.Mag(); //unit vector perp to Zaxis and MuVec
	    Double_t t=(-P.y()*V.x()/P.x()+V.y())/(P.y()/P.x()*MuVec.x()-MuVec.y());
	    //For small Py/Px and Vx<<Vy, MuVecx<MuVecy, this is ~-V.y()/MuVec.y()
	    //Double_t t=-V.Dot(MuVec)/MuVec.Mag2();
	    Double_t AlMSz0=V.z()+t*MuVec.z()*cos(LocPhi);
	    Double_t AlTh=MStheta[ll]+dTh;
	    //std::cout<<"AlTh,AlMSz0 "<<AlTh<<", "<<AlMSz0<<std::endl;
	    //Double_t AlMSz0=Z+0.6-(Rmu+6.8)/tan(MStheta[ll]);
	    //std::cout<<"Final AlMSz0, IDz0 "<<AlMSz0<<", "<<IDz0[ll]<<std::endl;
	    h6->Fill(AlMSz0-IDz0[ll]);
	    Double_t Origz0=MSz0[ll]-Rmu/tan(MStheta[ll]); //Test - can be removed
	    //std::cout<<"Origz0,ALMSz0,IDz0 "<<Origz0<<", "<<AlMSz0<<", "<<IDz0[ll]<<std::endl;
	  }
	  Double_t diff=AlMSz0-IDz0[ll];
	  h4->Fill(Zmu);
	  AlDelZ[ii]->Fill(diff);
	  AlCotvsDzprof[ii]->Fill(1./tan(IDtheta[ll]),diff);
	  DzvsPhi->Fill(MSz0[ll],LocPhi);
	  //	  AlDelCt[ii]->Fill(diffCt);
	  AlDelCt[ii]->Fill(MStheta[ll]+dTh-IDtheta[ll]); //Currently filling with DelTheta
	  PhiTr=IDphi[ll];
	  if(PhiTr<0) PhiTr+=6.28319256; 
	  AlDzvsPhi->Fill(diff,PhiTr);
	  AlDzvsZmuprof[ii]->Fill(diff,Zmu);
      }
    }//SectorList
   }//loop over ii
   for(int ii=0; ii<MaxSector; ii++) {
     bin[ii]=ii+1; //Number sectors from 1
     mean[ii]=AlDelZ[ii]->GetMean();
     orgmean[ii]=DelZ[ii]->GetMean();

     rmean2[ii]=AlDelZ[ii]->GetRMS();  //removed: /sqrt(count[ii])
     AlDiffZ[ii]=fabs(orgmean[ii])-fabs(mean[ii]);
     DCtmean[ii]=AlDelCt[ii]->GetMean();
     DorgCtmean[ii]=DelCt[ii]->GetMean();
     rCtmean2[ii]=AlDelCt[ii]->GetRMS(); //removed: /sqrt(count[ii])
     AlDiffCt[ii]=fabs(DorgCtmean[ii])-fabs(DCtmean[ii]);
   }

   TCanvas *c10 = new TCanvas("c10","Delta Z vs. CotTh B&A",200,10,1200,800);
   c10->Divide(1,3);
   c10->cd(1);
   CotvsDzprof[0]->SetMarkerStyle(20);
   CotvsDzprof[0]->SetMarkerColor(1);
   CotvsDzprof[0]->Draw();
   CotvsDzprof[0]->SetMarkerColor(2);
   AlCotvsDzprof[0]->SetMarkerStyle(21);
   AlCotvsDzprof[0]->Draw("same");
   c10->cd(2);
   CotvsDzprof[4]->SetMarkerColor(1);
   CotvsDzprof[4]->SetMarkerStyle(20);
   CotvsDzprof[4]->Draw();
   CotvsDzprof[4]->SetMarkerColor(2);
   AlCotvsDzprof[4]->SetMarkerStyle(21);
   AlCotvsDzprof[4]->Draw("same");
   c10->cd(3);
   CotvsDzprof[8]->SetMarkerColor(1);
   CotvsDzprof[8]->SetMarkerStyle(20);
   CotvsDzprof[8]->Draw();
   CotvsDzprof[8]->SetMarkerColor(2);
   AlCotvsDzprof[8]->SetMarkerStyle(21);
   AlCotvsDzprof[8]->Draw("same");
   cout<<" TOTAL EVENTS ANALYZED: "<<EvtCnt<<endl;
   //Results in readable format
   for(int ii=0; ii<16; ii++){
     cout<<"Sector= "<<ii+1<<" DT= "<<Dt[ii]<<" Dz= "<<Dz[ii]<<" Dth= "<<Dth[ii]<<endl;
     fprintf(tout,"Sector= %d Nev= %d, Nev(no cuts)= %d, Dy= %.3f +/- %.3f, Dz= %.3f +/- %.3f, Dth= %.6f +/- %.6f\n",ii+1,count[ii],NoCutCount[ii],Dt[ii],Dterr[ii],Dz[ii],Dzerr[ii],Dth[ii],Dtherr[ii]);
   }

   cout<<endl;
   //And now as it's needed in the code:
   cout<<"DT"<<endl;
   for(int ii=0; ii<16; ii++){
     cout<<Dt[ii]<<",";
   }
   cout<<endl;
   cout<<"Dz"<<endl;
   for(int ii=0; ii<16; ii++){
     cout<<Dz[ii]<<",";
   }
   cout<<endl;
   cout<<"Dth"<<endl;
   for(int ii=0; ii<16; ii++){
     cout<<Dth[ii]<<",";
   }
   cout<<endl;
   double dDy[16];
   Float_t IBin[16];
   Double_t CotMean[16];
   for(Int_t ki=0; ki<16; ki++) {
     dDy[ki]=Dt[ki]-EggShift[ki];
     if(SectorList[ki]) {
       DeltaDY->Fill(dDy[ki]);
       DeltaDZ->Fill(Dz[ki]); //Just Dz since expected=0.0
       DeltaDPhi->Fill(Dph[ki]);
       DeltaDTheta->Fill(Dth[ki]);
     }
     IBin[ki]=ki;
     CotMean[ki]=CotcovZZ[ki]->GetMean(1);
   }
   /*
   TGraphErrors *gr900 = new TGraphErrors(16,bin,dDy,xerr,Dterr);
   TCanvas *c11 = new TCanvas("c11","Aligned shifts minus expected shifts",200,10,800,600);
   //   c11->Divide(1,2);
   c11->cd(1);
   gr900->SetMarkerStyle(21);
   gr900->SetMarkerSize(1.1);
   gr900->SetTitle("Aligned shifts minus expected shifts");
   gr900->SetMaximum(2.0);
   gr900->SetMinimum(-2.0);
   gr900->Draw("AP");
   TCanvas *c12 = new TCanvas("c12","Mean ID Cotan per sector",200,10,800,600);
   c12->cd(1);
   DeltaDY->Draw();
   */
   fclose(tout);
}

void Align_3::minexam()
{

 Int_t ierflg = 0;
  TStopwatch timer;

   // Initialize TMinuit via generic fitter interface with a maximum of 5 params
   //TVirtualFitter *minuit = TVirtualFitter::Fitter(0, 3);
   TMinuit *old = gMinuit;
   TMinuit *minuit = new TMinuit(NPAR);  

printf("Starting timer\n");
   timer.Start();
   minuit->SetFCN(fcnk0);
   minuit->mnparm(0, "dS",    0,     0.1, -100,100,ierflg); //Displacement along S_sector
   minuit->mnparm(1, "dT",    0,     0.1, -100,100,ierflg); //Displacement along T_sector
   minuit->mnparm(2, "dZ",    0,     0.1, -100,100,ierflg); //Displacement along Z_atlas=Z_sector
   minuit->mnparm(3, "dTh",   0,     0.005, -0.2,0.2,ierflg); //rotation in theta (about S)
   minuit->mnparm(4, "dPh",   0,     0.005, -0.2,0.2,ierflg); //rotation in phi

   Double_t arglist[100];
   arglist[0] = 0;
   printf("Calling fcnk0\n");
   minuit->mnexcm("CALL FCN", arglist ,0,ierflg);
   printf("Done calling fcnk0\n");
   
   
   //arglist[0] = 1;
   //minuit->mnexcm("FIX ",arglist, 1, ierflg); //Fixing dS for now
   //arglist[0] = 3;
   //minuit->mnexcm("FIX ",arglist, 1, ierflg); //Fixing dZ for now
   //arglist[0] = 4;
   //minuit->mnexcm("FIX ",arglist, 1, ierflg); //Fixing dTh for now
   //arglist[0] = 5;
   //minuit->mnexcm("FIX ",arglist, 1, ierflg); //Fixing dPh for now
   
   
   arglist[0] = 0;
   minuit->mnexcm("SET PRINT", arglist, 1,ierflg);
   arglist[0] = 0;
   minuit->mnexcm("MIGRAD", arglist, 0,ierflg);
   minuit->mnexcm("MINOS", arglist, 0,ierflg);

   minuit->GetParameter(0,dS,dSerr);
   minuit->GetParameter(1,dT,dTerr);
   minuit->GetParameter(2,dZ,dZerr);
   minuit->GetParameter(3,dTh,dTherr);
   minuit->GetParameter(4,dPh,dPherr);

      arglist[0]=2;
      minuit->mnexcm("SCAN",arglist,2,ierflg);
      TCanvas *c99 = new TCanvas("c99","dT Scan",200,10,800,600);
      TGraph *gr0 = (TGraph*)minuit->GetPlot();
      gr0->SetMarkerStyle(21);
      gr0->Draw("alp");

      arglist[0]=3;
      minuit->mnexcm("SCAN",arglist,3,ierflg);
      TCanvas *c999 = new TCanvas("c999","dZ Scan",200,10,800,600);
      TGraph *gr00 = (TGraph*)minuit->GetPlot();
      gr00->SetMarkerStyle(21);
      gr00->Draw("alp");

   printf("Time at the end of job = %f seconds\n",timer.CpuTime());
   //   return 0;
}

void fcnk0(Int_t &, Double_t *, Double_t &f, Double_t *x, Int_t iflag)
{
  Double_t DelT=0;
  Double_t DelZ=0;
  Double_t VecArray[3];
  TVector3 S,V,T; //S,T are the AMDB sector coords, V is the muon vector
  TVector3 MuVec,Zaxis,P;
  Double_t X,Y,Z; //Vector from ATLAS global origin to muon track as MS entrance
  Double_t Xmu,Ymu,Zmu; //Unit vector in direction of muon track, origin at track at MS entrance
  Double_t Rmu;
  Double_t chisq;
  Double_t sigd0,sigthet0,sigz0,sigphi;
  Double_t IDCotTh;
  TMatrixD* CovM = new TMatrixD(2,2);
  Double_t LocPhi;
  Double_t CovEntries[4];
  Double_t DVec[2];

   chisq=0;
   sigd0=1;
   sigthet0=rorgCtmean2[Nsector];
   sigphi=1;
   sigz0=rorgmean2[Nsector];

   //   Double_t MeanSectorPhi=PhiBound[Nsector]-0.19635;
   Double_t MeanSectorPhi=Nsector*(TMath::Pi()/8);

   //Parameters: x[0]=dS, x[1]=dT, x[2]=dZ, x[3]=dTheta (around S), x[4]=dPhi (around Z)

for (int j=0; j< jj ; j++)
  {
    if(MStheta[j]!=0 && IDtheta[j]!=0){
      CovEntries[0]=MScovZZ[j];
      CovEntries[1]=MScovZT[j];
      CovEntries[2]=CovEntries[1];
      CovEntries[3]=MScovTT[j];
      CovM->SetMatrixArray(CovEntries); //2x2 covariance matrix
      CovM->Invert(); //Invert covanriance matrix
      double* CovI= CovM->GetMatrixArray(); //CovI is an *array* of elements of inverted matrix
      LocPhi=IDphi[j];
      if(LocPhi<0) LocPhi += 2*TMath::Pi();

      if(LocPhi>=PhiBound[15]) {
	 LocPhi= 2*TMath::Pi()-LocPhi;
      }
      else{
	LocPhi=fabs(MeanSectorPhi-LocPhi);
      }
      if(cos(LocPhi)!=0){
	Rmu=RSurfsec[Nsector]/cos(LocPhi); //Distance in x-y plane to muon
	//Rmu=Rsec[Nsector]/cos(LocPhi);
      }
      else{
	std::cout<<"BAD LOCPHI!!!"<<std::endl;
	Rmu=RSurfsec[Nsector];
      }
      T.SetMagThetaPhi(Rsec[Nsector],TMath::Pi()/2,MeanSectorPhi); //In the ATLAS x-y plane
      S.SetMagThetaPhi(Rsec[Nsector],TMath::Pi()/2,MeanSectorPhi); //In the ATLAS x-y plane
      S.RotateZ(TMath::Pi()/2); //S_sector direction
      //Vector (V) from ATLAS origin to muon track at MS
      X=Rmu*cos(IDphi[j]);
      Y=Rmu*sin(IDphi[j]);
      Z=MSz0[j];
      V.SetXYZ(X,Y,Z); //exit point of muon gets transformed

      //Muon unit vector
      Xmu=cos(IDphi[j])*sin(MStheta[j]);
      Ymu=sin(IDphi[j])*sin(MStheta[j]);
      Zmu=cos(MStheta[j]);
      MuVec.SetXYZ(Xmu,Ymu,Zmu);
      Zaxis.SetXYZ(0,0,1);
      /*
      std::cout<<"Start:"<<std::endl;
      std::cout<<"V.x,y,z "<<V.x()<<", "<<V.y()<<", "<<V.z()<<std::endl;
      std::cout<<"MuVec.x,y,z "<<MuVec.x()<<", "<<MuVec.y()<<", "<<MuVec.z()<<std::endl;
      std::cout<<"MuVec.Theta,Phi "<<MuVec.Theta()*degrees<<", "<<MuVec.Phi()*degrees<<std::endl;
      std::cout<<"End:"<<std::endl;
      */
      V.Rotate(x[3],S); //Rotation by x[3] around S
      V.RotateZ(x[4]); //Rotation by x[4] around Z

      //Same for MuVec
      MuVec.Rotate(x[3],S);
      MuVec.RotateZ(x[4]);

      V.GetXYZ(VecArray);
      //Note that T & S have remained fixed at their nominal orientations. 
      //Now translations..First along T (radial) direction
      X = VecArray[0] + x[1]*cos(T.Phi());
      Y = VecArray[1] + x[1]*sin(T.Phi());
      //Next along z
      Z = VecArray[2] + x[2];
      //And now along S
      X += x[0]*cos(S.Phi());
      Y += x[0]*sin(S.Phi());
      V.SetXYZ(X,Y,Z);

      //Same for MuVec
      /* MuVec.GetXYZ(VecArray);
      Xmu = VecArray[0] + x[1]*cos(T.Phi());
      Ymu = VecArray[1] + x[1]*sin(T.Phi());
      //Next along z
      Zmu = VecArray[2] + x[2];
      //And now along S
      Xmu += x[0]*cos(S.Phi());
      Ymu += x[0]*sin(S.Phi());
      MuVec.SetXYZ(Xmu,Ymu,Zmu);
      */

      //Find Z0
      P=MuVec.Cross(Zaxis);
      P=P/P.Mag(); //unit vector perp to Zaxis and MuVec
      Double_t t=(-P.y()*V.x()/P.x()+V.y())/(P.y()/P.x()*MuVec.x()-MuVec.y());
      //For small Py/Px and Vx<<Vy, MuVecx<MuVecy, this is ~-V.y()/MuVec.y()
      //Double_t t=-V.Dot(MuVec)/MuVec.Mag2();
      Double_t z0=V.z()+t*MuVec.z()*cos(LocPhi);
      Double_t AlTh=MStheta[j]+x[3];
      DVec[0]=z0-IDz0[j];
      DVec[1]=AlTh-IDtheta[j];
      chisq+= CovI[0]*DVec[0]*DVec[0]+CovI[1]*DVec[0]*DVec[1]+CovI[2]*DVec[1]*DVec[0]+CovI[3]*DVec[1]*DVec[1];
      /*
      std::cout<<"x 0,1,2,3,4,chisq "<<x[0]<<", "<<x[1]<<", "<<x[2]<<", "<<x[3]<<", "<<x[4]<<", "<<chisq<<std::endl;
      std::cout<<"V.x,y,z "<<V.x()<<", "<<V.y()<<", "<<V.z()<<std::endl;
      std::cout<<"MuVec.x,y,z "<<MuVec.x()<<", "<<MuVec.y()<<", "<<MuVec.z()<<std::endl;
      std::cout<<"MuVec.Theta,Phi "<<MuVec.Theta()*degrees<<", "<<MuVec.Phi()*degrees<<std::endl;
      std::cout<<"P.x,P.y,P.z,t "<<P.x()<<", "<<P.y()<<", "<<P.z()<<", t: "<<t<<std::endl;
      std::cout<<"z0, IDzo, MStheta, IDtheta "<<z0<<", "<<IDz0[j]<<", "<<MStheta[j]*degrees<<", "<<IDtheta[j]*degrees<<std::endl;
      std::cout<<"Extr MS z0 "<<Exz0[j]<<std::endl;
      */
      //std::cout<<"DVec[0],[1] chisq "<<DVec[0]<<", "<<DVec[1]<<", "<<chisq<<std::endl;
      //std::cout<<"CovI[0],[1],[2],[3] "<<CovI[0]<<", "<<CovI[1]<<", "<<CovI[2]<<", "<<CovI[3]<<std::endl;
      //chisq+=(z0-IDz0[j])*(z0-IDz0[j])/(MScovZZ[j]);
	//+(AlTh-IDtheta[j])*(AlTh-IDtheta[j])/MScovTT[j]
	//      +(MSCotTh-1/tan(IDtheta[j]))*(MSCotTh-1/tan(IDtheta[j]))/(sigthet0*sigthet0);
      }
  }
// cout<<" chisq = "<<chisq<<" x[1]= "<<x[1]<<" x[2]= "<<x[2]<<" x[3]="<<x[3]<<" x[4]="<<x[4]<<endl;
  f = chisq;
   }