//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class LSLTrackRep
//      see LSLTrackRep.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "LSLTrackRep.h"

// C/C++ Headers ----------------------
#include <iostream>

// Collaborating Class Headers --------
#include "Nystrom.h"
#include "AbsRecoHit.h"
#include "AbsBFieldIfc.h"
#include "LSLEQM.h"
#include "AbsNystromEQM.h"

// Class Member definitions -----------

LSLTrackRep::LSLTrackRep()
  : AbsTrackRep(5)
{
  _eqm=new LSLEQM(0);
}

LSLTrackRep::LSLTrackRep(double z, double x, double y, 
			 double dxdz, double dydz, double invp,
			 double sigx, double sigy, 
			 double sigdxdz, double sigdydz, 
			 double siginvp,
			 AbsBFieldIfc* field) 
  : AbsTrackRep(5)
{
  s=z;
  state[0][0]=x;
  state[1][0]=y;
  state[2][0]=dxdz;
  state[3][0]=dydz;
  state[4][0]=invp;

  cov[0][0]=sigx;
  cov[1][1]=sigy;
  cov[2][2]=sigdxdz;
  cov[3][3]=sigdydz;
  cov[4][4]=siginvp;
  
  _eqm=new LSLEQM(field);
}

LSLTrackRep::LSLTrackRep(const LSLTrackRep& rep) 
  : AbsTrackRep(rep)
{
  _eqm=new LSLEQM(*reinterpret_cast<LSLEQM*>(rep._eqm));
}


LSLTrackRep::~LSLTrackRep()
{
  if(_eqm!=NULL)delete _eqm;
}

void
LSLTrackRep::init(const TVector3& pos,
		  double dxdz, double dydz, double invp,
		  double sigx, double sigy, 
		  double sigdxdz, double sigdydz, 
		  double siginvp, 
		  AbsBFieldIfc* field) 
{
  s=pos.Z();
  state[0][0]=pos.X();
  state[1][0]=pos.Y();
  state[2][0]=dxdz;
  state[3][0]=dydz;
  state[4][0]=invp;

  cov[0][0]=sigx;
  cov[1][1]=sigy;
  cov[2][2]=sigdxdz;
  cov[3][3]=sigdydz;
  cov[4][4]=siginvp;

  if(_eqm!=NULL)delete _eqm;
  _eqm=new LSLEQM(field);
}

/*
TVectorT<double> 
LSLTrackRep::eqm(const TVectorT<double>& u,
		 const TVectorT<double>& uprim,
		 const TVectorT<double>& par) // equ of motion
{
  // u=(x,y,z) uprim=(dx/dz,dy/dz,1) par[0]=q/P

  // default values in case no bfield is given
  double Bx=0.; double By=0.;
  double Bz=2.;
  //if(b!=NULL){ 
  // TVectorT<double> B=b->get(u);
  // Bx=B[0];By=B[1];Bz=B[2];
  // }

  if(u[2]>200.)Bz=0.;
  if(u[2]>350. && u[2]<450.)By=0.5;

  double xprim2=uprim[0]*uprim[0];
  double yprim2=uprim[1]*uprim[1];
  double dsdz=sqrt(1.+xprim2+yprim2);
  double kappP=0.00299792458*par[0]; // [GeV/c T^-1 cm^-1]
  TVectorT<double> result(u);

  result[0]=kappP*dsdz*(uprim[0]*uprim[1]*Bx-(1+xprim2)*By+uprim[1]*Bz);
  result[1]=kappP*dsdz*(-uprim[0]*uprim[1]*By+(1+yprim2)*Bx-uprim[0]*Bz);
  result[2]=0;
  return result;
}
*/


void 
LSLTrackRep::SetBField(AbsBFieldIfc* b)
{
  if(_eqm!=NULL)delete _eqm;
  _eqm=new LSLEQM(b);
}

void 
LSLTrackRep::extrapolate(const DetPlane& pl, 
			 TMatrixT<double>& statePred)
{
  double sExtrapolateTo=pl.getO().Z();
  Nystrom rungeKutta(_eqm);
  //prepare the vectors
  TVectorT<double> u(3);u[0]=state[0][0];u[1]=state[1][0];u[2]=s;
  TVectorT<double> uprim(3);uprim[0]=state[2][0];uprim[1]=state[3][0];uprim[2]=1;
  TVectorT<double> par(1);par[0]=state[4][0];

  TVectorT<double> unew(3);
  TVectorT<double> uprimnew(3);
  rungeKutta.propagate(s,sExtrapolateTo,
		       u, uprim, par,
		       unew, uprimnew);
  // write results into statePred
  statePred[0][0]=unew[0];
  statePred[1][0]=unew[1];
  statePred[2][0]=uprimnew[0];
  statePred[3][0]=uprimnew[1];
  statePred[4][0]=state[4][0];
  //std::cout<<"unew[2]=z="<<unew[2];

}

