// -------------------------------------------------------------------------
//			CbmTrackParP source file 
//			Created by M. Al-Turany  06.02.2007
// -------------------------------------------------------------------------

#include <iostream>
#include "CbmTrackParP.h"
#include "TMath.h"
using namespace std;

// -----   Default constructor   -------------------------------------------
CbmTrackParP::CbmTrackParP(): CbmTrackPar() {
 Reset(); 
}


// -----   Constructor with track parameters   -----------------------------------
CbmTrackParP::CbmTrackParP(Double_t x, Double_t y, Double_t z,
				   Double_t tx, Double_t ty, Double_t qp,
				   Double_t CovMatrix[15])
  : CbmTrackPar(x,y,z,0,0,0,0)
{
 
  fTx = tx;
  fTy = ty;
  fQp = qp;
  Double_t P  = TMath::Abs(1/fQp);
  fq= P * fQp;
  for(Int_t i=0;i<15;i++)  {
	fCovMatrix[i]=CovMatrix[i];
  }
  fPz = TMath::Sqrt( P*P / ( fTx*fTx + fTy*fTy + 1 ) );
  fPx = fTx * fPz;
  fPy = fTy * fPz;
  
  fDQp  = TMath::Sqrt(fCovMatrix[0]);
  fDTx  = TMath::Sqrt(fCovMatrix[5]);
  fDTy  = TMath::Sqrt(fCovMatrix[9]);
  fDX   = TMath::Sqrt(fCovMatrix[12]);
  fDY   = TMath::Sqrt(fCovMatrix[14]);
  fDZ   = 0;
  fDPx= GetDPx(); 
  fDPy= GetDPy(); 
  fDPz= GetDPz(); 
  
}
// -------------------------------------------------------------------------
 void CbmTrackParP::SetTrackPar(Double_t X,  Double_t Y,  Double_t Z,
		Double_t Px, Double_t Py, Double_t Pz, Double_t Q,
		Double_t  CovMatrix[15])

{
  Reset();
  SetPx(Px);
  SetPy(Py);
  SetPz(Pz);

  SetX(X); //x (lab)
  SetY(Y); //y (lab)
  SetZ(Z); //z (lab)
  
  Double_t P = TMath::Sqrt(fPx*fPx +fPy*fPy +fPz*fPz );

//  cout << " X " << fX << " Y " << fY << " Z " << fZ << endl;
//  cout << " X " << pos.x() << " Y " << pos.y() << " Z " << pos.z() << endl; 
 
  if(Q!=0)fq= Q/TMath::Abs(Q);     

  if(fPz!=0){
  	fTx = fPx/fPz;
  	fTy = fPy/fPz;
	fQp = fq /P;
  }else {
  	fTx = 0;
  	fTy = 0;  
	fQp = 0;
  }

  for(Int_t i=0;i<15;i++)  {
	fCovMatrix[i]=CovMatrix[i];
  }


  fDQp  = TMath::Sqrt(fCovMatrix[0]);
  fDTx  = TMath::Sqrt(fCovMatrix[5]);
  fDTy  = TMath::Sqrt(fCovMatrix[9]);
  fDX   = TMath::Sqrt(fCovMatrix[12]);
  fDY   = TMath::Sqrt(fCovMatrix[14]);
  fDZ   = 0;
  fDPx= GetDPx(); 
  fDPy= GetDPy(); 
  fDPz= GetDPz(); 

  cout << " fDQp " << fDQp << " fDTx " << fDTx << " fDTy " << fDTy << endl;  

}
// -----   Constructor with point parameters -----------------------------------
CbmTrackParP::CbmTrackParP(TVector3 pos, TVector3 Mom ,TVector3 posErr, TVector3 MomErr ,Double_t q)
: CbmTrackPar(pos.x(),pos.y(),pos.z(),Mom.x(),Mom.y(),Mom.z(),q)
{

  Reset();
  SetPx(Mom.x());
  SetPy(Mom.y());
  SetPz(Mom.z());

  SetX(pos.x()); //x (lab)
  SetY(pos.y()); //y (lab)
  SetZ(pos.z()); //z (lab)

//  cout << " X " << fX << " Y " << fY << " Z " << fZ << endl;
//  cout << " X " << pos.x() << " Y " << pos.y() << " Z " << pos.z() << endl; 
 
  fq= q/TMath::Abs(q);     

  if(fPz!=0){
  	fTx = fPx/fPz;
  	fTy = fPy/fPz;
	fQp = fq /(TMath::Sqrt(fPx*fPx +fPy*fPy +fPz*fPz ));

  }else {
  	fTx = 0;
  	fTy = 0;  
	fQp = 0;
  }
  fDPx= MomErr.x(); //dpx
  fDPy= MomErr.y(); //dpy
  fDPz= MomErr.z(); //dpz

  fDX   = posErr.x(); //dx
  fDY   = posErr.x(); //dy
  fDZ   = posErr.x(); //dz

  CalCov();

}


