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

#include "CbmTrackParP.h"
#include "CbmGeaneUtil.h"
#include "CbmRunAna.h"
#include "CbmField.h"

#include "TMath.h"

#include <iostream>

using std::cout;
using std::endl;

ClassImp(CbmTrackParP)

// -----   Default constructor   -------------------------------------------
CbmTrackParP::CbmTrackParP(): CbmTrackPar() {
  Reset();
  //    for(Int_t i=0;i<15;i++)  {
  //	fCovMatrix[i]=0;
  //  }
}

// -----   Constructor with track parameters   -----------------------------------
CbmTrackParP::CbmTrackParP(Double_t v, Double_t w, Double_t Tv,
			   Double_t Tw, Double_t qp,
			   Double_t CovMatrix[15],
                           TVector3 o, TVector3 dj, TVector3 dk)
  : CbmTrackPar()
{
  Reset();
  SetPlane(o, dj, dk); 
  fV = v;
  fW = w;
  fTV = Tv;
  fTW = Tw;
  fQp = qp;

  Double_t P  = TMath::Abs(1/fQp);
  fq= P * fQp;
  for(Int_t i=0;i<15;i++)  {
    fCovMatrix[i]=CovMatrix[i];
  }

  // CHECK -----------------------
   fPx_sd = TMath::Sqrt( P*P / ( fTV*fTV + fTW*fTW + 1 ) );
   fPy_sd = fTV * fPx_sd;
   fPz_sd = fTW * fPx_sd;
//   TVector3 p_mars = TVector3(fPx_sd, fPy_sd, fPz_sd);
//   TVector3 u_mars = TVector3(fDI[0], dj.X(), dk.X());
//   TVector3 v_mars = TVector3(fDI[1], dj.Y(), dk.Y());
//   TVector3 w_mars = TVector3(fDI[2], dj.Z(), dk.Z());
//   fPx = p_mars.Dot(u_mars);
//   fPy = p_mars.Dot(v_mars);
//   fPz = p_mars.Dot(w_mars);
  
  fU = 0.; // CHECK la u e' fissata dal piano --> prendo la u = 0

  CbmGeaneUtil util;
  TVector3 position = util.FromSDToMARSCoord(TVector3(fU, fV, fW), forigin, fiver, fjver, fkver);  
  fX = position.X(); // CHECK 
  fY = position.Y(); // CHECK 
  fZ = position.Z(); // CHECK 

  fDQp  = TMath::Sqrt(fabs(fCovMatrix[0]));
  fDTV  = TMath::Sqrt(fabs(fCovMatrix[5]));
  fDTW  = TMath::Sqrt(fabs(fCovMatrix[9]));
  fDV   = TMath::Sqrt(fabs(fCovMatrix[12]));
  fDW   = TMath::Sqrt(fabs(fCovMatrix[14]));
  
  // CHECK da calcolare ---
  Double_t PD[3],RD[6][6],H[3],CH,PC[3],RC[15], SP1, DJ1[3], DK1[3];
  PC[0]   = 1./P;
  PC[1]   = fTV; 
  PC[2]   = fTW;
  
  for(Int_t i=0;i<15;i++)  {
	RC[i]=fCovMatrix[i];
  }

  // retrieve field
  Double_t pnt[3];
  pnt[0] = fX; 
  pnt[1] = fY;
  pnt[2] = fZ;
  CbmRunAna *fRun = CbmRunAna::Instance();
  fRun->GetField()->GetFieldValue(pnt, H);

  CH=fq;
  //  DJ1[0] = fDJ[0];
  //   DJ1[1] = fDJ[1];
  //   DJ1[2] = fDJ[2];
  //   DK1[0] = fDK[0];
  //   DK1[1] = fDK[1];
  //   DK1[2] = fDK[2];
  
  //  spu = sign[p·(DJ1 x DK1)]     
  //TVector3 mom = TVector3(fPx., fPy, fPz);
  TVector3 momsd = TVector3(fPx_sd,fPy_sd,fPz_sd);
  SP1 = (momsd.Dot(fjver.Cross(fkver)))/TMath::Abs(momsd.Dot(fjver.Cross(fkver))); // CHECK 
  fSPU=SP1;
					  
  util.FromSDToMars(PC, RC, H, CH, SP1, fDJ, fDK, PD, RD);

  fPx = PD[0];
  fPy = PD[1];
  fPz = PD[2];
  
  fDPx = TMath::Sqrt(fabs(RD[0][0])); // CHECK 
  fDPy = TMath::Sqrt(fabs(RD[1][1])); // CHECK 
  fDPz = TMath::Sqrt(fabs(RD[2][2])); // CHECK 
  fDX  = TMath::Sqrt(fabs(RD[3][3])); // CHECK 
  fDY  = TMath::Sqrt(fabs(RD[4][4])); // CHECK 
  fDZ  = TMath::Sqrt(fabs(RD[5][5])); // CHECK 
  
}

