// File and Version Information:
// $Id$
//
// Description:
//      Implementation of resCalc
//      Uses existing GFTracks and calculates
//      residua in XY.
//      Developed as part of the GEM-TPC project
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Felix Boehmer                     (original author)
//      Physik Department E18, TUM
//
//-----------------------------------------------------------

#include "CdcTrackTpcHitResCalc.h"
#include "TClonesArray.h"
#include "TpcCluster.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "TpcSPHit.h"
#include "TpcAlignmentManager.h"
#include "TpcDigiPar.h"
#include "TpcDevmapCyl.h"

#include "CdcHit.h"
#include "CdcTrack.h"

#include "MatchingTuple.h"

#include "GFTools.h"
#include "GFTrack.h"
#include "GFException.h"
#include "GFAbsTrackRep.h"
#include "GFRecoHitFactory.h"
#include "GFRecoHitProducer.h"
#include "RKTrackRep.h"

#include <FairRootManager.h>
#include <FairRun.h>
#include <FairRuntimeDb.h>

#include <TClass.h>
#include <TString.h>
#include <TProfile.h>
#include <string>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdlib.h>
#include <set>

double CdcTrackTpcHitResCalc::phiDifference(double angle1, double angle2) {
  double difference = angle1 - angle2;
  difference = difference * 180 / TMath::Pi();
  if (difference > 180) {
    difference -= 360;
  } else if (difference < -180) {
    difference += 360;
  }
  return difference;
}
TString CdcTrackTpcHitResCalc::getHistoFileName() const
{
  return fHistoFileName;
}

void CdcTrackTpcHitResCalc::setHistoFileName(const TString &value)
{
  fHistoFileName = value;
}
bool CdcTrackTpcHitResCalc::getCheckFopiTrack() const
{
  return fCheckFopiTrack;
}

void CdcTrackTpcHitResCalc::setCheckFopiTrack(bool value)
{
  fCheckFopiTrack = value;
}





CdcTrackTpcHitResCalc::CdcTrackTpcHitResCalc()
  : fCdcGFTrackBranchName("CdcTrackPostFit"),
    fCdcTrackBranchName("CdcTrack"),
    fCdcHitBranchName("CdcHit"),
    fTpcTrackBranchName("TpcTrackPostFit"),
    fTpcSPHitBranchName("TpcSPHit"),
    fTpcClusterBranchName("TpcCluster"),
    fFopiTupleBranchName("bla"),
    fFopiTrackBranchName("FopiTrackPostFit"),
    fUseFelixMatching(false) ,
    fAlMan(NULL),
    fOrigTransfName("tpc"),
    fNRep(-1),
    fDetID(-1),
    fUseClusters(false),
    fAlignedMatching(true),
    fThetaCut(-1.),
    fCdcExtrapZ(false),
    fCutOnRpc(true),
    fTrackdPhiCut(10),
    fCutNSectors(0),
    fCheckFopiTrack(true),
    fCdcTrackPtCut(0.5),
    fCdcTrackMaxPtCut(99),
    fTpcTrackPtCut(0.5),
    sectorTrackSelection(false),
    minAngle(0),
    maxAngle(0),
    fMinR(7), 
    fMaxR(13),
    fMassCut(-1),
    fMinHitsCdc(45),
    fApplyDevMap(false),
    fHistoFileName("CdcTrackTpcHitResCalcQA.root"),
    fDevMapPath(""),
    fDevMap(NULL),
    fQA(NULL),
    fVerbose(0)
{
  //  initHists();
  fPhiCuts.push_back(std::make_pair(0,20));
  fPhiCuts.push_back(std::make_pair(175,181));
  fPhiCuts.push_back(std::make_pair(-181,-165));
  fNExpectedBranches = 3;
}

CdcTrackTpcHitResCalc::~CdcTrackTpcHitResCalc() {}

