#include "PndFtsHoughSpace.h"

#include <iostream>

#include "TMath.h"
#include <math.h>
#include <algorithm>

#include <set>
#include <vector>
#include <map>

// FTS
#include "PndFtsHit.h"
#include "FairHit.h"



// (Hough) tracking
#include "PndTrackCand.h"
#include "PndTrack.h"
#include "FairTrackParP.h"
#include "PndFtsHoughTrackCand.h"

// histogramming / plotting
#include "TH1.h"
#include "TH2.h"
#include "TGraph.h"

// peak finder
#include "TSpectrum2.h"

// root IO
#include "FairRunAna.h"
#include "FairRootManager.h"
#include "FairRuntimeDb.h"
#include "FairTask.h"

#include "PndFtsHit.h"
#include "TString.h"
#include "FairHit.h"
#include "PndFtsHit.h"

#include "PndFtsTube.h"
#include "FairEventHeader.h"






ClassImp(PndFtsHoughSpace);




std::ostream& operator <<(std::ostream& os, const IdxPath& outVector){
	os << "[";
	Int_t lastIdx = outVector.size()-1;
	if(lastIdx < 0){
		os << "]" ;
		return os;
	}

	for (UInt_t iVec = 0; iVec < lastIdx; ++iVec){
		os << outVector[iVec] << ", ";
	}
	os << outVector[lastIdx] << "]";
	return os;
}

std::ostream& operator <<(std::ostream& os, const HitIdxPathMap& outMap)
{
	os << '\n';
	if(outMap.begin() == outMap.end()){
		os << "{ , [] }";
		return os;
	}
	for (HitIdxPathMap::const_iterator itMap = outMap.begin(); itMap != outMap.end(); ++itMap){
		os << "{ "<< itMap->first << ", ";
		os << itMap->second;
		os << " }\n\n";
	}
	return os;
}


inline void PndFtsHoughSpace::AddHitToHS(UInt_t hitId, Double_t rho)
{
	const PndFtsHit *const hitToAdd = fTrackerTask->GetFtsHit(hitId);
	const Int_t tubeIdToAdd = hitToAdd->GetTubeID();


	if (kFALSE == IsHitFromTubeIdAlreadyAdded(tubeIdToAdd)) {
		fHitId.push_back(PndTrackCandHit(fFtsBranchId, hitId, rho));
	}
}


inline void PndFtsHoughSpace::AddHitToHS(FairLink link, Double_t rho)
{
	const PndFtsHit *const hitToAdd = fTrackerTask->GetFtsHit(link.GetIndex());
	const Int_t tubeIdToAdd = hitToAdd->GetTubeID();

	if (kFALSE == IsHitFromTubeIdAlreadyAdded(tubeIdToAdd)) {
		fHitId.push_back(PndTrackCandHit(link.GetType(), link.GetIndex(), rho));
	}
}


inline Bool_t PndFtsHoughSpace::IsHitFromTubeIdAlreadyAdded(const Int_t tubeIdToAdd)
{
	//	return kFALSE; // uncomment this line to switch off testing for duplicates
	for (int iTestHit = 0; iTestHit < GetNHits(); ++iTestHit)
	{
		const PndFtsHit *const myTestHit = getHitFromHS(iTestHit);

		const Int_t tubeIdTestHit = myTestHit->GetTubeID();

		if ( tubeIdToAdd == tubeIdTestHit ) return kTRUE;

	} // for loop over all hits which have already been added to the Hough space
	return kFALSE;
};




PndFtsHoughSpace::PndFtsHoughSpace(
		const char *name,
		const Int_t refIndex,

		PndFtsHoughSpaceBinning binning,

		Double_t zRefPos,
		Double_t interceptZx,

		PndFtsHoughTrackCand *associatedTrackCand,

		PndFtsHoughTrackerTask *trackerTask
) :
			fTrackerTask(trackerTask),
			fRefIndex(refIndex),

			fZRefPos(zRefPos),
			fInterceptZx(interceptZx),

			TH2S(name, name,
					binning.getNBinsTheta(), binning.getThetaRadLow(), binning.getThetaRadHigh(),
					binning.getNBinsY(),binning.getYLow(),binning.getYHigh()),

			// set from tracker task
			fFtsBranchId(0),
			fVerbose(0),
			fField(0),

			fAssociatedTrackCand(associatedTrackCand)
{
	if (0==fTrackerTask){
		std::cerr << "PndFtsHoughSpace FATAL ERROR Tracker task pointer not set.\n";
	} else {
		fVerbose = fTrackerTask->GetVerbose();
		if(3<fVerbose) std::cout << "PndFtsHoughSpace called with tracker ptr " << fTrackerTask << '\n';
		fFtsBranchId = fTrackerTask->getFtsBranchId();
		fField = fTrackerTask->getMagneticFieldPtr();

		setParametersForHsOption();
		filterInputHits();
	}
}

PndFtsHoughSpace::~PndFtsHoughSpace()
{

}


Bool_t PndFtsHoughSpace::setParametersForHsOption()
{
	// use option instead of GetName() or fName (even though it is the same)
	const TString option = GetName();

	fKeepBConstant = kTRUE; // kFALSE only for testing

	// set parameters according to the Hough transform I want to do
	if ("lineBeforeDipole" == option)
	{
		// make sure hits are not shifted for line hough transform
		if (0!=fInterceptZx) {
			std::cout << "PndFtsHoughSpace: " << "fInterceptZx was set to " << fInterceptZx << " That is not correct for line HT of stations before dipole field!\n";
			std::cout << "Will set interceptZx to 0 for " << option << '\n';
			fInterceptZx = 0.;
		}

		fUseNonSkewedStraws = kTRUE;
		fUseSkewedStraws = kFALSE;

		// Line for stations 1+2
		fOnlyUseHitsFromZ = 100.; // Set = 100. if you want to use all FTS hits, higher if you want to exclude hits that are closer to the interaction point than the value
		fOnlyUseHitsUpToZ = 380.; // Set = 1000. if you want to use all FTS hits, lower if you want to exclude hits that are further away from the interaction point than the value

		GetXaxis()->SetTitle("#theta [rad]");
		GetYaxis()->SetTitle("x [cm]");
	}
	else if ("parabola" == option)
	{
		fUseNonSkewedStraws = kTRUE;
		fUseSkewedStraws = kFALSE;

		// parabola for stations 3-5
		fOnlyUseHitsFromZ = 380.; // Set = 100. if you want to use all FTS hits, higher if you want to exclude hits that are closer to the interaction point than the value
		fOnlyUseHitsUpToZ = 630.; // Set = 1000. if you want to use all FTS hits, lower if you want to exclude hits that are further away from the interaction point than the value

		GetXaxis()->SetTitle("#theta [rad]");
		GetYaxis()->SetTitle("x_{LP} [cm]");
	}
	else if ("parabolapz" == option)
	{
		fUseNonSkewedStraws = kTRUE;
		fUseSkewedStraws = kFALSE;

		// parabola for stations 3-5
		fOnlyUseHitsFromZ = 380.; // Set = 100. if you want to use all FTS hits, higher if you want to exclude hits that are closer to the interaction point than the value
		fOnlyUseHitsUpToZ = 630.; // Set = 1000. if you want to use all FTS hits, lower if you want to exclude hits that are further away from the interaction point than the value

		GetXaxis()->SetTitle("#theta [rad]");
		GetYaxis()->SetTitle("x_{LP} [cm]");
	}
	else if ("lineBehindDipole" == option)
	{
		// make sure hits are not shifted for line hough transform
		if (0!=fInterceptZx) {
			std::cout << "PndFtsHoughSpace: " << "fInterceptZx was set to " << fInterceptZx << " That is not correct for line HT of stations after dipole field!\n";
			std::cout << "Will set interceptZx to 0 for " << option << '\n';
			fInterceptZx = 0.;
		}

		fUseNonSkewedStraws = kTRUE;
		fUseSkewedStraws = kFALSE;

		// Line for stations 5+6
		fOnlyUseHitsFromZ = 550.; // Set = 100. if you want to use all FTS hits, higher if you want to exclude hits that are closer to the interaction point than the value
		fOnlyUseHitsUpToZ = 1000.; // Set = 1000. if you want to use all FTS hits, lower if you want to exclude hits that are further away from the interaction point than the value

		GetXaxis()->SetTitle("#theta [rad]");
		GetYaxis()->SetTitle("x [cm]");
	}
	else if ("lineZy" == option)
	{
		// make sure hits are not shifted for line hough transform
		if (0!=fInterceptZx) {
			std::cout << "PndFtsHoughSpace: " << "fInterceptZx was set to " << fInterceptZx << " That is not correct for line HT of all stations in zy plane!\n";
			std::cout << "Will set interceptZx to 0 for " << option << '\n';
			fInterceptZx = 0.;
		}

		fUseNonSkewedStraws = kFALSE;
		fUseSkewedStraws = kTRUE;

		// Line for all FTS stations
		fOnlyUseHitsFromZ = 100.; // Set = 100. if you want to use all FTS hits, higher if you want to exclude hits that are closer to the interaction point than the value
		fOnlyUseHitsUpToZ = 1000.; // Set = 1000. if you want to use all FTS hits, lower if you want to exclude hits that are further away from the interaction point than the value

		GetXaxis()->SetTitle("#theta [rad]");
		GetYaxis()->SetTitle("y [cm]");
	}
	else
	{
		throwError("in PndFtsHoughSpace! option " + option + " is not implemented!");
	}

	if (3<fVerbose)
	{
		std::cout << "HoughSpace parameters successfully set for option " << option << '\n';
		if (10<fVerbose)
		{
			std::cout << "fUseNonSkewedStraws " << fUseNonSkewedStraws << '\n';
			std::cout << "fUseSkewedStraws " << fUseSkewedStraws << '\n';
			std::cout << "fOnlyUseHitsFromZ " << fOnlyUseHitsFromZ << '\n';
			std::cout << "fOnlyUseHitsUpToZ " << fOnlyUseHitsUpToZ << '\n';

			std::cout << "fInterceptZx " << fInterceptZx << '\n';
		}
	}
	return kTRUE;
}



