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

#include "GFTrack.h"
#include "GFException.h"
#include "GFAbsTrackRep.h"
#include "GFRecoHitFactory.h"
#include "GFRecoHitProducer.h"
#include "RKTrackRep.h"
#include "GFTrackCand.h"
#include <FairRootManager.h>

#include <TClass.h>


#include <string>
#include <iostream>


ClassImp(TpcRefGFTrkTpcClusterResCalc)

TpcRefGFTrkTpcClusterResCalc::TpcRefGFTrkTpcClusterResCalc()
    : fGFTrackBranchName("GFTrack"),
      fTpcClusterBranchName("TpcCluster"),
      fTpcSPHitBranchName("TpcCluster"),
      fNRep(-1),
      fDetID(-1),
      fRequireGoodFit(true),
      fMomCutRefTrack(0.5),
      fQAFileName("TpcRefGFTrkTpcClusterResCalcQA.root"),
      fIdealTpcTrackTpcClusterMatching(false)

{
    fNExpectedBranches = 2;
    rHist=new TH1D("rHist","rHist",1000,0,100);
    rHistCut=new TH1D("rHistCut","rHistCut",1000,0,100);
    momHist=new TH1D("momHist","momHist",1000,0,5);
    momHistCut=new TH1D("momHistCut","momHistCut",1000,0,5);
}

TpcRefGFTrkTpcClusterResCalc::~TpcRefGFTrkTpcClusterResCalc() {
}

