//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class GeaneTrackRep
//      see GeaneTrackRep.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 "GeaneTrackRep.h"
#include "CbmGeaneUtil.h"
#include "CbmTrackParH.h"

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

// Collaborating Class Headers --------
#include "AbsRecoHit.h"
#include "CbmGeanePro.h"

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



GeaneTrackRep::GeaneTrackRep()
  : AbsTrackRep(5)
{
  std::cout<<"GeaneTrackRep::Standard Ctor"<<std::endl;
}

GeaneTrackRep::GeaneTrackRep(CbmGeanePro* geane, 
			     const TVector3& pos,
			     const TVector3& mom,
			     const TVector3& poserr,
			     const TVector3& momerr,
			     double q,
			     DetPlane& plane) 
  : AbsTrackRep(5), _geane(geane)
{
  double spu=1.;
  CbmTrackParP par(pos,mom,poserr,momerr,q,plane.getO(),plane.getU(),plane.getV());

  state[0][0]=par.GetQp();
  state[1][0]=par.GetTV();
  state[2][0]=par.GetTW();
  state[3][0]=par.GetV();
  state[4][0]=par.GetW();

  // blow up cov-array: ROOT does not support init with symmetric data
  // See ROOT docu source-file for TMatrixTSym
  // i=row, j=collumn
  double* covarray=par.GetCov();
  int count=0;
  for(int i=0;i<5;++i){
    for(int j=i;j<5;++j){
      cov[i][j]=covarray[count];
      if(i!=j)cov[j][i]=covarray[count];
      ++count;
    }
  }

  _refPlane=plane;
}

//  GeaneTrackRep::GeaneTrackRep(const GeaneTrackRep& rep) 
//   : AbsTrackRep(rep)
// {
//   _geane=rep._geane;
// }


GeaneTrackRep::~GeaneTrackRep()
{
  
}


void 
GeaneTrackRep::extrapolate(const DetPlane& pl, 
			   TMatrixT<double>& statePred)
{
  TMatrixT<double> covPred(5,5);
  TMatrixT<double> j(5,5);
  extrapolate(pl,statePred,covPred,j);
  //! TODO: make this faster by neglecting covariances ?
}