CbmTrackParP::CbmTrackParP(Double_t v, Double_t w, Double_t Tv,
			   Double_t Tw, Double_t qp,
			   Double_t CovMatrix[15],
                           TVector3 o, TVector3 dj, TVector3 dk, Double_t spu)
  : CbmTrackPar()
{
  Reset();
  SetPlane(o, dj, dk); 
  fV = v;
  fW = w;
  fTV = Tv;
  fTW = Tw;
  fQp = qp;
  fSPU = spu;
  Double_t P  = TMath::Abs(1/fQp);
  fq= P * fQp;
  for(Int_t i=0;i<15;i++)  {
    fCovMatrix[i]=CovMatrix[i];
  }

  // CHECK -----------------------
   fPx_sd = fSPU*TMath::Sqrt( P*P / ( fTV*fTV + fTW*fTW + 1 ) );
   fPy_sd = fTV * fPx_sd;
   fPz_sd = fTW * fPx_sd;
//   TVector3 p_mars = TVector3(fPx_sd, fPy_sd, fPz_sd);
//   TVector3 u_mars = TVector3(fDI[0], dj.X(), dk.X());
//   TVector3 v_mars = TVector3(fDI[1], dj.Y(), dk.Y());
//   TVector3 w_mars = TVector3(fDI[2], dj.Z(), dk.Z());
//   fPx = p_mars.Dot(u_mars);
//   fPy = p_mars.Dot(v_mars);
//   fPz = p_mars.Dot(w_mars);
  
  fU = 0.; // CHECK la u e' fissata dal piano --> prendo la u = 0

  CbmGeaneUtil util;
  TVector3 position = util.FromSDToMARSCoord(TVector3(fU, fV, fW), forigin, fiver, fjver, fkver);  
  fX = position.X(); // CHECK 
  fY = position.Y(); // CHECK 
  fZ = position.Z(); // CHECK 

  fDQp  = TMath::Sqrt(fabs(fCovMatrix[0]));
  fDTV  = TMath::Sqrt(fabs(fCovMatrix[5]));
  fDTW  = TMath::Sqrt(fabs(fCovMatrix[9]));
  fDV   = TMath::Sqrt(fabs(fCovMatrix[12]));
  fDW   = TMath::Sqrt(fabs(fCovMatrix[14]));
  
  // CHECK da calcolare ---
  Double_t PD[3],RD[6][6],H[3],CH,PC[3],RC[15], SP1, DJ1[3], DK1[3];
  PC[0]   = 1./P;
  PC[1]   = fTV; 
  PC[2]   = fTW;
  
  for(Int_t i=0;i<15;i++)  {
	RC[i]=fCovMatrix[i];
  }

  // retrieve field
  Double_t pnt[3];
  pnt[0] = fX; 
  pnt[1] = fY;
  pnt[2] = fZ;
  CbmRunAna *fRun = CbmRunAna::Instance();
  fRun->GetField()->GetFieldValue(pnt, H);

  CH=fq;
  //  DJ1[0] = fDJ[0];
  //   DJ1[1] = fDJ[1];
  //   DJ1[2] = fDJ[2];
  //   DK1[0] = fDK[0];
  //   DK1[1] = fDK[1];
  //   DK1[2] = fDK[2];
  
  //  spu = sign[p·(DJ1 x DK1)]     
  //TVector3 mom = TVector3(fPx., fPy, fPz);
  TVector3 momsd = TVector3(fPx_sd,fPy_sd,fPz_sd);
  SP1 = (momsd.Dot(fjver.Cross(fkver)))/TMath::Abs(momsd.Dot(fjver.Cross(fkver))); // CHECK 
  SP1 = fSPU; // correct calculation of SP1
	  
  util.FromSDToMars(PC, RC, H, CH, SP1, fDJ, fDK, PD, RD);

  fPx = PD[0];
  fPy = PD[1];
  fPz = PD[2];
  
  fDPx = TMath::Sqrt(fabs(RD[0][0])); // CHECK 
  fDPy = TMath::Sqrt(fabs(RD[1][1])); // CHECK 
  fDPz = TMath::Sqrt(fabs(RD[2][2])); // CHECK 
  fDX  = TMath::Sqrt(fabs(RD[3][3])); // CHECK 
  fDY  = TMath::Sqrt(fabs(RD[4][4])); // CHECK 
  fDZ  = TMath::Sqrt(fabs(RD[5][5])); // CHECK 

}
CbmTrackParP::CbmTrackParP(TVector3 pos, TVector3 Mom, TVector3 posErr, TVector3 MomErr, Double_t Q, TVector3 o, TVector3 dj, TVector3 dk)
  : CbmTrackPar(pos.x(),pos.y(),pos.z(),Mom.x(),Mom.y(),Mom.z(),Q)
{
  Reset();
  SetPlane(o, dj, dk);

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

  SetX(pos.x()); //x (lab)
  SetY(pos.y()); //y (lab)
  SetZ(pos.z()); //z (lab)
  Double_t P = TMath::Sqrt(fPx*fPx +fPy*fPy +fPz*fPz );
  if(Q!=0) fq= Q/TMath::Abs(Q);     

  
 //   cout << "CbmTrackParP " << endl;
//    cout << "origin " << o.X() << " " << o.Y() << " " << o.Z() << endl;
//    cout << "di " << fDI[0] << " " << fDI[1] << " " << fDI[2] << endl;
//    cout << "dj " << dj.X() << " " << dj.Y() << " " << dj.Z() << endl;
//    cout << "dk " << dk.X() << " " << dk.Y() << " " << dk.Z() << endl;
//    cout << "position " << fX << " " << fY << " " << fZ << endl;
//    cout << "momentum " << fPx << " " << fPy << " " << fPz << endl;
//   

   CbmGeaneUtil util;
   TVector3 positionsd = util.FromMARSToSDCoord(TVector3(fX, fY, fZ), forigin, fiver, fjver, fkver);
   fU = positionsd.X(); // CHECK
   fV = positionsd.Y(); // CHECK 
   fW = positionsd.Z(); // CHECK 
   fQp = fq/P;
  

   //    if(fPz!=0){
   //      fTV = fPy/fPx;
   //      fTW = fPz/fPx;
   //      fQp = fq /P;
   //    }
   //    else {
   //      fTV = 0.;
   //      fTW = 0.;  
   //      fQp = 0.;
   //    }
//    TVector3 p_mars = TVector3(fPx, fPy, fPz); // CHECK pendenza
//    TVector3 di = TVector3(fDI[0], fDI[1], fDI[2]); 
//    fPx_sd = p_mars.Dot(di);
//    fPy_sd = p_mars.Dot(dj);
//    fPz_sd = p_mars.Dot(dk);
//    fTV = fPy_sd/fPx_sd;
//    fTW = fPz_sd/fPx_sd;

   fDPx = MomErr.x(); //dpx
   fDPy = MomErr.y(); //dpy
   fDPz = MomErr.z(); //dpz
   fDX  = posErr.x(); //dpx
   fDY  = posErr.y(); //dpy
   fDZ  = posErr.z(); //dpz
   
   Double_t PD[3], RD[6][6], H[3], CH, SP1, PC[3], RC[3];
   Int_t IERR;
   
   PD[0] = fPx;
   PD[1] = fPy;
   PD[2] = fPz;
   
   for(int i = 0; i < 6; i++)  for(int j = 0; j < 6; j++) RD[i][j] = 0.;
   RD[0][0] = fDPx * fDPx;
   RD[1][1] = fDPy * fDPy;
   RD[2][2] = fDPz * fDPz;
   RD[3][3] = fDX * fDX;
   RD[4][4] = fDY * fDY;
   RD[5][5] = fDZ * fDZ;
   
   // retrieve field
   Double_t pnt[3];
   pnt[0] = fX; 
   pnt[1] = fY;
   pnt[2] = fZ;
   CbmRunAna *fRun = CbmRunAna::Instance();
   fRun->GetField()->GetFieldValue(pnt, H);
   
   CH=fq;
   
   util.FromMarsToSD(PD, RD, H, CH, fDJ, fDK, IERR, SP1, PC, RC);
   
   for(Int_t i=0;i<15;i++) fCovMatrix[i] = RC[i];

   fTV = PC[1];
   fTW = PC[2];
   
   fDQp  = TMath::Sqrt(fabs(fCovMatrix[0]));
   fDTV  = TMath::Sqrt(fabs(fCovMatrix[5]));
   fDTW  = TMath::Sqrt(fabs(fCovMatrix[9]));
   fDV   = TMath::Sqrt(fabs(fCovMatrix[12]));
   fDW   = TMath::Sqrt(fabs(fCovMatrix[14])); 
   
   fSPU = SP1;


}



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],
			       TVector3 o, TVector3 di, TVector3 dj, TVector3 dk)
{

  
//   cout << "SetTrackPar " << endl;
//   cout << "origin " << o.X() << " " << o.Y() << " " << o.Z() << endl;
//   cout << "di " << di.X() << " " << di.Y() << " " << di.Z() << endl;
//   cout << "dj " << dj.X() << " " << dj.Y() << " " << dj.Z() << endl;
//   cout << "dk " << dk.X() << " " << dk.Y() << " " << dk.Z() << endl;
//   cout << "position " << X << " " << Y << " " << Z << endl;
//   cout << "momentum " << Px << " " << Py << " " << Pz << endl;
  

  Reset();
  SetPlane(o, dj, dk);

  Double_t P =TMath::Sqrt(Px*Px+Py*Py+Pz*Pz);

  if (Q!=0) fq = TMath::Abs(Q)/Q;
  fQp    = fq/P;

  SetX(X);  
  SetY(Y);
  SetZ(Z);

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

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

  CbmGeaneUtil util;
  //cout << "punto estrapolato " << fX << " " << fY << " " << fZ << endl;
  TVector3 positionsd = util.FromMARSToSDCoord(TVector3(fX, fY, fZ), forigin, fiver, fjver, fkver);
  fU = positionsd.X(); // CHECK
  fV = positionsd.Y(); // CHECK 
  fW = positionsd.Z(); // CHECK 
  fQp = fq/P;
  
  // if(fPx!=0){
  //    fTV = fPy/fPx;
  //     fTW = fPz/fPx;
  //     fQp = fq /P;
  //   }
  //   else {
  //     fTV = 0.;
  //     fTW = 0.;  
  //     fQp = 0.;
  //}
//   TVector3 p_mars = TVector3(fPx, fPy, fPz); // CHECK pendenza
//   fPx_sd = p_mars.Dot(di);
//   fPy_sd = p_mars.Dot(dj);
//   fPz_sd = p_mars.Dot(dk);
//   fTV = fPy_sd/fPx_sd;
//   fTW = fPz_sd/fPx_sd;

  //................... CHECK controlla qui
  Double_t PD[3], RD[6][6], H[3], CH, SP1, PC[3], RC[3];
  Int_t IERR;
  PD[0] = Px;
  PD[1] = Py;
  PD[2] = Pz;

  for(int i = 0; i < 6; i++)  for(int j = 0; j < 6; j++) RD[i][j] = 0.;
  // retrieve field
  Double_t pnt[3];
  pnt[0] = fX; 
  pnt[1] = fY;
  pnt[2] = fZ;
  CbmRunAna *fRun = CbmRunAna::Instance();
  fRun->GetField()->GetFieldValue(pnt, H);
  CH=fq;
  util.FromMarsToSD(PD, RD, H, CH, fDJ, fDK, IERR, SP1, PC, RC);
  fTV = PC[1];
  fTW = PC[2]; 
  //...............................
  //cout << IERR <<  " estrap sd " << fTV << " " << fTW  <<endl; 

  fDQp  = TMath::Sqrt(fabs(fCovMatrix[0]));
  fDTV  = TMath::Sqrt(fabs(fCovMatrix[5]));
  fDTW  = TMath::Sqrt(fabs(fCovMatrix[9]));
  fDV   = TMath::Sqrt(fabs(fCovMatrix[12]));
  fDW   = TMath::Sqrt(fabs(fCovMatrix[14])); 

 
  //  Double_t PD[3],RD[6][6],H[3],CH,PC[3],RC[15], SP1;
  // Int_t IERR;
  //   PC[0]   = 1./P;
  //   PC[1]   = fTV; 
  //   PC[2]   = fTW;
  for(Int_t i=0;i<15;i++) RC[i] = fCovMatrix[i];
  // // retrieve field
  //   Double_t pnt[3];
  //   pnt[0] = fX; 
  //   pnt[1] = fY;
  //   pnt[2] = fZ;
  //   CbmRunAna *fRun = CbmRunAna::Instance();
  //   fRun->GetField()->GetFieldValue(pnt, H);
  //   CH=fq;
  //  spu = sign[p·(DJ1 x DK1)]     
  // TVector3 mom = TVector3(fPx, fPy, fPz);
  // SP1 = (mom.Dot(fjver.Cross(fkver)))/TMath::Abs(mom.Dot(fjver.Cross(fkver))); // CHECK 
  
  // cout << "CH " << CH << endl;
 
//   cout << "from SD " << PC[0] << " " << PC[1] << " " << PC[2] << endl;
//   cout << RC[0] << " " << RC[1] << " " << RC[2] << " " << RC[3] << " " << RC[4] << endl;
//   cout << RC[5] << " " << RC[6] << " " << RC[7] << " " << RC[8] << " " << RC[9] << endl;
//   cout << RC[10] << " " << RC[11] << " " << RC[12] << " " << RC[13] << " " << RC[14] << endl;
//   
  util.FromSDToMars(PC, RC, H, CH, SP1, fDJ, fDK, PD, RD);
  
//   cout << "to mars " << PD[0] << " " << PD[1] << " " << PD[2] << endl;
//   cout << RD[0][0] << " " << RD[1][0] << " " << RD[2][0] << " " << RD[3][0] << " " << RD[4][0] << " " << RD[5][0] << endl;


  fDPx = TMath::Sqrt(fabs(RD[0][0])); // CHECK da calcolare
  fDPy = TMath::Sqrt(fabs(RD[1][1])); // CHECK da calcolare
  fDPz = TMath::Sqrt(fabs(RD[2][2])); // CHECK da calcolare
   fDX = TMath::Sqrt(fabs(RD[3][3])); // CHECK da calcolare
   fDY = TMath::Sqrt(fabs(RD[4][4])); // CHECK da calcolare
   fDZ = TMath::Sqrt(fabs(RD[5][5])); // CHECK da calcolare
   
   fSPU = SP1;


}

