//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class PndMvdKalmanTask
//      see PndMvdKalmanTask.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//      Ralf Kliemt, TU Dresden             (Copied for MVD use)
//      modified by Elisabetta Prencipe, 19/5/2014
//-----------------------------------------------------------

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

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

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

// Collaborating Class Headers --------
#include "FairRootManager.h"
#include "TClonesArray.h"
#include "FairRunAna.h"
#include "Track.h"
#include "TDatabasePDG.h"
#include "PndHypHit.h"
#include "FairMCPoint.h"
#include "../../pnddata/SdsData/PndSdsMCPoint.h"
#include "../../pnddata/SdsData/PndSdsHit.h"
#include "../../GenfitTools/recohits/PndSdsRecoHit.h"
//#include "PndHypRecoHit.h"
#include "PndHypRecoSPHit.h"
#include "PndGeoHandling.h"

#include "FairField.h"
#include "PndFieldAdaptor.h"
#include "FieldManager.h"

#include "MeasurementFactory.h"
#include "KalmanFitter.h"
#include "Exception.h"
#include "TH1D.h"
#include "TFile.h"
#include "TGeoTrack.h"
#include "TGeoManager.h"
#include "TLorentzVector.h"
#include "DetPlane.h"

#include "TDatabasePDG.h"
#include "FairTrackParH.h"
#include "RKTrackRep.h"
//#include "LSLTrackRep.h"
//#include "GeaneTrackRep.h"
#include "FairGeanePro.h"

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


PndHypKalmanTask::PndHypKalmanTask()
  : FairTask("Kalman Filter"), fPersistence(kFALSE)
{
  fTrackBranchName = "HypTrackCand";
  //PndGeoHandling::Instance();
}


PndHypKalmanTask::~PndHypKalmanTask()
{
  if(fPH!=NULL)delete fPH;
  if(fChi2H!=NULL)delete fChi2H;
}

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

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

  // Build hit factory -----------------------------
  fTheRecoHitFactory = new genfit::MeasurementFactory();

  std::map<unsigned int,TString>::iterator iter=fHitBranchMap.begin();
  
  while(iter!=fHitBranchMap.end()){
    

     TClonesArray* har=(TClonesArray*) ioman->GetObject("HypHit");
   
    //cout<<" "<<iter->second<<endl;

    if(har==0){
      
      Error("PndHypKalmanTask::Init","Hit array not found");
    }
    else{ 
      
      if(iter->first==2)fTheRecoHitFactory->addProducer(iter->first,new genfit::MeasurementProducer<PndHypHit,PndHypRecoSPHit>(har));
     
    }

    TClonesArray* sar=(TClonesArray*) ioman->GetObject("MVDHit");
     if(sar==0){
      
      Error("PndHypKalmanTask::Init","Hit array not found");
    }
    else{ 
      
      if(iter->first==3)fTheRecoHitFactory->addProducer(iter->first,new genfit::MeasurementProducer<PndSdsHit,PndSdsRecoHit>(sar));
    }

   ++iter;
  }//end loops over hit types

// create and register output array
  fTrArray = new TClonesArray("Track"); 
  ioman->Register("Track","GenFit",fTrArray,fPersistence);

   //fPro = new FairGeanePro();

  // setup histograms
  fPH=new TH1D("pH","p",100,0.4,0.6);
  fChi2H=new TH1D("chi2H","chi2",100,0,20);
  

  return kSUCCESS;
}


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

  Int_t ntracks=fTrackArray->GetEntriesFast();
 // Detailed output
 
 
  
  if(ntracks>20){
    std::cout<<"ntracks="<<ntracks<<" Evil Event! skipping"<<std::endl;
    return;
  }

  // Fitting ---------------- can go to another task!
  genfit::KalmanFitter fitter;

  // std::vector<TLorentzVector*> particles;
  //std::vector<Int_t> signs;
  
  for(Int_t itr=0;itr<ntracks;++itr){

    std::cout<<"starting track"<<itr<<std::endl;
    genfit::TrackCand* trcnd = (genfit::TrackCand*)fTrackArray->At(itr);
    unsigned int detid=12345, index=12345;
    
    // setting the magnetic field properly
     fField= FairRunAna::Instance()->GetField();
     PndFieldAdaptor* b= new PndFieldAdaptor(fField);
     FieldManager * fb;
     //fb->getInstance();
     //fb->init(b);

     genfit::AbsTrackRep* rep = 0;
    
     std::cout<<trcnd->getNHits()<<std::endl;
   // Starting values for guessing

    Int_t PDGCode= trcnd->getPdgCode();//2212;
    TVector3 StartPos    = trcnd->getPosSeed();//TVector3 (1.0,0.0,0.0);//cmn
    TVector3 StartPosErr = TVector3(0,0,0);
    TVector3 StartMom    = trcnd->getDirSeed();//TVector3 (1.,0.,1.);

    TVector3 StartMomErr = TVector3(0,0,0);
    TDatabasePDG *fdbPDG= TDatabasePDG::Instance();
    TParticlePDG *fParticle= fdbPDG->GetParticle(PDGCode);
    Double_t  fCharge= fParticle->Charge();
     // calc momentum projections
    TVector3 dir=StartMom.Unit();
    double dxdz=dir.X()/dir.Z();
    double dydz=dir.Y()/dir.Z();
    
    rep=new genfit::RKTrackRep(StartPos.Z(),StartPos.X(),StartPos.Y(),dxdz,dydz,trcnd->getQoverPseed(),
			       StartPosErr.X(),StartPosErr.Y(),0.1,0.1,0.1,b);//NULL instead of b field

 
    genfit::Track* trk=new((*fTrArray)[fTrArray->GetEntriesFast()]) genfit::Track(rep);

    if(trk!=NULL)trk->setCandidate(*(genfit::TrackCand*)fTrackArray->At(itr));
   
    else {
      std::cout<<" "<< "caca de vaca "<<std::endl;
      continue;}
    
  
	try {
	  trk->addHitVector(fTheRecoHitFactory->createMany(*trcnd));
	 
    }
	catch(genfit::Exception& e) {
	  std::cout << e.what();
	  throw e;
	}
    // Start Fitter
    try{
      std::cout<<"starting fit"<<std::endl;
      fitter.processTrack(trk);
    }
    catch (genfit::Exception e){
      std::cout<<e.what()<<std::endl;
    }

    // Print Track Parameters after fit
    if(trk->getTrackRep(0)->getStatusFlag()==0){
      //trk->getTrackRep(0)->Print();
      genfit::DetPlane plane(TVector3(0,0,0.1),TVector3(1,0,0),TVector3(0,1,0));
      TVector3 p3=trk->getTrackRep(0)->getMom(plane);
      Double_t p=trk->getMom().Mag();
      std::cout<<" oye"<<p<<std::endl;
      
      fPH->Fill(p);
      
    

      Double_t chi2=trk->getChiSqu();
      fChi2H->Fill(chi2);

    }

  }

  std::cout<<"Fitting done"<<std::endl;



  return;
}

void 
PndHypKalmanTask::WriteHistograms(const TString& filename){
  TFile* file = new TFile(filename,"UPDATE");
//   if(file->cd("Kalman")==false) file->mkdir("Kalman");
//   file->cd("Kalman");

  if(file->cd("Kalman")) file->Delete("Kalman;*");
  file->mkdir("Kalman");
  file->cd("Kalman");


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

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

  

  file->Close();
  delete file;
}


ClassImp(PndHypKalmanTask)