//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Residual calculator for existing GFTracks
//      Just extracts residuals from the GFTrack object
//      for hits inside the TPC
//     
// Environment:
//      Software developed for the GEM-TPC detector
//
// Author List:
//	Felix Boehmer    	TUM     (original author)
//
//-----------------------------------------------------------


#include "TpcGFTrackResCalc.h"

#include "TClonesArray.h"
#include "TClass.h"
#include "CdcTrack.h"
#include "TpcCluster.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "TpcSPHit.h"

//#include "GFRecoHitFactory.h"
#include "GFTrack.h"
#include "GFTools.h"
#include "RecoHits/GFAbsRecoHit.h"

#include "TMatrixT.h"

#include <FairRootManager.h>

TpcGFTrackResCalc::TpcGFTrackResCalc()
    :  fSmoothedInput(false),
      fRepID(-1),
      fClBranchName("TpcCluster"),
      //fFactory(NULL),
      //fInitTracks(false),
      fRequireGoodFit(true)
{
    fNExpectedBranches = 1;
}

TpcGFTrackResCalc::~TpcGFTrackResCalc()
{
    //  if(fFactory!=NULL)
    //  delete fFactory;
    ;
}


bool
TpcGFTrackResCalc::init() {

    if(fBranchMap.size() != fNExpectedBranches) {
        std::cout<<"TpcGFTrackResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong number of input branches ("
               <<fBranchMap.size()<<")!"<<std::endl;
        return true;
    }
    //check for proper data types:
    std::string trackType("GFTrack");
    bool foundTracks=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());
        if(type.find(trackType)<type.size()) {
            fTrackArray = it->second;
            fGFTrackBranchName = it->first;
            foundTracks=true;
            std::cout<<"TpcGFTrackResCalc::init(): "<<std::endl
                    <<"   INFO: got "<<type<<" as Object type for GFTracks"
                   <<std::endl;
        }
    }
    if(!foundTracks) {
        std::cout<<"TpcGFTrackResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
        return true;
    }

    //if(fInitTracks) {
    //  fFactory = new GFRecoHitFactory();
    //}

    //all ok
  return false;  
}

//main functionality
int TpcGFTrackResCalc::calc() {

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

    fResults.clear();

    unsigned int nTracks = fTrackArray->GetEntriesFast();
    int TpcID = FairRootManager::Instance()->GetBranchId(fClBranchName);

    // GFTrack loop
    for(unsigned int itr=0; itr<nTracks; itr++) {
        GFTrack* itrack = (GFTrack*)((*fTrackArray)[itr]);
        int status;
    if(fRepID < 0) 
            status = itrack->getCardinalRep()->getStatusFlag();
        else
            status = itrack->getTrackRep(fRepID)->getStatusFlag();
        if(fRequireGoodFit && status > 0)
            continue;

        //create residualCollection for this track
        fResults.push_back(new TpcRefResidualCollection(fGFTrackBranchName.Data(),itr));
        //get list of hits lying in the TPC
        std::vector<int> tpcHits = itrack->getCand().getHitIDs(TpcID);

        //decide whether or not to calc. unbiased residuals:
        if(fSmoothedInput) {  //unbiased residuals

            TMatrixT<double> smState;
            TMatrixT<double> smCov;
            GFDetPlane smPlane;
            TVector3 trackPos;

            //loop over TPC hits:
            for(unsigned int ih=0; ih<tpcHits.size(); ih++) {
                //TODO: check for repID of cardinal rep. waiting for GENFIT feature here...
                if(fRepID < 0)
                    trackPos = GFTools::getSmoothedPosXYZ(itrack,0,tpcHits[ih]);
                else
                    trackPos = GFTools::getSmoothedPosXYZ(itrack,fRepID,tpcHits[ih]);
                GFAbsRecoHit* recoHit = itrack->getHit(ih);
                //test if TPC hit:
                TpcSPHit* test = dynamic_cast<TpcSPHit*>(recoHit);
                if(test==NULL)
                    continue;
                TVectorD rc(3);
                rc = recoHit->getRawHitCoord();
                TMatrixT<double> cov = recoHit->getRawHitCov();
                TVector3 hpos = TVector3(rc[0], rc[1], rc[2]);
                TpcResidual* theRes = new TpcResidual();
                theRes->setResXYZ(hpos-trackPos);
                theRes->setRefPos(trackPos);
                theRes->setHitPos(hpos);
                theRes->setHitIndex(ih);
                theRes->setHitCov(cov);
                fResults.back()->addResidual(theRes);
            }
        }  //end unbiased res calc.

        else {  //biased residuals: TESTED AND VERIFIED WITH LASER TRACKS
            GFAbsTrackRep* rep;
      if(fRepID < 0) 
                rep = itrack->getCardinalRep();
            else
                rep = itrack->getTrackRep(fRepID);
            //loop over TPC hits:
            for(unsigned int ih=0; ih<tpcHits.size(); ih++) {
                GFAbsRecoHit* recoHit = itrack->getHit(ih);
                //test if TPC hit:
                TpcSPHit* test = dynamic_cast<TpcSPHit*>(recoHit);
                if(test==NULL)
                    continue;
                TVectorD rc(3);
                rc = recoHit->getRawHitCoord();
                TMatrixT<double> cov = recoHit->getRawHitCov();

                TVector3 hpos = TVector3(rc[0], rc[1], rc[2]);
                TVector3 poca, norm;
                bool ok=true;
                try {
                    rep->extrapolateToPoint(hpos, poca, norm);
                }
                catch(GFException& ex) {
                    std::cout<<"TpcGFTrackResCalc::calc(): Error during extrapolation: \n"
                            <<"     "<<ex.what()<<std::endl;
                    ok=false;
                }
                if(ok) {  //add residual
                    TpcResidual* theRes = new TpcResidual();
                    theRes->setResXYZ(hpos-poca);
                    theRes->setRefPos(poca);
                    theRes->setHitPos(hpos);
                    theRes->setHitIndex(ih);
                    theRes->setHitCov(cov);
                    fResults.back()->addResidual(theRes);
                }
            }

        } //end biased res calc.

    }//end GFTrack loop

}