#include <iostream>

#include "TMatrix.h"
#include "Stntuple/obj/TStnTrack.hh"
#include "Stntuple/data/TCalTower.hh"
#include "Stntuple/data/TCesCluster.hh"

#ifdef CDF1
#include "run1cpp/trk/trks_bank.hh"
#include "run1cpp/top/TTop.hh"
#endif


ClassImp(TStnTrack)

namespace {
  const double BF = 1.4116 ;
}

//______________________________________________________________________________
void TStnTrack::ReadV51(TBuffer &R__b) {
  // read version 51 of TStnTrack: fake reading in beam-constrained 4-momentum

  TArrayF obsolete;
  obsolete.Set(4);

  TObject::Streamer(R__b);
					// here go the floats (17)

  R__b.ReadFastArray(&fCot,17);
					// covariance and beam-constrained 
					// parameters (to be optimized)
  obsolete.Streamer(R__b);

  fCov.Streamer(R__b);
					// integer section (10 numbers)
  R__b.ReadFastArray(&fAlgorithm,10);
					// now we need to restore track 
					// 4-momentum from track fit parameters
					// (assume it is a pion for a moment)

  fMomentum.SetPtEtaPhiM(fPt,fEta,fPhi0,0.1396);
}

//______________________________________________________________________________
void TStnTrack::ReadV52(TBuffer &R__b) {
  // read version 52 of TStnTrack: fake reading in beam-constrained 4-momentum
  // this version doesn't have chi**2, which have been added in the V53

  TObject::Streamer(R__b);
					// here go the floats (17)
  R__b.ReadFastArray(&fCot,kNFloats52);
					// covariance and beam-constrained 
					// parameters (to be optimized)
  fCov.Streamer(R__b);
					// integer section (10 numbers)

  R__b.ReadFastArray(&fAlgorithm,kNIntegers52);

					// now we need to restore track 
					// 4-momentum from track fit parameters
					// (assume it is a pion for a moment)

  fMomentum.SetPtEtaPhiM(fPt,fEta,fPhi0,0.1396);

					// chi**2's have been added in V53
					// set them such that all the cuts
					// would always be passed
  fChi2    = -1;
  fChi2Cot = -1;
  fChi2Svx = -1;
}

//______________________________________________________________________________
void TStnTrack::ReadV53(TBuffer &R__b) {
  // read version 53 of TStnTrack: fake reading in beam-constrained 4-momentum
  // this version doesn't have genp and obsp numbers set, which have been 
  // added as a new Int_t and to fNumber respectively, in the V54

  TObject::Streamer(R__b);
					// here go the floats (20)
  R__b.ReadFastArray(&fCot,kNFloats53);
					// covariance and beam-constrained 
					// parameters (to be optimized)
  fCov.Streamer(R__b);
					// integer section (10 numbers)

  R__b.ReadFastArray(&fAlgorithm,kNIntegers53);

					// now we need to restore track 
					// 4-momentum from track fit parameters
					// (assume it is a pion for a moment)

  fMomentum.SetPtEtaPhiM(fPt,fEta,fPhi0,0.1396);

}

//______________________________________________________________________________
void TStnTrack::ReadV54_56(TBuffer &R__b) {
  // read version 54-56 of TStnTrack

  TObject::Streamer(R__b);
					// here go the floats (20)
  R__b.ReadFastArray(&fCot,kNFloats54);
					// covariance and beam-constrained 
					// parameters (to be optimized)
  fCov.Streamer(R__b);
					// integer section (11 numbers)

  R__b.ReadFastArray(&fAlgorithm,kNIntegers54);

					// now we need to restore track 
					// 4-momentum from track fit parameters
					// (assume it is a pion for a moment)

  fMomentum.SetPtEtaPhiM(fPt,fEta,fPhi0,0.1396);

}

//______________________________________________________________________________
void TStnTrack::ReadV57_58(TBuffer &R__b) {

  TObject::Streamer(R__b);
  R__b.ReadFastArray(&fCot,kNFloats57);
  fCov.Streamer(R__b);
  R__b.ReadFastArray(&fAlgorithm,kNIntegers57);
  fMomentum.SetPtEtaPhiM(fPt,fEta,fPhi0,0.1396);

}