void CbmTrackParP::CalCov()
{
  Double_t P =sqrt(fPx*fPx+fPy*fPy+fPz*fPz);
  
  Double_t fDP= (2*fPx*fDPx+2*fPy*fDPy+2*fPz*fDPz)/(2*P); //dp 
  fDQp = - fDP/(P*P);  // d(1/p)

  

  fDTx= fDPx/fPz - (fPx*fDPz)/pow(fPz,2);

  fDTy= fDPy/fPz - (fPy*fDPz)/pow(fPz,2);
  
  Double_t fDPQDTx = -(fTx/(fPz*pow(1 + pow(fTx,2) + pow(fTy,2),1.5)));

  Double_t fDPQDTy = -(fTy/(fPz*pow(1 + pow(fTx,2) + pow(fTy,2),1.5)));
 
  Double_t fDTxDTy = fTx/fTy;
  
  Double_t fDXDY   = fTx/fTy;


  if(fDQp==0 && fDTx==0 && fDTy==0 ) {
      for (Int_t i=0 ;i<15; i++ ) fCovMatrix[i]=0; 

  }else{
	fCovMatrix[0]  =fDQp*fDQp; 
	fCovMatrix[1]  =fDPQDTx;
	fCovMatrix[2]  =fDPQDTy;
	fCovMatrix[3]  =0;
	fCovMatrix[4]  =0;
	fCovMatrix[5]  =fDTx*fDTx;
	fCovMatrix[6]  =fDTxDTy;
	fCovMatrix[7]  =0;
	fCovMatrix[8]  =0;
	fCovMatrix[9]  =fDTy*fDTy;
	fCovMatrix[10] =0;
	fCovMatrix[11] =0;
	fCovMatrix[12] =fDX*fDX;
	fCovMatrix[13] =fDXDY;
	fCovMatrix[14] =fDY*fDY;
  }

}
CbmTrackParP::CbmTrackParP(CbmTrackPar &Trkbase)
{
	
  fX = Trkbase.GetX();
  fY = Trkbase.GetY();
  fZ = Trkbase.GetZ();
  fPx = Trkbase.GetPx();
  fPy = Trkbase.GetPy();
  fPz = Trkbase.GetPz();
  fq = Trkbase.GetQ();
  Double_t fP=TMath::Sqrt(fPx*fPx +fPy*fPy +fPz*fPz );
  if (fP!=0) fQp = fq/fP;
  else fQp=0;
  fTx = fPx/fPz;
  fTy = fPy/fPz;
  fDPx  = Trkbase.GetDPx();
  fDPy  = Trkbase.GetDPy();
  fDPz  = Trkbase.GetDPz();
  fDX   = Trkbase.GetDX();
  fDY   = Trkbase.GetDY();
  fDZ   = Trkbase.GetDZ();
  
  CalCov();
}
// -----   Destructor   ----------------------------------------------------
CbmTrackParP::~CbmTrackParP() {}
// -------------------------------------------------------------------------


// -----   Public method Print   -------------------------------------------
void CbmTrackParP::Print() {
  cout << "Position : (";
  cout.precision(2);
  cout << fX << ", " << fY << ", " << fZ << ")" << endl;
  cout << "Slopes : dx/dz = " << fTx << ", dy/dz = " << fTy << endl;
  cout << "q/p = " << fQp << endl;
}
// -------------------------------------------------------------------------

Double_t CbmTrackParP::GetDX()   
{ return fDX; }
Double_t CbmTrackParP::GetDY()   
{ return fDY; }
Double_t CbmTrackParP::GetDZ()  
{ return fDZ; }


Double_t CbmTrackParP::GetDPz()  
{ 
  if(fDPz !=0) return fDPz;
  Double_t fP =sqrt(fPx*fPx+fPy*fPy+fPz*fPz);
  Double_t fDP= fDQp *fP*fP;
  fDPz= ((2*fP*fDP)/(1 + pow(fTx,2) + pow(fTy,2)) -(pow(fP,2)*(2*fTx*fDTx + 2*fTy*fDTy))/
       pow(1 + pow(fTx,2) + pow(fTy,2),2))/(2.*TMath::Sqrt(pow(fP,2)/(1 + pow(fTx,2) + pow(fTy,2))))  ;

  return fDPz;
}

Double_t CbmTrackParP::GetDPy()  
{  
  if(fDPy !=0) return fDPy;
  if(fDPz==0) fDPz=GetDPz();
  fDPy = fTy * fDPz + fPz*fDTy;
  return fDPy;

}

Double_t CbmTrackParP::GetDPx()  
{ 
  if(fDPx !=0) return fDPx;
  if(fDPz==0) fDPz=GetDPz();
  fDPx=  fTx * fDPz + fPz*fDTx;
  return fDPx;
}

Double_t CbmTrackParP::GetDQp()  
{ 
  return fDQp;
}
 void  CbmTrackParP::Reset()
{
   fTx= fTy=0;
   fDTx= fDTy=0;

   for(Int_t i= 0;i<15;i++)  { fCovMatrix[i] =0; }
   //base class members
   fX= fY= fZ=0;
   fDX= fDY= fDZ=0;
   fPx= fPy= fPz=0;
   fDPx= fDPy= fDPz=0;
   fQp=fDQp=fq=0;
 

}


ClassImp(CbmTrackParP)