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


ClassImp(TpcTrackShieldResCalc)

    TpcTrackShieldResCalc::TpcTrackShieldResCalc()
      : fGFTrackBranchName("GFTrack"),
      fShieldBranchName("TpcShieldPoint"),
      fNRep(-1),
      fDetID(-1),
      fRequireGoodFit(true),
      fQAFileName("TpcTrackShieldResCalcQA.root")
{
  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);

}

TpcTrackShieldResCalc::~TpcTrackShieldResCalc() {

}

bool TpcTrackShieldResCalc::init() {
  if(fBranchMap.size() != fNExpectedBranches) {
    std::cout<<"TpcTrackShieldResCalc::init(): "<<std::endl
        <<"   ERROR: Wrong number of input branches ("
        <<fBranchMap.size()<<")!"<<std::endl;
    return true;
  }
  //check for proper data types:
  bool foundGFTrack=false;
  bool foundShield=false;
  std::string GFTrackRefType("GFTrack");
  std::string tpcShieldRefType("TpcShieldPoint");
  alMan=TpcAlignmentManager::getInstance();
  //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());
    std::cout<<type<<std::endl;
    if(type.find(GFTrackRefType)<type.size()) {
      fGFTrackArray=it->second;
      fGFTrackBranchName = it->first;
      foundGFTrack=true;
      std::cout<<"TpcTrackShieldResCalc::init(): "<<std::endl
          <<"   INFO: got "<<type<<" as Object type for GFTracks with branch name "<<it->first
          <<std::endl;
    }
    if(type.find(tpcShieldRefType)<type.size()) {
      fShieldArray=it->second;
      foundShield=true;
      std::cout<<"TpcTrackShieldResCalc::init(): "<<std::endl
          <<"   INFO: got "<<type<<" as Object type for TPC Shield point with branch name "<<it->first
          <<std::endl;
    }
  }
  if(!(foundGFTrack&&foundShield)) {
    std::cout<<"TpcTrackShieldResCalc::init(): "<<std::endl
        <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
    return true;
  }

  //all ok
  return false;
}


//main functionality
int TpcTrackShieldResCalc::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();
    int val = rep->getStatusFlag();
    if(val!=0 && fRequireGoodFit)
      continue;
    int mcid=itrack->getCand().getMcTrackId();
    if(rep->getPDG()>10000)
       continue;
    fResults.push_back(new TpcRefResidualCollection(fShieldBranchName.Data(),itr));

    //calculate residuals in XYZ --------------------------
    unsigned int ncl = fShieldArray->GetEntriesFast();
    TVector3 poca, norm;
    for(unsigned int icl=0; icl<ncl; icl++) {

      TpcShieldPoint* point= (TpcShieldPoint*) fShieldArray->At(icl);
      if(point->GetTrackID()==mcid){
        TVector3 hpos;
        TVector3 pos1, mom1, mom;
        point->Position(hpos);
        //hpos.Print();
        hpos=TpcAlignmentManager::getInstance()->localToMaster("shield",hpos);
        //hpos.Print();
        double tracklength=0;
        bool ok=true;
        TMatrixDSym tcov,cov6;
        try {
          mom=rep->getMom();
          //std::cout<<"mom: ";
          //mom.Print();
          rep->extrapolateToPoint(hpos, poca, norm);
          TVectorT<double> dummy;
          GFDetPlane plane(poca,norm);
          //        plane.setO(poca);
          //        plane.setNormal(norm);
          //        RKTrackRep* trackrepCopy=(RKTrackRep*)rep->clone();
          //tracklength=rep->extrapolate(plane,dummy);
//          std::cout<<"before extrap"<<std::endl;
          //tracklength=rep->extrapolate(plane,dummy,tcov);
          //rep->getPosMomCov(pos1,mom1,cov6);
          //std::cout<<"testing"<<std::endl;
          //cov6.Print();
          tracklength=rep->extrapolate(plane);
          rep->getPosMomCov(pos1,mom1,cov6);
          //cov6.Print();
          //std::cout<<"mom1: ";
          //mom.Print();
//          std::cout<<"after extrap"<<std::endl;
        }
        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-TVector3(0,0,13));
          theRes->setHitPos(hpos-TVector3(0,0,13));
          theRes->setHitIndex(icl);
          theRes->setTrackDir(mom1.Unit());
//          std::cout<<"before passing cov"<<std::endl;
//          tcov.Print();
//          cov6.Print();
//          cov3.Print();
          TMatrixD cov3=cov6.GetSub(0,2,0,2);

          theRes->setHitCov(cov3);
//          std::cout<<"after passing cov"<<std::endl;
          theRes->setTrackLength(tracklength);
          fResults.back()->addResidual(theRes);
        }
        break;
      }
    } //end Cluster loop


  }//end GF trackfit loop

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