//3456789012345678901234567890123456789012345678901234567890123456789012
/** Implementation of the CbmKFParticle class
 *
 * @author  S.Gorbunov, I.Kisel
 * @version 1.0
 * @since   06.02.06
 *
 */

#include "CbmKFParticle.h"
#include "CbmKFTrack.h"
#include "CbmKFParticleDatabase.h"

#include <cmath>
#include <vector>

using namespace std;

CbmKFParticle::CbmKFParticle( CbmKFTrackInterface* Track, Double_t *z0, Int_t  *qHypo, Int_t *pdg):
  fId(-1),
  fDaughtersIds(),
  fPDG(-1),
  NDF(0),
  Chi2(0), Q(0),
  AtProductionVertex(0)
{

  fDaughtersIds.push_back( Track->Id() );

  CbmKFTrack Tr( *Track );
  
  z0=0;
  
  const Double_t *m = Tr.GetTrack();
  const Double_t *V = Tr.GetCovMatrix();

  //* Fit of vertex track slopes and momentum (a,b,qp) to xyz0 vertex

//  if( z0 ) Tr.Extrapolate( *z0 );

  Double_t a = m[2], b = m[3], qp = m[4];
  
  //* convert the track to (x,y,px,py,pz,e,t/p) parameterization

  double Mass = Tr.GetMass();
  if(pdg)
  {
    Mass=CbmKFParticleDatabase::Instance()->GetMass(*pdg);
  }


  double c2 = 1./(1. + a*a + b*b);
  double pq = 1./qp;
  if(qHypo) pq *= *qHypo;
  double p2 = pq*pq;
  double pz = sqrt(p2*c2);
  double px = a*pz;
  double py = b*pz;
  double E = sqrt( Mass*Mass + p2 );
    
  double H[3] = { -px*c2, -py*c2, -pz*pq };
  double HE = -pq*p2/E;
	  
  r[0] = m[0];
  r[1] = m[1];
  r[2] = m[5];
  r[3] = px;
  r[4] = py;
  r[5] = pz;
  r[6] = E;
  r[7] = 0;

  double cxpz = H[0]*V[ 3] + H[1]*V[ 6] + H[2]*V[10];
  double cypz = H[0]*V[ 4] + H[1]*V[ 7] + H[2]*V[11];
  double capz = H[0]*V[ 5] + H[1]*V[ 8] + H[2]*V[12];
  double cbpz = H[0]*V[ 8] + H[1]*V[ 9] + H[2]*V[13];
  double cqpz = H[0]*V[12] + H[1]*V[13] + H[2]*V[14];
  double cpzpz = H[0]*H[0]*V[5] +H[1]*H[1]*V[9] + H[2]*H[2]*V[14] 
    + 2*( H[0]*H[1]*V[8] +H[0]*H[2]*V[12] +H[1]*H[2]*V[13]);

  C[ 0] = V[0];
  C[ 1] = V[1];
  C[ 2] = V[2];
  C[ 3] = 0;
  C[ 4] = 0;
  C[ 5] = 0;
  C[ 6] = V[3]*pz + a*cxpz; 
  C[ 7] = V[4]*pz + a*cypz; 
  C[ 8] = 0;
  C[ 9] = V[5]*pz*pz + 2*a*pz*capz + a*a*cpzpz;
  C[10] = V[6]*pz+b*cxpz; 
  C[11] = V[7]*pz+b*cypz; 
  C[12] = 0;
  C[13] = V[8]*pz*pz + a*pz*cbpz + b*pz*capz + a*b*cpzpz;
  C[14] = V[9]*pz*pz + 2*b*pz*cbpz + b*b*cpzpz;
  C[15] = cxpz; 
  C[16] = cypz; 
  C[17] = 0;
  C[18] = capz*pz + a*cpzpz;
  C[19] = cbpz*pz + b*cpzpz;
  C[20] = cpzpz;
  C[21] = HE*V[10];
  C[22] = HE*V[11];
  C[23] = 0;
  C[24] = HE*( V[12]*pz + a*cqpz );
  C[25] = HE*( V[13]*pz + b*cqpz );
  C[26] = HE*cqpz;
  C[27] = HE*HE*V[14];	    
  C[28] = C[29] = C[30] = C[31] = C[32] = C[33] = C[34] = 0;
  C[35] = 1.;

  NDF = Tr.GetRefNDF();
  Chi2 = Tr.GetRefChi2();
  Q = (qp>0.) ?1 :( (qp<0) ?-1 :0);
  if(qHypo) Q *= *qHypo;
  AtProductionVertex = 1;
}

void CbmKFParticle::GetMomentum( Double_t &P, Double_t &Error_ )
{
  Double_t x = r[3];
  Double_t y = r[4];
  Double_t z = r[5];
  Double_t x2 = x*x;
  Double_t y2 = y*y;
  Double_t z2 = z*z;
  Double_t p2 = x2+y2+z2;
  P = sqrt(p2);
  Error_ = sqrt( (x2*C[9]+y2*C[14]+z2*C[20] 
		 + 2*(x*y*C[13]+x*z*C[18]+y*z*C[19]) )/p2 );
}

void CbmKFParticle::GetMass( Double_t &M, Double_t &Error_ )
{
  // S = sigma^2 of m2/2

  Double_t S = (  r[3]*r[3]*C[9] + r[4]*r[4]*C[14] + r[5]*r[5]*C[20] 
		  + r[6]*r[6]*C[27] 
		+2*( + r[3]*r[4]*C[13] + r[5]*(r[3]*C[18] + r[4]*C[19]) 
		     - r[6]*( r[3]*C[24] + r[4]*C[25] + r[5]*C[26] )   )
		 ); 
  Double_t m2 = r[6]*r[6] - r[3]*r[3] - r[4]*r[4] - r[5]*r[5];
  M     = (         m2>1.e-20 ) ? sqrt(m2)   :0.  ;
  Error_ = ( S>=0 && m2>1.e-20 ) ? sqrt(S/m2) :1.e4;
}


void CbmKFParticle::GetDecayLength( Double_t &L, Double_t &Error_ )
{
  Double_t x = r[3];
  Double_t y = r[4];
  Double_t z = r[5];
  Double_t t = r[7];
  Double_t x2 = x*x;
  Double_t y2 = y*y;
  Double_t z2 = z*z;
  Double_t p2 = x2+y2+z2;
  L = t*sqrt(p2);
  Error_ = sqrt( p2*C[35] + t*t/p2*(x2*C[9]+y2*C[14]+z2*C[20]
				   + 2*(x*y*C[13]+x*z*C[18]+y*z*C[19]) )
		+ 2*t*(x*C[31]+y*C[32]+z*C[33]) 
		);
}

void CbmKFParticle::GetLifeTime( Double_t &TauC, Double_t &Error_ ){
  Double_t m, dm;
  GetMass( m, dm );
  Double_t cTM = (-r[3]*C[31] - r[4]*C[32] - r[5]*C[33] + r[6]*C[34]);
  TauC = r[7]*m;
  Error_ = sqrt( m*m*C[35] + 2*r[7]*cTM + r[7]*r[7]*dm*dm);
}

Double_t CbmKFParticle::GetRapidity() const 
{ 
  return 0.5*log((r[6] + r[5])/(r[6] - r[5])); 
}
Double_t CbmKFParticle::GetPt()       const 
{ 
  return sqrt(r[3]*r[3] + r[4]*r[4]);
}
Double_t CbmKFParticle::GetTheta()    const 
{ 
  return atan2(GetPt(),r[5]);
}
Double_t CbmKFParticle::GetPhi()      const 
{ 
  return atan2(r[4],r[5]);
}