void  CbmTrackParP::SetTrackPar(Double_t v, Double_t w, Double_t Tv,
				Double_t Tw, Double_t qp,
				Double_t CovMatrix[15],
				TVector3 o, TVector3 di, TVector3 dj, TVector3 dk, Double_t spu)
{
  Reset();
  SetPlane(o, dj, dk); 
  fU = 0; // CHECK
  fV = v;
  fW = w;
  fTV = Tv;
  fTW = Tw;
  fQp = qp;
  fSPU = spu;

  Double_t P  = TMath::Abs(1/fQp);
  fq= P * fQp;
  for(Int_t i=0;i<15;i++)  {
	fCovMatrix[i]=CovMatrix[i];
  }
  
//   // CHECK -----------------------
   fPx_sd = fSPU*TMath::Sqrt( P*P / ( fTV*fTV + fTW*fTW + 1 ) );
   fPy_sd = fTV * fPx_sd;
   fPz_sd = fTW * fPx_sd;
//   TVector3 p_mars = TVector3(fPx_sd, fPy_sd, fPz_sd);
//   TVector3 u_mars = TVector3(fDI[0], dj.X(), dk.X());
//   TVector3 v_mars = TVector3(fDI[1], dj.Y(), dk.Y());
//   TVector3 w_mars = TVector3(fDI[2], dj.Z(), dk.Z());
//   fPx = p_mars.Dot(u_mars);
//   fPy = p_mars.Dot(v_mars);
//   fPz = p_mars.Dot(w_mars);

  CbmGeaneUtil util;
  TVector3 position = util.FromSDToMARSCoord(TVector3(fU, fV, fW), forigin, fiver, fjver, fkver);  
  fX = position.X(); // CHECK 
  fY = position.Y(); // CHECK 
  fZ = position.Z(); // CHECK 

  fDQp = TMath::Sqrt(fabs(fCovMatrix[0]));
  fDTV = TMath::Sqrt(fabs(fCovMatrix[5]));
  fDTW = TMath::Sqrt(fabs(fCovMatrix[9]));
  fDV  = TMath::Sqrt(fabs(fCovMatrix[12]));
  fDW  = TMath::Sqrt(fabs(fCovMatrix[14]));

  // CHECK da calcolare ---
  Double_t PD[3],RD[6][6],H[3],CH,PC[3],RC[15], SP1, DJ1[3], DK1[3];
  PC[0]   = 1./P;
  PC[1]   = fTV; 
  PC[2]   = fTW;
  
  for(Int_t i=0;i<15;i++)  {
	RC[i]=fCovMatrix[i];
  }

  // retrieve field
  Double_t pnt[3];
  pnt[0] = fX; 
  pnt[1] = fY;
  pnt[2] = fZ;
  CbmRunAna *fRun = CbmRunAna::Instance();
  fRun->GetField()->GetFieldValue(pnt, H);

  CH=fq;
 
  //  spu = sign[p·(DJ1 x DK1)]     
  //TVector3 mom = TVector3(fPx, fPy, fPz);
  TVector3 momsd = TVector3(fPx_sd,fPy_sd,fPz_sd);
  SP1 = (momsd.Dot(fjver.Cross(fkver)))/TMath::Abs(momsd.Dot(fjver.Cross(fkver))); // CHECK 
  SP1=fSPU;
			  
  util.FromSDToMars(PC, RC, H, CH, SP1, fDJ, fDK, PD, RD);
  
  fPx = PD[0];
  fPy = PD[1];
  fPz = PD[2];

  fDPx = TMath::Sqrt(fabs(RD[0][0])); // CHECK 
  fDPy = TMath::Sqrt(fabs(RD[1][1])); // CHECK 
  fDPz = TMath::Sqrt(fabs(RD[2][2])); // CHECK 
  fDX  = TMath::Sqrt(fabs(RD[3][3])); // CHECK 
  fDY  = TMath::Sqrt(fabs(RD[4][4])); // CHECK 
  fDZ  = TMath::Sqrt(fabs(RD[5][5])); // CHECK 

}