void PndFtsHoughSpace::filterInputHits()
{
	if (3<fVerbose) {
		std::cout << "All FTS hits in event " << fTrackerTask->GetNFtsHits() << "\n";
	}

	for (int iHit = 0; iHit < fTrackerTask->GetNFtsHits(); iHit++)
	{
		const PndFtsHit *const myHit = fTrackerTask->GetFtsHit(iHit);

		// Skip hits from skewed or non-skewed straws (if I wish not to use them)
		if (0 != myHit->GetSkewed())
		{
			// hit comes from skewed straw
			if (kFALSE == fUseSkewedStraws)
			{
				if (3<fVerbose) {std::cout << "Skipping hit with index " << iHit << ", because it comes from a skewed straw! LayerID = " << myHit->GetLayerID() << '\n';}
				continue;
			}

		} else {
			// hit comes from non-skewed straw
			if (kFALSE == fUseNonSkewedStraws)
			{
				if (3<fVerbose) {std::cout << "Skipping hit with index " << iHit << ", because it comes from a non-skewed straw! LayerID = " << myHit->GetLayerID() << '\n';}
				continue;
			}

		}



		// only hits with z component between fOnlyUseHitsFromZ and fOnlyUseHitsUpToZ will be used in the Hough transform
		const Double_t hitZLabSys = myHit->GetZ();
		if ( (hitZLabSys < fOnlyUseHitsFromZ) || (hitZLabSys > fOnlyUseHitsUpToZ) ){
			if (3<fVerbose) {std::cout << "Skipping hit with index " << iHit << " , because its z " << hitZLabSys << " is not in [" << fOnlyUseHitsFromZ << ", " << fOnlyUseHitsUpToZ << "]\n";}
			continue;
		}

		// add surviving hits to the hit vector (z coordinate as sorting parameter chosen)
		if (2<fVerbose) { std::cout << "Adding hit with index " << iHit << " to the Hough space hit vector.\n"; }
		AddHitToHS(iHit,hitZLabSys);

	} // for loop over all hits
	if (1<fVerbose) std::cout << GetNHits() << " hits in Hough space.\n";
}






Bool_t PndFtsHoughSpace::FillHoles(
		const Int_t lastBinX,
		const Int_t lastBinY,
		const Int_t currentBinY,
		IdxPath * ptrThetaYIdxPathVec
)
{
	// determine how many holes we have to fill
	const Int_t nHolesToFill = abs(currentBinY-lastBinY)-1;
	for (Int_t iCorrect = 1; iCorrect <= nHolesToFill; ++iCorrect)
	{
		const Int_t xCorrect = round(float(iCorrect)/float(nHolesToFill)); // gives 0 or 1
		Int_t yCorrect;
		if (currentBinY > lastBinY)
		{
			// we go up in the second value, therefore, we need to add to the yValue
			yCorrect = iCorrect;
		}
		else
		{
			yCorrect = -iCorrect;
		}


		const Int_t interpolateBinX = lastBinX+xCorrect;
		const Int_t interpolateBinY = lastBinY+yCorrect;

		const Int_t interpolatedGlobalBin = GetBin(interpolateBinX,interpolateBinY);
		AddBinContent(interpolatedGlobalBin);

		// save interpolated globalBin into path vector
		ptrThetaYIdxPathVec->push_back(interpolatedGlobalBin);

		if (8<fVerbose)
		{
			std::cout << "I am filling hole " << iCorrect << " of " << nHolesToFill << '\n';
			std::cout << "interpolatedGlobalBin = " << interpolatedGlobalBin << '\n';
			std::cout << "(lastBinX, lastBinY) = (" << lastBinX << ", " << lastBinY << ")" << '\n';
			std::cout << "(lastBinX+1, currentBinY)     = (" << lastBinX+1 << ", " << currentBinY << ")" << '\n';
			std::cout << "xCorrect = " << xCorrect << "  yCorrect = " << yCorrect << '\n';
			std::cout << "(interpolateBinX, interpolateBinY) = (" << interpolateBinX << ", " << interpolateBinY << ")" << '\n';
		}
	}// for iCorrect
	return kTRUE;
}



