#include "PndMvdRiemannTrackFinderTask.h"

#include <iostream>

// Root includes
#include "TROOT.h"
#include "TString.h"
#include "TClonesArray.h"
#include "TParticlePDG.h"

// framework includes
#include "FairRootManager.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"

// PndMvd includes
#include "PndTrackCand.h"
#include "PndMvdRiemannTrackFinder.h"


PndMvdRiemannTrackFinderTask::PndMvdRiemannTrackFinderTask() :
	FairTask("MVD Riemann Track Finder"), fMaxSZChi2(1), fMaxSZDist(10), fMinPointDist(1), fMaxDist(1), fEventNr(0), fB(2.0), fInitDone(kFALSE), fCutChi2H(0), fCutDistH(0), fPersistence(kTRUE)
{
  PndGeoHandling::Instance();
}

PndMvdRiemannTrackFinderTask::~PndMvdRiemannTrackFinderTask()
{
}

void PndMvdRiemannTrackFinderTask::SetParContainers()
{
}

InitStatus PndMvdRiemannTrackFinderTask::ReInit()
{
  InitStatus stat=kERROR;
  return stat;
}

// -----   Public method Init   --------------------------------------------
InitStatus PndMvdRiemannTrackFinderTask::Init()
{
  FairRootManager* ioman = FairRootManager::Instance();

  if ( ! ioman )
    {
      std::cout << "-E- PndMvdRiemannTrackFinderTask::Init: "
     << "RootManager not instantiated!" << std::endl;
      return kFATAL;
    }

  // Get input array
  if (fHitBranch.size() == 0){
	  std::cout << "-W- PndMvdRiemannTrackFinderTask::Init: " << "No Branch Names given with AddHitBranch(TString branchName)! Standard BranchNames taken!" << std::endl;
	  fHitBranch.push_back("MVDHitsPixel");
	  fHitBranch.push_back("MVDHitsStrip");
  }

  for (int i = 0; i < (int)fHitBranch.size(); i++){
	  TClonesArray* tempArray = (TClonesArray*) ioman->GetObject(fHitBranch[i]);
	  if (tempArray == 0){
		  std::cout << "-W- PndMvdRiemannTrackFinderTask::Init: " << "No hitArray for BranchName " << fHitBranch[i].Data() << std::endl;
		  return kERROR;
	  }
	  fHitArray.push_back(tempArray);
  }



  fTrackCandArray = new TClonesArray("PndTrackCand");
  ioman->Register("MVDRiemannTrackCand", "MVD", fTrackCandArray, fPersistence);

  fTrackArray = new TClonesArray("PndTrack");
  ioman->Register("MVDTrack", "MVD", fTrackArray, fPersistence);

  fRiemannTrackArray = new TClonesArray("PndRiemannTrack");
//  ioman->Register("MVDRiemannTrack", "MVD", fRiemannTrackArray, kTRUE);


  std::cout << "-I- PndMvdRiemannTrackFinderTask: Initialisation successfull" << std::endl;
  fInitDone = kTRUE;
  return kSUCCESS;
}

void PndMvdRiemannTrackFinderTask::AddHitBranch(TString branchName)
{
	if (fInitDone == kFALSE)
		fHitBranch.push_back(branchName);
	else
		std::cout << "-W- AddHitBranch has to be called before the Init() of the task!" << std::endl;
}

// -----   Public method Exec   --------------------------------------------
void PndMvdRiemannTrackFinderTask::Exec(Option_t* opt)
{

  // Reset output array
  if ( ! fTrackCandArray )
    Fatal("Exec", "No trackCandArray");

  fTrackCandArray->Delete();
  fTrackArray->Delete();
 // fRiemannTrackArray->Delete();

  PndMvdRiemannTrackFinder trackFinder;
  trackFinder.SetVerbose(fVerbose);

  FairRootManager *ioman = FairRootManager::Instance();

 // std::cout << std::endl;
//  std::cout << "------------- event " << fEventNr << "----------------" << std::endl;

  for (int i = 0; i < (int)fHitBranch.size(); i++)
	  trackFinder.AddHits(fHitArray[i], ioman->GetBranchId(fHitBranch[i]));
  trackFinder.SetMaxSZChi2(fMaxSZChi2);
  trackFinder.SetMinPointDist(fMinPointDist);
  trackFinder.SetMaxPlaneDistance(fMaxDist);
  trackFinder.SetMaxSZDist(fMaxSZDist);
  trackFinder.SetVerbose(fVerbose);
  trackFinder.SetMinNumberOfHits(4);
  //trackFinder.SetGeoH(fGeoH);

  trackFinder.SetCutDistH(fCutDistH);
  trackFinder.SetCutChi2H(fCutChi2H);

  trackFinder.FindTracks();


  std::cout << "Found Tracks: " << trackFinder.NTracks() << " in event no. " << fEventNr++ << std::endl;
  std::cout << "----------------" << std::endl;


  for (int i = 0; i < trackFinder.NTracks(); i++){
	  new ((*fTrackCandArray)[i])PndTrackCand(trackFinder.GetTrackCand(i));
	  if (fVerbose > 0)trackFinder.GetTrack(i).Print();
	  //PndRiemannTrack myTrack = trackFinder.GetTrack(i);

	  PndTrack* myTrack = new ((*fTrackArray)[i])PndTrack(trackFinder.GetPndTrack(i, fB));
	  myTrack->SetLink(FairLink("MVDRiemannTrackCand", i));
	  if (fVerbose > 0) {
		  std::cout << i << ": ";
		  myTrack->Print();
	  }
//	  PndRiemannTrack* newTrack = new ((*fRiemannTrackArray)[i])PndRiemannTrack();
//	  TVectorD origin = myTrack.orig();
//	  newTrack->init(origin[0], origin[1], myTrack.r(), myTrack.dip(), 0);
//	  for (int j = 0; j < myTrack.getNumHits(); j++){
//		  newTrack->addHit(*myTrack.getHit(j));
//	  }
  }
}

void PndMvdRiemannTrackFinderTask::FinishEvent()
{
	fTrackCandArray->Delete();
	fTrackArray->Delete();
//	fRiemannTrackArray->Delete();
}

ClassImp(PndMvdRiemannTrackFinderTask);