bool TpcRefGFTrkTpcClusterResCalc::init() {
    if(fIdealTpcTrackTpcClusterMatching){
        fNExpectedBranches=3;
    }
    if(fBranchMap.size() != fNExpectedBranches) {
        std::cout<<"TpcRefGFTrkTpcClusterResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong number of input branches ("
               <<fBranchMap.size()<<")!"<<std::endl;
        return true;
    }
    //check for proper data types:
    bool foundGFTrack=false;
    bool foundTpcCluster=false;
    bool foundTpcSPHit=false;
    std::string GFTrackRefType("GFTrack");
    std::string tpcClusterRefType("TpcCluster");
    std::string tpcSPHitType("TpcSPHit");
    //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(GFTrackRefType)<type.size()) {
            fGFTrackArray=it->second;
            fGFTrackBranchName = it->first;
            foundGFTrack=true;
            std::cout<<"TpcRefGFTrkTpcClusterResCalc::init(): "<<std::endl
                    <<"   INFO: got "<<type<<" as Object type for GFTracks with branch name "<<it->first
                   <<std::endl;
        }
        if(type.find(tpcClusterRefType)<type.size()) {
            fTpcClusterArray=it->second;
            foundTpcCluster=true;
            std::cout<<"TpcRefGFTrkTpcClusterResCalc::init(): "<<std::endl
                    <<"   INFO: got "<<type<<" as Object type for TPC Clusters with branch name "<<it->first
                   <<std::endl;
        }
        if(fIdealTpcTrackTpcClusterMatching){
            if(type.find(tpcSPHitType)<type.size()) {
                fTpcSPHitArray=it->second;
                foundTpcSPHit=true;
                std::cout<<"TpcRefGFTrkTpcClusterResCalc::init(): "<<std::endl
                        <<"   INFO: got "<<type<<" as Object type for TPC SP Hits with branch name "<<it->first
                       <<std::endl;
            }
        }
    }
    if((!(foundGFTrack&&foundTpcCluster))&&(fIdealTpcTrackTpcClusterMatching||foundTpcSPHit)) {
        std::cout<<"TpcRefGFTrkTpcClusterResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
        return true;
    }

    //all ok
  return false;  
}


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

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

    fResults.clear();


    unsigned int nTracks = fGFTrackArray->GetEntriesFast();
    // GF trackfit loop
    for(unsigned int itr=0; itr<nTracks; itr++) {
        GFTrack* itrack = (GFTrack*)((*fGFTrackArray)[itr]);
        GFAbsTrackRep* rep;
        if(fNRep>=0)
            rep = itrack->getTrackRep(fNRep);
        else
            rep = itrack->getCardinalRep();
        GFTrackCand cand = itrack->getCand();

        int val = rep->getStatusFlag();
    if(val!=0 && fRequireGoodFit) 
            continue;
        TVector3 refMom=rep->getMom();
        TVector3 refPos=rep->getPos();
        rHist->Fill(refPos.Perp());
        momHist->Fill(refMom.Mag());
        if(refMom.Mag()<fMomCutRefTrack)
            continue;
        rHistCut->Fill(refPos.Perp());
        momHistCut->Fill(refMom.Mag());
        //create residualCollection for this track
        fResults.push_back(new TpcRefResidualCollection(fTpcClusterBranchName.Data(),itr));

        //calculate residuals in XYZ --------------------------
        unsigned int ncl =0;
        if(fIdealTpcTrackTpcClusterMatching){
            ncl=cand.getNHits();
            if(abs(rep->getPDG())>10000){
                continue;
            }
        }else{
            ncl = fTpcClusterArray->GetEntriesFast();
        }
        TVector3 poca, norm;
        for(unsigned int icl=0; icl<(ncl-2); icl+=3) {

            TMatrixT<double> cov(3,3);
            TpcCluster* theCl;
            if(fIdealTpcTrackTpcClusterMatching){
                int detID;
                int hitID;
                cand.getHit(icl,detID,hitID);
                TpcSPHit* hit= (TpcSPHit*) fTpcSPHitArray->At(hitID);
                theCl=(TpcCluster*) fTpcClusterArray->At(hit->getClusterID());
                if(theCl==NULL){
                    std::cout<<"########################## "
                            <<"detID: "<<detID<<" hitID: "<<hitID<<" totCl: "<<fTpcClusterArray->GetEntriesFast()
                           <<" ###########################"<<std::endl;
                    continue;
                }
            }else{
                theCl= (TpcCluster*) fTpcClusterArray->At(icl);
            }
            TVector3 pos =theCl->pos();
            //pos.Print();
            TVector3 sig = theCl->sig();
            cov[0][0] = sig.X()*sig.X();
            cov[1][1] = sig.Y()*sig.Y();
            cov[2][2] = sig.Z()*sig.Z();
            pos = TpcAlignmentManager::getInstance()->localToMaster("tpc",pos);
            cov = TpcAlignmentManager::getInstance()->localToMaster("tpc",cov);

            TVector3 hpos =pos;
            double tracklength=0;
            bool ok=true;
            TVector3 pos1, mom1;
            TMatrixDSym cov6;
            try {
                //std::cout<<" "<<icl<<" r= "<<theCl->pos().Perp();
                rep->extrapolateToPoint(hpos, poca, norm);
                TVectorT<double> dummy;
                GFDetPlane plane(poca,norm);
                tracklength=rep->extrapolate(plane);
                rep->getPosMomCov(pos1,mom1,cov6);
            }
            catch(GFException& ex) {
                std::cout<<" TpcRefGFTrackResCalc::calc(): Error during extrapolation: to point #"
                           <<icl<<", r="<<theCl->pos().Perp()<<"\n"
                        <<"     "<<ex.what()<<"\n";
                ok=false;
            }
            if(ok) {  //add residual
                TpcResidual* theRes = new TpcResidual();
                theRes->setResXYZ(poca-hpos);
                theRes->setRefPos(poca);
                theRes->setHitPos(hpos);
                theRes->setHitIndex(icl);
                theRes->setTrackDir(mom1.Unit());
                theRes->setHitCov(cov);
                TMatrixD cov3=cov6.GetSub(0,2,0,2);
                theRes->setTrackCov(cov3);
                theRes->setTrackLength(tracklength);
                fResults.back()->addResidual(theRes);
            }
            //delete recoHit;
        } //end Cluster loop


    }//end GF trackfit loop

}
void TpcRefGFTrkTpcClusterResCalc::writeHists(){
    TFile* file = new TFile(fQAFileName,"recreate");
    file->cd();
    rHist->Write();
    rHistCut->Write();
    momHist->Write();
    momHistCut->Write();
    file->Close();
}