//-----------------------------------------------------------
// 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 "TpcRefGFTrkResCalc_Alignment.h"
#include "TClonesArray.h"
#include "TpcCluster.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "TpcSPHit.h"
#include "TpcAlignmentManager.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 <string>
#include <iostream>
#include <fstream>




double TpcRefGFTrkResCalc_Alignment::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;
}

TpcRefGFTrkResCalc_Alignment::TpcRefGFTrkResCalc_Alignment()
    : fGFTrackBranchName("TpcTrackPostFit"),
      fTpcSPHitBranchName("TpcSPHit"),
      fRefGFTrackBranchName("CdcTrackPostFit"),
      fTpcClusterBranchName("TpcCluster"),
      fNRep(-1),
      fDetID(-1),
      fAlignedMatching(true),
      fRequireGoodFit(true),
      fUseClusters(false),
      fRefTrackMomentumCut(0.5),
      fRefTrackPosCut(40.),
      fTpcTrackMomCut(0.5),
      fTrackdPhiCut(10),
      fOrigTransfName("tpc"),
      fAlMan(NULL),
      fQAFileName("TpcRefGFTrkResCalc_AlignmentQA.root"),
      sectorTrackSelection(false),minAngle(0),maxAngle(0)
{
    
    fHistoFile = new TFile(fQAFileName,"recreate");
    rRefHist=new TH1D("Ref_RHist","Ref_RHist;r (cm)",1000,0,150);
    hists.push_back(rRefHist);
    xyRefHist=new TH2D("Ref_XYHist","Ref_XYHist;x (cm);y (cm)",1000,-150,150,1000,-150,150);
    hists.push_back(xyRefHist);
    momRefHist=new TH1D("Ref_MomHist","Ref_MomHist;p (GeV/c)",1000,0,10);
    hists.push_back(momRefHist);
    phiRefHist=new TH1D("Ref_PhiHist","Ref_PhiHist; #phi (deg)",1000,-360,360);
    hists.push_back(phiRefHist);
    dPhiRefTrkPosHist=new TH1D("Ref_dPhiMomPos","Ref_dPhiMomPos; #phi (deg)",1000,-360,360);
    hists.push_back(dPhiRefTrkPosHist);

    rRefCutHist=new TH1D("RefMomCut_RHist","RefMomCut_RHist;r (cm)",1000,0,150);
    hists.push_back(rRefCutHist);
    xyRefCutHist=new TH2D("RefMomCut_XYHist","RefMomCut_XYHist;x (cm);y (cm",1000,-150,150,1000,-150,150);
    hists.push_back(xyRefCutHist);
    momRefCutHist=new TH1D("RefMomCut_MomHist","RefMomCut_MomHist; p (GeV/c)",1000,0,10);
    hists.push_back(momRefCutHist);
    phiRefCutHist=new TH1D("RefMomCut_PhiHist","RefMomCut_PhiHist;#phi (deg)",1000,360,360);
    hists.push_back(phiRefCutHist);
    dPhiRefCutTrkPosHist=new TH1D("RefMomCut_dPhiMomPos","RefMomCut_dPhiMomPos; d#phi (deg)",1000,-360,360);
    hists.push_back(dPhiRefCutTrkPosHist);
    dPhiRefCutTrkPosHist2=new TH1D("RefMomCut_dPhiMomPos_rot","RefMomCut_dPhiMomPos_rot; d#phi (deg)",1000,-360,360);
    hists.push_back(dPhiRefCutTrkPosHist2);


    rHist=new TH1D("TpcTrack_RHist","rHist;r (cm)",1000,0,150);
    hists.push_back(rHist);
    momHist=new TH1D("TpcTrack_MomHist","TpcTrack_MomHist; p (GeV/c)",1000,0,10);
    hists.push_back(momHist);
    phiHist=new TH1D("TpcTrack_PhiHist","TpcTrack_PhiHist; #phi (deg)",1000,-360,360);
    hists.push_back(phiHist);
    dPhiTpcTrkPosHist=new TH1D("TpcTrack_dPhiMomPos","TpcTrack_dPhiMomPos; d#phi (deg)",1000,-360,360);
    hists.push_back(dPhiTpcTrkPosHist);

    dPhiHist=new TH1D("dPhiMom_Tpc-Ref","dPhiMom_Tpc-Ref",1000,-180,180);
    hists.push_back(dPhiHist);
    dPhiVsPhiHist=new TH2D("dPhiMom_Tpc-RefvsTpcPhi","dPhiMom_Tpc-RefvsTpcPhi",500,-180,180,500,-10,10);
    hists.push_back(dPhiVsPhiHist);
    dPhiPosHist=new TH1D("dPhiPos_Tpc-Ref","dPhiPos_Tpc-Ref",2000,-180,180);
    hists.push_back(dPhiPosHist);
    dPhiVsPhiPosHist=new TH2D("dPhiPos_Tpc-RefvsTpcPhi","dPhiPos_Tpc-RefvsTpcPhi",500,-180,180,500,-10,10);
    hists.push_back(dPhiVsPhiPosHist);

    //************************************************************************************************
    // Positive charge
    //************************************************************************************************
    dPhiPlusHist=new TH1D("dPhiPlusMom_Tpc-Ref","dPhiPlusMom_Tpc-Ref",1000,-180,180);
    hists.push_back(dPhiPlusHist);
    dPhiPlusVsPhiHist=new TH2D("dPhiPlusMom_Tpc-RefvsTpcPhi","dPhiPlusMom_Tpc-RefvsTpcPhi",500,-180,180,500,-10,10);
    hists.push_back(dPhiPlusVsPhiHist);
    dPhiPlusPosHist=new TH1D("dPhiPlusPos_Tpc-Ref","dPhiPlusPos_Tpc-Ref",2000,-180,180);
    hists.push_back(dPhiPlusPosHist);
    dPhiPlusVsPhiPosHist=new TH2D("dPhiPlusPos_Tpc-RefvsTpcPhi","dPhiPlusPos_Tpc-RefvsTpcPhi",500,-180,180,500,-10,10);
    hists.push_back(dPhiPlusVsPhiPosHist);
    
    
    //signedResiduals
    signedXYResidual= new TH1D("signedResidual", "signedResidual",500, -20,20);
    hists.push_back(signedXYResidual);
    signedXYResidualPlus= new TH1D("signedResidualPlus", "signedResidualPlus",500, -20,20);
    hists.push_back(signedXYResidualPlus);
    signedXYResidualMinus= new TH1D("signedResidualMinus", "signedResidualMinus",500, -20,20);
    hists.push_back(signedXYResidualMinus);
    signedXYResidualVsPhi= new TH2D("signedResidualVsPhi", "signedResidualVsPhi",360,-180,180,500, -20,20);
    hists.push_back(signedXYResidualVsPhi);
    signedXYResidualVsPhiPlus= new TH2D("signedResidualVsPhiPlus", "signedResidualVsPhiPlus",360,-180,180,500, -20,20);
    hists.push_back(signedXYResidualVsPhiPlus);
    signedXYResidualVsPhiMinus= new TH2D("signedResidualVsPhiMinus", "signedResidualVsPhiMinus",360,-180,180,500, -20,20);
    hists.push_back(signedXYResidualVsPhiMinus);
    
    zSlices=10;
    zStart=-30;
    zEnd=40;
    double zRange=zEnd-zStart;
    double zBin=zRange/((double)zSlices);
    double zBinStart=zStart;
    double zBinEnd=zBinStart;
    for(int i =0;i<zSlices;++i){
        std::cout<<"i: "<<i<<" ";
        zBinEnd+=zBin;
        TString name=TString::Format("dPhiPlusPos_vsTpcPhi_ZSlice_%i",i);
        TString name2=TString::Format("dPhiPlusMom_vsTpcPhi_ZSlice_%i",i);
        TString name3=TString::Format("dPhiMinusPos_vsTpcPhi_ZSlice_%i",i);
        TString name4=TString::Format("dPhiMinusMom_vsTpcPhi_ZSlice_%i",i);
        TString prettyName=TString::Format("dPhiPlusPos_vsTpcPhi_ZSlice %f - %f",zBinStart,zBinEnd);
        TString prettyName2=TString::Format("dPhiPlusMom_vsTpcPhi_ZSlice  %f - %f",zBinStart,zBinEnd);
        TString prettyName3=TString::Format("dPhiMinusPos_vsTpcPhi_ZSlice  %f - %f",zBinStart,zBinEnd);
        TString prettyName4=TString::Format("dPhiMinusMom_vsTpcPhi_ZSlice  %f - %f",zBinStart,zBinEnd);
        zBinStart=zBinEnd;
        TH2D* hist=new TH2D(name,prettyName,500,-180,180,500,-10,10);
        TH2D* hist2=new TH2D(name2,prettyName2,500,-180,180,500,-10,10);
        TH2D* hist3=new TH2D(name3,prettyName3,500,-180,180,500,-10,10);
        TH2D* hist4=new TH2D(name4,prettyName4,500,-180,180,500,-10,10);
        histMap[name.Data()]=hist;
        histMap[name2.Data()]=hist2;
        histMap[name3.Data()]=hist3;
        histMap[name4.Data()]=hist4;
        sliceNamesPlusPos.push_back(name.Data());
        sliceNamesPlusMom.push_back(name2.Data());
        sliceNamesMinusPos.push_back(name3.Data());
        sliceNamesMinusMom.push_back(name4.Data());
        std::cout<<name<<std::endl;
    }
    //************************************************************************************************
    // Neg charge
    //************************************************************************************************
    dPhiMinusHist=new TH1D("dPhiMinusMom_Tpc-Ref","dPhiMinusMom_Tpc-Ref",1000,-180,180);
    hists.push_back(dPhiMinusHist);
    dPhiMinusVsPhiHist=new TH2D("dPhiMinusMom_Tpc-RefvsTpcPhi","dPhiMinusMom_Tpc-RefvsTpcPhi",500,-180,180,500,-10,10);
    hists.push_back(dPhiMinusVsPhiHist);
    dPhiMinusPosHist=new TH1D("dPhiMinusPos_Tpc-Ref","dPhiMinusPos_Tpc-Ref",2000,-180,180);
    hists.push_back(dPhiMinusPosHist);
    dPhiMinusVsPhiPosHist=new TH2D("dPhiMinusPos_Tpc-RefvsTpcPhi","dPhiMinusPos_Tpc-RefvsTpcPhi",500,-180,180,500,-10,10);
    hists.push_back(dPhiMinusVsPhiPosHist);

    //**************************************************************************************************
    dTheta=new TH1D("dTheta","dTheta",1000,-180,180);
    hists.push_back(dTheta);
    dThetaVTheta=new TH2D("dThetaVTheta","dThetaVTheta",500,-180,180,500,-180,180);
    hists.push_back(dThetaVTheta);
    dThetaCut=new TH1D("dThetaCut","dThetaCut",1000,-180,180);
    hists.push_back(dThetaCut);
    dThetaVThetaCut=new TH2D("dThetaVThetaCut","dThetaVThetaCut",500,-180,180,500,-180,180);
    hists.push_back(dThetaVThetaCut);

    dThetaPos=new TH1D("dThetaPos","dThetaPos",1000,-180,180);
    hists.push_back(dThetaPos);
    dThetaPosVThetaPos=new TH2D("dThetaPosVThetaPos","dThetaPosVThetaPos",500,-180,180,500,-180,180);
    hists.push_back(dThetaPosVThetaPos);
    dThetaPosCut=new TH1D("dThetaPosCut","dThetaPosCut",1000,-180,180);
    hists.push_back(dThetaPosCut);
    dThetaPosVThetaPosCut=new TH2D("dThetaPosVThetaPosCut","dThetaPosVThetaPosCut",500,-180,180,500,-180,180);
    hists.push_back(dThetaPosVThetaPosCut);
    fNExpectedBranches = 3;

}

