//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcVertexingTask
//      see TpcVertexingTask.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Johannes Rauch    TUM            (original author)
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "TpcVertexingTask.h"

// C/C++ Headers ----------------------
#include <map>

// Collaborating Class Headers --------
#include "FairRootManager.h"
#include "FairRuntimeDb.h"



#include "GFTrack.h"
#include "RKTrackRep.h"

#include "FairRunAna.h"
#include "GFDetPlane.h"
#include "GFFieldManager.h"
#include "GFException.h"

#include "TFile.h"
#include "TH1D.h"
#include "TH2D.h"
#include "TH3D.h"
#include "TVector3.h"
#include "TMath.h"
#include "TDatabasePDG.h"
#include "TClonesArray.h"

#include "PndDetectorList.h"
#include "TpcVertex.h"
#include "GFRaveVertexFactory.h"
#include "GFRaveVertex.h"

#include <cmath>

using namespace std;

// Class Member definitions -----------

ClassImp(TpcVertexingTask)

TpcVertexingTask::TpcVertexingTask()
  : FairTask("TpcVertexing"),
    _persistence(kFALSE),
    fUseVacuumPropagator(false),
    _trackBranchName("TrackPostFit"),
    _tpcVertexBranchName("TpcVertex"),
    _vertexBranchName("GFVertex"),
    fMethod("kalman-smoothing:1"),
    fUseBeamspot(false),
    fBeamCov(3,3)
  {
    fVerbose = 0;
  }

TpcVertexingTask::~TpcVertexingTask(){
}


InitStatus
TpcVertexingTask::Init()
{
//Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();

  if(ioman==0){
    Error("TpcVertexingTask::Init","RootManager not instantiated!");
    return kERROR;
  }

  // Get input collection
  _trackArray=(TClonesArray*) ioman->GetObject(_trackBranchName);
  if(_trackArray==0){
    Error("TpcVertexingTask::Init","GFTrack-array not found!");
    return kERROR;
  }

  // register output arrays
  _vertexArray = new TClonesArray("GFRaveVertex");
  ioman->Register(_vertexBranchName,"Tpc",_vertexArray,_persistence);
  _tpcVertexArray = new TClonesArray("TpcVertex");
  ioman->Register(_tpcVertexBranchName,"Tpc",_tpcVertexArray,_persistence);



  fVertexBuffer = new std::vector < GFRaveVertex* >;

  return kSUCCESS;
}


void
TpcVertexingTask::SetParContainers() {
  ;
}


void
TpcVertexingTask::SetMethod(const TString& method){
  // TString to std::string
  std::string stdMethod(method.Data());
  fMethod = stdMethod;
}


void
TpcVertexingTask::SetBeamspot(const TVector3 & pos, const TMatrixDSym& cov3x3){
  fBeamPos = pos;
  fBeamCov = cov3x3;
  fUseBeamspot = true;
}


void
TpcVertexingTask::SetBeamspot(const TVector3 & pos, double err){
  fBeamPos = pos;
  fBeamCov.UnitMatrix();
  fBeamCov *= err;
  fUseBeamspot = true;
}


void
TpcVertexingTask::Exec(Option_t* opt)
{
  static unsigned int counter(0);
  //std::cout<<"TpcVertexingTask::Exec - Event Nr. " << ++counter << "\n";
  // init vertex factory
  fVertexFactory = new GFRaveVertexFactory(fVerbose, fUseVacuumPropagator);
  //std::cout<<"TpcVertexingTask::Exec - Event Nr. " << counter << "\n";
  if(fUseBeamspot){
    fVertexFactory->setBeamspot(fBeamPos, fBeamCov);
  }
  //std::cout<<"TpcVertexingTask::Exec - Event Nr. " << counter << "\n";
  fVertexFactory->setMethod(fMethod);




  std::map<GFTrack*, unsigned int> trackMap;
  // Reset output Array
  if(_vertexArray==0) Fatal("TpcVertexingTask::Exec","No GFRaveVertex Array");
  _vertexArray->Delete();
  if(_tpcVertexArray==0) Fatal("TpcVertexingTask::Exec","No TpcVertex Array");
  _tpcVertexArray->Delete();


  // clear buffers
  for (unsigned int i=0; i<fVertexBuffer->size(); ++i){
    delete (*fVertexBuffer)[i];
  }
  fVertexBuffer->clear();
  fTrackBuffer.clear();

  // skip if not enough tracks
  unsigned int nTrks=_trackArray->GetEntriesFast();
  if (nTrks < 2) {
    if (fVerbose > 0) std::cout << "less than 2 tracks, skipping ...\n";
    return;
  }

  // put GFTracks into vector
  fTrackBuffer.reserve(nTrks);
  for(unsigned int i=0; i<nTrks; ++i){
    if(((GFTrack*)_trackArray->At(i))->getCardinalRep()->getStatusFlag()==0){
      fTrackBuffer.push_back((GFTrack*)_trackArray->At(i));
      trackMap[(GFTrack*)_trackArray->At(i)]=i;
    }
  }

  // create vertices
  if (fVerbose > 0){
    std::cout << "################################################################################################ ";
    std::cout << " TpcVertexingTask find vertices from " << fTrackBuffer.size() << " tracks";
    std::cout << "################################################################################################ "<<std::endl;
  }
  fVertexFactory->findVertices(fVertexBuffer, fTrackBuffer, fUseBeamspot);

  // copy vertices into output array
  unsigned int nVert(fVertexBuffer->size());
  for (unsigned int i=0; i<nVert; ++i){
    GFRaveVertex* theVtx= new((*_vertexArray)[i])  GFRaveVertex(*(*fVertexBuffer)[i]);
    std::vector<unsigned int> tracks;
    if (fVerbose > 0){
      std::cout << "################################################################################################ ";
      std::cout << " vertex contains " << ((*fVertexBuffer)[i])->getNTracks() << " tracks";
      std::cout << "################################################################################################ "<<std::endl;
    }
    for(unsigned int itrack=0;itrack<((*fVertexBuffer)[i])->getNTracks();++itrack){
      if(trackMap.count(((*fVertexBuffer)[i])->getParameters(itrack)->getTrack())){
        tracks.push_back(trackMap[((*fVertexBuffer)[i])->getParameters(itrack)->getTrack()]);
      }else{
        std::cerr<<"####################################################################################"<<std::endl;
        std::cerr<<"TpcVertexingTask::Exec() invalid track index!, this should NEVER happen"<<std::endl;
        std::cerr<<"####################################################################################"<<std::endl;
      }
    }
    TpcVertex* theVtx2=new((*_tpcVertexArray)[i])  TpcVertex(theVtx->getPos(),
                                                             theVtx->getCov(),
                                                             tracks,
                                                             _trackBranchName,
                                                             i,_vertexBranchName);
    theVtx2->setErr(theVtx->getChi2()/theVtx->getNdf());
    if (fVerbose > 1) (*_vertexArray)[_vertexArray->GetLast()]->Print();
  }

  std::cout << nVert << " vertices found\n";
  delete fVertexFactory;
}