//______________________________________________________________________________
void TStnTrack::Streamer(TBuffer &R__b)
{
  // Stream an object of class TStnTrack. From now on we are using custom 
  // Streamer. Run 1 data do not affect the streaming. Start from the simpliest
  // implementation possible, later on custom Streamer for TStnTrackBlock
  // will obsolete the need in this one
  // version 57: adds SiExpected
  // current version: 57 

  if (R__b.IsReading()) {
//-----------------------------------------------------------------------------
//  read section
//-----------------------------------------------------------------------------
    Version_t R__v = R__b.ReadVersion(); 
    if      (R__v <= 51) ReadV51   (R__b);
    else if (R__v == 52) ReadV52   (R__b);
    else if (R__v == 53) ReadV53   (R__b);
    else if (R__v <= 56) ReadV54_56(R__b);
    else if (R__v <= 58) ReadV57_58(R__b);
    else {
					
      TObject::Streamer(R__b);
					// here go the floats

      R__b.ReadFastArray(&fCot,kNFloats59);

					// covariance and beam-constrained 
					// parameters (to be optimized)
      fCov.Streamer(R__b);
					// integer section (13 numbers)

      R__b.ReadFastArray(&fAlgorithm,kNIntegers59);

					// now we need to restore track 
					// 4-momentum from track fit parameters
					// (assume it is a pion for a moment)

      fMomentum.SetPtEtaPhiM(fPt,fEta,fPhi0,0.1396);
    }
  } 
  else {
//-----------------------------------------------------------------------------
//  write section
//-----------------------------------------------------------------------------
    R__b.WriteVersion(TStnTrack::IsA());
    TObject::Streamer(R__b);
					// floats: fCot-fUIso4

    R__b.WriteFastArray(&fCot,kNFloats59);

    fCov.Streamer(R__b);
					// integers

    R__b.WriteFastArray(&fAlgorithm,kNIntegers59);
  }
}


//_____________________________________________________________________________
TStnTrack::TStnTrack(Int_t Number)
  :fMomentum(), fCot(0.0F), fCurv(0.0F), fZ0(0.0F), fD0(0.0F), fPhi0(0.0F), fPt(0.0F), 
   fEta(0.0F), fT0(0.0F), fT0Sigma(0.0F), 
   fBcZ0(0.0F), fBcD0(0.0F), fBcPhi0(0.0F), fBcC0(0.0F), fBcLam0(0.0F), 
  fIso4(0.0F), fUd0(0.0F), fUiso4(0.0F), fChi2(0.0F), fChi2Cot(0.0F), fChi2Svx(0.0F), fCov(), 
  fAlgorithm(0), fVind(0), fBcvind(0), fCotHitWord(0), fSvxHitWord(0), 
   fSvxHitMask(0), fCdfTrackId(0), fCdfTrack(NULL), fSiExpWord(0), 
   fdEdxWord(0), fSide(-1),fWedge(-1),fSideBc(-1),fWedgeBc(-1)
 {
  // 'Number' can be -1 ...
  // Note this makes obsp number default to 0 (unset)
  // and default genp number -1 (unset)
  fNumber        = (Number & 0xffff) | 0xffff0000  ;
  fGenpNumber    = -1;
  fCesCluster[0] = NULL;
  fCesCluster[1] = NULL;
  fXCes[0]       = 999.;
  fXCes[1]       = 999.;
  fDelXCes[0]    = 999.;
  fDelXCes[1]    = 999.;
  fCesShower     = NULL;
  fCesDist       = 999.;
  fXCesBc[0]     = 999.;
  fXCesBc[1]     = 999.;
  fDelXCesBc[0]  = 999.;
  fDelXCesBc[1]  = 999.;
  fDZ0           = 999.;
}

