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

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

// This Class' Header ------------------
#include "HeliKalmanTask.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 "TpcCluster.h"
#include "TpcPlanarRecoHit.h"
#include "TpcSPHit.h"

#include "PseudoSpacePoint.h"

#include "GFRecoHitFactory.h"
#include "GFKalman.h"
#include "GFException.h"
#include "TH1D.h"
#include "TFile.h"
#include "TGeoTrack.h"
#include "TGeoManager.h"

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


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



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

 
HeliKalmanTask::HeliKalmanTask()
  : FairTask("Kalman Filter"),
    _persistence(kTRUE),
    _numIt(5), 
    _trackBranchName("HeliPlawaTrackPreFit"),
    _outBranchName("HeliPlawaTrackPostFit")
{
  _verbose = kFALSE;
}

 
HeliKalmanTask::~HeliKalmanTask()
{
}

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

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

   _trackOutArray = new TClonesArray("GFTrack");
   ioman->Register(_outBranchName,"", _trackOutArray, _persistence);

  return kSUCCESS;
}


void
HeliKalmanTask::Exec(Option_t* opt)
{
  if (_verbose)
    fprintf(stderr,"\nHeliKalmanTask::Exec\n");

  // define variables
  TVector3 pos, mom;
  TMatrixT<double> cov;  
  GFDetPlane plane1;
  GFAbsTrackRep *rep = NULL;

  // Reset output Array
  _trackOutArray->Delete();
  if(_trackArray==0) Fatal("Kalman::Exec)","No TrackArray");


  Int_t ntracks=_trackArray->GetEntriesFast();
  
  if(ntracks>20000){
    std::cout<<"ntracks="<<ntracks<<" Evil Event! skipping"<<std::endl;
    return;
  }

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

  for(Int_t itr=0;itr<ntracks;++itr){
    GFTrack* trk=(GFTrack*)_trackArray->At(itr);
    
   // pre-fit momentum
    double mom0=trk->getMom().Mag();
    
    if (_verbose) {
      fprintf(stderr,"....................................................\n");
      fprintf(stderr,"Track %i - Number of hits on track: %i\n",
        itr,trk->getNumHits());
    }

    // Start Fitter
    try{
      fitter.processTrack(trk);
    }
    catch (GFException& e){
      std::cout<<e.what()<<std::endl;
    }
    
    // save track
    Int_t size = _trackOutArray->GetEntriesFast();
    new((*_trackOutArray)[size]) GFTrack(*trk);
    
    // Print Track Parameters after fit
    Int_t nreps = trk->getNumReps();
    for (Int_t kk=0; kk<nreps; kk++) {
      rep = trk->getTrackRep(kk);
      
      fprintf(stderr,"Representation %i: %i\n",kk,rep->getStatusFlag());
      fprintf(stderr,"Chi2: %f/%f\n",rep->getChiSqu(),rep->getRedChiSqu());
      fprintf(stderr,"Charge: %f\n",rep->getCharge());
      fprintf(stderr,"Initial/Fitted momentum: %f/%f\n",
        mom0,rep->getMom().Mag());
    }
    
    if (_verbose) {
      
      Double_t chi2 = 0.;
      for (Int_t kk=0; kk<trk->getNumHits(); kk++) {
        TVector3 hitpos =
          TVector3(trk->getHit(kk)->getRawHitCoord().GetMatrixArray());
        plane1 = GFDetPlane(
          TVector3(0.,0.,hitpos.Z()),
          TVector3(1.,0.,0.),
          TVector3(0.,1.,0.));
        try {
          trk->getPosMomCov(plane1,pos,mom,cov);
        } catch (GFException& e) {
          e.what();
        }
        fprintf(stderr,"hit[%i]: %8.3f/%8.3f %8.3f/%8.3f %8.3f/%8.3f %8.3f\n",
          kk,hitpos.X(),pos.X(),hitpos.Y(),pos.Y(),hitpos.Z(),pos.Z(),
          (hitpos-pos).Mag());
        chi2 += (hitpos-pos).Mag2();
      }
    }
  }
  if (_verbose)
    std::cout<<"Fitted "<< _trackOutArray->GetEntriesFast() << " tracks" << std::endl;

  return;
}

ClassImp(HeliKalmanTask)