TpcRefGFTrkResCalc_Alignment::~TpcRefGFTrkResCalc_Alignment() {

}

bool TpcRefGFTrkResCalc_Alignment::init() {
    if(fBranchMap.size() != fNExpectedBranches) {
        std::cout<<"TpcRefGFTrkResCalc_Alignment::init(): "<<std::endl
                <<"   ERROR: Wrong number of input branches ("
               <<fBranchMap.size()<<")!"<<std::endl;
        return true;
    }
    //check for proper data types:
    bool foundGFTrack=false;
    bool foundRefGFTrack=false;
    bool foundSPHitArray=false;
    bool foundClusterArray=false;
    std::string GFTrackRefType("GFTrack");
    std::string RefGFTrackRefType("GFTrack");
    std::string tpcSPHitType("TpcSPHit");
    std::string tpcClusterType("TpcCluster");

    if(fBranchMap.count(fRefGFTrackBranchName)){
        std::cout<<"reftrack branch found:"<<fRefGFTrackBranchName<<std::endl;
        std::string type(fBranchMap[fRefGFTrackBranchName]->GetClass()->GetName());
        if(type.find(RefGFTrackRefType)<type.size()){
            fRefGFTrackArray=fBranchMap[fRefGFTrackBranchName];
            foundRefGFTrack=true;
        }
    }
    if(fBranchMap.count(fGFTrackBranchName)){
        std::cout<<"track branch found:"<<fRefGFTrackBranchName<<std::endl;
        std::string type(fBranchMap[fGFTrackBranchName]->GetClass()->GetName());
        if(type.find(GFTrackRefType)<type.size()){
            fGFTrackArray=fBranchMap[fGFTrackBranchName];
            foundGFTrack=true;
        }
    }
    if(fBranchMap.count(fTpcSPHitBranchName)){
        std::cout<<"SPHit branch found:"<<fTpcSPHitBranchName<<std::endl;
        std::string type(fBranchMap[fTpcSPHitBranchName]->GetClass()->GetName());
        if(type.find(tpcSPHitType)<type.size()){
            fTpcSPHitArray=fBranchMap[fTpcSPHitBranchName];
            foundSPHitArray=true;
        }
    }
    if(!(foundGFTrack&&foundSPHitArray&&foundRefGFTrack)) {
        std::cout<<"TpcRefGFTrkResCalc_Alignment::init(): "<<std::endl
                <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
        return true;
    }

    fAlMan=TpcAlignmentManager::getInstance();
    if(fUseClusters){

        if(fBranchMap.count(fTpcClusterBranchName)){
            std::cout<<"TpcCluster branch found:"<<fTpcClusterBranchName<<std::endl;
            std::string type(fBranchMap[fTpcClusterBranchName]->GetClass()->GetName());
            if(type.find(tpcClusterType)<type.size()){
                fTpcClusterArray=fBranchMap[fTpcClusterBranchName];
                foundClusterArray=true;
            }
        }
        if(!foundClusterArray){
            std::cout<<"TpcRefGFTrkResCalc_Alignment::init(): "<<std::endl
                    <<"   ERROR: TpcClusters enabled but not found!"<<std::endl;
            return true;
        }
    }
    std::ofstream txtoutfile("TpcRefGFTrkResCalc_Alignment_settings.txt");
    txtoutfile<<"Settings for TpcRefGFTrkResCalc_Alignment"<<std::endl;
    txtoutfile<<"Require good fits: "<<fRequireGoodFit<<std::endl;
    txtoutfile<<"Original applied alignment: "<<fOrigTransfName<<std::endl;
    txtoutfile<<"Use SPHits(0) or Cluster(1): "<<fUseClusters<<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<<"Momentum cut, minimum momentum for ref tracks: "<<fRefTrackMomentumCut<<std::endl;
    txtoutfile<<"Momentum cut, minimum momentum for Tpc tracks: "<<fTpcTrackMomCut<<std::endl;
    txtoutfile<<"Position cut, max radial position of ref track start pos: "<<fRefTrackPosCut<<std::endl;
    //All ok
    return false;
}


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

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

    fResults.clear();


    unsigned int nRefTracks = fRefGFTrackArray->GetEntriesFast();
    // GF Referencetrackfit op
    //std::cout<<"nRefTracks "<<nRefTracks<<std::endl;
    for(unsigned int irtr=0; irtr<nRefTracks; ++irtr) {
        //std::cout<<"reftrack #"<<irtr<<std::endl;
        GFTrack* iRefTrack = (GFTrack*)((*fRefGFTrackArray)[irtr]);
        GFAbsTrackRep* refRep;
        if(fNRep>=0)
            refRep = iRefTrack->getTrackRep(fNRep);
        else
            refRep = iRefTrack->getCardinalRep();
        int val = refRep->getStatusFlag();

        if(val!=0 && fRequireGoodFit)
            continue;
        double charge=refRep->getCharge();
        TVector3 refMom=refRep->getMom();
        TVector3 refPos=refRep->getPos();
        
        //*********************************************************************
        dPhiRefTrkPosHist->Fill((refMom.Phi()-refPos.Phi())*180/TMath::Pi());
        rRefHist->Fill(refPos.Perp());
        xyRefHist->Fill(refPos.X(),refPos.Y());
        momRefHist->Fill(refMom.Mag());
        phiRefHist->Fill(refMom.Phi()*180/TMath::Pi());
        //*********************************************************************

        if(refMom.Perp()<fRefTrackMomentumCut or refPos.Perp()>fRefTrackPosCut){
            continue;
        }
        try{
            refRep->extrapolateToCylinder(15,refPos,refMom); 
        }catch(GFException& ex){
            std::cout<<"TpcRefGFTrackResCalc::calc(): Error during CDC extrap to cylinder: \n"
                    <<"     "<<ex.what()<<std::endl;
            continue;
        }

        //*********************************************************************
        double dPosMomPhi=phiDifference(refMom.Phi(),refPos.Phi());
        dPhiRefCutTrkPosHist->Fill(dPosMomPhi);
        rRefCutHist->Fill(refPos.Perp());
        xyRefCutHist->Fill(refPos.X(),refPos.Y());
        momRefCutHist->Fill(refMom.Mag());
        phiRefCutHist->Fill(refMom.Phi()*180/TMath::Pi());
        //*********************************************************************

        double refPhi=refMom.Phi();
        // if(fabs(dPosMomPhi)>150){
        //     refMom.RotateZ(TMath::Pi());
        //     refPhi=refMom.Phi();
        //     std::cerr<<"Error:: Rotating track!"<<std::endl;
        // }

        //*********************************************************************
        double dPosMomPhi2=phiDifference(refMom.Phi(),refPos.Phi());
        dPhiRefCutTrkPosHist2->Fill(dPosMomPhi2);
        //*********************************************************************
        
        //create residualCollection for this track
        fResults.push_back(new TpcRefResidualCollection(fTpcSPHitBranchName.Data(),irtr));
        unsigned int nTracks = fGFTrackArray->GetEntriesFast();
        for(unsigned int itr=0;itr<nTracks;++itr){
            GFTrack* iTrack=(GFTrack*)((*fGFTrackArray)[itr]);
            GFAbsTrackRep* rep= iTrack->getCardinalRep();
            if(rep->getStatusFlag()!=0){
                continue;
            }
            TVector3 mom, pos;
            TMatrixDSym cov_;
            const TMatrixDSym lastCov=rep->getLastCov();
            rep->getPosMomCov(pos,mom,cov_);
            if(mom.Perp()<fTpcTrackMomCut or rep->getStatusFlag()!=0){
                continue;
            }
            RKTrackRep* lastRep = (RKTrackRep*) rep->clone();
            GFDetPlane tpcLastPl = rep->getLastPlane();
            TVectorD lastState = lastRep->getLastState();
            try{
                lastRep->setData(lastState,tpcLastPl,&lastCov,lastRep->getAuxInfo(lastRep->getReferencePlane()));
            }catch(GFException& ex){
                std::cout<<"TpcRefGFTrackResCalc::calc():during getting of AuxInfo: \n"
                        <<"     "<<ex.what()<<std::endl;
                continue;
            }
            TVector3 lastTpcMom=((GFAbsTrackRep*)lastRep)->getMom();
            delete lastRep;
            if(lastTpcMom.Mag()<0.1){
                std::cout<<"################################################################################"<<std::endl;
                std::cout<<"Tpc last state broken continue cdc#: "<<irtr<<", tpc# "<<itr<<std::endl;
                std::cout<<"First Mom: "<<mom.Mag()<<", lastMom: "<<lastTpcMom.Mag()<<std::endl;
                std::cout<<"################################################################################"<<std::endl;
                continue;
            }
            //Rotate and shift TpcTrack using suggested geometry
            if(fOrigTransfName!="tpc"){
                mom=fAlMan->masterToLocalVect(fOrigTransfName,mom);
                mom=fAlMan->localToMasterVect("tpc",mom);
                pos=fAlMan->masterToLocal(fOrigTransfName,pos);
                pos=fAlMan->localToMaster("tpc",pos);
                rep->setPosMomCov(pos,mom,cov_);
            }
            try{
                rep->extrapolateToCylinder(15,pos,mom);
            }catch(GFException& ex){
                std::cout<<"TpcRefGFTrackResCalc::calc(): Error during extrap to cylinder mom: \n"
                        <<"     "<<ex.what()<<std::endl;
                continue;
            }
            //*********************************************************************
            TVector3 residual= refPos-pos;
            int resDir=phiDifference(refPos.Phi(),pos.Phi());
            resDir=resDir/fabs(resDir);
            signedXYResidual->Fill(residual.Perp()*resDir);
            signedXYResidualVsPhi->Fill(pos.Phi()*180/TMath::Pi(),residual.Perp()*resDir);
            if(charge>0){
                signedXYResidualPlus->Fill(residual.Perp()*resDir);
                signedXYResidualVsPhiPlus->Fill(pos.Phi()*180/TMath::Pi(),residual.Perp()*resDir);
            }else{
                signedXYResidualMinus->Fill(residual.Perp()*resDir);
                signedXYResidualVsPhiMinus->Fill(pos.Phi()*180/TMath::Pi(),residual.Perp()*resDir);
            }
            rHist->Fill(pos.Perp());
            momHist->Fill(mom.Mag());
            phiHist->Fill(mom.Phi()*180/TMath::Pi());

            dPhiTpcTrkPosHist->Fill(phiDifference(mom.Phi(),pos.Phi()));

            dPhiVsPhiHist->Fill(mom.Phi()*180/TMath::Pi(),phiDifference(mom.Phi(),refPhi));
            dPhiHist->Fill(phiDifference(mom.Phi(),refMom.Phi()));
            dThetaVTheta->Fill(mom.Theta()*180/TMath::Pi(),phiDifference(mom.Theta(),refMom.Theta()));
            dTheta->Fill((mom.Theta()-refMom.Theta())*180/TMath::Pi());

            dPhiVsPhiPosHist->Fill(pos.Phi()*180/TMath::Pi(),phiDifference(pos.Phi(),refPos.Phi()));
            dPhiPosHist->Fill(phiDifference(pos.Phi(),refPos.Phi()));
            dThetaPosVThetaPos->Fill(pos.Theta()*180/TMath::Pi(),phiDifference(pos.Theta(),refPos.Theta()));
            dThetaPos->Fill(phiDifference(pos.Theta(),refPos.Theta()));

            //Negative tracks
            if(refRep->getCharge()<0){
                dPhiMinusVsPhiHist->Fill(mom.Phi()*180/TMath::Pi(),phiDifference(mom.Phi(),refPhi));
                dPhiMinusHist->Fill(phiDifference(mom.Phi(),refMom.Phi()));
                dPhiMinusVsPhiPosHist->Fill(pos.Phi()*180/TMath::Pi(),phiDifference(pos.Phi(),refPos.Phi()));
                dPhiMinusPosHist->Fill(phiDifference(pos.Phi(),refPos.Phi()));
                double zBinEnd=zStart;
                double zBin=(zEnd-zStart)/((double)zSlices);
                double zPos=pos.Z();
                for(int i=0;i<zSlices;++i){
                    zBinEnd+=zBin;
                    //std::cout<<zBinEnd<<std::endl;
                    if(zPos<zBinEnd){
                        histMap[sliceNamesMinusPos[i]]->Fill(pos.Phi()*180/TMath::Pi(),phiDifference(pos.Phi(),refPos.Phi()));
                        histMap[sliceNamesMinusMom[i]]->Fill(mom.Phi()*180/TMath::Pi(),phiDifference(mom.Phi(),refPhi));
                        break;
                    }
                }
            }
            //Plus tracks
            if(refRep->getCharge()>0){
                dPhiPlusVsPhiHist->Fill(mom.Phi()*180/TMath::Pi(),phiDifference(mom.Phi(),refPhi));
                dPhiPlusHist->Fill(phiDifference(mom.Phi(),refMom.Phi()));
                dPhiPlusVsPhiPosHist->Fill(pos.Phi()*180/TMath::Pi(),phiDifference(pos.Phi(),refPos.Phi()));
                dPhiPlusPosHist->Fill(phiDifference(pos.Phi(),refPos.Phi()));
                double zBinEnd=zStart;
                double zBin=(zEnd-zStart)/((double)zSlices);
                double zPos=pos.Z();
                for(int i=0;i<zSlices;++i){
                    zBinEnd+=zBin;
                    //std::cout<<zBinEnd<<std::endl;
                    if(zPos<zBinEnd){
                        histMap[sliceNamesPlusPos[i]]->Fill(pos.Phi()*180/TMath::Pi(),phiDifference(pos.Phi(),refPos.Phi()));
                        histMap[sliceNamesPlusMom[i]]->Fill(mom.Phi()*180/TMath::Pi(),phiDifference(mom.Phi(),refPhi));
                        break;
                    }
                }
            }

            //*********************************************************************

            if((fabs(pos.Phi()-refPos.Phi())*180/TMath::Pi()>(this->fTrackdPhiCut/2)) or
                    (fabs(mom.Phi()-refMom.Phi())*180/TMath::Pi()>this->fTrackdPhiCut)){
                continue;
            }
            //*********************************************************************
            dThetaVThetaCut->Fill(mom.Theta()*180/TMath::Pi(),(mom.Theta()-refMom.Theta())*180/TMath::Pi());
            dThetaCut->Fill((mom.Theta()-refMom.Theta())*180/TMath::Pi());
            dThetaPosVThetaPosCut->Fill(pos.Theta()*180/TMath::Pi(),(pos.Theta()-refPos.Theta())*180/TMath::Pi());
            dThetaPosCut->Fill((pos.Theta()-refPos.Theta())*180/TMath::Pi());
            //*********************************************************************

            //debug
            //continue;

            //loop over hits from matched track
            GFTrackCand cand=iTrack->getCand();
            unsigned int nHits = cand.getNHits();
            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<<std::endl;
                    std::cout<<fTpcSPHitArray->GetEntries()<<std::endl;
                }
                
                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()<<std::endl;
                        std::cout<<"hitid: "<<hitId<<" ihit: "<<iHit<<" nhit: "<<fTpcSPHitArray->GetEntriesFast()<<std::endl;
                        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()<7){
                            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 {
                    refRep->extrapolateToCylinder(hPos.Perp(),poca,norm);
                    TVectorT<double> dummy;
                    GFDetPlane plane(poca,norm);
                    tracklength=refRep->extrapolate(plane);
                    refRep->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(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(charge);
                    
                    fResults.back()->addResidual(theRes);
                }
            }//end Hit loop
        }//end GFTrack loop
    }//end refGFTrack loop
}

void TpcRefGFTrkResCalc_Alignment::writeHists(){
    
    fHistoFile->cd();
    for(unsigned int i =0;i<hists.size();++i){
        hists[i]->Write();
        if(hists[i]->InheritsFrom("TH2D")){
                TProfile* temp = ((TH2D*)hists[i])->ProfileX();
                temp->Write();
        }
    }
    for(std::map<std::string,TH1*>::iterator it =histMap.begin();it!=histMap.end();++it){
      it->second->Write();
      if(it->second->InheritsFrom("TH2D")){
          TProfile* temp = ((TH2D*)it->second)->ProfileX();
          temp->Write();
      }
    }
    fHistoFile->Close();
}

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