/*
void LSLTrackRep::extrapolate(const DetPlane& pl,
			      const TMatrixT<double>& stateFrom, 
			      TMatrixT<double>& stateResult) {
  stateResult.ResizeTo(5,1);
  double s=
  Nystrom rungeKutta(_eqm);
  //prepare the vectors
  TVectorT<double> u(3);
  u[0]=stateFrom[0][0];
  u[1]=stateFrom[1][0];
  u[2]=sExtrapolateFrom;
  TVectorT<double> uprim(3);uprim[0]=stateFrom[2][0];uprim[1]=stateFrom[3][0];uprim[2]=1;
  TVectorT<double> par(1);par[0]=stateFrom[4][0];

  TVectorT<double> unew(3);
  TVectorT<double> uprimnew(3);
  rungeKutta.propagate(sExtrapolateFrom,sExtrapolateTo,
					   u, uprim, par,
					   unew, uprimnew);
  // write results into statePred
  stateResult[0][0]=unew[0];
  stateResult[1][0]=unew[1];
  stateResult[2][0]=uprimnew[0];
  stateResult[3][0]=uprimnew[1];
  stateResult[4][0]=state[4][0];
  //std::cout<<"unew[2]=z="<<unew[2];

}
*/

void 
LSLTrackRep::extrapolate(const DetPlane& pl, 
			 TMatrixT<double>& statePred,
			 TMatrixT<double>& covPred,
			 TMatrixT<double>& jacobian)
{
  //std::cout << "Extr from To: " << s << " " << sExtrapolateTo << std::endl;
  extrapolate(pl,statePred);
  // covPred=JCovJ^T with J being Jacobian
  jacobian.ResizeTo(5,5);
  Jacobian(pl,statePred,jacobian);
  TMatrixT<double> dummy(cov,TMatrixT<double>::kMultTranspose,jacobian);
  covPred=jacobian*dummy;
  //covPred=cov;
}

void 
LSLTrackRep::predict(const DetPlane& pl, 
		     TMatrixT<double>& statePred,
		     TMatrixT<double>& covPred,
		     TMatrixT<double>& jacobian)
{
  extrapolate(pl,statePred,covPred,jacobian);
}

void
LSLTrackRep::Jacobian(const DetPlane& pl,
		      const TMatrixT<double>& statePred,
		      TMatrixT<double>& jacResult){
  
  TMatrixT<double> difPred(statePred);
  // do column wise differntiation:
  for(int icol=0;icol<5;++icol){
    // choose step
    double h=fabs(state[icol][0])*1.e-4;
    if(h<1e-13)h=1.e-13;
    // vary the state
    state[icol][0]+=h;
    extrapolate(pl,difPred);
    // difference:
    difPred-=statePred;
    // remove variation from state
    state[icol][0]-=h;
    // fill jacobian with difference quotient
    for(int irow=0;irow<5;++irow)jacResult[irow][icol]=difPred[irow][0]/h;
  }
}
		

TVector3 
LSLTrackRep::getPos(const DetPlane& pl)
{
  double z=pl.getO().Z();
  TMatrixT<double> statePred(state);
  DetPlane p(TVector3(0,0,z),TVector3(1,0,0),TVector3(0,1,0));
  extrapolate(p,statePred);
  return TVector3(statePred[0][0],statePred[1][0],z);
}
 
TVector3 
LSLTrackRep::getMom(const DetPlane& pl)
{
  double z=pl.getO().Z();
  TMatrixT<double> statePred(state);
  //statePred.Print();
  DetPlane p(TVector3(0,0,z),TVector3(1,0,0),TVector3(0,1,0));
  extrapolate(p,statePred);
  //statePred.Print();
  TVector3 result(statePred[2][0],statePred[3][0],1);
  result.SetMag(1./statePred[4][0]);
  return result;
}
	      
ClassImp(LSLTrackRep)