//_____________________________________________________________________________
TStnTrack::TStnTrack(double* Par, double* Cov, int Flag):
  fMomentum(), 
  fPt(0.0F), 
  fEta(0.0F), fT0(0.0F),  fT0Sigma(0.0F),
  fBcZ0(0.0F), fBcD0(0.0F), fBcPhi0(0.0F), fBcC0(0.0F), fBcLam0(0.0F), 
  fIso4(0.0F), fUd0(0.0F), fUiso4(0.0F), 
  fChi2(0.0F), fChi2Cot(0.0F), fChi2Svx(0.0F), fCov(), 
  fAlgorithm(0), fVind(0), fBcvind(0), fCotHitWord(0), fSvxHitWord(0), 
  fSvxHitMask(0), fCdfTrackId(0), fCdfTrack(NULL), fSiExpWord(0),
  fdEdxWord(0),fSide(-1),fWedge(-1),fSideBc(-1),fWedgeBc(-1)
{
   // Flag=1: Cov is 5x5, Flag=2: Cov is the upper triangle
  // 'Number' can be -1 ...
  // Note this makes obsp number default to 0 (unset)
  // and default genp number -1 (unset)

  int    loc;
  double cov[25];

  fNumber        = 0xffffffff  ;
  fGenpNumber    = -1;
  fCesCluster[0] = NULL;
  fCesCluster[1] = NULL;
  fXCes[0]       = 0;
  fXCes[1]       = 0;
  fDelXCes[0]    = 999.;
  fDelXCes[1]    = 999.;
  fCesShower     = NULL;
  fCesDist       = 999.;
  fXCesBc[0]     = 999.;
  fXCesBc[1]     = 999.;
  fDelXCesBc[0]  = 999.;
  fDelXCesBc[1]  = 999.;
  fDZ0           = 999.;

  SetParameters(Par);
  if (Flag == 1) SetCovariance(Cov);
  else {
    loc = 0;
    for (int i=0; i<5; i++) {
      for (int j=i; j<5; j++) {
	cov[5*i+j] = Cov[loc];
	cov[5*j+i] = Cov[loc];
	loc++;
      }
    }
    SetCovariance(cov);
  }
}


// TStnTrack::TStnTrack(TrksBank* trks) {

//   int    num, bc, flag3d, nasl, nssl, bcstatus;
//   float  d0_old, d0, cv, dz, zt, qtp[4];

//   fObjectType = kTrackObject;
//   fCharge     = trks->charge();
//   fChi2       = trks->chi2();

//   fPar.Set(5);
//   fCov.Set(15);
//   fNHits.Set(9);

//   for (int i=0; i<9; i++) {
//     fNHits.fArray[i] = trks->nHits(i);
//   }
//   fNHitsTotal = trks->nHitsTotal();

//   for (int i=0; i< 5; i++) fPar.fArray[i] = trks->par(i);
//   for (int i=0; i<15; i++) fCov.fArray[i] = trks->cov(i);

//   bc = 0;
//   fNumber = trks->number();
//   TTop::Trackq(fNumber,bc,flag3d,d0_old,fD0,nasl,nssl,cv,fDz,zt,qtp,bcstatus);

//   LORENTZ_VECTOR::setV4M(qtp[0],qtp[1],qtp[2],float(0.1396));

//   fPt    = trks->pt();
//   fPhi   = trks->phi0();
//   fLam   = trks->lam0();
//   fD0old = trks->d0();
//   fZ0    = trks->z0();
//   fEta   = log(tan(0.5*atan(1./fabs(fLam))));
//   if (fLam < 0) fEta = -fEta;
	       
// }


//_____________________________________________________________________________
TStnTrack::~TStnTrack() {
  // play tricks to avoid problems with TMatrix::~TMatrix
  //  fCov.Invalidate();
}


//_____________________________________________________________________________
Int_t TStnTrack::NCotHits(Int_t Isl) const {
  // 12 layers per SL

  int il1 = Isl*12;
  int il2 = il1+12;

  int nhits = 0;
  for (int il=il1; il<il2; il++) {
    if (HasCotHit(il)) nhits++;
  }
  return nhits;
}


//_____________________________________________________________________________
Int_t TStnTrack::NCotAxSeg(Int_t MinHits) const {
  // 12 layers per SL

  //  return NAxSeg();

  int nseg = 0;
  for (int i=1; i<=8; i+=2) { 
    if (NCotHits(i) >= MinHits) nseg++; 
  }

  return nseg;
}

//_____________________________________________________________________________
Int_t TStnTrack::NCotStSeg(Int_t MinHits) const {

  //  return NStSeg();

  int nseg = 0;

  for (int i=0; i<8; i+=2) { 
    if (NCotHits(i) >= MinHits) nseg++; 
  }

  return nseg;
}

//_____________________________________________________________________________
Int_t TStnTrack::GetBcMomentum(TLorentzVector* BcMom) {

  double s   = (fBcLam0 >= 0) ? 1 : -1;
  double eta = -s*TMath::Log(TMath::Tan(0.5*TMath::ATan(1/TMath::Abs(fBcLam0))));

  BcMom->SetPtEtaPhiM(fPt*fCurv/fBcC0,eta,fBcPhi0,0.1396);
  return 0;
}

//_____________________________________________________________________________
Int_t TStnTrack::GetMomentum(TLorentzVector* Mom) {
  *Mom = fMomentum;
  return 0;
}