void PndFtsHoughSpace::FillHoughSpace()
{
	// make sure we have hits in the Hough space
	if (0==GetNHits()){
		if(1<fVerbose) Info("FillHoughSpace","No hits in Hough space.");
		return;
	}


	// make Hough space according to the Hough transform I want to do
	const TString option = GetName();


	// for storing the value to be calculated in Hough transform (yValue = offset for line, yValue = Q/p_{zx} for parabola)
	Double_t yVal = 0.;

	// store bin numbers for last and current entry (for making sure that there are no holes in the histogram)
	Int_t globalBin = 0;
	Int_t currentBinX = 0, currentBinY = 0, currentBinZ = 0;
	Int_t lastBinX = 0, lastBinY = 0;

	Bool_t firstEntry = kTRUE; // is used to indicate when holes in histogram have to be filled, set this to kTRUE for the first entry FOR EACH HIT



	// This produces the Hough space for a parabola or a line (with constant B field or with B field read from field maps)
	for (int iHit = 0; iHit < GetNHits(); iHit++)
	{
		firstEntry = kTRUE;
		const PndFtsHit *const myHit = getHitFromHS(iHit);


		// get hit position
		TVector3 hitPos = GetRawOrCalculatedHitPos(myHit);
		Double_t hitXLabSys = hitPos.X();
		Double_t hitYLabSys = hitPos.Y();
		Double_t hitZLabSys = hitPos.Z();
		Double_t hitXShifted = hitXLabSys - fInterceptZx; // shifts all x positions of hits so that they go through x=0 at z=zOffset (for parabola)
		Double_t hitZShifted = hitZLabSys - fZRefPos; // z coordinate in local coordinate system (for parabola and for line)



		if (1<fVerbose) {
			std::cout << "Doing " << option << " Hough transform for hit (hitZLabSys, hitXLabSys) = (" << hitZLabSys << ", " << hitXLabSys << ") cm";
			if (kTRUE == fKeepBConstant) { std::cout << " ignoring B field maps\n"; } else { std::cout << " reading B field maps\n"; }
		}

		// y component of B field
		Double_t By = getByFromBField(hitXLabSys, hitYLabSys, hitZLabSys);


		//------------------
		// theta scan
		//------------------

		// get indices for first and last bins on x-axis
		Int_t iThetaFirst  = fXaxis.GetFirst();
		Int_t iThetaLast   = fXaxis.GetLast();

		// save path for each hit
		IdxPath globalBinPathVec;

		// calculate Hough transform for hit iHit
		// for each hit a scan in theta is done by going through the x-axis of the Hough space from lower to higher values
		for (Int_t iTheta = iThetaFirst; iTheta < iThetaLast; ++iTheta)
		{
			// get theta value corresponding to iTheta
			Double_t thetaRad = fXaxis.GetBinCenter(iTheta); // theta has to be stored in rad
			if (9<fVerbose)	{ std::cout << " for thetaRad = " << thetaRad << '\n'; }

			// calculate yVal depending on which Hough transform we need
			if ("parabola" == option)
			{
				// Use shifted x and shifted z for parabola
				yVal = equationParabola(thetaRad, hitZShifted, hitXShifted, By);
				if (9<fVerbose)	{ std::cout << "Q/pzx = " << yVal; }
			}
			else if ("parabolapz" == option)
			{
				// Use shifted x and shifted z for parabola
				yVal = equationParabolaPz(thetaRad, hitZShifted, hitXShifted, By);
				if (9<fVerbose)	{ std::cout << "pz/Q = " << yVal; }
			}
			else if ( ("lineBeforeDipole" == option) || ("lineBehindDipole" == option) )
			{
				// Use real x and shifted z for line
				yVal = equationLineZxOrZy(thetaRad, hitZShifted, hitXLabSys);
				if (9<fVerbose) { std::cout << "xLP/PL = " << yVal; }
			}
			else if ("lineZy" == option)
			{
				// Use real x and shifted z for line
				yVal = equationLineZxOrZy(thetaRad, hitZShifted, hitYLabSys);
				if (9<fVerbose) { std::cout << "yLine = " << yVal; }
			}
			else
			{
				// should never happen
				throwError("in FillHoughSpace! option " + option + " is not implemented!");
			}





			// Insert "point" into Hough space and check in which bins we filled
			globalBin = Fill(thetaRad,yVal);
			if (5<fVerbose) { std::cout << "Hough point was filled into Hough space. globalbin = " << globalBin << " for option" << option <<'\n'; }
			// Find currentBinX(=iTheta) and currentBinY from globalBin
			GetBinXYZ(globalBin, currentBinX, currentBinY, currentBinZ);



			// if Fill was into a real bin (and not into over-/underflow) remove holes in Hough space (by assuming a straight line in between neighboring points)
			// for each theta 1 yVal is calculated, so holes will only appear in yVal, not in theta
			if (globalBin>=0) // -1 would mean over- or underflow
			{
				if (5<fVerbose) { std::cout << "OK! Hough point was NOT written to over- or underflow of histogram. Setting firstEntry to kFALSE now. "<< option <<'\n'; }

				// TODO Remove the following check for optimization, it should always be true
				if (currentBinX != iTheta){
					std::cerr << "\n\nError in FillHoughSpace! Hough point was filled into xBin " << currentBinX << " and not in " << iTheta << "\n";
					std::cerr << "iThetaFirst = " << iThetaLast << " iThetaLast = " << iThetaLast << "\n";
					std::cerr << "Over- or underflow on y-axis or FATAL error!\n\n\n";
				}

				// fill holes if the current Hough point is not the first entry in Hough space for the hit
				if (kFALSE == firstEntry)
				{
					if (5<fVerbose)
					{
						std::cout << "This is not the first point of the hit in the histogram. I will fix all holes which might be between this entry and the last one in the histogram"<<'\n';
					}

					FillHoles(lastBinX, lastBinY, currentBinY, &globalBinPathVec);

				} // if not first entry to be written into histogram
				else
				{
					//		++nHitsInHoughSpace; // count hits for making of Hough space (only once per hit)
					if (5<fVerbose) { std::cout << "This is the first point of the hit in the histogram. I will not try to fix any holes in the " << option << " histogram"<<'\n';}
				}


				firstEntry = kFALSE;

				// save calculated globalBin into path vector
				globalBinPathVec.push_back(globalBin);

			} // if NOT filled into over- or underflow
			else
			{
				if (9<fVerbose)	{ std::cout << "Watch out! Point was written to over- or underflow of histogram. firstEntry is set to kTRUE. "<< option <<'\n'; }
				firstEntry = kTRUE; // otherwise, algorithm connects first point which does not go into over-/underflow with (0,0)
			}

			if (9<fVerbose)	{ std::cout << "currentBinY = " << currentBinY << "  lastBinY = " << lastBinY << '\n'; }
			lastBinX = currentBinX;
			lastBinY = currentBinY;


		} // for theta
		//		std::cout << "map before iHit " << iHit << ": " << fHitThetaYIdxPath << '\n';
		// save the final path for the hit
		if ( 0 < globalBinPathVec.size() ){
			HitIdxPathPair hitPathPair( iHit, globalBinPathVec );
			fHitThetaYIdxPath.insert( hitPathPair );
		}
	} // for iHit
	if (1<fVerbose) std::cout << "map after all hits: " << fHitThetaYIdxPath << '\n';
	if (0 < fTrackerTask->GetSaveDebugInfo()) {
		WriteHistoOfHoughSpace();
		WriteHistoOfAllPaths();
		WriteHistoOfAllPathsForEachMcTruthTrack();
	}
}

TH2S PndFtsHoughSpace::MakeEmptyHistoOfSameDimensions(TString specifier, Int_t index) const {
	// for getting rid of warning about potential memory leak (same name for mutliple different histos)
	static Int_t histoCounter = 0;
	TString newname = GetName();
	newname += histoCounter;
	//	std::cout<<newname << '\n';
	++histoCounter;

	TString newTitle = GetName();
	if (""!=specifier){
		newTitle += " ";
		newTitle += specifier;
	}
	if (-1 < index){
		newTitle += " ";
		newTitle += index;
	}
	TH2S peaks(newname, newTitle, fXaxis.GetNbins(), fXaxis.GetXmin(),
			fXaxis.GetXmax(), fYaxis.GetNbins(), fYaxis.GetXmin(),
			fYaxis.GetXmax());
	return peaks;
}

void PndFtsHoughSpace::WriteHistoOfAllPeaks(const std::vector< PndFtsHoughSpacePeak >& peaksToPlot) const {
	// test if we need to do anything here
	Bool_t saveEachSeparately = !(fTrackerTask->GetSaveDebugInfo() % PndFtsHoughTrackerTask::kEachFoundPeakSeparately);
	Bool_t saveAllTogether = !(fTrackerTask->GetSaveDebugInfo() % PndFtsHoughTrackerTask::kAllFoundPeaksTogether);
	if ( (kTRUE == saveEachSeparately) && (kTRUE == saveAllTogether) )return;

	// write out histograms containing found peaks
	// filling all peaks in separate histos and all together in one histo
	TH2S allPeaks = MakeEmptyHistoOfSameDimensions("AllPeaks");
	for (UInt_t iPeak = 0; iPeak < peaksToPlot.size(); ++iPeak) {
		TH2S onePeak = MakeEmptyHistoOfSameDimensions("Peak", iPeak);
		const Double_t currHeight = peaksToPlot[iPeak].getHeight();
		const std::set<Int_t>& binsInPeak = peaksToPlot[iPeak].getBins();
		for (std::set<Int_t>::iterator itBin = binsInPeak.begin(); itBin != binsInPeak.end(); ++itBin) {
			onePeak.SetBinContent(*itBin, currHeight);
			allPeaks.SetBinContent(*itBin, currHeight);
		}
		TString outNameOne = GetDebugOutName(onePeak.GetTitle(), currHeight);
		if ( kTRUE == saveEachSeparately ) onePeak.SaveAs(outNameOne, "LEGO2");
	}
	TString outNameAll = GetDebugOutName(allPeaks.GetTitle());
	if ( kTRUE == saveAllTogether ) allPeaks.SaveAs(outNameAll, "LEGO2");
}

