#include "PndIsochroneTrackFinderTask.h"

#include <iostream>

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

// framework includes
#include "FairRootManager.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"
#include "FairRunAna.h"
#include "FairEventHeader.h"
#include "FairTimeStamp.h"
#include "FairTSBufferFunctional.h"

// PndMvd includes
#include "PndTrackCand.h"
#include "PndSttMapCreator.h"
#include "PndSttHit.h"
#include "PndSttMapCreator.h"

#include "PndSdsHit.h"

using std::cout;
using std::endl;

ClassImp(PndIsochroneTrackFinderTask);

void PndIsochroneTrackFinderTask::SetParContainers() {
	FairRuntimeDb* rtdb = FairRunAna::Instance()->GetRuntimeDb();
	fSttParameters = (PndGeoSttPar*) rtdb->getContainer("PndGeoSttPar");
}

InitStatus PndIsochroneTrackFinderTask::Init() {
	FairRootManager* ioman = FairRootManager::Instance();

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

	fEventHeader = (TClonesArray*) ioman->GetObject("EventHeader.");
	if (!fEventHeader) {
		cout
				<< "-W-  PndJetteAnalysisTask::Init: No EventHeader array! Needed for EventNumber"
				<< endl;
		return kERROR;
	}

	if (fHitBranch.size() == 0) {
		std::cout << "-W- PndIsochroneTrackFinderTask::Init: "
				<< "No Branch Names given with AddHitBranch(TString branchName)! Standard BranchNames taken!"
				<< std::endl;
		fHitBranch.push_back("STTHit");
		//fHitBranch.push_back("STTCombinedSkewedHits");
	}


	for (int i = 0; i < (int) fHitBranch.size(); i++) {
		InitHitArray(fHitBranch[i]);
	}

	PndSttMapCreator *mapper = new PndSttMapCreator(fSttParameters);
	fSttTubeArray = mapper->FillTubeArray();
	fTrackFinder.SetTubeArray(fSttTubeArray);


	fSttSkewedHitsArray = ioman->Register("CorrectedSkewedHits", "FairHit", "STT", fPersistence);
	fTrackCandArray = ioman->Register("TrackCandsIsochrone", "PndTrackCand", "STT", fPersistence);
	fTrackArray = ioman->Register("TracksIsochrone", "PndTrack", "STT", fPersistence);
	fTrackFinder.SetSkewedBranchName("CorrectedSkewedHits");
	fTrackFinder.SetSkewedArray(fSttSkewedHitsArray);

	std::cout << "-I- PndIsochroneTrackFinderTask: Initialisation successfull"
			<< std::endl;
	fSaveHistograms = false;

//	fStartFunctor = new StopTime();
//	fStopFunctor = new StopTime();

	//fInitDone = kTRUE;
	return kSUCCESS;
}

void PndIsochroneTrackFinderTask::InitHitArray(TString branchName)
{
	  TClonesArray* tempArray = (TClonesArray*) FairRootManager::Instance()->GetObject(branchName);
	  if (tempArray == 0){
		  std::cout << "-W- PndMvdRiemannTrackFinderTask::Init: " << "No hitArray for BranchName " << branchName.Data() << std::endl;
	  }
	  fHitArray.push_back(tempArray);
}

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

	FairEventHeader* myEventHeader = (FairEventHeader*) fEventHeader;
	int eventNumber = myEventHeader->GetMCEntryNumber();

//	if (fVerbose > 0) {
	cout << "====================Begin PndIsochroneTrackFinderTask::Exec======================="	<< endl;
	cout << "Event #" << eventNumber << endl;
//	}


	FairRootManager *ioman = FairRootManager::Instance();


	fTrackFinder.Reset();

	for (int i = 0; i < (int)fHitBranch.size(); i++){
	  fTrackFinder.AddHits(fHitArray[i], ioman->GetBranchId(fHitBranch[i]));
	  if (fVerbose > 0) std::cout << "TrackFinder.AddHits: " << fHitBranch[i] << " Number: " << ioman->GetBranchId(fHitBranch[i]) << std::endl;
	}
	fTrackFinder.SetAngleSteps(0.5);
	//std::vector<PndTrack> tracks =  fTrackFinder.FindTracks();
	std::vector<PndTrack> tracks = fTrackFinder.FindMvdTracks();
	if (fSaveHistograms == true) {
		TH2D* histo = fTrackFinder.GetHoughHisto();
		TGraph peakGraph = fTrackFinder.GetPeakCoordinateGraph();
		TGraph peakGraphMvd = fTrackFinder.GetPeakCoordinateGraphMvd();
		gROOT->SetBatch();
		if (histo != 0) {
			TString histoName("HoughHisto_");
			histoName += eventNumber;
			histo->SetName(histoName.Data());

			TCanvas * tempCanvas = new TCanvas("canvasHough","canvasHough",0, 0, 1000, 800);
			TString canvasName = tempCanvas->GetName();
			canvasName += eventNumber;
			tempCanvas->SetName(canvasName.Data());

			histo->Draw("COLZ");
			peakGraph.Draw("P");
			peakGraphMvd.Draw("P");
			tempCanvas->Update();
			histo->Write();
			tempCanvas->Write();
		}
	std::vector<PndTrackCand> trackCands = fTrackFinder.GetTrackCands();
	std::cout << "nTrackCands " << trackCands.size() << std::endl;
	for (int j = 0; j < trackCands.size(); j++){
		PndTrackCand* aTrackCand = new ((*fTrackCandArray)[fTrackCandArray->GetEntriesFast()]) PndTrackCand();		//why the copy constructor cannot be used is not clear for me. If I use it a Streamer Error is created
		std::vector<PndTrackCandHit> sortedHits = trackCands[j].GetSortedHits();
		for (int k = 0; k < sortedHits.size(); k++){
			aTrackCand->AddHit((FairLink)sortedHits[k], sortedHits[k].GetRho());
		}
	}

	std::cout << "nTracks: " << tracks.size() << std::endl;
	for (int i = 0; i < tracks.size(); i++){
		PndTrack* aTrack = new((*fTrackArray)[fTrackArray->GetEntriesFast()]) PndTrack(tracks[i]);
		aTrack->AddLink(FairLink(-1, ioman->GetEntryNr(), "TrackCandsIsochrone", i));
	}

}

void PndIsochroneTrackFinderTask::FinishEvent()
{
	fTrackArray->Delete();
	if (fTrackCandArray != 0)
		fTrackCandArray->Delete();
}