//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of resCalc
//      Uses existing GFTracks as reference and calculates
//      residua in XY for CdcHits.
//
// Environment:
//      Software NOT developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sverre Doerheim(adapting author)
//
//      Physik Department E18, TUM
//
//-----------------------------------------------------------
#include "TpcTrackCdcHitResCalc.h"

#include "TClonesArray.h"
#include "TpcCluster.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "TpcSPHit.h"
#include "TpcAlignmentManager.h"

#include "CdcSpacepoint.h"
#include "CdcHit.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 <TClass.h>
#include <TString.h>
#include <TProfile.h>
#include <TH2D.h>
#include <TH1D.h>

#include <string>
#include <iostream>
#include <fstream>



TpcTrackCdcHitResCalc::TpcTrackCdcHitResCalc()
    : fCdcGFTrackBranchName("CdcTrackPostFit"),
      fCdcGFHitBranchName("CdcSpacepoint"),
      fTpcGFTrackBranchName("TpcTrackPostFit"),
      fCdcHitBranchName("CdcHitBr"),
      fNRep(-1),
      fDetID(-1),
      fAlignedMatching(true),
      fUseCdcHits(false),
      fTpcPtCut(0.5),
      fCdcPtCut(0.5),
      fTrackdPhiCut(10),
      fOrigTransfName("tpc"),
      fAlMan(NULL),
      cutOnSameCharge(true),
      cutOnPtRes(true),
      ptResCut(0.100),
      fQA("TpcTrackCdcHitResCalcQA.root")
{
    fNExpectedBranches = 3;
    
}

TpcTrackCdcHitResCalc::~TpcTrackCdcHitResCalc() {
    
}

bool TpcTrackCdcHitResCalc::init() {
    if(fBranchMap.size() != fNExpectedBranches) {
        std::cout<<"TpcTrackCdcHitResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong number of input branches ("
               <<fBranchMap.size()<<")!"<<std::endl;
        return true;
    }
    //check for proper data types:
    bool foundCdcGFTrack=false;
    bool foundTpcGFTrack=false;
    bool foundCdcSPHitArray=false;
    bool foundCdcHitArray=false;
    std::string CdcGFTrackRefType("GFTrack");
    std::string TpcGFTrackRefType("GFTrack");
    std::string cdcSPhitType("CdcSpacepoint");
    std::string cdcHitType("CdcHit");
    
    if(fBranchMap.count(fTpcGFTrackBranchName)){
        std::cout<<"tpctrack branch found:"<<fTpcGFTrackBranchName<<std::endl;
        std::string type(fBranchMap[fTpcGFTrackBranchName]->GetClass()->GetName());
        if(type.find(TpcGFTrackRefType)<type.size()){
            fTpcGFTrackArray=fBranchMap[fTpcGFTrackBranchName];
            foundTpcGFTrack=true;
        }
    }
    if(fBranchMap.count(fCdcGFTrackBranchName)){
        std::cout<<"track branch found:"<<fCdcGFTrackBranchName<<std::endl;
        std::string type(fBranchMap[fCdcGFTrackBranchName]->GetClass()->GetName());
        if(type.find(CdcGFTrackRefType)<type.size()){
            fCdcGFTrackArray=fBranchMap[fCdcGFTrackBranchName];
            foundCdcGFTrack=true;
        }
    }
    if(fBranchMap.count(fCdcGFHitBranchName)){
        std::cout<<"CdcSpacepoint branch found:"<<fCdcGFHitBranchName<<std::endl;
        std::string type(fBranchMap[fCdcGFHitBranchName]->GetClass()->GetName());
        if(type.find(cdcSPhitType)<type.size()){
            fCdcGFHitArray=fBranchMap[fCdcGFHitBranchName];
            foundCdcSPHitArray=true;
        }
    }
    if(!(foundCdcGFTrack&&foundCdcSPHitArray&&foundTpcGFTrack)) {
        std::cout<<"TpcTrackCdcHitResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
        return true;
    }
    
    fAlMan=TpcAlignmentManager::getInstance();
    if(fUseCdcHits){
        
        if(fBranchMap.count(fCdcHitBranchName)){
            std::cout<<"TpcCluster branch found:"<<fCdcHitBranchName<<std::endl;
            std::string type(fBranchMap[fCdcHitBranchName]->GetClass()->GetName());
            if(type.find(cdcHitType)<type.size()){
                fCdcHitArray=fBranchMap[fCdcHitBranchName];
                foundCdcHitArray=true;
            }
        }
        if(!foundCdcHitArray){
            std::cout<<"TpcTrackCdcHitResCalc::init(): "<<std::endl
                    <<"   ERROR: TpcClusters enabled but not found!"<<std::endl;
            return true;
        }
    }
    std::ofstream txtoutfile("TpcTrackCdcHitResCalc_settings.txt");
    txtoutfile<<"Settings for TpcTrackCdcHitResCalc"<<std::endl;
    txtoutfile<<"Original applied alignment: "<<fOrigTransfName<<std::endl;
    txtoutfile<<"Use SPHits(0) or CdcHits(1): "<<fUseCdcHits<<std::endl;
    txtoutfile<<"dPhi cuts on the track tangentials used for matching: "<<fTrackdPhiCut<<std::endl;
    txtoutfile<<"dPhi cuts on the track positions used for matching: "<<fTrackdPhiCut/2<<std::endl;
    txtoutfile<<"Pt cut, minimum momentum for Tpc tracks: "<<fCdcPtCut<<std::endl;
    txtoutfile<<"Pt cut, minimum momentum for Cpc tracks: "<<fCdcPtCut<<std::endl;
        //All ok
    return false;
}