void PndFtsHoughSpace::WriteHistoOfAllPaths() const {
	// test if we need to do anything here
	Bool_t saveExclusively = !(fTrackerTask->GetSaveDebugInfo() % PndFtsHoughTrackerTask::kHitCurvesExclusively);
	Bool_t saveProjected = !(fTrackerTask->GetSaveDebugInfo() % PndFtsHoughTrackerTask::kHitCurvesProjected);
	if ( (kTRUE == saveExclusively) && (kTRUE == saveProjected) )return;

	// filling each path in separate histo
	for (HitIdxPathMap::const_iterator itPath = fHitThetaYIdxPath.begin(); itPath != fHitThetaYIdxPath.end(); ++itPath) {
		const Int_t currHit = itPath->first;
		TH2S onePathProjected = MakeEmptyHistoOfSameDimensions("PathProjected", currHit);
		TH2S onePathExclusively = MakeEmptyHistoOfSameDimensions("PathExclusively", currHit);
		const IdxPath& currPath = itPath->second;

		Int_t lastBinNumber = -1;
		for (Int_t iGlobalBin = 0; iGlobalBin < currPath.size(); ++iGlobalBin) {
			const Int_t currBinNumber = currPath[iGlobalBin];
			// warn if we accidently saved the same bin twice in the path
			if ( currBinNumber == lastBinNumber ) std::cerr << "FATAL! Bin " << currBinNumber << " twice in a row! in hit " << currHit << '\n';
			lastBinNumber = currBinNumber;

			if ( kTRUE == saveProjected){
				const Double_t currHeight = GetBinContent(currBinNumber); // get height of Hough space
				onePathProjected.SetBinContent(currBinNumber, currHeight);
			}

			if ( kTRUE == saveExclusively){
				Int_t currBinX = 0, currBinY = 0, notUsedBinZ = 0;
				GetBinXYZ(currBinNumber, currBinX, currBinY, notUsedBinZ); // Find currBinX and currBinY from globalBin
				const Double_t currXVal = GetXaxis()->GetBinCenter(currBinX);
				const Double_t currYVal = GetYaxis()->GetBinCenter(currBinY);
				onePathExclusively.Fill(currXVal,currYVal); // exclusive fill
			}
		}
		TString outNameProj = GetDebugOutName(onePathProjected.GetTitle());
		if ( kTRUE == saveProjected ) onePathProjected.SaveAs(outNameProj, "LEGO2");
		TString outNameExcl = GetDebugOutName(onePathExclusively.GetTitle());
		if ( kTRUE == saveExclusively ) onePathExclusively.SaveAs(outNameExcl, "LEGO2");
	}
}





void PndFtsHoughSpace::WriteHistoOfAllPathsForEachMcTruthTrack() const {
	// test if we need to do anything here
	Bool_t saveExclusively = !(fTrackerTask->GetSaveDebugInfo() % PndFtsHoughTrackerTask::kMcTruthPeaksExclusively);
	Bool_t saveProjected = !(fTrackerTask->GetSaveDebugInfo() % PndFtsHoughTrackerTask::kMcTruthPeaksProjected);
	if ( (kTRUE == saveExclusively) && (kTRUE == saveProjected) )return;


	// create map collecting histos of all hit indices from same Mc truth track
	std::map<Int_t, std::pair < TH2S, TH2S > > mcTruthIdHistos;

	for (HitIdxPathMap::const_iterator itPath = fHitThetaYIdxPath.begin(); itPath != fHitThetaYIdxPath.end(); ++itPath) {

		// figure out to which MC Truth track hit belongs
		const Int_t currHit = itPath->first;
		const Int_t hitId = getHitIdFromHS(currHit);
		const Int_t mcTruthId = fTrackerTask->getMcTruthIdForHitId(hitId);

		// if MC truth index not yet in map add new histo
		std::map<Int_t, std::pair < TH2S, TH2S > >::iterator itFind;
		itFind = mcTruthIdHistos.find(mcTruthId);
		if ( mcTruthIdHistos.end() == itFind ){// not found
			TH2S newHisto = MakeEmptyHistoOfSameDimensions("McTruthPeak projected", mcTruthId);
			TH2S newHisto2 = MakeEmptyHistoOfSameDimensions("McTruthPeak exclusive", mcTruthId);
			std::pair<TH2S, TH2S > histoPair( newHisto, newHisto2 );
			std::pair<Int_t, std::pair<TH2S, TH2S > > mcTruthIdHistosPair( mcTruthId, histoPair );
			mcTruthIdHistos.insert( mcTruthIdHistosPair );
			itFind = mcTruthIdHistos.find(mcTruthId); // now histos can be found in map
		}

		// Fill current path into correct histo (first as projection, second as if only hits from same mc truth track were filled)
		const IdxPath& currPath = itPath->second;
		for (Int_t iGlobalBin = 0; iGlobalBin < currPath.size(); ++iGlobalBin) {
			Int_t currBinNumber = currPath[iGlobalBin];
			std::pair<TH2S, TH2S >& histoPair = itFind->second;

			if ( kTRUE == saveProjected ){
				const Double_t currHeight = GetBinContent(currBinNumber); // get height of Hough space
				histoPair.first.SetBinContent(currBinNumber, currHeight); // projection
			}

			if ( kTRUE == saveExclusively ){
				Int_t currBinX = 0, currBinY = 0, notUsedBinZ = 0;
				GetBinXYZ(currBinNumber, currBinX, currBinY, notUsedBinZ); // Find currBinX and currBinY from globalBin
				const Double_t currXVal = GetXaxis()->GetBinCenter(currBinX);
				const Double_t currYVal = GetYaxis()->GetBinCenter(currBinY);
				histoPair.second.Fill(currXVal,currYVal); // exclusive fill
			}
		}
	}
	// loop over all histos that have been created
	for ( std::map<Int_t, std::pair<TH2S, TH2S > >::const_iterator itHisto = mcTruthIdHistos.begin(); itHisto != mcTruthIdHistos.end(); ++itHisto) {
		const std::pair<TH2S, TH2S >& histoPair = itHisto->second;
		TString outNameProjection = GetDebugOutName(histoPair.first.GetTitle());
		TString outNameExclusive = GetDebugOutName(histoPair.second.GetTitle());
		if ( kTRUE == saveProjected ) histoPair.first.SaveAs(outNameProjection, "LEGO2");
		if ( kTRUE == saveExclusively ) histoPair.second.SaveAs(outNameExclusive, "LEGO2");
	}
}



void PndFtsHoughSpace::WriteHistoOfHoughSpace() const{
	if (0 != fTrackerTask->GetSaveDebugInfo() % PndFtsHoughTrackerTask::kHoughSpaces) return;

	//	Int_t index = fHoughSpaces->GetEntriesFast();
	//	PndFtsHoughSpace* myHoughSpace = new ((*fHoughSpaces)[index])PndFtsHoughSpace(*houghSpace);
	TString title = GetTitle();
	title += " histo";
	TString outName = GetDebugOutName(title);
	SaveAs(outName, "LEGO2"); // resulting files need to have PndFtsHoughSpace replaced with TH2S
	// sed -i 's/PndFtsHoughSpace/TH2S/g' *.rtg


	//	fOutFile = FairRootManager::Instance()->GetOutFile();
	//	if (0==fOutFile)
	//	{
	//		std::cout << "WriteHistograms: Cannot get outfile.\n";
	//	}
	//	else
	//	{
	//		//			fOutFile->cd();
	//		//			fOutFile->cd("PndFtsHoughTrackerTask");
	//		if(3<fVerbose) std::cout << "WriteHistograms: Got outfile for debugging output.\n";
	//		if (0!=houghSpace)
	//		{
	//			TString histNameOld = houghSpace->GetName();
	//			TString histNameNew = houghSpace->GetName();
	//			histNameNew+=fEventNr;
	//			houghSpace->SetName(histNameNew);
	//			houghSpace->Write();
	//			houghSpace->SetName(histNameOld);
	//		}
	//		//			fOutFile->cd();
	//	}
}