void 
GeaneTrackRep::extrapolate(const DetPlane& pl, 
			   TMatrixT<double>& statePred,
			   TMatrixT<double>& covPred,
			   TMatrixT<double>& jacobian)
{

  TVector3 o=pl.getO();
  TVector3 u=pl.getU();
  TVector3 v=pl.getV();

  TVector3 ofrom=_refPlane.getO();
  TVector3 ufrom=_refPlane.getU();
  TVector3 vfrom=_refPlane.getV();

  std::cout<<"From DetPlane"<<std::endl;
  ofrom.Print();
  ufrom.Print();
  vfrom.Print();
  
  std::cout<<"To DetPlane"<<std::endl;
  o.Print();
  u.Print();
  v.Print();
  
  //_geane->SetPoint(og);
  _geane->PropagateFromPlane(ufrom,vfrom);

  _geane->PropagateToPlane(o,u,v);

  CbmTrackParP result;
  CbmTrackParH result2;
  
  std::cout<<"Before prop:"<<std::endl;
  Print();

  double cova[15];
  int count=0;;
  for(int i=0; i<5;++i){
    for(int j=i;j<5;++j){
      cova[count++]=cov[i][j];
    }
  }
  // protect against low momentum:
  if(fabs(state[0][0])>10){
    statePred=state;
    covPred=cov;
    statusFlag=10;
    std::cout<<"*** PROTECT AGAINST LOW MOMENTA ***"<<std::endl;
    return;
  }

  // protect against (x,y)=(0,0)
  if(state[3][0]==0)state[3][0]=1E-4;
  if(state[4][0]==0)state[4][0]=1E-4;
  
  double spu=1; //! sign of z-component of momentum
  
  CbmTrackParP par(state[3][0],state[4][0],state[1][0],state[2][0],state[0][0],cova,ofrom,ufrom,vfrom,spu);

  std::cout<<"qp="<<par.GetQp()<<"  Dqp="<<par.GetDQp()<<std::endl;
  std::cout<<"TV="<<par.GetTV()<<"  DTV="<<par.GetDTV()<<std::endl;
  std::cout<<"TW="<<par.GetTW()<<"  DTW="<<par.GetDTW()<<std::endl;
  std::cout<<"V="<<par.GetV()<<"  DV="<<par.GetDV()<<std::endl;
  std::cout<<"W="<<par.GetW()<<"  DW="<<par.GetDW()<<std::endl;

  // Todo: put pdgcode into constructor!
  Bool_t prop = kTRUE;
  prop = _geane->Propagate(&par,&result,13);   //211
  if (prop==kFALSE) std::cout<<"propagation failed"<<std::endl;

  TVector3 pcap=_geane->GetPCAOnTrack();
  //  std::cout<<"pcaontrack="<<pcap.X()<<"\t"<<pcap.Y()<<"\t"<<pcap.Z()<<std::endl;

  //    CbmGeaneUtil *fUtil = new CbmGeaneUtil();
  //    Double_t PD[3], RD[15], H[3], CH, SPU, DJ[3], DK[3], PC[3], RC[15];
  //    Int_t IERR;
       
  // 	 PD[0] = result.GetQp()/result.GetQ();
  // 	 PD[1] = result.GetV();
  // 	 PD[2] = result.GetW();
       
  // 	 result.GetCov(RD);
        
  // 	 H[0] = 0; H[1] = 0; H[2] = 0;
  // 	 CH = _ParP.GetQ()/TMath::Abs(_ParP.GetQ());
        
  // 	 DJ[0] = result.GetJVer().X();
  // 	 DJ[1] = result.GetJVer().Y();
  // 	 DJ[2] = result.GetJVer().Z();
  // 	 DK[0] = result.GetKVer().X();
  // 	 DK[1] = result.GetKVer().Y();
  // 	 DK[2] = result.GetKVer().Z();
	 
  // //	 cout << "error alpha" << fRes->GetDPx() << endl;
       
  // 	 fUtil->FromSDToSC(PD, RD, H, CH, SPU, DJ, DK, IERR, PC, RC);

  //      result2.SetQp(PC[0]);
  //      result2.SetLambda(PC[1]);
  //      result2.SetPhi(PC[2]);

  
  statePred[0][0]=result.GetQp();
  statePred[1][0]=result.GetTV();
  statePred[2][0]=result.GetTW();
  statePred[3][0]=result.GetV();
  statePred[4][0]=result.GetW();

  double* rescov=result.GetCov();
  count=0;
  for(int i=0;i<5;++i){
    for(int j=i;j<5;++j){
      covPred[i][j]=rescov[count];
      if(i!=j)covPred[j][i]=rescov[count];
      ++count;
    }
  }

  // //   statePred[0][0]=result2.GetQp();
  // //   statePred[1][0]=result2.GetLambda();
  // //   statePred[2][0]=result2.GetPhi();
  // //   statePred[3][0]=result2.GetY();
  // //   statePred[4][0]=result2.GetZ();


  // //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;
  
  std::cout<<"After prop:"<<std::endl;
  statePred.Print();
  
  
}

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

		

TVector3 
GeaneTrackRep::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);
  throw;
  return TVector3(0,0,0);
}
 
TVector3 
GeaneTrackRep::getMom(const DetPlane& pl)
{
  TMatrixT<double> statePred(state);
  //   //statePred.Print();
  //   DetPlane p(TVector3(0,0,z),TVector3(1,0,0),TVector3(0,1,0));
  extrapolate(pl,statePred);
  //   //statePred.Print();
//   TVector3 result(statePred[3][0],statePred[4][0],1);
//   if (statePred[0][0]!=0)result.SetMag(TMath::Abs(1./statePred[0][0]));
//   else  result.SetMag(0);
  if (statePred[0][0]!=0) {
    Double_t pu = TMath::Sqrt((1./statePred[0][0])*(1./statePred[0][0])/(1.+statePred[1][0]*statePred[1][0]+statePred[2][0]*statePred[2][0]));
    Double_t pv = pu*statePred[1][0];
    Double_t pw = pu*statePred[2][0];
    TVector3 result(pu,pv,pw);
    result.SetMag(TMath::Sqrt(pu*pu+pv*pv+pw*pw));
    return result;
  }
  else {
    TVector3 result(0.,0.,0.);
    result.SetMag(0);
    return result;
  }
}
	      
ClassImp(GeaneTrackRep)