//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of resCalc
//      Uses existing CDC circle fits (XY) and calculates
//      residua in XY.
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sverre Doerhein                     (original author)
//
//      Physik Department E18, TUM
//
//-----------------------------------------------------------

//Header
#include "TpcCdcTrackTrackCylResCalc.h"

//ROOT
#include "TClonesArray.h"
#include "TClass.h"

//Collaborating classes
#include "CdcTrack.h"
#include "TpcCluster.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "TpcAlignmentManager.h"

//Genfit
#include "GFTrack.h"
#include "RKTrackRep.h"
#include "GFAbsTrackRep.h"
#include "GFException.h"
#include "GFTools.h"
//Standard lib
#include <string>
#include <iostream>
#include <cmath>


TpcCdcTrackTrackCylResCalc::TpcCdcTrackTrackCylResCalc()
    :fAlignment(false),fRadius(20),fPhiCut(TMath::Pi()/12),
      fCdcTrackBranchName("CdcTrackPostFit"),
      fTpcTrackBranchName("TpcTrackPostFit")
{
    fNExpectedBranches = 2;
}

TpcCdcTrackTrackCylResCalc::~TpcCdcTrackTrackCylResCalc()
{;}

bool TpcCdcTrackTrackCylResCalc::init() {
    if(fBranchMap.size() != fNExpectedBranches) {
        std::cerr<<"TpcCdcTrackTrackCylResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong number of input branches ("
               <<fBranchMap.size()<<")!"<<std::endl;
        return true;
    }
    //check for proper data types:
    std::string cdcTrackRefType("GFTrack");
    std::string tpcTrackType("GFTrack");
    bool foundCdcTrack=false;
    bool foundTpcTrack=false;

    //identify the needed branches in the map
    if(fBranchMap.count(fCdcTrackBranchName)){
        std::cout<<"reftrack branch found: "<<fCdcTrackBranchName<<std::endl;
        std::string type(fBranchMap[fCdcTrackBranchName]->GetClass()->GetName());
        if(type.find(cdcTrackRefType)<type.size()){
            fCdcTrackArray=fBranchMap[fCdcTrackBranchName];
            foundCdcTrack=true;
        }
    }
    if(fBranchMap.count(fTpcTrackBranchName)){
        std::cout<<"reftrack branch found: "<<fTpcTrackBranchName<<std::endl;
        std::string type(fBranchMap[fTpcTrackBranchName]->GetClass()->GetName());
        if(type.find(tpcTrackType)<type.size()){
            fTpcTrackArray=fBranchMap[fTpcTrackBranchName];
            foundTpcTrack=true;
        }
    }
    if(!(foundCdcTrack&&foundTpcTrack)) {
        std::cerr<<"TpcCdcTrackTrackCylResCalc::init(): "<<std::endl
                <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
        return true;
    }
    //all ok
    return false;
}


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

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

    fResults.clear();

    //get instance of alignment manager
    //TpcAlignmentManager* alMan = TpcAlignmentManager::getInstance();

    unsigned int nCdcTracks = fCdcTrackArray->GetEntriesFast();
    unsigned int nTpcTracks = fTpcTrackArray->GetEntriesFast();
    std::vector<TVector3> tpcPos;
    std::vector<TMatrixD> tpcCov;
    bool firstCdcTrack=true;
    //std::cout<<"calc(): nCdcTracks:"<<nCdcTracks<<", nTpcTracks:"<<nTpcTracks<<std::endl;
    for(unsigned int iCdc=0;iCdc<nCdcTracks;++iCdc){
        //std::cout<<"cdcTrack #"<<iCdc<<std::endl;
        GFTrack* cdcTrack=(GFTrack*)fCdcTrackArray->UncheckedAt(iCdc);
        RKTrackRep* cdcRep=(RKTrackRep*) cdcTrack->getCardinalRep();
        if(cdcRep->getStatusFlag()){
            continue;
        }

        TVector3 cpos,cmom;
        TVector3 cposErr,cmomErr;
        TMatrixDSym ccov;
        try{
            cdcRep->extrapolateToCylinder(fRadius,cpos,cmom);
            GFDetPlane plane(cpos,cmom);
            cdcRep->getPosMomCov(plane,cposErr,cmomErr,ccov);
        }catch(GFException exp){
            // std::cerr<<__FILE__<<": "<<__LINE__<<": "<<": "<<exp.what()<<std::endl;
            continue;
        }
        if(cmom.Mag()<0.5){
            continue;
        }
        ////std::cout<<"adding collection "<<std::endl;
        fResults.push_back(new TpcRefResidualCollection(fCdcTrackBranchName.Data(),iCdc));

        for(unsigned int iTpc=0;iTpc<nTpcTracks;++iTpc){
            ////std::cout<<" Tpc Track #"<<iTpc;
            if(firstCdcTrack){
                tpcPos.push_back(TVector3());
                tpcCov.push_back(TMatrixD(3,3));
                GFTrack* tpcTrack=(GFTrack*)fTpcTrackArray->UncheckedAt(iTpc);
                RKTrackRep* tpcRep=(RKTrackRep*) tpcTrack->getCardinalRep();
                if(tpcRep->getStatusFlag()){
                    continue;
                }
                GFTrackCand cand = tpcTrack->getCand();
                int irep=tpcTrack->getCardinalRepID();
                int ihit=cand.getNHits()-1;
                if(tpcTrack->getSmoothing()){
                    GFTools::updateRepSmoothed(tpcTrack,irep,ihit);
                }
                TVector3 tpos,tmom;
                TVector3 tposErr,tmomErr;
                TMatrixDSym tcov;
                try{
                    tpcRep->extrapolateToCylinder(fRadius,tpos,tmom);
                    GFDetPlane plane(tpos,tmom);
                    tpcRep->getPosMomCov(plane,tposErr,tmomErr,tcov);
                }catch(GFException exp){
                    //  std::cerr<<__FILE__<<": "<<__LINE__<<": "<<exp.what()<<std::endl;
                    continue;
                }
                TMatrixD ccov3=ccov.GetSub(0,2,0,2);
                TMatrixD tcov3=tcov.GetSub(0,2,0,2);
                tpcCov[iTpc]=tcov3;
                tpcPos[iTpc]=tposErr;
                if(TMath::Abs(tposErr.Phi()-cposErr.Phi()) > fPhiCut){
                    continue;
                }
                TpcResidual* theRes=new TpcResidual();
                theRes->setResXYZ(cposErr-tposErr);
                theRes->setRefPos(cposErr);
                theRes->setHitPos(tposErr);
                theRes->setTrackCov(ccov3);
                theRes->setHitCov(tcov3);
                theRes->setTrackDir(cmomErr.Unit());
                ////std::cout<<" adding res";
                fResults.back()->addResidual(theRes);
            }else{
                //std::cout<<"using old extrap "<<iCdc<<", "<<iTpc<<std::endl;
                if(tpcPos[iTpc].X()==0){
                    //std::cout<<"old extrap failed, continue"<<std::endl;
                    continue;
                }
                TMatrixD ccov3=ccov.GetSub(0,2,0,2);
                if(TMath::Abs(tpcPos[iTpc].Phi()-cposErr.Phi()) > fPhiCut){
                    continue;
                }
                TpcResidual* theRes=new TpcResidual();
                theRes->setResXYZ(cposErr-tpcPos[iTpc]);
                theRes->setRefPos(cposErr);
                theRes->setHitPos(tpcPos[iTpc]);
                theRes->setTrackCov(ccov3);
                theRes->setHitCov(tpcCov[iTpc]);
                theRes->setTrackDir(cmomErr.Unit());
                ////std::cout<<" adding res";
                fResults.back()->addResidual(theRes);
            }

        }
        firstCdcTrack=false;
    }
    ////std::cout<<fResults.size()<<std::endl;
}