#include"PndGenfitAdapters.h"

#include"GFTrack.h"
#include"GFAbsTrackRep.h"
#include"GFTrackCand.h"
#include"PndTrack.h"
#include"PndTrackCand.h"
#include"GFDetPlane.h"
#include"TMatrixT.h"
#include"FairTrackParP.h"

#include"GeaneTrackRep.h"
#include"RKTrackRep.h"
#include <cmath>

PndTrackCand* GenfitTrackCand2PndTrackCand(const GFTrackCand* cand){
  PndTrackCand* retVal = new PndTrackCand();
  unsigned int nhits = cand->getNHits();
  unsigned detId,hitId;
  double rho;
  for(unsigned int i=0;i<nhits;++i){
    cand->getHit(i,detId,hitId,rho);
    retVal->AddHit(detId,hitId,rho);
  }
  retVal->setMcTrackId(cand->getMcTrackId());
  retVal->setTrackSeed(cand->getPosSeed(),
		       cand->getDirSeed(),
		       cand->getQoverPseed());
  return retVal;
}

GFTrackCand* PndTrackCand2GenfitTrackCand(PndTrackCand* cand){
  GFTrackCand* retVal = new GFTrackCand();
  unsigned int nhits = cand->GetNHits();
  for(unsigned int i=0;i<nhits;++i){
    PndTrackCandHit candHit = cand->GetSortedHit(i);
    retVal->addHit(candHit.GetDetId(),candHit.GetHitId(),candHit.GetRho(),0);
  }
  retVal->setMcTrackId(cand->getMcTrackId());
  retVal->setTrackSeed(cand->getPosSeed(),
		       cand->getDirSeed(),
		       cand->getQoverPseed());
  return retVal;
}

PndTrack* GenfitTrack2PndTrack(const GFTrack* tr){
  GFAbsTrackRep* clone = tr->getCardinalRep()->clone();
  TMatrixT<double> firstState = clone->getFirstState();
  TMatrixT<double> lastState = clone->getLastState();
  TMatrixT<double> firstCov = clone->getFirstCov();
  TMatrixT<double> lastCov = clone->getLastCov();
  GFDetPlane firstPlane = clone->getFirstPlane();
  GFDetPlane lastPlane = clone->getLastPlane();
  
  GFAbsTrackRep* gtr;
  if (dynamic_cast<GeaneTrackRep*>(clone)!=NULL)
	  gtr = dynamic_cast<GeaneTrackRep*>(clone);
  else if (dynamic_cast<RKTrackRep*>(clone)!=NULL)
	  gtr = dynamic_cast<RKTrackRep*>(clone);
  else {
    std::cerr << " GenfitGFAbsTrackRep2PndTrack() can currently only handle GeaneTrackRep and RKTrackRep" << std::endl;
    throw;
  }

  //make the FairTrackParP for first and last hit
  double firstCova[15];
  int count=0;;
  for(int i=0; i<5;++i){
     for(int j=i;j<5;++j){
	   firstCova[count++]=firstCov[i][j];
     }
  }
  double lastCova[15];
  count=0;;
  for(int i=0; i<5;++i){
    for(int j=i;j<5;++j){
	  lastCova[count++]=lastCov[i][j];
    }
  }

    //  calculation of spu = sign[p·(DJ x DK)]  
    double first_pro = gtr->getMom(firstPlane).Dot(firstPlane.getNormal());
    double first_spu = first_pro/fabs(first_pro);
    double last_pro = gtr->getMom(lastPlane).Dot(lastPlane.getNormal());
    double last_spu = last_pro/fabs(last_pro);
    
    FairTrackParP first(firstState[3][0],firstState[4][0],firstState[1][0],firstState[2][0],firstState[0][0],firstCova,firstPlane.getO(),firstPlane.getU(),firstPlane.getV(),first_spu);
    FairTrackParP last(lastState[3][0],lastState[4][0],lastState[1][0],lastState[2][0],lastState[0][0],lastCova,lastPlane.getO(),lastPlane.getU(),lastPlane.getV(),last_spu);
    
    //copy the trackCand
    GFTrackCand genfitCand = tr->getCand();
    PndTrackCand* pndCand = GenfitTrackCand2PndTrackCand(&genfitCand);
    PndTrack* retVal =  new PndTrack(first,last,*pndCand);
    retVal->SetChi2(tr->getChiSqu());
    retVal->SetNDF(tr->getNDF());
    if (tr->getNDF()==0)
      {
	retVal->SetFlag(-1);
      }
    else
      {
	retVal->SetFlag(1);
      }
    delete pndCand;
    return retVal;
  
}