//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of TpcCdc2DMatchedResCalc
//      
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Felix Boehmer                     (original author)
//      Physik Department E18, TUM
//
//-----------------------------------------------------------

#include "TpcCdc2DMatchedResCalc.h"

#include "MatchingTuple.h"
#include "TpcSPHit.h"
#include "FairRootManager.h"
#include "GFTrack.h"
#include "CdcTrack.h"
#include "TpcResidual.h"
#include "TClonesArray.h"

#include <set>


TpcCdc2DMatchedResCalc::TpcCdc2DMatchedResCalc()
{
  fNExpectedBranches = 2;
}

bool TpcCdc2DMatchedResCalc::init() {
  if(fBranchMap.size() != fNExpectedBranches) {
    std::cout<<"TpcCdc2DMatchedResCalc::init(): "<<std::endl
	     <<"   ERROR: Wrong number of input branches ("
	     <<fBranchMap.size()<<")!"<<std::endl;
    return true;
  }
  //check for proper data types:
  std::string matchingRefType("MatchingTuple");
  std::string tpcSPRefType("TpcSPHit");
  bool foundTuples=false;
  bool foundTpcSPHit=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(matchingRefType)<type.size()) {
      fMatchingArray=it->second;
      fMatchingBranch = it->first;
      foundTuples = true;
      std::cout<<"TpcCdc2DMatchedResCalc::init(): "<<std::endl
	       <<"   INFO: got "<<type<<" as Object type for Tpc-Cdc MatchingPairs"
	       <<std::endl;
    }
    if(type.find(tpcSPRefType)<type.size()) {
      fSPArray=it->second;
      fSPBranch = it->first;
      foundTpcSPHit = true;
      std::cout<<"TpcCdc2DMatchedResCalc::init(): "<<std::endl
	       <<"   INFO: got "<<type<<" as Object type for TpcSPHits"
	       <<std::endl;
    }
  }
    
  
  if(!(foundTuples && foundTpcSPHit )) {
    std::cout<<"TpcCdc2DMatchedResCalc::init(): "<<std::endl
	     <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
    return true;
  }
  //all ok
  return false;  
}


//main functionality
int TpcCdc2DMatchedResCalc::calc() {
  
  FairRootManager* ioman= FairRootManager::Instance();
  if(ioman==NULL) 
    Fatal("TpcCdc2DMatchedResCalc","No FairRootManager!");
  
  //clear results
  for(unsigned int i=0; i<fResults.size(); i++) 
    delete fResults[i];
  
  fResults.clear();
  
  unsigned int nTuples = fMatchingArray->GetEntriesFast();
  // Tuple loop
  for(unsigned int itup=0; itup<nTuples; itup++) {
    MatchingTuple* iTup = (MatchingTuple*) (*fMatchingArray)[itup];
    if(!iTup->isMatched())
      continue;
    //get registered branches:
    unsigned int tpcID;
    unsigned int cdcID;
    GFTrack* iTpcTr;
    CdcTrack* iCdcTr;
    //std::vector<TString> branchNames = iTup->getListOfBranchNames();
    //for(unsigned int ibr=0; ibr<branchNames.size(); ibr++)
    //  std::cout<<branchNames[ibr].Data()<<std::endl;
    
    if(iTup->hasBranch(fTpcTrackBranch) && iTup->hasBranch(fCdcTrackBranch)) {
      tpcID = iTup->getIdFromBranch(fTpcTrackBranch);
      fTpcTrackArray = (TClonesArray*) ioman->GetObject(fTpcTrackBranch);
      iTpcTr = (GFTrack*) (*fTpcTrackArray)[tpcID];
      cdcID = iTup->getIdFromBranch(fCdcTrackBranch);
      fCdcTrackArray = (TClonesArray*) ioman->GetObject(fCdcTrackBranch);
      iCdcTr = (CdcTrack*) (*fCdcTrackArray)[cdcID];
    }
    else 
      Fatal("TpcCdc2DMatchedResCalc","Something is wrong with the Tuple! Wrong Branches!");
    
    //create residualCollection for this track
    fResults.push_back(new TpcRefResidualCollection(fCdcTrackBranch.Data(),cdcID));
    
    double mx  = iCdcTr->GetMx();
    double my  = iCdcTr->GetMy();
    double r = std::fabs(iCdcTr->GetRadius());
    
    GFTrackCand cand = iTpcTr->getCand();
    unsigned int nhits = cand.getNHits();
    
    //just a cross-check:
    std::set<unsigned int> detIDs;
    TpcSPHit* iSp;
    
    for(unsigned int isp=0; isp<nhits; isp++) {
      int detID;
      int hitID;
      cand.getHit(isp,detID,hitID);
      detIDs.insert(detID);
      iSp = (TpcSPHit*) (*fSPArray)[hitID];
      TVectorD rawP(3);
      rawP = iSp->getRawHitCoord();
      TVector3 hitPos(rawP[0],rawP[1],rawP[2]);

      TVector3 copy = hitPos;
      copy.SetZ(0.);
      TVector3 M(mx, my, 0.);
      TVector3 diff = copy-M;
      double dist = diff.Mag() - r;

      //save residual 
      diff.SetMag(dist);  //flips the vector for negative dist
      TpcResidual* resi = new TpcResidual();
      resi->setResXYZ(diff);
      resi->setHitPos(hitPos);
      resi->setHitIndex(hitID);
      TVector3 pP = copy - diff; //reference position on the circle
      resi->setRefPos(pP);
      resi->setHitCov(iSp->getRawHitCov());
      fResults.back()->addResidual(resi);
    }
    //end residual calculation  -------------------------

    //cross-checks:
    if(detIDs.size()>1)
      Fatal("TpcCdc2DMatchedResCalc","More than 1 detID in the TpcTrack cand! Something is wrong!");
    unsigned int tpcBranchID = *(detIDs.begin());
    TString hitBranchName = ioman->GetBranchName(tpcBranchID);
    if(hitBranchName != fSPBranch) {
      TString err("SPBranchName '");
      err+= fSPBranch;
      err+="' does not match the one got from candidate detID ('";
      err+=hitBranchName;
      err+="')";
      Fatal("TpcCdc2DMatchedResCalc",err);
    }
    
  }//end MatchingTuple loop
  
}