//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class DemoKalmanTask
//      see DemoKalmanTask.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 "DemoKalmanTask.h"

// C/C++ Headers ----------------------
#include <algorithm>
#include <iostream>
#include <assert.h>

// Collaborating Class Headers --------
#include "CbmRootManager.h"
#include "TClonesArray.h"
#include "Track.h"
//#include "PndTpcPoint.h"
#include "DemoRecoHit.h"
#include "CbmMCPoint.h"
#include "LSLTrackRep.h"
#include "RecoHitFactory.h"
#include "Kalman.h"
#include "FitterExceptions.h"
#include "TH1D.h"
#include "TFile.h"
#include "TGeoTrack.h"
#include "TGeoManager.h"

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


DemoKalmanTask::DemoKalmanTask()
  : CbmTask("Kalman Filter"), _persistence(kFALSE)
{
  _trackBranchName = "TrackPreFit";
}


DemoKalmanTask::~DemoKalmanTask()
{
  if(_pH!=NULL)delete _pH;
  if(_chi2H!=NULL)delete _chi2H;
}

InitStatus
DemoKalmanTask::Init()
{
  //Get ROOT Manager
  CbmRootManager* ioman= CbmRootManager::Instance();

  if(ioman==0)
    {
      Error("DemoKalmanTask::Init","RootManager not instantiated!");
      return kERROR;
    }
  
  // Get input collection
  _trackArray=(TClonesArray*) ioman->GetObject(_trackBranchName);
  
  if(_trackArray==0)
    {
      Error("DemoKalmanTask::Init","track-array not found!");
      return kERROR;
    }
  
  // create and register output array
  //_trackArray = new TClonesArray("Track"); 
  //ioman->Register("TrackPreFit","GenFit",_trackArray,_persistence);
 
  // Build hit factory -----------------------------
  _theRecoHitFactory = new RecoHitFactory();
  std::map<unsigned int,TString>::iterator iter=_hitBranchMap.begin();
  while(iter!=_hitBranchMap.end()){
   TClonesArray* ar=(TClonesArray*) ioman->GetObject(iter->second);
   if(ar==0){
     Error("DemoKalmanTask::Init","point-array %s not found!",iter->second.Data());
   }
   else{ 
     // the next lines is not general because it will work only for CmMCPoints!
     _theRecoHitFactory->addProducer(iter->first,new RecoHitProducer<CbmMCPoint,DemoRecoHit>(ar));
   }
  ++iter;
  }//end loops over hit types

  // setup histograms
  _pH=new TH1D("pH","p",100,-2,2);
  _chi2H=new TH1D("chi2H","chi2",100,0,20);

  return kSUCCESS;
}


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

  Int_t ntracks=_trackArray->GetEntriesFast();
  
  // Fitting ---------------- can go to another task!
  Kalman fitter;
  
  for(Int_t itr=0;itr<ntracks;++itr){
    Track* trk=(Track*)_trackArray->At(itr);
    // Load RecoHits 
	try {
	  trk->addHitVector(_theRecoHitFactory->createMany(trk->getCand()));
	  std::cout<<trk->getNumHits()<<" hits in track "
			   <<itr<<std::endl;
    }
	catch(FitterException& e) {
	  std::cout << e.what() << std::endl;
	  throw e;
	}
    // Start Fitter
    try{
      fitter.processTrack(trk);
    }
    catch (FitterException e){
      std::cout<<e.what()<<std::endl;
    }

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

      // get GeoTrack
      assert(gGeoManager!=0);
      TVirtualGeoTrack* geotrk=gGeoManager->GetTrack(gGeoManager->AddTrack(_trackcount,13));
      trk->fillGeoTrack(geotrk);
      ++_trackcount;
    }
  }
   
  return;
}

void 
DemoKalmanTask::WriteHistograms(const TString& filename){
  TFile* file = new TFile(filename,"UPDATE");
  file->mkdir("DemoKalman");
  file->cd("DemoKalman");

  _pH->Write();
  delete _pH;
  _pH=NULL;

  _chi2H->Write();
  delete _chi2H;
  _chi2H=NULL;

  file->Close();
  delete file;
}


ClassImp(DemoKalmanTask)