bool CdcTrackTpcHitResCalc::init() {
  if (fFopiTupleBranchName != "") {
    fUseFelixMatching = true;
    fNExpectedBranches = 5;
    if (fUseClusters) {
      fNExpectedBranches = 6;
    }
    if(fCutNSectors>0){
      fNExpectedBranches+=2;
    }
  } else if (fUseClusters) {
    fNExpectedBranches = 4;
  }
  fQA=new CdcTpcMatchingQA(fHistoFileName);
  if (fApplyDevMap) {
    FairRun* run = FairRun::Instance();
    if (!run) Fatal("SetParContainers", "No analysis run");
        
    FairRuntimeDb* db = run->GetRuntimeDb();
    if (!db) Fatal("SetParContainers", "No runtime database");
        
    // Get Tpc digitisation parameter container
    TpcDigiPar* fpar = (TpcDigiPar*)db->getContainer("TpcDigiPar");
    if (fDevMapPath == "") {
      std::cerr << "CdcTrackTpcHitResCalc::init() Apply DevMap option set, but "
	"path is empty" << "\n";
      throw;
    }
    std::cout << "#############################################################"
      "############################################\n";
    std::cout << "#############################################################"
      "############################################\n\n\n\n";
    std::cout << fpar->getGas()->VDrift() << "\n";
    std::cout << "\n\n\n#######################################################"
      "##################################################\n";
    std::cout << "#############################################################"
      "############################################\n";
    fDevMap = new TpcDevmapCyl(fDevMapPath, fpar->getGas()->VDrift(), kTRUE);
    if (!fDevMap->loaded()) {
      std::cerr
	<< "CdcTrackTpcHitResCalc::init() trouble loading deviation map "
	<< "\n";
      throw;
    }
    fDevMap->printc();
  }
    
    
  // check for proper data types:
  bool foundTpcTrack = false;
  bool foundCdcGFTrack = false;
  bool foundSPHitArray = false;
  bool foundClusterArray = false;
  std::string TpcTrackRefType("GFTrack");
  std::string CdcGFTrackRefType("GFTrack");
  std::string tpcSPHitType("TpcSPHit");
  std::string tpcClusterType("TpcCluster");
    
  if (fBranchMap.count(fCdcGFTrackBranchName)) {
    std::cout << "cdctrack branch found:" << fCdcGFTrackBranchName << "\n";
    std::string type(fBranchMap[fCdcGFTrackBranchName]->GetClass()->GetName());
    if (type.find(CdcGFTrackRefType) < type.size()) {
      fCdcGFTrackArray = fBranchMap[fCdcGFTrackBranchName];
      foundCdcGFTrack = true;
    }
  }
  if (fBranchMap.count(fTpcTrackBranchName)) {
    std::cout << "track branch found:" << fTpcTrackBranchName << "\n";
    std::string type(fBranchMap[fTpcTrackBranchName]->GetClass()->GetName());
    if (type.find(TpcTrackRefType) < type.size()) {
      fTpcTrackArray = fBranchMap[fTpcTrackBranchName];
      foundTpcTrack = true;
    }
  }
  if (fBranchMap.count(fTpcSPHitBranchName)) {
    std::cout << "SPHit branch found:" << fTpcSPHitBranchName << "\n";
    std::string type(fBranchMap[fTpcSPHitBranchName]->GetClass()->GetName());
    if (type.find(tpcSPHitType) < type.size()) {
      fTpcSPHitArray = fBranchMap[fTpcSPHitBranchName];
      foundSPHitArray = true;
    }
  }
  if (!(foundTpcTrack && foundSPHitArray && foundCdcGFTrack)) {
    std::cout << "CdcTrackTpcHitResCalc::init(): " << "\n"
	      << "   ERROR: Wrong Class Types in input branches!" << "\n";
    return true;
  }
    
  fAlMan = TpcAlignmentManager::getInstance();
  if (fUseClusters) {
    if (fBranchMap.count(fTpcClusterBranchName)) {
      std::cout << "TpcCluster branch found:" << fTpcClusterBranchName
		<< "\n";
      std::string type(
		       fBranchMap[fTpcClusterBranchName]->GetClass()->GetName());
      if (type.find(tpcClusterType) < type.size()) {
	fTpcClusterArray = fBranchMap[fTpcClusterBranchName];
	foundClusterArray = true;
      }
    }
    if (!foundClusterArray) {
      std::cout << "CdcTrackTpcHitResCalc::init(): " << "\n"
		<< "   ERROR: TpcClusters enabled but not found!" << "\n";
      return true;
    }
  }
  bool foundFopiTrackArray = false;
  if (fUseFelixMatching) {
    bool foundTupleArray = false;
    if (fBranchMap.count(fFopiTupleBranchName)) {
      std::cout << "FopiTuple branch found:" << fFopiTupleBranchName
		<< "\n";
      std::string type(fBranchMap[fFopiTupleBranchName]->GetClass()->GetName());
      if (type.find("MatchingTuple") < type.size() or
	  type.find("MatchingPair") < type.size()) {
	fFopiTupleArray = fBranchMap[fFopiTupleBranchName];
	foundTupleArray = true;
      }
    }
        
    if (fBranchMap.count(fFopiTrackBranchName)) {
      std::cout << "FopiTrack branch found:" << fFopiTrackBranchName
		<< "\n";
      std::string type(fBranchMap[fFopiTrackBranchName]->GetClass()->GetName());
      if (type.find("GFTrack") < type.size()) {
	fFopiTrackArray = fBranchMap[fFopiTrackBranchName];
	foundFopiTrackArray = true;
      }
    }
    if (!(foundTupleArray )) {
      std::cerr<< "CdcTrackTpcHitResCalc::init(): " << "\n"
	       << "   ERROR: FopiMatchingTuples and tracks enabled but not found!"
	       << "\n";
      return true;
    }
  }
  if(fCutNSectors>0 or fMassCut>0){
    bool foundCdcHits=false;
    bool foundCdcTracks=false;
    if(fBranchMap.count(fCdcTrackBranchName)){
      std::cout << "CdcTrack branch found " << fCdcTrackBranchName
		<<"\n";
      std::string type(fBranchMap[fCdcTrackBranchName]->GetClass()->GetName());
      if(type.find("CdcTrack")< type.size()){
	fCdcTrackArray = fBranchMap[fCdcTrackBranchName];
	foundCdcTracks=true;
      }
    }
    if(fBranchMap.count(fCdcHitBranchName)){
      std::cout << "CdcHit branch found " << fCdcHitBranchName
		<<"\n";
      std::string type(fBranchMap[fCdcHitBranchName]->GetClass()->GetName());
      if(type.find("CdcHit")< type.size()){
	fCdcHitArray = fBranchMap[fCdcHitBranchName];
	foundCdcHits=true;
      }
    }
    if(!(foundCdcHits and foundCdcTracks)){
      std::cerr<< "CdcTrackTpcHitResCalc::init(): " << "\n"
	       << "   ERROR: CdcHits and Tracks enabled but not found! "
	       <<  fCdcTrackBranchName << ", "<< fCdcHitBranchName
	       << "\n";
      return true;
    }
        
  }
  if (fBranchMap.size() != fNExpectedBranches  and !(fUseFelixMatching and !foundFopiTrackArray ) ){
    std::cout << "CdcTrackTpcHitResCalc::init(): " << "\n"
	      << "   ERROR: Wrong number of input branches ("
	      << fBranchMap.size() << ")!" << "\n";
    return true;
  }
  std::ofstream txtoutfile("CdcTrackTpcHitResCalc_settings.txt");
  txtoutfile << "Settings for CdcTrackTpcHitResCalc" << "\n";
  txtoutfile << "Original applied alignment: \t" << fOrigTransfName << "\n";
  txtoutfile << "Use SPHits(0) or Cluster(1): \t" << fUseClusters << "\n";
  txtoutfile << "Use FELIX(1) matching or manual dynamic(0) \t"
	     << fUseFelixMatching << "\n";
  txtoutfile << "dPhi cuts on the track tangentials used for matching: \t"
	     << fTrackdPhiCut << "\n";
  txtoutfile << "dPhi cuts on the track positions used for matching: \t"
	     << fTrackdPhiCut / 2 << "\n";
  txtoutfile << "Momentum cut, minimum momentum for cdc tracks: \t"
	     << fCdcTrackPtCut << ", " << fCdcTrackMaxPtCut << "\n";
  txtoutfile << "Momentum cut, minimum momentum for Tpc tracks: \t"
	     << fTpcTrackPtCut << "\n";
  txtoutfile << "Extra thetaCut (<0:no cut applied): \t" << fThetaCut << "\n";
  txtoutfile << "Force succesfull extrap to z-axis: \t" << fCdcExtrapZ<< std::endl
	     << "Discard tracks without RPC hit: \t"<<fCutOnRpc<<std::endl
	     << "Discard cdc tracks with less than: \t"<<fMinHitsCdc<<std::endl
	     << "Discard tpc points with phi in between these limits:"<<std::endl;
  for(unsigned int iCut=0;iCut<fPhiCuts.size();++iCut){
    txtoutfile<<fPhiCuts[iCut].first<<",\t"<<fPhiCuts[iCut].second<<std::endl;
  }
  // All ok
  return false;
}