void PndFtsHoughSpace::AddHitsToTrackletByCalculating(PndFtsHoughTracklet *currentTracklet, Int_t locmax){
	///////////////////////////////////////////
	// TODO this is messy, because the code is very similar to FillHoughSpace. Probably, I should find a way to merge it
	// add hits which are in the peak to the tracklet
	// 1 calculate the 2nd value for the next higher/lower theta bin of peak theta
	// 2 If peak 2nd value is within [min hit 2nd value - half width,  max hit 2nd value + half width] add the hit to the tracklet
	// 3 otherwise the hit is not in the peak

	// This is more complicated to check, but also true
	// A calculate the 2nd value for peak theta
	// B If hit 2nd value is within [peak 2nd value - peakSecondHw,  peak 2nd value + peakSecondHw] add the hit
	// C If hit 2nd value is < peak 2nd value - peakSecondHw, hit is in peak if the 2nd value of the next higher/lower theta bin is >= peak 2nd value
	// D If hit 2nd value is > peak 2nd value + peakSecondHw, hit is in peak if the 2nd value of the next lower/higher theta bin is <= peak 2nd value
	// E otherwise the hit is not in the peak

	const Int_t xfirst  = fXaxis.GetFirst();
	const Int_t xlast   = fXaxis.GetLast();
	const Int_t yfirst  = fYaxis.GetFirst();
	const Int_t ylast   = fYaxis.GetLast();

	// 1 calculate the 2nd value for the next higher/lower theta bin of peak theta
	UInt_t thetaBinLo = locmax-1;
	UInt_t thetaBinHi = locmax+1;
	// special case if we are at the edge of the Hough space
	if (thetaBinLo>xfirst) {
		thetaBinLo=xfirst;
	}
	if (thetaBinHi>xlast) {
		thetaBinHi=xlast;
	}

	// get theta values
	const Double_t thetaRadLo = fXaxis.GetBinCenter(thetaBinLo);
	const Double_t thetaRadHi = fXaxis.GetBinCenter(thetaBinHi);

	// for storing the values to be calculated in Hough transform (yValue = offset for line, yValue = Q/pzx for parabola)
	Double_t yValLo = 0.;
	Double_t yValHi = 0.;


	for (int iHit = 0; iHit < GetNHits(); iHit++)
	{
		const PndFtsHit* myHit = getHitFromHS(iHit);

		// get hit position
		const TVector3 hitPos = GetRawOrCalculatedHitPos(myHit);

		Double_t hitXLabSys = hitPos.X();
		Double_t hitYLabSys = hitPos.Y();
		Double_t hitZLabSys = hitPos.Z();
		Double_t hitXShifted = hitXLabSys - fInterceptZx; // shifts all x positions of hits so that they go through x=0 at z=zOffset (for parabola)
		Double_t hitZShifted = hitZLabSys - fZRefPos; // z coordinate in local coordinate system (for parabola and for line)

		// y component of B field
		Double_t By = getByFromBField(hitXLabSys, hitYLabSys, hitZLabSys);

		const TString option = GetName();
		if ("parabola" == option)
		{
			// Use shifted x and shifted z for parabola
			yValLo = equationParabola(thetaRadLo, hitZShifted, hitXShifted, By);
			yValHi = equationParabola(thetaRadHi, hitZShifted, hitXShifted, By);
			if (9<fVerbose)	{
				std::cout << "Q/pzx = " << yValLo << '\n';
				std::cout << "Q/pzx = " << yValHi << '\n';
			}
		}
		else if ("parabolapz" == option)
		{
			yValLo = equationParabolaPz(thetaRadLo, hitZShifted, hitXShifted, By);
			yValHi = equationParabolaPz(thetaRadHi, hitZShifted, hitXShifted, By);

			if (9<fVerbose)	{
				std::cout << "pz/Q = " << yValLo << '\n';
				std::cout << "pz/Q = " << yValHi << '\n';
			}
		}
		else if ( ("lineBeforeDipole" == option) || ("lineBehindDipole" == option) )
		{
			// Use real x and shifted z for line

			yValLo = equationLineZxOrZy(thetaRadLo, hitZShifted, hitXLabSys);
			yValHi = equationLineZxOrZy(thetaRadHi, hitZShifted, hitXLabSys);

			if (9<fVerbose) {
				std::cout << "xLP/PL = " << yValLo << '\n';
				std::cout << "xLP/PL = " << yValHi << '\n';
			}
		}
		else if ("lineZy" == option)
		{
			// Use real x and shifted z for line

			yValLo = equationLineZxOrZy(thetaRadLo, hitZShifted, hitYLabSys);
			yValHi = equationLineZxOrZy(thetaRadHi, hitZShifted, hitYLabSys);

			if (9<fVerbose) {
				std::cout << "xLP = " << yValLo << '\n';
				std::cout << "xLP = " << yValHi << '\n';
			}
		}
		else
		{
			throwError("Error in FillHoughSpace! option " + option + " is not implemented!");
		}


		// 2 If peak 2nd value is within [min hit 2nd value - half width,  max hit 2nd value + half width] add the hit to the tracklet
		const Double_t yMin = std::min(yValLo,yValHi);
		const Double_t yMax = std::max(yValLo,yValHi);
		const Double_t yMinHw = fYaxis.GetBinWidth(yMin)/2.;
		const Double_t yMaxHw = fYaxis.GetBinWidth(yMax)/2.;


		const Double_t peakSecondVal = currentTracklet->getSecondVal();
		if ( (yMin-yMinHw <= peakSecondVal) && (yMax+yMaxHw >= peakSecondVal) ){
			currentTracklet->AddHit(fFtsBranchId, getHitIdFromHS(iHit), hitZLabSys);
		}

	} // loop over hits
}




std::vector<PndFtsHoughTracklet> PndFtsHoughSpace::FindAllPeaksBinsWoMergingWithSearchWindow(const UInt_t minHeight, const Int_t vicinityLength)
{
	std::vector<PndFtsHoughTracklet> tracklets; // for output

	// check if Hough space has at least one entry
	if (1>GetEntries())
	{
		if(1<fVerbose) std::cout << "Hough Space is empty. No peak can be found. Return empty tracklet vector." << '\n';
		return tracklets;
	}


	// peak finder based on weighted means (only for 2D histograms implemented)


	// the following is an adaptation of the TH1::GetMaximumBin() code



	// define vicinityLength for search window which determines an effective height of the bin
	// set vicinityLength 0 for not using a search window
	// search window has size (2*vicinityLength+1)^2


	// move search window over histogram and select highest value
	Int_t locmax, locmay, locmaz; // location of maximum (x,y,z)

	//   -*-*-*-*-*Return location of bin with maximum value in the range*-*
	//             ======================================================
	Int_t binNumber, binx, biny, binz=0;
	// get values for first and last bins on each axis and take into account that we might want a search window (make sure search window stays within histogram)
	const Int_t xfirst  = fXaxis.GetFirst()+vicinityLength;
	const Int_t xlast   = fXaxis.GetLast()-vicinityLength;
	const Int_t yfirst  = fYaxis.GetFirst()+vicinityLength;
	const Int_t ylast   = fYaxis.GetLast()-vicinityLength;
	//		   Int_t zfirst  = fZaxis->GetFirst()+vicinityLength;
	//		   Int_t zlast   = fZaxis->GetLast()-vicinityLength;

	// find all peaks that have a height >= minHeight
	Double_t currentHeight;
	locmax = locmay = locmaz = 0;
	//		   for (binz=zfirst;binz<=zlast;binz++) {
	for (biny=yfirst;biny<=ylast;biny++) {
		for (binx=xfirst;binx<=xlast;binx++) {
			currentHeight = 0.;
			// iterate over vicinity
			for (Int_t xVicinityBin = -vicinityLength; xVicinityBin <= vicinityLength; ++xVicinityBin)
			{
				for (Int_t yVicinityBin = -vicinityLength; yVicinityBin <= vicinityLength; ++yVicinityBin)
				{
					binNumber = GetBin(binx+xVicinityBin,biny+yVicinityBin); //,binz);
					currentHeight += 1./(1.+std::max(abs(xVicinityBin),abs(yVicinityBin)))*GetBinContent(binNumber); // linear, unendlich norm
					//							currentHeight += 1./(1.+abs(xVicinity) +abs(yVicinity))*houghspace->GetBinContent(binNumber); // linear
					// currentHeight += 1./(1.+pow(max(xVicinity,yVicinity),2))*houghspace->GetBinContent(binNumber); // quadratic, unendlich norm
				}
			}
			if (currentHeight >= minHeight) {
				locmax  = binx;
				locmay  = biny;
				//		               locmaz  = binz;
				// get values corresponding to the peak
				Double_t peakThetaVal = fXaxis.GetBinCenter(locmax);
				Double_t peakSecondVal = fYaxis.GetBinCenter(locmay);

				Int_t binmaxglobal = Fill(peakThetaVal, peakSecondVal, 0); // returns binnumber without modifying the histogram
				Double_t peakThetaHw = fXaxis.GetBinWidth(peakThetaVal)/2.;
				Double_t peakSecondHw = fYaxis.GetBinWidth(peakSecondVal)/2.;

				// create tracklet and push it back to output
				PndFtsHoughTracklet currentTracklet(fZRefPos, fTrackerTask);
				currentTracklet.SetHoughTransformResults(peakThetaVal, peakSecondVal, currentHeight, peakThetaHw, peakSecondHw);

				// Add hits to tracklet
				AddHitsToTrackletByCalculating(&currentTracklet, locmax);

				tracklets.push_back(currentTracklet);
			} // height is big enough
		} // x loop
	} // y loop
	//		   } // z loop


	if (1<fVerbose) PrintFoundTracklets(tracklets);
	return tracklets;
}





