//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TBKalmanTask
//      see TBKalmanTask.h for details
//
// Environment:
//      Software developed for the Test Chamber
//
// Author List:
//      Maxence Vandenbroucke    TUM            (original author)
//
//
//-----------------------------------------------------------

// This Class' Header ------------------
#include "TBKalmanTask.h"
#include <cmath>
// C/C++ Headers ----------------------
#include <algorithm>
#include <iostream>
#include <assert.h>

// Collaborating Class Headers --------
#include "FairRootManager.h"
#include "TClonesArray.h"
#include "GFTrack.h"

#include "GFRecoHitFactory.h"
#include "GFKalman.h"
#include "GFException.h"
#include "TFile.h"
#include "TGeoTrack.h"
#include "TGeoManager.h"
#include "TLorentzVector.h"
#include "GFDetPlane.h"

#include "TBGEMRecoHit.h"
#include "TBSIRecoHit.h"

#include "GFAbsRecoHit.h"
#include "TVector3.h"

#include <signal.h>
#include <stdlib.h>

void sighandler(int sig){
  std::cerr << "sighandler for sig " << sig << std::endl;
  abort();
}

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

TBKalmanTask::TBKalmanTask()
  : FairTask("TB Kalman Filter"), _persistence(kTRUE),_lazy(0),_numIt(1), _trackBranchName("TBTrackPreFit"), _outBranchName("TBTrackPostFit"), fSIBranchName("TBSICluster"), fGEMBranchName("TBGEMCluster")
{
  fVerbose = 1;
}


TBKalmanTask::~TBKalmanTask()
{
}

InitStatus
TBKalmanTask::Init()
{
  //Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();
  
  if(ioman==0) {
    Error("TBKalmanTask::Init","RootManager not instantiated!");
    return kERROR;
  }
  
  // Get input collection
  _trackArray=(TClonesArray*) ioman->GetObject(_trackBranchName);
  if(_trackArray==0) {
    Error("TBKalmanTask::Init","track-array not found!");
    return kERROR;
  }
  

  // Build hit factories -----------------------------
  _theRecoHitFactory = new GFRecoHitFactory();
  
  TClonesArray* SIArray=(TClonesArray*) ioman->GetObject(fSIBranchName);
  if(SIArray==0){
    Error("PndFwdTBKalmanTask::Init","TB SI array not found");
  } else {
    _theRecoHitFactory->addProducer(FairRootManager::Instance()->GetBranchId(fSIBranchName),
				    new GFRecoHitProducer<TBSICluster,TBSIRecoHit>(SIArray));
  }
  
  TClonesArray* GEMArray=(TClonesArray*) ioman->GetObject(fGEMBranchName);
  if(GEMArray==0){
    Error("PndFwdTBKalmanTask::Init","TB GEM array not found");
  } else {
    _theRecoHitFactory->addProducer(FairRootManager::Instance()->GetBranchId(fGEMBranchName),
				    new GFRecoHitProducer<TBGEMCluster,TBGEMRecoHit>(GEMArray));
  }
  
  _trackOutArray = new TClonesArray("GFTrack");
  ioman->Register(_outBranchName,"GenFit", _trackOutArray, _persistence);
  
  return kSUCCESS;
}

void
TBKalmanTask::Exec(Option_t* opt)
{
  std::cout<<"TBKalmanTask::Exec"<<std::endl;
  
  // Reset output Array
  _trackOutArray->Delete();
  if(_trackArray==0) Fatal("Kalman::Exec)","No TrackArray");

  Int_t ntracks=_trackArray->GetEntriesFast();
  
  if(ntracks>2){
    std::cout<<"ntracks="<<ntracks<<" More than one tracks from SI, Evil Event! skipping"<<std::endl;
    return;
  }

  // Fitting ---------------- can go to another task!
  GFKalman fitter;
  //  fitter.setLazy(_lazy);
  fitter.setNumIterations(_numIt);

  //std::vector<TLorentzVector*> particles;
  //std::vector<int> signs;
  
  for(Int_t itr=0;itr<ntracks;++itr){
    if (fVerbose) std::cout<<"starting track"<<itr<<std::endl;
    GFTrack* trk=(GFTrack*)_trackArray->At(itr);
    
    // Load RecoHits 
    try {
      trk->addHitVector(_theRecoHitFactory->createMany(trk->getCand()));
      if (fVerbose) std::cout<<trk->getNumHits()<<" hits in track " <<itr<<std::endl;
    }
    catch(GFException& e) {
      std::cout << e.what() << std::endl;
      e.info();
      throw e;
    }
    if (fVerbose) std::cout<<"*** Number of clusters in track: "<<trk->getNumHits()<<" ***"<<std::endl;

    Int_t size = _trackOutArray->GetEntriesFast();


    std::cout << "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" << std::endl;
    trk->getCand().Print();
    std::cout << "###########@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" << std::endl;
    GFTrack* trkCopy = new((*_trackOutArray)[size]) GFTrack(*trk);
    trkCopy->getCand().Print();
    std::cout << "$$$$$$$$$###########@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" << std::endl;

    
    
    // Start Fitter
    try{
      if (fVerbose) std::cerr << "TBKalmanTask::Exec - Calling processTrack" << std::endl;
      fitter.processTrack(trkCopy);
    }
    catch (GFException& e){
      std::cout<<"TBKalmanTask::Exec - While Calling processTrack: "<<e.what()<<std::endl;
    }
    

    // Print Track Parameters after fit
    if(trkCopy->getTrackRep(0)->getStatusFlag()==0){
      if(fVerbose) trkCopy->getTrackRep(0)->Print();
      double p=trkCopy->getMom().Mag();
      // _pH->Fill(p);
      
      double chi2=trkCopy->getChiSqu();
      //_chi2H->Fill(chi2);
    }

  }

  if (fVerbose) std::cout<<"Fitting done"<<std::endl;

  std::cout<<"Fitted "<< _trackOutArray->GetEntriesFast() << " tracks" << std::endl;

  return;
}

ClassImp(TBKalmanTask)