// main functionality
int CdcTrackTpcHitResCalc::calc() {
  // clear results
  for (unsigned int i = 0; i < fResults.size(); i++) {
    delete fResults[i];
  }
    
  fResults.clear();
    
  if (fUseFelixMatching) {
    int nTuples = fFopiTupleArray->GetEntriesFast();
    std::map<int, std::vector<int> > matchedTpcTracks;
    if(fVerbose>1){
      std::cout<<"nTuples="<<nTuples<<"\n";
    }
    for (unsigned iTuple = 0; iTuple < nTuples; ++iTuple) {
      MatchingTuple* theTuple = (MatchingTuple*)fFopiTupleArray->At(iTuple);
      if (theTuple->isMatched()) {
	if (theTuple->hasBranch("CdcTrack") and
	    theTuple->hasBranch(fTpcTrackBranchName)) {
	  if(fVerbose>1){
	    std::cout<<iTuple<<", "<<theTuple->getIdFromBranch(fCdcTrackBranchName)<<", "
	             <<theTuple->getIdFromBranch(fTpcTrackBranchName)<<"\n";
	  }
	  matchedTpcTracks[theTuple->getIdFromBranch("CdcTrack")]
	    .push_back(theTuple->getIdFromBranch(fTpcTrackBranchName));
	}
      }
    }
        
    for (unsigned iTuple = 0; iTuple < nTuples; ++iTuple) {
      MatchingTuple* theTuple = (MatchingTuple*)fFopiTupleArray->At(iTuple);
      if (!(theTuple->hasBranch("CdcTrack") and
	    theTuple->hasBranch(fTpcTrackBranchName))) {
	if(fVerbose>1){
	  std::cout<<"cdc or tpc track missing"<<"\n";
	}
	continue;
      }
      if(!theTuple->isMatched()){
	if(fVerbose>1){
	  std::cout<<"skipping, not matched"<<"\n";
	}
	continue;
      }
      int cdcId = theTuple->getIdFromBranch("CdcTrack");
      int tpcId = theTuple->getIdFromBranch(fTpcTrackBranchName);
      if(fCheckFopiTrack && fFopiTrackArray!=NULL && fFopiTrackArray->GetEntries()>0 ){
	GFTrack* combTrack = (GFTrack*)fFopiTrackArray->At(iTuple);
	GFAbsTrackRep* fopiRep = combTrack->getCardinalRep();
	if(fopiRep->getStatusFlag()!=0){
	  if(fVerbose>1){
	    std::cout<<"skipping fopiRep statusflag !=0"<<"\n";
	  }
	  continue;
	}
      }
      GFTrack* cdcTrack = (GFTrack*)fCdcGFTrackArray->At(cdcId);
      GFTrack* tpcTrack = (GFTrack*)fTpcTrackArray->At(tpcId);
      GFAbsTrackRep* cdcRep;
      if (fNRep >= 0) {
	cdcRep = cdcTrack->getTrackRep(fNRep);
      } else {
	cdcRep = cdcTrack->getCardinalRep();
      }
      if (cdcRep->getStatusFlag() != 0) {
	if(fVerbose>1){
	  std::cout<<"skipping cdcRep statusflag !=0"<<"\n";
	}
	continue;
      }
      GFAbsTrackRep* tpcRep = tpcTrack->getCardinalRep();
      if (tpcRep->getStatusFlag() != 0) {
	if(fVerbose>1){
	  std::cout<<"skipping tpcRep statusflag !=0"<<"\n";
	}
	continue;
      }
      if(abs(((RKTrackRep*)tpcRep)->getPDG())>5000 ){
	if(fVerbose>1){
	  std::cout<<"tpc pdg id= "<<((RKTrackRep*)tpcRep)->getPDG()<<" ,  skipping "<<"\n";
	}
	continue;
      }
      if(abs(((RKTrackRep*)cdcRep)->getPDG())>5000 ){
	if(fVerbose>1){
	  std::cout<<"tpc pdg id= "<<((RKTrackRep*)cdcRep)->getPDG()<<" ,  skipping "<<"\n";
	}
	continue;
      }
      double cdcCharge = cdcRep->getCharge();
      TVector3 cdcMom, cdcPos;
      TMatrixDSym cdcCov_;
      cdcRep->getPosMomCov(cdcPos, cdcMom, cdcCov_);
      TVector3 tpcMom, tpcPos;
      TMatrixDSym tpcCov_;
      tpcRep->getPosMomCov(tpcPos, tpcMom, tpcCov_);
            
      // Fill basic hits before any cuts have been done
      // Only cdc charge is reliable
      fQA->fillBasicHists("cdc", cdcMom, cdcPos, cdcCharge);
      fQA->fillBasicHists("cdc", cdcMom, cdcPos, 0);
      fQA->fillBasicHists("tpc", tpcMom, tpcPos, cdcCharge);
      fQA->fillBasicHists("tpc", tpcMom, tpcPos, 0);
            
            
            
      if (cdcMom.Perp() < fCdcTrackPtCut or cdcMom.Perp() > fCdcTrackMaxPtCut) {
	if(fVerbose>1){
	  std::cout<<"skipping momCut "<<cdcMom.Perp()<<"\n";
	}
	continue;
      }
      std::vector<int> detIds=cdcTrack->getCand().getDetIDs();
      std::set<int> detIdSet;
      for(unsigned int iId=0; iId < detIds.size();++iId){
	detIdSet.insert(detIds[iId]);
      }
      if(fCutOnRpc and !detIdSet.count(12)){
	if(fVerbose>1){
	  std::cout<<"no rpc hit \n";
	}
	continue;
      }
      try {
	cdcRep->extrapolateToCylinder(15, cdcPos, cdcMom);
      } catch (GFException& ex) {
	if(fVerbose>1){
	  std::cout << "CdcTrackTpcHitResCalc::calc(): Error during CDC extrap "
	    "to cylinder: r=15 \n"
		    << "     " << ex.what() << "\n";
	}
	continue;
      }
      TVector3 cdcPos20,cdcMom20;
      try {
	cdcRep->extrapolateToCylinder(20, cdcPos20, cdcMom20);
      } catch (GFException& ex) {
	if(fVerbose>1){
	  std::cout << "CdcTrackTpcHitResCalc::calc(): Error during CDC extrap "
	    "to cylinder: r=20 \n"
		    << "     " << ex.what() << "\n";
	}
	continue;
      }
      if (fCdcExtrapZ) {
	TVector3 pocaZ, normVec, pocaOnWire;
	try {
	  cdcRep->extrapolateToLine(TVector3(0, 0, -100), TVector3(0, 0, 100),
				    pocaZ, normVec, pocaOnWire);
	} catch (GFException& ex) {
	  if(fVerbose>1){
	    std::cout << "CdcTrackTpcHitResCalc::calc(): Error during CDC extrap "
	      "to Z-axis: \n"
		      << "     " << ex.what() << "\n";
	  }
	  continue;
	}
      }
            
      //      ################################################################################
      // To be able to fill the tpc tracks before the cdc pt cut
      if (cdcMom.Perp() < fCdcTrackPtCut or cdcMom.Perp() > fCdcTrackMaxPtCut) {
	if(fVerbose>1){
	  std::cout<<"skipping momCut "<<cdcMom.Perp()<<"\n";
	}
	continue;
      }
      fQA->fillBasicHists("cdc_cutPt", cdcMom, cdcPos, cdcCharge);
      fQA->fillBasicHists("cdc_cutPt", cdcMom, cdcPos, 0);
      //            if (tpcMom.Perp() < fTpcTrackPtCut) {
      //  continue;
      //            }
      // Making a copy of the track rep, and giving it the last state.
      RKTrackRep* lastTpcRep =
	(RKTrackRep*)tpcRep->clone();  //      delete lastTpcRep;!!!
      TMatrixDSym lastTpcCov = lastTpcRep->getLastCov();
      GFDetPlane lastTpcPl = lastTpcRep->getLastPlane();
      TVectorD lastTpcState = lastTpcRep->getLastState();
      try {
	lastTpcRep->setData(
                            lastTpcState, lastTpcPl, &lastTpcCov,
                            lastTpcRep->getAuxInfo(lastTpcRep->getReferencePlane()));
      } catch (GFException& ex) {
	if(fVerbose>1){
	  std::cout
	    << "CdcTrackTpcHitResCalc::calc():during getting of AuxInfo: \n"
	    << "     " << ex.what() << "\n";
	}
	delete lastTpcRep;
	continue;
      }
      TVector3 lastTpcMom, lastTpcPos;
      ((GFAbsTrackRep*)lastTpcRep)
	->getPosMomCov(lastTpcPos, lastTpcMom, lastTpcCov);
      // LastMom << firstMom
      if (lastTpcMom.Perp() < tpcMom.Perp() / 4 && lastTpcMom.Perp() < 0.01) {
	if(fVerbose>1){
	  std::cout << "#########################################################"
	    "#######################" << "\n";
	  std::cout << "Tpc last state broken continue cdc#: "
		    << "blurgh"
		    << ", tpc# "
		    << "blurgh" << "\n";
	  std::cout << "First mom Pt: " << tpcMom.Perp()
		    << ", lastMom pt: " << lastTpcMom.Perp() << "\n";
	  std::cout << "#########################################################"
	    "#######################" << "\n";
	}
	delete lastTpcRep;
	continue;
      }
      // Rotate and shift TpcTrack using alignment
      if (fOrigTransfName != "tpc") {
	lastTpcMom = fAlMan->masterToLocalVect(fOrigTransfName, lastTpcMom);
	lastTpcMom = fAlMan->localToMasterVect("tpc", lastTpcMom);
	lastTpcPos = fAlMan->masterToLocal(fOrigTransfName, lastTpcPos);
	lastTpcPos = fAlMan->localToMaster("tpc", lastTpcPos);
	lastTpcRep->setPosMomCov(lastTpcPos, lastTpcMom, lastTpcCov);
      }
      try {
	lastTpcRep->extrapolateToCylinder(15, tpcPos, tpcMom);
      } catch (GFException& ex) {
	if(fVerbose>1){
	  std::cout << "CdcTrackTpcHitResCalc::calc(): Error during TPC extrap "
	    "to cylinder: \n"
		    << "     " << ex.what() << "\n";
	}
	delete lastTpcRep;
	continue;
      }
      //            if (lastTpcMom.Perp() < fTpcTrackPtCut) {
      //  delete lastTpcRep;
      //  continue;
      //}
      fQA->fillBasicHists("tpc_cutPt", tpcMom, tpcPos, cdcCharge);
      fQA->fillBasicHists("tpc_cutPt", tpcMom, tpcPos, 0);
      //################################################################################
      // Cdc-Tpc- Matching!
      //*********************************************************************
      // std::cout<<"filling hists before matching"<<"\n";
            
      fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, false, 0);
      fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, false, cdcCharge);
      if (!(theTuple->isMatched())) {
	if(fVerbose>1){
	  std::cout<<"theTuple->isMatched()"<<theTuple->isMatched()<<"\n";
	}
	// theTuple->print();
	continue;
      }
      //            else {
      //                std::cout<<"#######################################################################3"<<"\n";
      //                theTuple->print();
      //                std::cout<<"#######################################################################3"<<"\n";
      //            }
            
      if (matchedTpcTracks[cdcId].size() != 1) {
	if(fVerbose>1){
	  std::cout<<"double match, skipping track "<<matchedTpcTracks[cdcId].size()<<"\n";
	}
	continue;
      }
            
      if (cdcTrack->getCand().getNHits() < fMinHitsCdc) {
	if(fVerbose>1){
	  std::cout<<"reference track has to few hits: "<<cdcTrack->getCand().getNHits()<<"\n";
	}
	continue;
      }
      if (fThetaCut > 0 and
	  fabs(phiDifference(tpcMom.Theta(), cdcMom.Theta())) > fThetaCut) {
	if(fVerbose>1){
	  std::cout<<"theta cut failed"<<"\n";
	}
	continue;
      }
      if (fCutNSectors>0){
	CdcTrack* theTrack =(CdcTrack*) fCdcTrackArray->At(cdcId);
	int nSec=getNSectors(theTrack);
	if(nSec!=fCutNSectors){
	  continue;
	  //		    std::cout<<"nSec!=fCutNSectors "<<nSec<<", "<<fCutNSectors<<"\n";
	}
      }
      if (fMassCut>0){
	CdcTrack* theTrack =(CdcTrack*) fCdcTrackArray->At(cdcId);
	if(theTrack->GetMass()>fMassCut){
	  continue;
	}
      }
      fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, true, 0);
      fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, true, cdcCharge);
      fResults.push_back(new TpcRefResidualCollection(
						      fCdcGFTrackBranchName.Data(),
						      cdcId));
      GFTrackCand cand = tpcTrack->getCand();
      unsigned int nHits = cand.getNHits();
      //bool smoothing = tpcTrack->getSmoothing();
      //	    std::cout<<"looping over hits "<<nHits<<"\n";
      for (int iHit = (nHits - 1); iHit >= 0; iHit = (iHit - 1)) {
	int hitId, detId;
	cand.getHit(iHit, detId, hitId);
	TpcSPHit* recoHit = (TpcSPHit*)fTpcSPHitArray->At(hitId);
	if (recoHit == NULL) {
	  std::cout << "nooooooooooooooooooooooooooooooooo " << iHit << " "
		    << hitId << " " << detId << "\n";
	  std::cout << fTpcSPHitArray->GetEntries() << "\n";
	}
	double weight = 1;
	//                if (smoothing) {
	//                    TVectorD weight2 =
	//                            tpcTrack->getBK(-1)->getVector(GFBKKey_dafWeight, iHit);
	//                    // std::cout<<weight2[0]<<"\n";
	//                    weight = weight2[0];
	//                }
	TVector3 hPos;
	TMatrixT<double> cov(3, 3);
                
	if (fUseClusters) {
	  int iCl = recoHit->getClusterID();
	  TpcCluster* cl =
	    (TpcCluster*)fTpcClusterArray->At(recoHit->getClusterID());
	  if (cl == 0) {
	    std::cout << "no cluster at id: " << iCl
		      << " nCl: " << fTpcClusterArray->GetEntriesFast()
		      << "\n";
	    std::cout << "hitid: " << hitId << " ihit: " << iHit
		      << " nhit: " << fTpcSPHitArray->GetEntriesFast()
		      << "\n";
	    continue;
	  }
	  hPos = cl->pos();
	  hPos.SetZ(hPos.Z()-42.7);
	  ///hPos = fAlMan->localToMaster("tpc", hPos);
	  TVector3 sig = cl->sig();
	  cov[0][0] = sig.X() * sig.X();
	  cov[1][1] = sig.Y() * sig.Y();
	  cov[2][2] = sig.Z() * sig.Z();
	  //cov = fAlMan->localToMaster("tpc", cov);
	} else {
	  TVectorT<double> rc = ((GFAbsRecoHit*)recoHit)->getRawHitCoord();
	  cov = ((GFAbsRecoHit*)recoHit)->getRawHitCov();
	  hPos = TVector3(rc[0], rc[1], rc[2]);
	  if (fOrigTransfName != "tpc") {
	    hPos = fAlMan->masterToLocal(fOrigTransfName, hPos);
	    if (hPos.Perp() > fMaxR or hPos.Perp() < fMinR) {  // todo parameters!
	      continue;
	    }
	    double hPhi = hPos.Phi() * TMath::RadToDeg();
	    bool cont=false;
	    for(unsigned int iCut=0;iCut<fPhiCuts.size();++iCut){
	      if(hPhi > fPhiCuts[iCut].first and hPhi < fPhiCuts[iCut].second){
		cont=true;
		break;
	      }
	    }
	    if(cont){
	      continue;
	    }
                        
	    if (fApplyDevMap) {
	      TVector3 devPos = hPos;
	      // devPos.SetZ(devPos.Z()-60);
	      TVector3 corrval = fDevMap->value(devPos);
	      hPos = hPos - corrval;
	    }
	    hPos = fAlMan->localToMaster("tpc", hPos);
	    cov = fAlMan->masterToLocal(fOrigTransfName, cov);
	    cov = fAlMan->localToMaster("tpc", cov);
	  }
	}
                
	double tracklength = 0;
	bool ok = true;
	TVector3 poca, norm;
	TVector3 pos1, mom1, pos2;
	TMatrixDSym cov6, covPred;
	TVector3 posTpc, momTpc, pocaOnWire, dirInPoca;
	try {
	  // cdcRep->extrapolateToCylinder(hPos.Perp(),poca,norm);
	  TVector3 p1 = hPos;
	  TVector3 p2 = hPos;
	  p1.SetZ(-100);
	  p2.SetZ(100);
	  cdcRep->extrapolateToLine(p1, p2, poca, dirInPoca, pocaOnWire);
	  TVectorT<double> state;
                    
                    
	  GFDetPlane plane2(poca, TVector3(0,0,1),TVector3(0,0,1).Cross(dirInPoca));
	  cdcRep->extrapolate(plane2, state, covPred);
	  cdcRep->getPosMom(plane2,pos2,mom1);
	  lastTpcRep->getPosMom(plane2,posTpc,momTpc);
	  tracklength = cdcRep->extrapolate(plane2);
	  cdcRep->getPosMomCov(pos1, mom1, cov6);
                    
	} catch (GFException& ex) {
	  std::cout << "CdcTrackTpcHitResCalc::calc(): Error during cdc "
	    "extrapolation to hit:\n "
		    << "Cdc Pt: " << cdcMom.Perp()
		    << ", theta: " << cdcMom.Theta() * TMath::RadToDeg()
		    << "\n     " << ex.what();
	  ok = false;
	}
	if (ok) {  // add residual
	  TpcResidual* theRes = new TpcResidual();
	  theRes->setResXYZ(hPos-pos1); //measured(tpc) - expected(cdc)
	  theRes->setRefPos(poca);
	  //		    std::cout<<"hPos"<<"\n";
	  theRes->setHitPos(hPos);
	  theRes->setTrackDir(mom1.Unit());
	  theRes->setTrackDirLocal(momTpc.Unit());
	  // TODO, use correct assignment (cluster/SPHit)
	  if (fUseClusters) {
	    theRes->setHitIndex(recoHit->getClusterID());
	  } else {
	    theRes->setHitIndex(hitId);
	  }
	  // std::cout<<"cov6.Print(): "<<cov6.Determinant()<<"\n";
	  // cov6.Print();
	  //TMatrixDSym cov3 = cov6.GetSub(0, 2, 0, 2);
	  // cov3.Print();
	  theRes->setHitCov(cov);
	  theRes->setTrackCov(covPred);
	  theRes->setTrackLength(weight);
	  theRes->setCharge(cdcCharge);
	  theRes->setRefCylPosMom(cdcPos20,cdcMom20);
	  //std::cout<<"add residual"<<"\n";
	  fResults.back()->addResidual(theRes);
	}
      }  // end Hit loop
      delete lastTpcRep;
    }
  } else {
    //####################################################################################################
    //####################################################################################################
    //####################################################################################################
    // manual matching
    std::cerr<<"this should not happen anymore!!!!"<<"\n";
    throw;
    unsigned int nCdcTracks = fCdcGFTrackArray->GetEntriesFast();
    for (unsigned int irtr = 0; irtr < nCdcTracks; ++irtr) {
      GFTrack* iCdcTrack = (GFTrack*)((*fCdcGFTrackArray)[irtr]);
      GFAbsTrackRep* cdcRep;
      if (fNRep >= 0) {
	cdcRep = iCdcTrack->getTrackRep(fNRep);
      } else {
	cdcRep = iCdcTrack->getCardinalRep();
      }
      if (cdcRep->getStatusFlag() != 0) {
	continue;
      }
      if (iCdcTrack->getCand().getNHits() < 45) {
	//	std::cout<<"reference track has to few hits:
	//"<<cdcTrack->getCand().getNHits()<<"\n";
	continue;
      }
      double cdcCharge = cdcRep->getCharge();
      TVector3 cdcMom = cdcRep->getMom();
      TVector3 cdcPos = cdcRep->getPos();
      fQA->fillBasicHists("cdc", cdcMom, cdcPos, cdcCharge);
      fQA->fillBasicHists("cdc", cdcMom, cdcPos, 0);
            
      if (cdcMom.Perp() < fCdcTrackPtCut) {
	continue;
      }
      try {
	cdcRep->extrapolateToCylinder(15, cdcPos, cdcMom);
      } catch (GFException& ex) {
	// std::cout<<"TpcRefGFTrackResCalc::calc(): Error during CDC extrap to
	// cylinder: \n"
	//	   <<"     "<<ex.what()<<"\n";
	continue;
      }
      // just to be sure
      if (cdcMom.Perp() < fCdcTrackPtCut) {
	continue;
      }
      // fQA->fillBasicHists("cdc_cutPt",cdcMom,cdcPos,cdcCharge);
      // fQA->fillBasicHists("cdc_cutPt",cdcMom,cdcPos,0);
            
      // create residualCollection for this track
      fResults.push_back(
			 new TpcRefResidualCollection(fTpcSPHitBranchName.Data(), irtr));
      unsigned int nTpcTracks = fTpcTrackArray->GetEntriesFast();
      for (unsigned int itr = 0; itr < nTpcTracks; ++itr) {
	GFTrack* iTpcTrack = (GFTrack*)((*fTpcTrackArray)[itr]);
	GFAbsTrackRep* tpcRep = iTpcTrack->getCardinalRep();
	if (tpcRep->getStatusFlag() != 0) {
	  continue;
	}
	TVector3 tpcMom, tpcPos;
	TMatrixDSym tpcCov_;
	int tpcCharge = tpcRep->getCharge();
	tpcRep->getPosMomCov(tpcPos, tpcMom, tpcCov_);
                
	fQA->fillBasicHists("tpc", tpcMom, tpcPos, tpcCharge);
	fQA->fillBasicHists("tpc", tpcMom, tpcPos, 0);
	if (tpcMom.Perp() < fTpcTrackPtCut) {
	  continue;
	}
	// Making a copy of the track rep, and giving it the last state.
	RKTrackRep* lastTpcRep =
	  (RKTrackRep*)tpcRep->clone();  //      delete lastTpcRep;!!!
	TMatrixDSym lastTpcCov = lastTpcRep->getLastCov();
	GFDetPlane lastTpcPl = lastTpcRep->getLastPlane();
	TVectorD lastTpcState = lastTpcRep->getLastState();
	try {
	  lastTpcRep->setData(
			      lastTpcState, lastTpcPl, &lastTpcCov,
			      lastTpcRep->getAuxInfo(lastTpcRep->getReferencePlane()));
	} catch (GFException& ex) {
	  std::cout
	    << "CdcTrackTpcHitResCalc::calc():during getting of AuxInfo: \n"
	    << "     " << ex.what() << "\n";
	  delete lastTpcRep;
	  continue;
	}
	TVector3 lastTpcMom, lastTpcPos;
	((GFAbsTrackRep*)lastTpcRep)
	  ->getPosMomCov(lastTpcPos, lastTpcMom, lastTpcCov);
	// LastMom << firstMom
	if (lastTpcMom.Mag() < tpcMom.Mag() / 4) {
	  std::cout << "#######################################################"
	    "#########################" << "\n";
	  std::cout << "Tpc last state broken continue cdc#: " << irtr
		    << ", tpc# " << itr << "\n";
	  std::cout << "First Mom: " << tpcMom.Mag()
		    << ", lastMom: " << lastTpcMom.Mag() << "\n";
	  std::cout << "#######################################################"
	    "#########################" << "\n";
	  delete lastTpcRep;
	  continue;
	}
	// Rotate and shift TpcTrack using alignment
	if (fOrigTransfName != "tpc") {
	  lastTpcMom = fAlMan->masterToLocalVect(fOrigTransfName, lastTpcMom);
	  lastTpcMom = fAlMan->localToMasterVect("tpc", lastTpcMom);
	  lastTpcPos = fAlMan->masterToLocal(fOrigTransfName, lastTpcPos);
	  lastTpcPos = fAlMan->localToMaster("tpc", lastTpcPos);
	  lastTpcRep->setPosMomCov(lastTpcPos, lastTpcMom, lastTpcCov);
	}
	try {
	  lastTpcRep->extrapolateToCylinder(15, tpcPos, tpcMom);
	} catch (GFException& ex) {
	  std::cout << "CdcTrackTpcHitResCalc::calc(): Error during TPC extrap "
	    "to cylinder: \n"
		    << "     " << ex.what() << "\n";
	  delete lastTpcRep;
	  continue;
	}
	if (lastTpcMom.Perp() < fTpcTrackPtCut) {
	  delete lastTpcRep;
	  continue;
	}
	// fQA->fillBasicHists("tpc_cutPt",tpcMom,tpcPos,tpcCharge);
	// fQA->fillBasicHists("tpc_cutPt",tpcMom,tpcPos,0);
	//################################################################################
	// Cdc-Tpc- Matching!
	//*********************************************************************
	// std::cout<<"filling hists before matching"<<"\n";
	fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, false, 0);
	fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, false, cdcCharge);
	// std::cout<<"done filling hists before matching"<<"\n";
	double dPhiMom = phiDifference(tpcMom.Phi(), cdcMom.Phi());
	double dPhiPos = phiDifference(tpcPos.Phi(), cdcPos.Phi());
	// std::cout<<"dPhiPos: "<<dPhiPos<<", dPhiMom: "<<dPhiMom<<"\n";
	if ((fabs(dPhiPos) > (this->fTrackdPhiCut / 2)) or
	    (fabs(dPhiMom) > this->fTrackdPhiCut)) {
	  //	std::cout<<"failing cut "<<this->fTrackdPhiCut<<"\n";
	  delete lastTpcRep;
	  continue;
	}
	if (fThetaCut > 0 &&
	    fabs(phiDifference(tpcMom.Theta(), cdcMom.Theta())) > fThetaCut) {
	  delete lastTpcRep;
	  continue;
	}
	// std::cout<<"dPhiPos: "<<dPhiPos<<", dPhiMom: "<<dPhiMom<<"\n";
	// std::cout<<"tpcPos: ";tpcPos.Print();
	// std::cout<<"tpcMom: ";tpcMom.Print();
	// std::cout<<"cdcPos: ";cdcPos.Print();
	// std::cout<<"cdcMom: ";cdcMom.Print();
	// std::cout<<"filling hists after matching"<<"\n";
	fQA->fillBasicHists("tpc_cutPt", tpcMom, tpcPos, tpcCharge);
	fQA->fillBasicHists("tpc_cutPt", tpcMom, tpcPos, 0);
	fQA->fillBasicHists("cdc_cutPt", cdcMom, cdcPos, cdcCharge);
	fQA->fillBasicHists("cdc_cutPt", cdcMom, cdcPos, 0);
	fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, true, 0);
	fQA->fillMatchingHists(tpcMom, tpcPos, cdcMom, cdcPos, true, cdcCharge);
	// std::cout<<"done filling hists after matching"<<"\n";
	// loop over hits from matched track
	GFTrackCand cand = iTpcTrack->getCand();
	unsigned int nHits = cand.getNHits();
	bool smoothing = iTpcTrack->getSmoothing();
	for (int iHit = (nHits - 1); iHit >= 0; iHit = (iHit - 1)) {
	  int hitId, detId;
	  cand.getHit(iHit, detId, hitId);
	  TpcSPHit* recoHit = (TpcSPHit*)fTpcSPHitArray->At(hitId);
	  if (recoHit == NULL) {
	    std::cout << "nooooooooooooooooooooooooooooooooo " << iHit << " "
		      << hitId << " " << detId << "\n";
	    std::cout << fTpcSPHitArray->GetEntries() << "\n";
	  }
	  //double weight = 1;
	  if (smoothing) {
	    TVectorD weight2 =
	      iTpcTrack->getBK(-1)->getVector(GFBKKey_dafWeight, iHit);
	    std::cout << weight2[0] << "\n";
	  }
	  TVector3 hPos;
	  TMatrixT<double> cov(3, 3);
                    
	  if (fUseClusters) {
	    int iCl = recoHit->getClusterID();
	    TpcCluster* cl =
	      (TpcCluster*)fTpcClusterArray->At(recoHit->getClusterID());
	    if (cl == 0) {
	      std::cout << "no cluster at id: " << iCl
			<< " nCl: " << fTpcClusterArray->GetEntriesFast()
			<< "\n";
	      std::cout << "hitid: " << hitId << " ihit: " << iHit
			<< " nhit: " << fTpcSPHitArray->GetEntriesFast()
			<< "\n";
	      continue;
	    }
	    hPos = cl->pos();
	    hPos = fAlMan->localToMaster("tpc", hPos);
	    TVector3 sig = cl->sig();
	    cov[0][0] = sig.X() * sig.X();
	    cov[1][1] = sig.Y() * sig.Y();
	    cov[2][2] = sig.Z() * sig.Z();
	    cov = fAlMan->localToMaster("tpc", cov);
	  } else {
	    TVectorT<double> rc = ((GFAbsRecoHit*)recoHit)->getRawHitCoord();
	    cov = ((GFAbsRecoHit*)recoHit)->getRawHitCov();
	    hPos = TVector3(rc[0], rc[1], rc[2]);
	    if (fOrigTransfName != "tpc") {
	      hPos = fAlMan->masterToLocal(fOrigTransfName, hPos);
	      if (hPos.Perp() > 13 or hPos.Perp() < 6) {
		continue;
	      }
	      hPos = fAlMan->localToMaster("tpc", hPos);
	      cov = fAlMan->masterToLocal(fOrigTransfName, cov);
	      cov = fAlMan->localToMaster("tpc", cov);
	    }
	  }
                    
	  double tracklength = 0;
	  bool ok = true;
	  TVector3 poca, norm;
	  TVector3 pos1, mom1;
	  TMatrixDSym cov6;
	  try {
	    cdcRep->extrapolateToCylinder(hPos.Perp(), poca, norm);
	    TVectorT<double> dummy;
	    GFDetPlane plane(poca, norm);
	    tracklength = cdcRep->extrapolate(plane);
	    cdcRep->getPosMomCov(pos1, mom1, cov6);
                        
	  } catch (GFException& ex) {
	    std::cout << "CdcTrackTpcHitResCalc::calc(): Error during "
	      "extrapolation: \n"
		      << "     " << ex.what() << "\n";
	    ok = false;
	  }
	  if (ok) {  // add residual
	    TpcResidual* theRes = new TpcResidual();
	    theRes->setResXYZ(hPos-poca);
	    theRes->setRefPos(poca);
	    theRes->setHitPos(hPos);
	    theRes->setTrackDir(mom1.Unit());
	    // TODO, use correct assignment (cluster/SPHit)
	    if (fUseClusters) {
	      theRes->setHitIndex(recoHit->getClusterID());
	    } else {
	      theRes->setHitIndex(hitId);
	    }
	    TMatrixDSym cov3 = cov6.GetSub(0, 2, 0, 2);
	    // cov3.Print();
	    theRes->setHitCov(cov);
	    theRes->setTrackCov(cov3);
	    theRes->setTrackLength(tracklength);
	    theRes->setCharge(cdcCharge);
                        
	    fResults.back()->addResidual(theRes);
	  }
	}  // end Hit loop
	delete lastTpcRep;
      }  // end GFTrack loop
    }  // end refGFTrack loop
  }
}