void CbmTrackParP::CalCov()
{
  // CHECK da calcolare
}

// CbmTrackParP::CbmTrackParP(CbmTrackParP &Trkbase)
// {
//      cout << "CbmTrackParP::CbmTrackParP(CbmTrackParP &Trkbase)"     << endl;
// }

//CbmTrackParP::CbmTrackParP(CbmTrackPar &Trkbase)/
//{
//}

// -----   Destructor   ----------------------------------------------------
CbmTrackParP::~CbmTrackParP() {}
// -------------------------------------------------------------------------

// -----   Public method Print   -------------------------------------------
void CbmTrackParP::Print() {
  cout << "Position : (";
  cout.precision(2);
  cout << fX << ", " << fY << ", " << fZ << ")" << endl;
  cout << "Slopes : dx/dz = " << fTV << ", dy/dz = " << fTW << 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::GetV()   
{ 
	return fV; 
}

Double_t CbmTrackParP::GetW()   
{ 
	return fW; 
}
Double_t CbmTrackParP::GetTV()  
{ 
	return fTV; 
}
Double_t CbmTrackParP::GetTW()  
{ 
	return fTW; 
}
Double_t CbmTrackParP::GetDV()   
{ 
	return fDV; 
}

Double_t CbmTrackParP::GetDW()   
{ 
	return fDW; 
}
Double_t CbmTrackParP::GetDTV()  
{ 
	return fDTV; 
}
Double_t CbmTrackParP::GetDTW()  
{ 
	return fDTW; 
}
Double_t CbmTrackParP::GetX()    // CHECK da cambiare
{  
  return fX;
}
Double_t CbmTrackParP::GetY()   // CHECK da cambiare
{  
  return fY;
}

Double_t CbmTrackParP::GetZ()   // CHECK da cambiare
{  
  return fZ;
}

Double_t CbmTrackParP::GetDPx()  
{ 
  return fDPx;
}

Double_t CbmTrackParP::GetDPy()  
{
  return fDPy;
}

Double_t CbmTrackParP::GetDPz()  
{   
  return fDPz;
}

Double_t CbmTrackParP::GetDQp()  
{ 
  return fDQp;
}

TVector3 CbmTrackParP::GetOrigin()
{
  return forigin;
}
TVector3 CbmTrackParP::GetIVer()
{
  return fiver;
}
TVector3 CbmTrackParP::GetJVer()
{
  return fjver;
}

TVector3 CbmTrackParP::GetKVer()
{
  return fkver;
}



void  CbmTrackParP::Reset()
{
  fU   = 0.;
  fV   = 0.; 
  fW   = 0.;
  fTV  = 0.;
  fTW  = 0.;
  fDV  = 0.; 
  fDW  = 0.;
  fDTV = 0.;
  fDTW = 0.;
  for(Int_t i= 0;i<15;i++)  { fCovMatrix[i] = 0.; }
  fPx_sd = 0.; fPy_sd = 0.; fPz_sd = 0.; 

  //base class members
  fX   = fY   = fZ   = 0.;
  fDX  = fDY  = fDZ  = 0.;
  fPx  = fPy  = fPz  = 0.;
  fDPx = fDPy = fDPz = 0.;
  fQp  = fDQp = fq   = 0;
 
}


void CbmTrackParP::SetPlane(TVector3 o, TVector3 j, TVector3 k)
{
  // origin
  forigin = TVector3(o.X(), o.Y(), o.Z());
  
  // check unity
  j.SetMag(1.);
  k.SetMag(1.);
  // check orthogonality
  if(j * k != 0) {
   // cout << "*** NOT PERP ***" << endl;
    k = (j.Cross(k)).Cross(j); 
  }

  // plane
  // i
  TVector3 i = j.Cross(k); 
  i.SetMag(1.);
  fDI[0] = i.X();
  fDI[1] = i.Y();
  fDI[2] = i.Z();
  fiver = TVector3(fDI[0],fDI[1],fDI[2]);
  // j
  fDJ[0] = j.X();
  fDJ[1] = j.Y();
  fDJ[2] = j.Z();
  fjver = TVector3(fDJ[0],fDJ[1],fDJ[2]);j;
  // k
  fDK[0] = k.X();
  fDK[1] = k.Y();
  fDK[2] = k.Z();  
  fkver = TVector3(fDK[0],fDK[1],fDK[2]);
 
//  cout << "versori: " <<  fDI[0] << " " << fDI[1] << " " << fDI[2] << endl;
//  cout << "         " <<  fDJ[0] << " " << fDJ[1] << " " << fDJ[2] << endl;
//  cout << "         " <<  fDK[0] << " " << fDK[1] << " " << fDK[2] << endl;
 




}