std::vector<PndFtsHoughTracklet> PndFtsHoughSpace::FindAllPeaksScanPathsMergeBins(const UInt_t minHeight)
{
	std::vector<PndFtsHoughTracklet> tracklets; // for output

	// check if Hough space has at least one entry
	if (1>GetEntries())
	{
		if(1<fVerbose) std::cout << "Hough Space is empty. No peak can be found. Return empty tracklet vector." << '\n';
		return tracklets;
	}



	// this peak finder is only implemented for 2D histograms

	// for each hit scan the Hough space along the hit's path -> this reduces the 2d peak finding to nHits 1d problems plus peak merging.
	// require bin height >= minHeight
	// and that we are on a rising edge


	std::vector< PndFtsHoughSpacePeak > peaksForOneHit; // stores (possible) peaks for one hit
	std::vector< PndFtsHoughSpacePeak > mergedPeaksForAllHits; // stores merged peaks for all hits
	mergedPeaksForAllHits.clear();

	// hit loop
	for (HitIdxPathMap::const_iterator itMap = fHitThetaYIdxPath.begin(); itMap != fHitThetaYIdxPath.end(); ++itMap){
		const Int_t hitIdx = itMap->first;
		IdxPath path = itMap->second;
		peaksForOneHit.clear();

		// loop over bins along the path
		for (Int_t iGlobalBin = 0; iGlobalBin < path.size(); ++iGlobalBin){
			// current bin
			const Int_t currBinNumber = path[iGlobalBin];
			const Int_t currHeight = GetBinContent(currBinNumber);

			// check if we already found a peak candidate
			const Int_t nPeaksSoFar = peaksForOneHit.size();
			if ( 0 < nPeaksSoFar ){ // if we already have a peak, we might need to continue building it
				PndFtsHoughSpacePeak& currPeak = peaksForOneHit[nPeaksSoFar-1]; // get last peak
				if ( kFALSE == currPeak.isFinished() ){ // continue building up current peak
					if ( currHeight < currPeak.getHeight() ) currPeak.setFinished(kTRUE);
					if ( currHeight == currPeak.getHeight() ) currPeak.addBin(currBinNumber, hitIdx);
					if ( currHeight > currPeak.getHeight() ) currPeak.replaceBins(currHeight, currBinNumber, hitIdx);
					continue; // do not create a new peak as we were building an old one
				}
			} // we have already >= 1 peaks

			if ( minHeight <= currHeight ) { // Bin could belong to a peak


				// Make sure we are not on a falling edge by checking that the previous position was strictly lower!
				// previous bin
				const Int_t prevIdx = std::max(0, iGlobalBin-1); // make sure we stay within bounds of vector
				const Int_t prevBinNumber = path[prevIdx];
				const Int_t prevHeight = GetBinContent(prevBinNumber);

				Bool_t rising = prevHeight < currHeight;
				if ( 0 == iGlobalBin ) rising = kTRUE; // always save if we are at the beginning

				if ( kTRUE == rising ){ // we are on rising edge
					// Add a new peak (only if we do not continue a nonfinished existing one)
					// either we did not have any peaks or the last peak was already finished
					PndFtsHoughSpacePeak newPeak(currHeight, currBinNumber, hitIdx);
					peaksForOneHit.push_back(newPeak);
				}
			}


		}// loop over bins along the path

		// now merge peaksForOneHit with mergedPeaksForAllHits
		// loop over found peaks for latest hit and all merged hits, compare if they have overlaps, if so merge
		for (UInt_t iPeaksForOneHit = 0; iPeaksForOneHit < peaksForOneHit.size(); ++iPeaksForOneHit){
			Bool_t merged = kFALSE;
			for (UInt_t iPeaksForAllHits = 0; iPeaksForAllHits < mergedPeaksForAllHits.size(); ++iPeaksForAllHits){
				if ( mergedPeaksForAllHits[iPeaksForAllHits].binsOverlapWith(peaksForOneHit[iPeaksForOneHit]) ) {
					mergedPeaksForAllHits[iPeaksForAllHits].mergeWith(peaksForOneHit[iPeaksForOneHit]);
					merged = kTRUE;
					//continue; // skip checking with all other peaks as they cannot overlap anymore (TODO Check if that is true)
				}
			}
			if ( kFALSE==merged ) mergedPeaksForAllHits.push_back(peaksForOneHit[iPeaksForOneHit]); // add peaks which could not be merged with preexisting ones
		}
	}// hit loop

	if ( 0 < fTrackerTask->GetSaveDebugInfo() ) WriteHistoOfAllPeaks(mergedPeaksForAllHits);


	// Build up tracklets from peaks by going through vector of merged peaks
	for (UInt_t iPeaks = 0; iPeaks < mergedPeaksForAllHits.size(); ++iPeaks){
		const Double_t currentHeight = mergedPeaksForAllHits[iPeaks].getHeight();
		const std::set<Int_t>& binsInPeak = mergedPeaksForAllHits[iPeaks].getBins();
		// calculate center and halfwidth (HW) in theta and in secondVal for all global bins in peak
		// save min and max values
		Double_t minThetaVal = 0., maxThetaVal = 0., minSecondVal = 0., maxSecondVal = 0.;
		for (std::set< Int_t >::iterator itBin = binsInPeak.begin(); itBin != binsInPeak.end(); ++itBin){
			Int_t currentBinX = 0, currentBinY = 0, currentBinZ = 0;
			GetBinXYZ(*itBin, currentBinX, currentBinY, currentBinZ); // get bin numbers for x, y (and z) axis
			// get values for corresponding to global bin number
			const Double_t currThetaVal = fXaxis.GetBinCenter(currentBinX);
			const Double_t currSecondVal = fYaxis.GetBinCenter(currentBinY);

			if ( binsInPeak.begin() == itBin ){ // initialize values if this is the first bin
				minThetaVal = currThetaVal;
				maxThetaVal = currThetaVal;
				minSecondVal = currSecondVal;
				maxSecondVal = currSecondVal;
			} else { // save min and max
				minThetaVal = std::min(minThetaVal,currThetaVal);
				maxThetaVal = std::max(maxThetaVal,currThetaVal);
				minSecondVal = std::min(minSecondVal,currSecondVal);
				maxSecondVal = std::max(maxSecondVal,currSecondVal);
			}
		}

		const Double_t peakThetaVal = (maxThetaVal + minThetaVal)/2.;
		const Double_t peakSecondVal = (maxSecondVal + minSecondVal)/2.;
		const Double_t peakThetaHw = (maxThetaVal - minThetaVal)/2. + fXaxis.GetBinWidth(peakThetaVal)/2.;
		const Double_t peakSecondHw = (maxSecondVal - minSecondVal)/2. + fYaxis.GetBinWidth(peakSecondVal)/2.;



		// create tracklet, add hits and push it back to output
		PndFtsHoughTracklet currentTracklet(fZRefPos, fTrackerTask);
		currentTracklet.SetHoughTransformResults(peakThetaVal, peakSecondVal, currentHeight, peakThetaHw, peakSecondHw);
		// add hits to tracklet
		const std::set<Int_t>& hitIdsInPeak = mergedPeaksForAllHits[iPeaks].getHitIds();
		for (std::set< Int_t >::iterator itHitId= hitIdsInPeak.begin(); itHitId != hitIdsInPeak.end(); ++itHitId){
			const Int_t hitIndex = getHitIdFromHS(*itHitId);
			const PndFtsHit* myHit = getHitFromHS(*itHitId);
			// get hit position
			const TVector3 hitPos = GetRawOrCalculatedHitPos(myHit);
			Double_t hitZLabSys = hitPos.Z();
			currentTracklet.AddHit(fFtsBranchId, hitIndex, hitZLabSys);
		}
		tracklets.push_back(currentTracklet);
	}


	if (1<fVerbose) PrintFoundTracklets(tracklets);
	return tracklets;
}