void CdcTrackTpcHitResCalc::writeHists() { fQA->writeHists(); }

void CdcTrackTpcHitResCalc::setSectorTrackSelection(double minAngle_,
                                                    double maxAngle_) {
  if (minAngle_ < 0 or minAngle_ > 22.5 or maxAngle_ < 0 or maxAngle_ > 22.5) {
    std::cerr << "#############################################################"
      "###########################################################"
	      << "\n";
    std::cerr << "#" << "\n";
    std::cerr << "#" << "\n";
    std::cerr << "# angle range for modulo track selection outside sector "
      "size, 0<angle<22.5, minAngle: " << minAngle_
	      << ", maxAngle_: " << maxAngle_ << "\n";
    std::cerr << "#" << "\n";
    std::cerr << "#" << "\n";
    std::cerr << "#############################################################"
      "###########################################################"
	      << "\n";
    return;
  }
  if (minAngle_ < maxAngle_) {
    minAngle = maxAngle_;
    maxAngle = minAngle_;
  } else {
    maxAngle = maxAngle_;
    minAngle = minAngle_;
  }
  sectorTrackSelection = true;
}


int CdcTrackTpcHitResCalc::getCdcSector(TVector3 wirePos){
  double phi = wirePos.Phi()*TMath::RadToDeg();
  //std::cout<<"Phi: "<<phi;
  for(unsigned int i=0;i<16;++i){
    if(phi<(-155+i*22.5)){
      //  std::cout<<", sectorId: "<<i<<"\n";
      return i;
    }
  }
  return -1;
}
int CdcTrackTpcHitResCalc::getNSectors(CdcTrack *theTrack){
  //std::set<unsigned int> sectorIds;
  std::set<int> sectorIdsLeftRight;
  int nHits=theTrack->GetNpoint();
  for(int iHit=0;iHit<nHits;++iHit){
    CdcHit* hit= (CdcHit*)fCdcHitArray->At(theTrack->getNthHitID(iHit));
        
    if(hit!=NULL){
      TVector3 wirePos=hit->GetWirePosA();
      double drift=hit->GetHitDrift();
      unsigned int sectorId=getCdcSector(wirePos);
      int sectorIdLeftRight=sectorId;
      if(drift<0){
	sectorIdLeftRight*=-1;
	if(sectorIdLeftRight==0){
	  sectorIdLeftRight=-999;
	}
      }
      //sectorIds.insert(sectorId);
      sectorIdsLeftRight.insert(sectorIdLeftRight);
      //std::cout<<sectorId<<"\n";
    }else{
      std::cout<<"hit not found!!!!"<<"\n";
    }
  }
  return sectorIdsLeftRight.size();
}
ClassImp(CdcTrackTpcHitResCalc)