//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of MCTrackResCalc
//
// Environment:
//      Software NOT developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sverre Doerheim                     (original author)
//      Physik Department E18, TUM
//
//-----------------------------------------------------------
#include <iostream>

#include <TClonesArray.h>
#include <TError.h>


#include <FairRootManager.h>

#include "MCTrackResCalc.h"
#include "McId.h"

#include "GFTrack.h"
#include "GFTrackCand.h"
#include "GFTools.h"
#include "RKTrackRep.h"


#include "GFException.h"
#include "PndMCTrack.h"

MCTrackResCalc::MCTrackResCalc():fExtrapolate(false)
{
  fNExpectedBranches =2;

}


bool MCTrackResCalc::init(){
  if(fBranchMap.size() != 2) {
    std::cout<<"MCTrackResCalc::init(): "<<std::endl
       <<"   ERROR: Wrong number of input branches ("
       <<fBranchMap.size()<<")!"<<std::endl;
    return true;
  }
  //check for proper data types:
  std::string McTrackType("PndMCTrack");
  std::string GFTracType("GFTrack");
  bool foundMcTrack=false;
  bool foundGFTrack=false;

  //identify the needed branches in the map
  std::map<TString, TClonesArray*>::const_iterator it;
  for(it=fBranchMap.begin(); it!=fBranchMap.end(); it++) {
    std::string type((it->second)->GetClass()->GetName());
    std::cout<<"branchtype: "<<type<<std::endl;
    if(type.find(McTrackType)<type.size()) {
      fMcTrackArray=it->second;
      foundMcTrack=true;
      std::cout<<"MCTrackResCalc::init(): "<<std::endl
         <<"   INFO: got "<<type<<" as Object type for McTracks"
         <<std::endl;
    }
    if(type.find(GFTracType)<type.size()) {
      fGFTrackArray=it->second;
      foundGFTrack=true;
      std::cout<<"MCTrackResCalc::init(): "<<std::endl
         <<"   INFO: got "<<type<<" as Object type for GFTracks"
         <<std::endl;
    }
  }
  std::cout<<foundMcTrack<<" "<<foundGFTrack<<" "<<((foundMcTrack && foundGFTrack ))<<std::endl;
  if(!(foundMcTrack && foundGFTrack )) {
    std::cout<<"MCTrackResCalc::init(): "<<std::endl
       <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
    return true;
  }

  //all ok
  return false;
}

int MCTrackResCalc::calc(){

  //clear results
  for(unsigned int i=0; i<fResults.size(); i++)
    delete fResults[i];

  fResults.clear();

  unsigned int nTracks = fGFTrackArray->GetEntriesFast();
  unsigned int nMcTracks = fMcTrackArray->GetEntriesFast();
  for(unsigned int i_tr=0; i_tr<nTracks;++i_tr){
    GFTrack* track = (GFTrack*) fGFTrackArray->UncheckedAt(i_tr);
    const GFTrackCand cand = track->getCand();
    GFAbsTrackRep* rep = track->getCardinalRep();
    if(rep->getStatusFlag()!=0){
        continue;
    }
    int mcId=cand.getMcTrackId();
    PndMCTrack* mcTrack= (PndMCTrack*) fMcTrackArray->At(mcId);
    //DEBUG
    TVector3 mom=rep->getMom();
    TVector3 mcMom=mcTrack->GetMomentum();

    TpcGFTrackMCTrackResidual* res = new TpcGFTrackMCTrackResidual();
    fResults.push_back(res);
    GFDetPlane plane;
    TVector3 poca, normVec;
    try{
      rep->extrapolateToPoint(mcTrack->GetStartVertex(),poca,normVec);
      if(fExtrapolate){
        plane.setNormal(normVec);
        plane.setO(poca);
        mom=rep->getMom(plane);
      }

    }catch(GFException exp){
      continue;
    }
    res->setMotherId(mcTrack->GetMotherID());
    res->setPdgId(mcTrack->GetPdgCode());
    res->setMom(mcMom,mom);
    res->setStartPos(mcTrack->GetStartVertex(),poca);
    res->setFlag(rep->getStatusFlag());
  }
  return false;
}