//main functionality
int TpcTrackCdcHitResCalc::calc() {
    
    //clear results
    for(unsigned int i=0; i<fResults.size(); i++){
        delete fResults[i];
    }
    
    fResults.clear();
    
    
    unsigned int nTpcTracks = fTpcGFTrackArray->GetEntriesFast();
    // GF Referencetrackfit loop
    for(unsigned int irtr=0; irtr<nTpcTracks; ++irtr) {
      GFTrack* iTpcTrack = (GFTrack*)((*fTpcGFTrackArray)[irtr]);
        GFAbsTrackRep* tpcRep;
        if(fNRep>=0){
	  tpcRep = iTpcTrack->getTrackRep(fNRep);
        }else{
	  tpcRep = iTpcTrack->getCardinalRep();
        }
	if(tpcRep->getStatusFlag()!=0){
            continue;
        }
        
	double tpcCharge=tpcRep->getCharge();
        
        TVector3 tpcMom=tpcRep->getMom();
        TVector3 tpcPos=tpcRep->getPos();
	fQA.fillBasicHists("tpc",tpcMom,tpcPos,tpcCharge);
	fQA.fillBasicHists("tpc",tpcMom,tpcPos,0);
    
        if(tpcMom.Perp()<fTpcPtCut){
            continue;
        }
        //-----------------------------------------------------------------------------------
        //Prepare last state rep
        GFDetPlane tpcLastPl = tpcRep->getLastPlane();
        RKTrackRep* lastTpcRep=(RKTrackRep*)tpcRep->clone();
        lastTpcRep->setData(lastTpcRep->getLastState(),tpcLastPl,
                            &lastTpcRep->getLastCov(),lastTpcRep->getAuxInfo(lastTpcRep->getReferencePlane()));
        
        
        tpcMom=((GFAbsTrackRep*)lastTpcRep)->getMom();
        tpcPos=((GFAbsTrackRep*)lastTpcRep)->getPos();
        TMatrixD tpcCov=((GFAbsTrackRep*)lastTpcRep)->getCov();
        if(tpcMom.Perp()<fTpcPtCut){
	  delete lastTpcRep;
	  continue;
        }
	
        //-----------------------------------------------------------------------------------
        try{
	  lastTpcRep->extrapolateToCylinder(15,tpcPos,tpcMom); 
        }catch(GFException& ex){
            std::cout<<"TpcRefGFTrackResCalc::calc(): Error during TPC extrap to cylinder: \n"
                    <<"     "<<ex.what()<<std::endl;
	    delete lastTpcRep;
            continue;
        }
        if(tpcMom.Perp()<fTpcPtCut){
	  delete lastTpcRep;
	  continue;
        }
	fQA.fillBasicHists("tpc_cutPt",tpcMom,tpcPos,tpcCharge);
	fQA.fillBasicHists("tpc_cutPt",tpcMom,tpcPos,0);
	
        //create residualCollection for this track
        fResults.push_back(new TpcRefResidualCollection(fCdcGFHitBranchName.Data(),irtr));
        
        //loop over CDC tracks
        unsigned int nCdcTracks = fCdcGFTrackArray->GetEntriesFast();
        for(unsigned int itr=0;itr<nCdcTracks;++itr){
            GFTrack* iCdcTrack=(GFTrack*)((*fCdcGFTrackArray)[itr]);
            GFAbsTrackRep* cdcRep= iCdcTrack->getCardinalRep();
            double cdcCharge=cdcRep->getCharge();

            TVector3 cdcMom, cdcPos;
            TMatrixDSym cdcCov_;
            cdcRep->getPosMomCov(cdcPos,cdcMom,cdcCov_);
	     fQA.fillBasicHists("cdc",cdcMom,cdcPos,cdcCharge);
	     fQA.fillBasicHists("cdc",cdcMom,cdcPos,0);
	    //Only consider cases where tpc and cdc have the same charge
	    if(cdcCharge*tpcCharge<0 and cutOnSameCharge){
	      continue;
	    }
            if(cdcMom.Perp()<fCdcPtCut or cdcRep->getStatusFlag()!=0){
                continue;
            }
            
            try{
	      cdcRep->extrapolateToCylinder(15,cdcPos,cdcMom);
            }catch(GFException& ex){
                std::cout<<"TpcRefGFTrackResCalc::calc(): Error during extrap to cylinder mom: \n"
                        <<"     "<<ex.what()<<std::endl;
                continue;
            }
	    fQA.fillBasicHists("cdc_cutPt",cdcMom,cdcPos,cdcCharge);
	    fQA.fillBasicHists("cdc_cutPt",cdcMom,cdcPos,0);
	    //Phi matching
	    fQA.fillMatchingHists(tpcMom,tpcPos,cdcMom,cdcPos,false,0);
	    fQA.fillMatchingHists(tpcMom,tpcPos,cdcMom,cdcPos,false,cdcCharge);
            if((fabs(CdcTpcMatchingQA::phiDifference(cdcPos.Phi(),tpcPos.Phi()))>(this->fTrackdPhiCut/2)) or
	       (fabs(CdcTpcMatchingQA::phiDifference(cdcMom.Phi(),tpcMom.Phi()))>this->fTrackdPhiCut)){
                continue;
            }
	    if((fabs(cdcMom.Perp()-tpcMom.Perp())>ptResCut) and cutOnPtRes){
	      continue;
	    }

	    fQA.fillMatchingHists(tpcMom,tpcPos,cdcMom,cdcPos,true,0);
	    fQA.fillMatchingHists(tpcMom,tpcPos,cdcMom,cdcPos,true,cdcCharge);
            
            //loop over hits from matched track
            GFTrackCand cand=iCdcTrack->getCand();
            unsigned int nHits = cand.getNHits();
            for(int iHit=0;iHit<nHits;++iHit){
                int hitId, detId;
                cand.getHit(iHit,detId,hitId);
                CdcSpacepoint* recoHit = (CdcSpacepoint*) fCdcGFHitArray->At(hitId);
                if(recoHit==NULL){
                    std::cout<<"nooooooooooooooooooooooooooooooooo "<<iHit<<" "<<hitId<<" "<<detId<<std::endl;
                    std::cout<<fCdcGFHitArray->GetEntries()<<std::endl;
                }
                TVector3 hPos;
                TMatrixT<double> cov(3,3) ;
                
                if(fUseCdcHits){
                    int iCl=recoHit->getHitID();
                    CdcHit* cl= (CdcHit*) fCdcHitArray->At(iCl);
                    if(cl==0){
                        std::cout<<"no cluster at id: "<<iCl<<" nCl: "<<fCdcHitArray->GetEntriesFast()<<std::endl;
                        std::cout<<"hitid: "<<hitId<<" ihit: "<<iHit<<" nhit: "<<fCdcGFHitArray->GetEntriesFast()<<std::endl;
                        continue;
                    }
                    hPos=cl->GetHitPos();
                    TVector3 sigX = cl->GetXposErr();
                    TVector3 sigY = cl->GetYposErr();
                    TVector3 sigZ = cl->GetZposErr();
                    cov[0][0] = sigX.X()*sigX.X();
                    cov[1][1] = sigY.Y()*sigY.Y();
                    cov[2][2] = sigZ.Z()*sigZ.Z();
                    
                }else{
                    TVectorT<double> rc = ((GFAbsRecoHit*)recoHit)->getRawHitCoord();
                    cov =((GFAbsRecoHit*)recoHit)->getRawHitCov();
                    hPos = TVector3(rc[0], rc[1], rc[2]);
		    if(hPos.Perp()>30){
		      continue;
		    }
                    double tracklength=0;
                    bool ok=true;
                    TVector3 poca, norm;
                    TVector3 pos1,mom1;
                    TMatrixDSym cov6;
                    try {
		      tpcRep->extrapolateToCylinder(hPos.Perp(),poca,norm);
                        //TVectorT<double> dummy;
                        GFDetPlane plane(poca,norm);
                        tracklength=tpcRep->extrapolate(plane);
                        tpcRep->getPosMomCov(pos1,mom1,cov6);
                        
                    }
                    catch(GFException& ex) {
                        std::cout<<"TpcRefGFTrackResCalc::calc(): Error during extrapolation: \n"
                                <<"     "<<ex.what()<<std::endl;
                        ok=false;
                    }
                    if(ok) {  //add residual
                        TpcResidual* theRes = new TpcResidual();
                        theRes->setResXYZ(poca-hPos);
                        theRes->setRefPos(poca);
                        theRes->setHitPos(hPos);
                        theRes->setTrackDir(mom1.Unit());
                        //TODO, use correct assignment (cluster/SPHit)
                        if(fUseCdcHits){
                            theRes->setHitIndex(recoHit->getHitID());
                        }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
        }//end GFTrack loop
	delete lastTpcRep;
    }//end refGFTrack loop
}

void TpcTrackCdcHitResCalc::writeHists(){
  fQA.writeHists();

}

// void TpcTrackCdcHitResCalc::fillRefHistos(TVector3 refMom,TVector3 refPos){
//     histMap["histRefMom"]->Fill(refMom.Mag());
//     histMap["histRefPT"]->Fill(refMom.Perp());
// }
// void TpcTrackCdcHitResCalc::fillTrackHistos(TVector3 mom,TVector3 pos){
//     histMap["histCdcMom"]->Fill(mom.Mag());
//     histMap["histCdcPT"]->Fill(mom.Perp());
// }
// void TpcTrackCdcHitResCalc::fillMatchingHistos(TVector3 refMom, TVector3 refPos, TVector3 mom, TVector3 pos){
//     histMap["histMomRes"]->Fill(mom.Mag()-refMom.Mag());
//     histMap["histMomResVMom"]->Fill(mom.Mag(),mom.Mag()-refMom.Mag());
//     histMap["histPTRes"]->Fill(mom.Perp()-refMom.Perp());
//     histMap["histPTResVPT"]->Fill(mom.Perp(),mom.Perp()-refMom.Perp());
// }

ClassImp(TpcTrackCdcHitResCalc)