//_____________________________________________________________________________
void TStnTrack::SetParameters(Float_t Lam0, Float_t C0, Float_t Z0, 
			      Float_t Phi0, 
			      Float_t D0) 
{
  double px,py,pz, pt;

  fCot  = Lam0;
  fCurv = C0;
  fZ0   = Z0;
  fD0   = D0;
  fPhi0 = Phi0;
				//

  pt   = 0.5/TMath::Abs(C0)*0.0029979*BF;

  pz   = pt*fCot;
  px   = pt*TMath::Cos(fPhi0);
  py   = pt*TMath::Sin(fPhi0);
  
  fMomentum.SetXYZM(px,py,pz,0.1396);
}

//_____________________________________________________________________________
void TStnTrack::SetParameters(Float_t* Par) {
  Error("SetPar","Not implemented yet");
}

//_____________________________________________________________________________
void TStnTrack::SetParameters(Double_t* Par) {
  Error("SetParameters","Not implemented yet");
}

//_____________________________________________________________________________
void TStnTrack::SetCovariance(Float_t* Cov) {
  Error("SetCovariance","Not implemented yet");
}

//_____________________________________________________________________________
void TStnTrack::SetCovariance(Double_t* Cov) {
  Error("SetCovariance","Not implemented yet");
}

//_____________________________________________________________________________
void TStnTrack::SetCovariance(TMatrix55& Cov) {
  fCov.SetMatrixArray(Cov.GetMatrixArray(),"F");
}

//_____________________________________________________________________________
void TStnTrack::Print(Option_t* opt) const {

  int   ieta;
  float dx, dz, th, tan_th, deteta;

  if ((strstr(opt,"") == 0) || (strstr(opt,"banner") != 0)) {
    printf("----------------------------------------------------------------");
    printf("------------------------------");
    printf("-----------------------------------------------------------------\n");
    printf("trk alg     Pt    Eta    Phi       Px       Py");
    printf("    Lam0     D0       Z0   Iso  Chi2  C^2C (ah sh as ss)");
    printf("  C^2S (sa sa sz) OBSP");
    printf(" eta iw   Zces   Xces dx_ces dz_ces");
    printf("\n");
    printf("----------------------------------------------------------------");
    printf("------------------------------");
    printf("-----------------------------------------------------------------\n");
  }
  if ((strcmp(opt,"") == 0) || (strstr(opt,"data") != 0)) {

    float pt = fMomentum.Pt()*Charge();

    float chi2, chi2_cot, chi2_svx;

    chi2     = ( Chi2   () < 999.9 ) ? Chi2   () : 999.9 ;
    chi2_cot = ( Chi2Cot() < 999.9 ) ? Chi2Cot() : 999.9 ;
    chi2_svx = ( Chi2Svx() < 999.9 ) ? Chi2Svx() : 999.9 ;

    printf("%3i %2i %8.3f %6.3f %6.3f %8.3f %8.3f %6.3f %7.3f %7.2f %5.2f %5.1f %5.1f",
	   Number(), 
	   Algorithm(),
	   pt,  
	   fMomentum.Eta(), 
	   TVector2::Phi_0_2pi(fMomentum.Phi()),
	   fMomentum.Px (), 
	   fMomentum.Py (), 
	   Lam0(),
	   D0(),
	   Z0(), 
	   Iso4(),
	   chi2,
	   chi2_cot);

    printf(" (%2i %2i %2i %2i) %5.1f (%2i %2i %2i) %4i",
	   NCotHitsAx(),
	   NCotHitsSt(),
	   NAxSeg(),
	   NStSeg(),
	   chi2_svx,
	   NSvxRPhiHits(),
	   NSvxSASHits(),
	   NSvxZHits(),
	   ObspNumber());
//-----------------------------------------------------------------------------
// corrections for misalignment NOT yet applied ! this is the place!
//  [0]:Z, 1:[X] - hardware
//-----------------------------------------------------------------------------
    if (fCesCluster[0] == 0) dz = 999.9;
    else                     dz = fDelXCes[0];

    if (fCesCluster[1] == 0) dx = 999.9;
    else                     dx = fDelXCes[1];

    tan_th = fXCes[0]/184.15; // R(CES)

    th     = TMath::ATan(1/tan_th);
    deteta = -TMath::Log(TMath::Tan(th/2.));
    if (fSide == 0) deteta = -deteta;
    ieta   = TCalTower::IEta(deteta);

    printf("  %2i %2i %6.2f %6.2f %6.2f %6.2f\n",
	   ieta,fWedge,fXCes[0],fXCes[1],dx,dz);

  }
}