std::vector<PndFtsHoughTracklet> PndFtsHoughSpace::FindAllPeaksScanPathsMergeBinsCalculatingPaths(const UInt_t minHeight)
{
	std::vector<PndFtsHoughTracklet> tracklets; // for output

	// check if Hough space has at least one entry
	if (1>GetEntries())
	{
		if(1<fVerbose) std::cout << "Hough Space is empty. No peak can be found. Return empty tracklet vector." << '\n';
		return tracklets;
	}


	// code goes here
	throwError("This peak finder is not fully implemented!");

	// peak finder only for 2D histograms implemented

	// the following is an adaptation of the TH1::GetMaximumBin() code


	// get values for first and last bins on each axis
	Int_t xFirstBin  = fXaxis.GetFirst();
	Int_t xLastBin   = fXaxis.GetLast();
	Int_t yFirstBin  = fYaxis.GetFirst();
	Int_t yLastBin   = fYaxis.GetLast();
	//		   Int_t zfirst  = fZaxis->GetFirst();
	//		   Int_t zlast   = fZaxis->GetLast();

	// find all peaks that have a height >= minHeight
	// go from low to high in theta <- SUPER IMPORTANT
	Int_t currBinNumber;
	Double_t currHeight;
	Int_t peakBinX = 0;
	Int_t peakBinY = 0;
	Int_t peakBinZ = 0; // location of maximum (x,y,z)

	for (Int_t currBinX=xFirstBin; currBinX<=xLastBin; ++currBinX) {
		for (Int_t currBinY=yFirstBin; currBinY<=yLastBin; ++currBinY) {
			//		   for (binz=zfirst;binz<=zlast;binz++) {

			currBinNumber = GetBin(currBinX,currBinY); // binz
			currHeight = GetBinContent(currBinNumber);
			if (minHeight > currHeight) {
				// currBinNumber is not high enough to be considered a peak
				SetBinContent(currBinNumber,0); // DEBUG: Uncomment this if you want to produce nice Hough space plots for demonstration
				continue;
			}
			// I found a potential peak, so I check the neighbors in next higher theta bins
			// I save the currently highest peak
			// I set the last visited peak = current peak and move to the next neighbor
			// if neighbor < highest peak and neighbor <= last peak, but above the minHeight, I set it to 0. I save its height as the last height.

			// if neighbor is higher than the last peak, I set the last visited bin to 0 and move on
			// if neighbor is the same height as last peak, I find the middle and set all others to 0 and I widen the errors of the middle bin

			Int_t iBinX = 0; // for moving to neighbors in x direction

			// save info of highest peak found while searching neighbors
			Int_t combinedPeakBinXLow=currBinX;  // saves in which x (thetaRad) bin a peak starts
			Int_t combinedPeakBinXHigh=currBinX; // saves in which x (thetaRad) bin a peak ends
			Double_t combinedPeakHeight=currHeight;

			// save info of last visited neighbor
			Int_t lastVisitedBinX = currBinX;
			Int_t lastVisitedBinNumber = currBinNumber;
			Double_t lastVisitedHeight = currHeight;

			// storing the current neighbor bin
			Int_t currNeighborBinX = currBinX;
			Int_t currNeighborBinNumber = currBinNumber;
			Double_t currNeighborHeight = currHeight;

			do{
				++iBinX; // to move along the x bins
				// set current neighbor
				currNeighborBinX = currBinX+iBinX;
				if (currNeighborBinX>xLastBin){
					break;
				}
				currNeighborBinNumber = GetBin(currNeighborBinX,currBinY); // binz
				currNeighborHeight = GetBinContent(currNeighborBinNumber);

				if (minHeight > currNeighborHeight){
					// end of peak region in x direction found, I can get out of the loop
					SetBinContent(currNeighborBinNumber,0); // DEBUG: Uncomment this if you want to produce nice Hough space plots for demonstration
					break;
				}

				if (currNeighborHeight > combinedPeakHeight){
					// actually peak starts with neighbor (or even later), but current bin is not in peak
					for (Int_t xBinToDel=combinedPeakBinXLow; xBinToDel<= combinedPeakBinXHigh; ++xBinToDel){
						Int_t BinNumberToDel = GetBin(xBinToDel,currBinY);
						SetBinContent(BinNumberToDel,0);
					}
					combinedPeakBinXLow = currNeighborBinX;
					combinedPeakBinXHigh = currNeighborBinX;
					combinedPeakHeight = currNeighborHeight;
				} else if (currNeighborHeight < combinedPeakHeight){
					if (currNeighborHeight <= lastVisitedHeight){
						// peak is dropping, delete current bin (which is at the falling edge of the peak)
						SetBinContent(currNeighborBinNumber,0);
					} else {
						// next peak starts rising, I can end the loop
						break;
					}
				} else if (currNeighborHeight == combinedPeakHeight){
					// peak is spread over several bins
					combinedPeakBinXHigh = currNeighborBinX;
				}
				lastVisitedBinX = currNeighborBinX;
				lastVisitedBinNumber = currNeighborBinNumber;
				lastVisitedHeight = currNeighborHeight;

			} while(kTRUE);


			// DEBUG: uncomment this if you want to produce nice Hough space plots for demonstration
			// remove the peak which was found, otherwise it (or parts of it) might be found again
			for (Int_t xBinToDel=combinedPeakBinXLow; xBinToDel<= combinedPeakBinXHigh; ++xBinToDel){
				Int_t BinNumberToDel = GetBin(xBinToDel,currBinY);
				SetBinContent(BinNumberToDel,0);
			}




			// peak is in the middle
			peakBinX  = (combinedPeakBinXHigh+combinedPeakBinXLow)/2; // if sum is not an even number, I have to correct for that later
			peakBinY  = currBinY;
			// locmaz  = binz;
			// get values corresponding to the peak
			Double_t peakThetaVal = fXaxis.GetBinCenter(peakBinX);
			Double_t peakSecondVal = fYaxis.GetBinCenter(peakBinY);

			// correct peak value if mean for peak cannot be divided by 2 without remainder
			if (0!=(combinedPeakBinXHigh+combinedPeakBinXLow)%2){
				peakThetaVal+=fXaxis.GetBinWidth(peakThetaVal)/2.;
			}

			// get full width of peak = (highest bin + halfwidth) - (lowest bin - halfwidth)
			Double_t combinedPeakThetaLowEdge = fXaxis.GetBinCenter(combinedPeakBinXLow) - fXaxis.GetBinWidth(combinedPeakBinXLow)/2.;
			Double_t combinedPeakThetaHighEdge = fXaxis.GetBinCenter(combinedPeakBinXHigh) + fXaxis.GetBinWidth(combinedPeakBinXHigh)/2.;

			Double_t peakThetaHw = (combinedPeakThetaHighEdge-combinedPeakThetaLowEdge)/2.;
			Double_t peakSecondHw = fYaxis.GetBinWidth(peakSecondVal)/2.;

			// create tracklet and push it back to output
			PndFtsHoughTracklet currentTracklet(fZRefPos, fTrackerTask);
			currentTracklet.SetHoughTransformResults(peakThetaVal, peakSecondVal, currHeight, peakThetaHw, peakSecondHw);

			///////////////////////////////////////////
			// TODO this is messy, because the code is very similar to FillHoughSpace. Probably, I should find a way to merge it
			// add hits which are in the peak to the tracklet
			// 1 calculate the 2nd value for the next higher/lower theta bin of combined peak theta
			// 2 If peak 2nd value is within [min hit 2nd value - half width,  max hit 2nd value + half width] add the hit to the tracklet
			// 3 otherwise the hit is not in the peak

			// This is more complicated to check, but also true
			// A calculate the 2nd value for peak theta
			// B If hit 2nd value is within [peak 2nd value - peakSecondHw,  peak 2nd value + peakSecondHw] add the hit
			// C If hit 2nd value is < peak 2nd value - peakSecondHw, hit is in peak if the 2nd value of the next higher/lower theta bin is >= peak 2nd value
			// D If hit 2nd value is > peak 2nd value + peakSecondHw, hit is in peak if the 2nd value of the next lower/higher theta bin is <= peak 2nd value
			// E otherwise the hit is not in the peak

			// 1 calculate the 2nd value for the next higher/lower theta bin of peak theta
			UInt_t thetaBinLo = combinedPeakBinXLow-1;
			UInt_t thetaBinHi = combinedPeakBinXHigh+1;
			// special case if we are at the edge of the Hough space
			if (thetaBinLo>xFirstBin) {
				thetaBinLo=xFirstBin;
			}
			if (thetaBinHi>xLastBin) {
				thetaBinHi=xLastBin;
			}

			// get theta values
			const Double_t thetaRadLo = fXaxis.GetBinCenter(thetaBinLo);
			const Double_t thetaRadHi = fXaxis.GetBinCenter(thetaBinHi);

			// for storing the values to be calculated in Hough transform (yValue = offset for line, yValue = Q/pzx for parabola)
			Double_t yValLo = 0.;
			Double_t yValHi = 0.;

			for (int iHit = 0; iHit < GetNHits(); iHit++)
			{
				const PndFtsHit* myHit = getHitFromHS(iHit);

				// get hit position
				const TVector3 hitPos = GetRawOrCalculatedHitPos(myHit);


				Double_t hitXLabSys = hitPos.X();
				Double_t hitYLabSys = hitPos.Y();
				Double_t hitZLabSys = hitPos.Z();
				Double_t hitXShifted = hitXLabSys - fInterceptZx; // shifts all x positions of hits so that they go through x=0 at z=zOffset (for parabola)
				Double_t hitZShifted = hitZLabSys - fZRefPos; // z coordinate in local coordinate system (for parabola and for line)

				// y component of B field
				Double_t By = getByFromBField(hitXLabSys, hitYLabSys, hitZLabSys);

				const TString option = GetName();
				if ("parabola" == option)
				{
					// Use shifted x and shifted z for parabola
					yValLo = equationParabola(thetaRadLo, hitZShifted, hitXShifted, By);
					yValHi = equationParabola(thetaRadHi, hitZShifted, hitXShifted, By);
					if (9<fVerbose)	{
						std::cout << "Q/pzx = " << yValLo << '\n';
						std::cout << "Q/pzx = " << yValHi << '\n';
					}
				}
				else if ("parabolapz" == option)
				{
					yValLo = equationParabolaPz(thetaRadLo, hitZShifted, hitXShifted, By);
					yValHi = equationParabolaPz(thetaRadHi, hitZShifted, hitXShifted, By);

					if (9<fVerbose)	{
						std::cout << "pz/Q = " << yValLo << '\n';
						std::cout << "pz/Q = " << yValHi << '\n';
					}
				}
				else if ( ("lineBeforeDipole" == option) || ("lineBehindDipole" == option) )
				{
					// Use real x and shifted z for line

					yValLo = equationLineZxOrZy(thetaRadLo, hitZShifted, hitXLabSys);
					yValHi = equationLineZxOrZy(thetaRadHi, hitZShifted, hitXLabSys);

					if (9<fVerbose) {
						std::cout << "xLP/PL = " << yValLo << '\n';
						std::cout << "xLP/PL = " << yValHi << '\n';
					}
				}
				else if ("lineZy" == option)
				{
					// Use real x and shifted z for line

					yValLo = equationLineZxOrZy(thetaRadLo, hitZShifted, hitYLabSys);
					yValHi = equationLineZxOrZy(thetaRadHi, hitZShifted, hitYLabSys);

					if (9<fVerbose) {
						std::cout << "xLP = " << yValLo << '\n';
						std::cout << "xLP = " << yValHi << '\n';
					}
				}
				else
				{
					throwError("Error in FillHoughSpace! option " + option + " is not implemented!");
				}


				// 2 If peak 2nd value is within [min hit 2nd value - half width,  max hit 2nd value + half width] add the hit to the tracklet
				const Double_t yMin = std::min(yValLo,yValHi);
				const Double_t yMax = std::max(yValLo,yValHi);
				const Double_t yMinHw = fYaxis.GetBinWidth(yMin)/2.;
				const Double_t yMaxHw = fYaxis.GetBinWidth(yMax)/2.;



				if ( (yMin-yMinHw <= peakSecondVal) && (yMax+yMaxHw >= peakSecondVal) ){
					Int_t hitIndex = fHitId.at(iHit).GetHitId();
					currentTracklet.AddHit(fFtsBranchId, hitIndex, hitZLabSys);
				}

			} // loop over hits
			/////////////////////////////////////////////////
			tracklets.push_back(currentTracklet);
			// } // z loop
		} // y loop
	} // x loop

	if (1<fVerbose) PrintFoundTracklets(tracklets);
	return tracklets;
}




std::vector<PndFtsHoughTracklet> PndFtsHoughSpace::FindAllPeaksWithTSpectrum2(const UInt_t minHeight)
{
	std::vector<PndFtsHoughTracklet> tracklets; // for output

	// check if Hough space has at least one entry
	if (1>GetEntries())
	{
		if(1<fVerbose) std::cout << "Hough Space is empty. No peak can be found. Return empty tracklet vector." << '\n';
		return tracklets;
	}


	// code goes here
	throwError("This peak finder is not fully implemented!");

	// TODO: This peakfinder should be rechecked!
	// TODO: This peakfinder does not assign hits to the tracklets (so far)
	Int_t maxpeaks = 20;
	// Finding the peaks (as in example macro)
	TSpectrum2 s(maxpeaks,3); // second argument: higher = peaks can be closer together (1 enforces 3 sigma seperation between peaks)
	s.SetAverageWindow(3); // standard is 3 (for Markov smoothing)
	s.SetDeconIterations(100); // standard is 3, more is better
	// 2nd parameter: sigma of searched peaks = 2 standard
	// 4th parameter: threshold: (default=0.05)  peaks with amplitude less than threshold*highest_peak are discarded.  0<threshold<1
	//		Int_t nfound = s.Search(houghspace, 2,"nobackground,nomarkov",0.5); // works ok for line and for parabola, kind of...
	Int_t nfound = s.Search(this, 2,"",0.5); // TODO: does this work for my derived class?!?
	Double_t *xpeaks = (Double_t*)s.GetPositionX();
	Double_t *ypeaks = (Double_t*)s.GetPositionY();
	s.Print();

	// for output
	for (UInt_t iPeak = 0; iPeak < nfound; ++iPeak){
		Double_t peakThetaVal = xpeaks[iPeak];
		Double_t peakSecondVal = ypeaks[iPeak];

		Int_t binmaxglobal = Fill(peakThetaVal, peakSecondVal, 0); // returns binnumber without modifying the histogram
		Double_t peakThetaHw = fXaxis.GetBinWidth(peakThetaVal)/2.;
		Double_t peakSecondHw = fYaxis.GetBinWidth(peakSecondVal)/2.;


		Double_t currentHeight = GetBinContent(binmaxglobal);

		// create tracklet and push it back to output
		PndFtsHoughTracklet currentTracklet(fZRefPos, fTrackerTask);
		currentTracklet.SetHoughTransformResults(peakThetaVal, peakSecondVal, currentHeight, peakThetaHw, peakSecondHw);
		tracklets.push_back(currentTracklet);
	}
	//		binmaxglobal = houghspace->Fill(peakTheta, peakSecond, 0); // returns binnumber without modifying the histogram



	if (1<fVerbose) PrintFoundTracklets(tracklets);
	return tracklets;
}



std::vector<PndFtsHoughTracklet> PndFtsHoughSpace::FindAllPeaksBlanko(const UInt_t minHeight)
{
	std::vector<PndFtsHoughTracklet> tracklets; // for output

	// check if Hough space has at least one entry
	if (1>GetEntries())
	{
		if(1<fVerbose) std::cout << "Hough Space is empty. No peak can be found. Return empty tracklet vector." << '\n';
		return tracklets;
	}


	// code goes here
	throwError("This peak finder is not implemented!");




	if (1<fVerbose) PrintFoundTracklets(tracklets);
	return tracklets;
}





// ----------------------
// Ideas for other peak finders
// ----------------------

// 1. allPeaks>minHeight, merge peaks
// find bin >= minHeight, then merge with neighboring bins of equal height, set surrounding bins to 0 (otherwise tail of peaks will be found in addition to peak itself)