/*
 * PndIsochroneTrackFinder.cxx
 *
 *  Created on: 10.10.2014
 *      Author: Stockmanns
 */

#include "PndIsochroneTrackFinder.h"

//#include "PndSttHit.h"
#include "PndSttTube.h"
#include "FairRootManager.h"

#include "TClonesArray.h"
#include "TH2D.h"
#include "TVector2.h"
#include "TMath.h"
#include "TGraph.h"
#include "TF1.h"

ClassImp(PndIsochroneTrackFinder);

void PndPeakContent::AddHits(std::set<Int_t> hits){
	fHitsInPeaks.insert(hits.begin(), hits.end());
}

void PndPeakContent::AddPeak(Int_t peak){
	fPeakBins.insert(peak);
}

void PndPeakContent::AddPeaks(std::set<Int_t> peaks){
	fPeakBins.insert(peaks.begin(), peaks.end());
}


void PndPeakContent::AddPeakContent(const PndPeakContent& peak){
	AddHits(peak.fHitsInPeaks);
	AddPeaks(peak.fPeakBins);
}


TVector2 PndPeakContent::FillMeanPeakPos(TH2* h2){
	Double_t meanX = 0;
	Double_t meanY = 0;
	Int_t nPeaks = 0;
	for (std::set<Int_t>::iterator peakIter = fPeakBins.begin(); peakIter != fPeakBins.end(); peakIter++){
		Int_t binX, binY, binZ;
		h2->GetBinXYZ(*peakIter, binX, binY, binZ);
		meanX += h2->GetXaxis()->GetBinCenter(binX);
		meanY += h2->GetYaxis()->GetBinCenter(binY);
		nPeaks++;
	}
	fCircleCenter.Set(meanX/nPeaks, meanY/nPeaks);
	return fCircleCenter;
}

std::pair<Double_t, Double_t> PndPeakContent::FillPeaksRangeRPhi(TH2* h2){
	Double_t maxR = -1;
	Double_t minR = 500000;
	Double_t maxPhi = -1;
	Double_t minPhi = 5000000;
	Int_t nPeaks = 0;
	for (std::set<Int_t>::iterator peakIter = fPeakBins.begin(); peakIter != fPeakBins.end(); peakIter++){
		Int_t binX, binY, binZ;
		h2->GetBinXYZ(*peakIter, binX, binY, binZ);
		TVector2 vec(h2->GetXaxis()->GetBinCenter(binX), h2->GetYaxis()->GetBinCenter(binY));
		if (vec.Mod() > maxR){
			maxR = vec.Mod();
		}
		if (vec.Mod() < minR){
			minR = vec.Mod();
		}
		if (vec.Phi_0_2pi(vec.Phi()) > maxPhi){
			maxPhi = vec.Phi_0_2pi(vec.Phi());
		}
		if (vec.Phi_0_2pi(vec.Phi()) < minPhi){
			minPhi = vec.Phi_0_2pi(vec.Phi());
		}
	}
	Double_t diffR = maxR - minR;
	Double_t diffPhi = maxPhi - minPhi;
	if (diffR == 0){
//		diffR = fCircleCenter.Mod() * 0.2;
		diffR = 2;
	}
	if (diffPhi == 0){
		diffPhi = TMath::Pi() / 9;
	}
	fRPhiRange = std::make_pair(diffR, diffPhi);
	return fRPhiRange;

}

PndIsochroneTrackFinder::PndIsochroneTrackFinder():
		fBField(2.0),
		fHoughHisto(0),
		fHitsToFit(0),
		fSttTubeArray(0),
		fSttSkewedArray(0),
		fClockwise(kFALSE),
		fAngleRange(0., 360.),
		fAngleSteps(1.),
		fPtRange(0., 15.),
		fPtStepSize(0.1),
		fMvdHitsPixelArray(0),
		fMvdHitsStripArray(0),
		fSttHitArray(0),
		fGemHitArray(0),
		fCutMergePeakDistance(20.0),
		fMvdHitsPixelBranchId(-1),
		fMvdHitsStripBranchId(-1),
		fSttHitBranchId(-1),
		fGemHitBranchId(-1)
{
	fHoughHisto = new TH2D("histo","histo", 4001, -1000, 1000, 4001, -1000, 1000);
	fWeightMap["STTSortedHits_event"] = 1;
	fWeightMap["MVDHitsPixel_event"] = 2;
	fWeightMap["MVDHitsStrip_event"] = 2;
	fWeightMap["GEMHit_event"] = 2;

}

PndIsochroneTrackFinder::~PndIsochroneTrackFinder() {
	// TODO Auto-generated destructor stub
}

void PndIsochroneTrackFinder::AddHits(TClonesArray* hits, Int_t branchId){
	if (FairRootManager::Instance()->GetBranchName(branchId).Contains("STT")){
		fSttHitArray = hits;
		fSttHitBranchId = branchId;
	} else if (FairRootManager::Instance()->GetBranchName(branchId).Contains("MVDHitsPixel")){
		fMvdHitsPixelArray = hits;
		fMvdHitsPixelBranchId = branchId;
	} else if (FairRootManager::Instance()->GetBranchName(branchId).Contains("MVDHitsStrip")){
		fMvdHitsStripArray = hits;
		fMvdHitsStripBranchId = branchId;
	} else if (FairRootManager::Instance()->GetBranchName(branchId).Contains("GEMHit")){
		fGemHitArray = hits;
		fGemHitBranchId = branchId;
	}
}

void PndIsochroneTrackFinder::AddHitsToFit(TClonesArray* hits, Int_t branchId)
{
	FairRootManager* ioman = FairRootManager::Instance();
	if (branchId == FairRootManager::Instance()->GetBranchId("STTHit")){
		std::cout << "PndIsochroneTrackFinder::AddHits size() " << hits->GetEntriesFast() << std::endl;
		for (int i = 0; i < hits->GetEntriesFast(); i++){
			PndSttHit* mySttHit = (PndSttHit*)hits->At(i);
			mySttHit->SetEntryNr(FairLink(-1, ioman->GetEntryNr(), branchId, i));  // old FairHits
			std::cout << i << " : " << mySttHit->GetX() << "/" << mySttHit->GetY() << "/" << mySttHit->GetZ() << std::endl;
			PndSttTube* myTube = (PndSttTube*) fSttTubeArray->At(mySttHit->GetTubeID());
			PndGeneralHoughHit myHoughHit(*mySttHit);
			myHoughHit.SetIsochroneRadius(mySttHit->GetIsochrone());
			if (myTube->IsSkew()){
				fSkewedHits.push_back(mySttHit);
				std::cout << "SkewedHit" << std::endl;
			}
			else {
				fHitsToFit.push_back(myHoughHit);
				std::cout << "PndIsochroneTrackFinder::AddHits SttHit at " << fHitsToFit.size() - 1 << " " << mySttHit->GetEntryNr() << std::endl;
			}
			std::cout << i << " : " << mySttHit->GetX() << "/" << mySttHit->GetY() << "/" << mySttHit->GetZ() << std::endl;
			fMapFairLinkHit[mySttHit->GetEntryNr()] = myHoughHit;
		}
	} else {
		for (int i = 0; i < hits->GetEntriesFast(); i++){
			FairHit* myFairHit = (FairHit*)hits->At(i);
			myFairHit->SetEntryNr(FairLink(-1, ioman->GetEntryNr(), branchId, i));  // old FairHits
			
//			std::cout << "PndIsochroneTrackFinder::AddHits EntryNr FairHit: " << myFairHit->GetEntryNr() << std::endl;
			PndGeneralHoughHit myHoughHit(*myFairHit);
			// std::cout << "PndIsochroneTrackFinder::AddHits EntryNr HoughHit: " << myHoughHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)myHoughHit << std::endl;
			myHoughHit.SetIsochroneRadius(0);
			fHitsToFit.push_back(myHoughHit);
			std::cout << "PndIsochroneTrackFinder::AddHits FairHit at "<< fHitsToFit.size() - 1 << " " << myFairHit->GetEntryNr() << std::endl;
			std::cout << i << " : " << myFairHit->GetX() << "/" << myFairHit->GetY() << "/" << myFairHit->GetZ() << std::endl;

			fMapFairLinkHit[myFairHit->GetEntryNr()] = myHoughHit;
		}
	}
}
std::vector<Double_t> PndIsochroneTrackFinder::GeneratePtValues() {
	std::vector<Double_t> returnVector;
	Double_t ptStart = fPtRange.first;
	Double_t ptEnd = fPtRange.second;
	for (Double_t i = ptStart; i < ptEnd; i += fPtStepSize) {
		returnVector.push_back(i);
	}
	return returnVector;
}

std::set<Double_t> PndIsochroneTrackFinder::GeneratePhiValues(PndGeneralHoughHit& myHit) {
	std::set<Double_t> phiValues;
	std::vector<Double_t> ptValues = GeneratePtValues();

	for (std::vector<Double_t>::iterator it = ptValues.begin(); it != ptValues.end(); it++) {
		std::vector<Double_t> phiVal = CalcPhiForPt(myHit, *it);
		phiValues.insert(phiVal.begin(), phiVal.end());
	}
	return phiValues;
}

std::vector<PndTrack> PndIsochroneTrackFinder::FindTracks() {
	FairRootManager* ioman = FairRootManager::Instance();

	if (fMvdHitsPixelArray != 0) AddHitsToFit(fMvdHitsPixelArray, fMvdHitsPixelBranchId);
	if (fMvdHitsStripArray != 0) AddHitsToFit(fMvdHitsStripArray, fMvdHitsStripBranchId);
	if (fSttHitArray != 0) AddHitsToFit(fSttHitArray, fSttHitBranchId);
	if (fGemHitArray != 0) AddHitsToFit(fGemHitArray, fGemHitBranchId);

	for (int i = 0; i < fHitsToFit.size(); i++){
		Double_t xVal(.0), yVal(.0), isoChrone(.0);
		PndGeneralHoughHit myHit = fHitsToFit.at(i);
		std::cout << "PndIsochroneTrackFinder::FindTracks() myHit: " << i << " : " << myHit.GetEntryNr() << std::endl;

		xVal = myHit.GetX();
		yVal = myHit.GetY();
		isoChrone = myHit.GetIsochroneRadius();

		std::cout << "Processing Hit " << i << std::endl;
		//std::set<Double_t> phiValues = GeneratePhiValues(myHit);
		//std::vector<TVector2> circleCenters = GenerateHoughValuesPhi(myHit, phiValues);
		CalcHoughHist(myHit, i, kTRUE);

		if (myHit.GetIsochroneRadius() > 0) {
			CalcHoughHist(myHit, i, kFALSE);
		}
	}
	std::multimap<Int_t, Int_t> peakBins = FindPeaks(fBinsWithContent, fHoughHisto, 2);
	std::cout << "PeakFinder: " << std::endl;
	std::vector<PndTrack> result = PeaksToTracks(peakBins, 2);
	return result;
}


//std::vector<PndTrack> PndIsochroneTrackFinder::FindTracks() {
//	FairRootManager* ioman = FairRootManager::Instance();
//
//	if (fMvdHitsPixelArray != 0) AddHitsToFit(fMvdHitsPixelArray, ioman->GetBranchId("MVDHitsPixel"));
//	if (fMvdHitsStripArray != 0) AddHitsToFit(fMvdHitsStripArray, ioman->GetBranchId("MVDHitsStrip"));
//	if (fSttHitArray != 0) AddHitsToFit(fSttHitArray, ioman->GetBranchId("STTHit"));
//	if (fGemHitArray != 0) AddHitsToFit(fGemHitArray, ioman->GetBranchId("GEMHit"));
//
//	for (int i = 0; i < fHitsToFit.size(); i++){
//		Double_t xVal(.0), yVal(.0), isoChrone(.0);
//		PndGeneralHoughHit myHit = fHitsToFit.at(i);
//		std::cout << "PndIsochroneTrackFinder::FindTracks() myHit: " << i << " : " << myHit.GetEntryNr() << std::endl;
//
//		xVal = myHit.GetX();
//		yVal = myHit.GetY();
//		isoChrone = myHit.GetIsochroneRadius();
//
//		std::cout << "Processing Hit " << i << std::endl;
//		std::set<Double_t> phiValues = GeneratePhiValues(myHit);
//		std::vector<TVector2> circleCenters = GenerateHoughValuesPhi(myHit, phiValues);
//		Double_t hitWeight = fWeightMap[FairRootManager::Instance()->GetBranchName(myHit.GetEntryNr().GetType())];
//		FillHoughHist(circleCenters, i, hitWeight);
//	}
//	std::multimap<Int_t, Int_t> peakBins = FindPeaks(fBinsWithContent, fHoughHisto, 4);
//	std::cout << "PeakFinder: " << std::endl;
//	std::vector<PndTrack> result = PeaksToTracks(peakBins);
//	return result;
//}


std::vector<PndTrack>  PndIsochroneTrackFinder::FindMvdBasedTracks(){
	FairRootManager* ioman = FairRootManager::Instance();
	if (fMvdHitsPixelArray != 0) AddHitsToFit(fMvdHitsPixelArray, fMvdHitsPixelBranchId);
	if (fMvdHitsStripArray != 0)AddHitsToFit(fMvdHitsStripArray, fMvdHitsStripBranchId);
	fHitsToFitAfterMvd = fHitsToFit.size();

	for (int i = 0; i < fHitsToFit.size(); i++){
		Double_t xVal(.0), yVal(.0), isoChrone(.0);
		PndGeneralHoughHit myHit = fHitsToFit.at(i);
		std::cout << "PndIsochroneTrackFinder::FindTracks() myHit: " << i << " : " << myHit.GetEntryNr() << std::endl;

		xVal = myHit.GetX();
		yVal = myHit.GetY();
		isoChrone = myHit.GetIsochroneRadius();

		std::cout << "Processing Hit " << i << std::endl;
		CalcHoughHist(myHit, i , kTRUE);

	}
	std::multimap<Int_t, Int_t> peakBins = FindPeaks(fBinsWithContent, fHoughHisto, 2);
	std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks nPeaks " << peakBins.size() << std::endl;
	for (std::multimap<Int_t, Int_t>::reverse_iterator peakIter = peakBins.rbegin(); peakIter != peakBins.rend(); ++peakIter) {
			Int_t binX, binY, binZ;
			fHoughHisto->GetBinXYZ(peakIter->second, binX, binY, binZ);
			TVector2 circle(fHoughHisto->GetXaxis()->GetBinCenter(binX), fHoughHisto->GetYaxis()->GetBinCenter(binY));
			std::set<Int_t> hitsInBin = fBinToHit[peakIter->second];
			std::cout << peakIter->first << " Hits in Peak: /";
			for (std::set<Int_t>::iterator hitIter = hitsInBin.begin(); hitIter != hitsInBin.end(); hitIter++){
				std::cout << *hitIter << "/";
			}
			std::cout << " circleCenter: " << circle.X() << "/" << circle.Y() << std::endl;
	}
	std::vector<PndPeakContent> mergedPeaks = MergePeaks(peakBins, 0.9, 2, kTRUE);
	std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks mergedPeaks: " << mergedPeaks.size() << std::endl;
	for (int count = 0; count < mergedPeaks.size(); count++){
		fPeakCoordinatesMvd.push_back(mergedPeaks[count].fCircleCenter);
		std::cout << count << " : " << mergedPeaks[count];
		std::cout << std::endl;

	}
	std::vector<PndCircleTime> circleTimes;
//	for (std::multimap<Int_t, Int_t>::reverse_iterator peakIter = peakBins.rbegin(); peakIter != peakBins.rend(); ++peakIter) {
//		PndTrackCand myTrackCand = FillTrackCand(peakIter->second);
//		if (myTrackCand.GetNHits() > 2)
//			circleTimes.push_back(FindCircleTime(peakIter->second));
//	}
//	std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: nCircleTimes " << circleTimes.size() << std::endl;

	if (fSttHitArray != 0) AddHitsToFit(fSttHitArray, fSttHitBranchId);
	if (fGemHitArray != 0) AddHitsToFit(fGemHitArray, fGemHitBranchId);
	fHitsUsed.clear();
	TVector2 zeroPos(0,0);
//	std::vector<Double_t> phiVal = CalcPhiForCircleTimes(zeroPos, circleTimes);
	std::vector<Double_t> phiVal = CalcPhiForPeakContents(zeroPos, mergedPeaks);
	for (int i = fHitsToFitAfterMvd; i < fHitsToFit.size(); i++){
			Double_t xVal(.0), yVal(.0), isoChrone(.0);
			PndGeneralHoughHit myHit = fHitsToFit.at(i);
			std::cout << "PndIsochroneTrackFinder::FindTracks() myHit: " << i << " : " << myHit.GetEntryNr() << std::endl;

			xVal = myHit.GetX();
			yVal = myHit.GetY();
			isoChrone = myHit.GetIsochroneRadius();

			std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: Processing Hit " << i << std::endl;

			std::cout << myHit.GetX() << "/" << myHit.GetY() << " r " << myHit.GetIsochroneRadius() << " angle: " << TMath::RadToDeg() * TMath::ATan(myHit.GetY()/myHit.GetX());
//			for (int j = 0; j < phiVal.size(); j++){
//				std::cout << " phi" << j << " : " << TMath::RadToDeg() * phiVal[j];
//			}
//			std::cout << std::endl;
		//	std::cout << IsoCalc(xVal, yVal, isoChrone, phiVal.first).Mod() << " 2: " << IsoCalc(xVal, yVal, isoChrone, phiVal.second).Mod() << std::endl;
			if (myHit.GetEntryNr().GetType() == FairRootManager::Instance()->GetBranchId("STTHit")){
				if (myHit.GetZ() > 35.5 || myHit.GetZ() < 34.5){
					continue;
				}
			}
//			std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: phiVal.size() " << phiVal.size() << std::endl;
			std::vector<TVector2> circleCenters;
			std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: MergedPeaks: size " << mergedPeaks.size() << std::endl;
			for (Int_t circleIndex = 0; circleIndex < mergedPeaks.size(); circleIndex++){
				Double_t lowerBound = mergedPeaks[circleIndex].fCircleCenter.Mod() - mergedPeaks[circleIndex].GetPeaksRangeRPhi().first/2;
				Double_t upperBound = mergedPeaks[circleIndex].fCircleCenter.Mod() + mergedPeaks[circleIndex].GetPeaksRangeRPhi().first/2;
				std::vector<TVector2> matchingCircleCenters;
				std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: LowerBound: " << lowerBound << " UpperBound: " << upperBound << std::endl;
				for (Double_t radius = lowerBound; radius < upperBound; radius += 1){
					std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: Radius: " << radius << std::endl;
					std::vector<TVector2> circleCenter = CalcCircleIntersections(zeroPos, radius, myHit.GetXYVector(), radius + myHit.GetIsochroneRadius());
					for (Int_t circleCenterIndex = 0; circleCenterIndex < circleCenter.size(); circleCenterIndex++){
						TVector2 vec = circleCenter[circleCenterIndex];
	//					std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: CircleCenter " << circleIndex << " : " << vec.X() << "/" << vec.Y() << " Phi: " << vec.Phi_0_2pi(vec.Phi()) * TMath::RadToDeg() << std::endl;
	//					std::cout << "PhiRange: " << 2 * circleIndex << " :> " << phiVal[2 * circleIndex] * TMath::RadToDeg()
	//						<< " " << 2 * circleIndex + 1  << " :< " << phiVal[2 * circleIndex + 1] *TMath::RadToDeg() << std::endl;
						if (vec.Phi_0_2pi(vec.Phi()) > phiVal[2 * circleIndex] && vec.Phi_0_2pi(vec.Phi()) < phiVal[2 * circleIndex + 1]){
	//						std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: Accepted!" << std::endl;
							circleCenters.push_back(vec);
						}
					}
					circleCenter = CalcCircleIntersections(zeroPos, radius, myHit.GetXYVector(), radius - myHit.GetIsochroneRadius());
					for (Int_t circleCenterIndex = 0; circleCenterIndex < circleCenter.size(); circleCenterIndex++){
						TVector2 vec = circleCenter[circleCenterIndex];
						std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: CircleCenter " << circleIndex << " : " << vec.X() << "/" << vec.Y() << " Phi: " << vec.Phi_0_2pi(vec.Phi()) * TMath::RadToDeg() << std::endl;
		//				std::cout << "PhiRange: " << 2 * circleIndex << " :> " << phiVal[2 * circleIndex] * TMath::RadToDeg()
		//					<< " " << 2 * circleIndex + 1  << " :< " << phiVal[2 * circleIndex + 1] *TMath::RadToDeg() << std::endl;
						if (vec.Phi_0_2pi(vec.Phi()) > phiVal[2 * circleIndex] && vec.Phi_0_2pi(vec.Phi()) < phiVal[2 * circleIndex + 1]){
		//					std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks: Accepted!" << std::endl;
							circleCenters.push_back(vec);
						}
					}
				}
			}
			Double_t hitWeight = fWeightMap[FairRootManager::Instance()->GetBranchName(myHit.GetEntryNr().GetType())];
			FillHoughHist(circleCenters, i, hitWeight);
		}
		peakBins = FindPeaks(fBinsWithContent, fHoughHisto, 6);
		std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks MergeStripPeaks" << std::endl;
		mergedPeaks = MergePeaks(peakBins, 0.8, 6, kFALSE);
		std::cout << "PeakFinder: " << std::endl;
		std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks MergedStripPeaks after general merge: " << mergedPeaks.size() << std::endl;
		for (int count = 0; count < mergedPeaks.size(); count++){
			std::cout << count << " : " << mergedPeaks[count];
			std::set<Int_t> hitsInPeak =  mergedPeaks[count].fHitsInPeaks;
			for (std::set<Int_t>::iterator hitIter = hitsInPeak.begin(); hitIter != hitsInPeak.end(); hitIter++){
				std::cout << fHitsToFit[*hitIter].GetEntryNr() << "/";
			}
			std::cout << std::endl << std::endl;

		}

		mergedPeaks = MergePeaksByPos(mergedPeaks, 4);
		std::cout << "PndIsochroneTrackFinder::FindMvdBasedTracks MergedStripPeaks after merge by pos: " << mergedPeaks.size() << std::endl;

		for (int count = 0; count < mergedPeaks.size(); count++){
			std::cout << count << " : " << mergedPeaks[count];
			std::set<Int_t> hitsInPeak =  mergedPeaks[count].fHitsInPeaks;
			for (std::set<Int_t>::iterator hitIter = hitsInPeak.begin(); hitIter != hitsInPeak.end(); hitIter++){
				std::cout << fHitsToFit[*hitIter].GetEntryNr() << "/";
			}
			std::cout << std::endl << std::endl;

		}
		std::vector<PndTrack> result = PeaksToTracks(mergedPeaks, 6);
		return result;

}

void PndIsochroneTrackFinder::CalcHoughHist(PndGeneralHoughHit& hit, Int_t hitIndex, Bool_t plusIsochrone)
{
	std::vector<TVector2> upperPart, lowerPart;
	for (Int_t ptIndex = 0; ptIndex < 5000; ptIndex++){
		TVector2 zeroPos(0,0);
		Double_t pt = ptIndex;
		Double_t ptIso;
		if (plusIsochrone)
			ptIso = pt + hit.GetIsochroneRadius();
		else
			ptIso = pt - hit.GetIsochroneRadius();
		std::vector<TVector2> circles = CalcCircleIntersections(zeroPos, pt, hit.GetXYVector(), ptIso);
		if (circles.size() == 1){
			upperPart.push_back(circles[0]);
			lowerPart.push_back(circles[0]);
		} else if (circles.size() == 2){
			upperPart.push_back(circles[0]);
			lowerPart.push_back(circles[1]);
		}
	}
	Double_t hitWeight = fWeightMap[FairRootManager::Instance()->GetBranchName(hit.GetEntryNr().GetType())];
	FillHoughHist(upperPart, hitIndex, hitWeight);
	FillHoughHist(lowerPart, hitIndex, hitWeight);
}


PndCircleTime PndIsochroneTrackFinder::FindCircleTime(Int_t peakBin){

	Int_t binX, binY, binZ;
	fHoughHisto->GetBinXYZ(peakBin, binX, binY, binZ);
	PndCircleTime circleTime;
	circleTime.fCircle.Set(fHoughHisto->GetXaxis()->GetBinCenter(binX), fHoughHisto->GetYaxis()->GetBinCenter(binY));
	circleTime.fTime = CalcTimeStampInBin(peakBin);
	return circleTime;
}

Double_t PndIsochroneTrackFinder::CalcTimeStampInBin(Int_t bin){
	std::set<Int_t> hitsInBin = fBinToHit[bin];
	Int_t nHits = hitsInBin.size();
	Double_t timeStamp = 0;
	for (std::set<Int_t>::iterator iter = hitsInBin.begin(); iter!= hitsInBin.end(); iter++){
		PndGeneralHoughHit hit = fHitsToFit[*iter];
		timeStamp += hit.GetTimeStamp();
	}
	return timeStamp/nHits;
}

std::vector<Double_t> PndIsochroneTrackFinder::CalcPhiForCircleTimes(TVector2& hit, std::vector<PndCircleTime>& circleTimes)
{
	std::vector<Double_t> result;
	for (Int_t i = 0; i < circleTimes.size(); i++){
		if (TMath::Abs((hit - circleTimes[i].fCircle).Mod() - circleTimes[i].fCircle.Mod()) > circleTimes[i].fCircle.Mod() * 0.1){
			continue;
		} else {
			std::vector<TVector2> tangPoints = CalcCircleIntersections(hit, (hit - circleTimes[i].fCircle).Mod(), circleTimes[i].fCircle, circleTimes[i].fCircle.Mod() * 0.1); //todo GetIsochroneRadius has to replaced by calculated isochroneRadius
			if (tangPoints.size() == 2){
				Double_t phi1 = tangPoints[0].Phi_0_2pi((tangPoints[0] - hit).Phi());
				Double_t phi2 = tangPoints[0].Phi_0_2pi((tangPoints[1] - hit).Phi());
				Double_t phiDiff = phi2 - phi1;
				if (phiDiff < 0) phiDiff += 2 * TMath::Pi();
				std::cout << "PndIsochroneTrackFinder::CalcPhiForCircleTimes " << circleTimes[i].fCircle.X()
						<< "/" << circleTimes[i].fCircle.Y() << " phi1: " << phi1 * TMath::RadToDeg() << " phi2 " << phi2 * TMath::RadToDeg() << std::endl;

				if (phiDiff < TMath::Pi()){
					result.push_back(phi1);
					result.push_back(phi2);
				} else {
					result.push_back(phi2);
					result.push_back(phi1);
				}
			}
		}
	}
	return result;
}

std::vector<Double_t> PndIsochroneTrackFinder::CalcPhiForPeakContents(TVector2& hit, std::vector<PndPeakContent>& peakContents)
{
	std::vector<Double_t> result;
	for (Int_t i = 0; i < peakContents.size(); i++){
		if (TMath::Abs((hit - peakContents[i].fCircleCenter).Mod() - peakContents[i].fCircleCenter.Mod()) > peakContents[i].GetPeaksRangeRPhi().first/2){
			continue;
		} else {
			std::vector<TVector2> tangPoints = CalcCircleIntersections(hit, (hit - peakContents[i].fCircleCenter).Mod(), peakContents[i].fCircleCenter, peakContents[i].GetPeaksRangeRPhi().first/2); //todo GetIsochroneRadius has to replaced by calculated isochroneRadius
			if (tangPoints.size() == 2){
				Double_t phi1 = tangPoints[0].Phi_0_2pi((tangPoints[0] - hit).Phi());
				Double_t phi2 = tangPoints[0].Phi_0_2pi((tangPoints[1] - hit).Phi());
				Double_t phiDiff = phi2 - phi1;
				if (phiDiff < 0) phiDiff += 2 * TMath::Pi();
				std::cout << "PndIsochroneTrackFinder::CalcPhiForpeakContents " << peakContents[i].fCircleCenter.X()
						<< "/" << peakContents[i].fCircleCenter.Y() << " phi1: " << phi1 * TMath::RadToDeg() << " phi2 " << phi2 * TMath::RadToDeg() << std::endl;

				if (phiDiff < TMath::Pi()){
					result.push_back(phi1);
					result.push_back(phi2);
				} else {
					result.push_back(phi2);
					result.push_back(phi1);
				}
			}
		}
	}
	return result;
}


std::vector<TVector2> PndIsochroneTrackFinder::CalcCircleIntersections(TVector2 circleCenter1, Double_t radius1, TVector2 circleCenter2, Double_t radius2)
{
	std::vector<TVector2> result;
	if ((circleCenter1-circleCenter2).Mod() > radius1 + radius2)
		return result;
	Double_t c = (circleCenter1.X()*circleCenter1.X() - circleCenter2.X()*circleCenter2.X() + circleCenter1.Y()*circleCenter1.Y() - circleCenter2.Y()*circleCenter2.Y() - radius1*radius1 + radius2*radius2)/(-2 * (circleCenter2.X() - circleCenter1.X()));
	Double_t d = (circleCenter2.Y() - circleCenter1.Y()) / (circleCenter2.X() - circleCenter1.X());
	Double_t p_half = -1*((c - circleCenter1.X())* d + circleCenter1.Y())/((d*d+1));
	Double_t q = (TMath::Power((c - circleCenter1.X()),2) + circleCenter1.Y()*circleCenter1.Y() - radius1*radius1)/(d*d + 1);
	if ((p_half)*(p_half) - q < 0){
		return result;
	} else {
		Double_t y1 = -p_half + TMath::Sqrt((p_half)*(p_half) - q);
		Double_t y2 = -p_half - TMath::Sqrt((p_half)*(p_half) - q);
		Double_t x1 = c - d * y1;
		Double_t x2 = c - d * y2;
		TVector2 v1(x1, y1);
		TVector2 v2(x2, y2);
		result.push_back(v1);
		result.push_back(v2);
//		std::cout << "PndIsochroneTrackFinder::CalcCircleIntersections circles: " << circleCenter1.X() << "/" << circleCenter1.Y() << "/" << radius1 << " "
//				<< circleCenter2.X() << "/" << circleCenter2.Y() << "/" << radius2 << " intersections: " << v1.X() << "/" << v1.Y() << " "  << v2.X() << "/" << v2.Y()
//				<< " c: " << c << " d " << d << std::endl;
		return result;
	}
}

std::vector<Double_t> PndIsochroneTrackFinder::CalcPhiForPt(PndGeneralHoughHit& hit, Double_t pt)
{
	Double_t s = PtToR(pt) + hit.GetIsochroneRadius();
	Double_t t1 = (hit.GetIsochroneRadius()*hit.GetIsochroneRadius() - hit.GetX() * hit.GetX() - hit.GetY() * hit.GetY() - 2 * s * hit.GetIsochroneRadius())/(2*s*hit.GetXYVector().Mod());
	Double_t t2 = (hit.GetIsochroneRadius()*hit.GetIsochroneRadius() - hit.GetX() * hit.GetX() - hit.GetY() * hit.GetY() + 2 * s * hit.GetIsochroneRadius())/(-2*s*hit.GetXYVector().Mod());

	std::vector<Double_t> result;
	std::pair<Double_t, Double_t> firstPair = CalcPhiForT(t1, hit.GetXYVector());
	result.push_back(firstPair.second);
	result.push_back(firstPair.first);

	if (hit.GetIsochroneRadius() > 0){
		std::pair<Double_t, Double_t> secondPair = CalcPhiForT(t2, hit.GetXYVector());
		result.push_back(secondPair.first);
		result.push_back(secondPair.second);
	}

	return result;
}

std::pair<Double_t, Double_t> PndIsochroneTrackFinder::CalcPhiForT(Double_t t, TVector2 hit)
{
	Double_t phi0 = TMath::ASin(t) - TMath::ATan(hit.X()/hit.Y());
	Double_t phi1 = TMath::ACos(t) + TMath::ATan(hit.Y()/hit.X());
	if (hit.Y() < 0)
		phi0 += TMath::Pi();
	if (hit.X() < 0)
		phi1 += TMath::Pi();
	if (phi0 > 2 * TMath::Pi())
		phi0 -= 2 * TMath::Pi();
	if (phi1 > 2 * TMath::Pi())
		phi1 -= 2 * TMath::Pi();
	if (phi0 < 0){
		phi0 += 2 * TMath::Pi();
	}
	if (phi1 < 0){
		phi1 += 2 * TMath::Pi();
	}

	return std::make_pair(phi0, phi1);
}

std::vector<TVector2> PndIsochroneTrackFinder::GenerateHoughValues(PndGeneralHoughHit& houghHit) {
	std::vector<TVector2> resultVector;
	std::cout << "PndIsochroneTrackFinder::GenerateHoughValues hit " << houghHit.GetX() << "/" << houghHit.GetY() << "/" << houghHit.GetIsochroneRadius() << std::endl;
	Double_t phi = fAngleRange.first;
	Double_t angleStepSize = TMath::Abs(fAngleRange.second - fAngleRange.first)/fAngleSteps;
	for (Int_t i = 0; i < fAngleSteps + 1; i++) {
		phi += angleStepSize;
		if (2 * TMath::Pi() < phi) {
			phi -= 2 * TMath::Pi();
		}
		TVector2 circle = IsoCalc(houghHit, phi);
		std::cout << "Phi: " << TMath::RadToDeg() * phi << " Circle: " << circle.X() << "/" << circle.Y() << std::endl;
		resultVector.push_back(circle);
	}
	return resultVector;
}


std::vector<TVector2> PndIsochroneTrackFinder::GenerateHoughValuesPhi(PndGeneralHoughHit& houghHit, std::set<Double_t> phiValues)
{
	std::vector<TVector2> resultVector;
	for (std::set<Double_t>::iterator iter = phiValues.begin(); iter != phiValues.end(); iter++){
		TVector2 circle = IsoCalc(houghHit, *iter);
		resultVector.push_back(circle);
	}
	return resultVector;
}

void PndIsochroneTrackFinder::FillHoughHist(std::vector<TVector2> circleCenters, Int_t hitNumber, Double_t hitWeight) {
	TVector2 oldCircleCenter(0,0);
	Bool_t firstRun = true;

//	std::cout << "PndIsochroneTrackFinder::FillHoughHist HitNumber " << hitNumber << " weight " << hitWeight << std::endl;
	for (Double_t iAllDegrees = 0; iAllDegrees < circleCenters.size(); iAllDegrees++){
		//Todo implement check for skewed layers
		TVector2 circleCenter = circleCenters[iAllDegrees];
	//	std::cout << "PndIsochroneTrackFinder::FillHoughHist " << circleCenter.X() << "/" << circleCenter.Y() << std::endl;
		if (true == firstRun) {
			Double_t x, y;
			x = circleCenter.X();
			y = circleCenter.Y();
			Int_t bin = fHoughHisto->Fill(x, y, hitWeight);
			fBinsWithContent.insert(bin);
			fBinToHit[bin].insert(hitNumber);
			oldCircleCenter = circleCenter;
			firstRun = false;
		} else { // false == firstRun
			if (TMath::Abs(oldCircleCenter.Mod()) < 2600 && TMath::Abs(circleCenter.Mod()) < 2600){
				//std::cout << "PndIsochroneTrackFinder::FindTracks weight for " << myHit.GetEntryNr() << " BranchName: " << FairRootManager::Instance()->GetBranchName(myHit.GetEntryNr().GetType()) << " weight: " << fWeightMap[FairRootManager::Instance()->GetBranchName(myHit.GetEntryNr().GetType())] << std::endl;
				InterpolateHisto(fHoughHisto, oldCircleCenter, circleCenter, hitNumber, hitWeight);
			}
			oldCircleCenter = circleCenter;
		}
	}
}

std::vector<PndTrack> PndIsochroneTrackFinder::PeaksToTracks(std::multimap<Int_t, Int_t> peakBins, Int_t lowerHitLimit) {
	std::vector<PndTrack> result;
	for (std::multimap<Int_t, Int_t>::reverse_iterator peakIter = peakBins.rbegin(); peakIter != peakBins.rend(); ++peakIter) {
		Int_t binX, binY, binZ;
		fHoughHisto->GetBinXYZ(peakIter->second, binX, binY, binZ);
		TVector2 circle(fHoughHisto->GetXaxis()->GetBinCenter(binX), fHoughHisto->GetYaxis()->GetBinCenter(binY));
		std::cout << peakIter->first << " : " << circle.X() << "/" << circle.Y() << " Bin: " << peakIter->second << std::endl;
		PndTrackCand myTrackCand = FillTrackCand(peakIter->second);
		if (myTrackCand.GetNHits() > lowerHitLimit) {
			//std::vector<std::vector<FairHit> > correctedSkewedHits = FindSkewedHits(circle);
			std::vector<std::vector<FairHit> > skewedHits = FindSkewedHits(circle);
//				for (int i = 0; i < skewedHits.size(); i++){
//					for (int j = 0; j < skewedHits[i].size(); j++){
//						Double_t rho = CalcArcLength(skewedHits[i][j], circleCenter);
//						trackCandHits[rho] = skewedHits[i][j];
//					}
//				}
			AssignSkewedHits(myTrackCand, skewedHits, circle);
			PndTrack myTrack = CalcPndTrack(myTrackCand, circle);
			fTrackCands.push_back(myTrackCand);
			result.push_back(myTrack);
			fPeakCoordinates.push_back(circle);
			std::cout << "PndTrack: " << myTrack << std::endl;
		}
	}
	return result;
}

std::vector<PndTrack> PndIsochroneTrackFinder::PeaksToTracks(std::vector<PndPeakContent>& mergedPeaks, Int_t lowerHitLimit) {
	std::vector<PndTrack> result;
	std::cout << "PndIsochroneTrackFinder::PeaksToTracks: nMergedPeaks: " << mergedPeaks.size() << std::endl;
	for (Int_t i = 0; i < mergedPeaks.size(); i++) {
		std::cout << i << " : " << mergedPeaks[i] << std::endl;
		TVector2 circle(mergedPeaks[i].GetMeanPeakPos());
		PndTrackCand myTrackCand = FillTrackCand(mergedPeaks[i]);
		if (myTrackCand.GetNHits() > lowerHitLimit) {
			//std::vector<std::vector<FairHit> > correctedSkewedHits = FindSkewedHits(circle);
			std::vector<std::vector<FairHit> > skewedHits = FindSkewedHits(circle);
//				for (int i = 0; i < skewedHits.size(); i++){
//					for (int j = 0; j < skewedHits[i].size(); j++){
//						Double_t rho = CalcArcLength(skewedHits[i][j], circleCenter);
//						trackCandHits[rho] = skewedHits[i][j];
//					}
//				}
			AssignSkewedHits(myTrackCand, skewedHits, circle);
			PndTrack myTrack = CalcPndTrack(myTrackCand, circle);
			fTrackCands.push_back(myTrackCand);
			result.push_back(myTrack);
			fPeakCoordinates.push_back(circle);
			std::cout << "PndTrack: " << myTrack << std::endl;
		}
	}
	return result;
}


std::vector<PndPeakContent> PndIsochroneTrackFinder::MergePeaksByPos(std::vector<PndPeakContent>& peaks, Double_t peakDistanceCut){
	std::cout << "PndIsochroneTrackFinder::MergePeaksByPos peaks size() " << peaks.size() << std::endl;
	for (Int_t rIter = peaks.size() - 1; rIter > 0; rIter--){
		for (Int_t fIter = 0; fIter < rIter; fIter++){
			std::cout << "PndIsochroneTrackFinder::MergePeaksByPos: " << rIter << " " << fIter << " distance:  "
					<< (peaks[rIter].fCircleCenter - peaks[fIter].fCircleCenter).Mod() << " cut " << peakDistanceCut << std::endl;
			if((peaks[rIter].fCircleCenter - peaks[fIter].fCircleCenter).Mod() < peakDistanceCut){
				peaks[fIter].AddPeakContent(peaks[rIter]);
				peaks.erase(peaks.begin() + rIter);

				break;
			}
		}
	}
	return peaks;
}



std::vector<std::vector<FairHit> > PndIsochroneTrackFinder::FindSkewedHits(TVector2 circle){

	std::vector<std::vector <FairHit> > result;
	for (int i = 0; i < fSkewedHits.size(); i++){
		std::vector<FairHit> tempResult;
		PndSttHit* myHit = fSkewedHits.at(i);
		if (myHit->GetTubeID() < fSttTubeArray->GetEntriesFast()) {
			PndSttTube* myTube = (PndSttTube*) fSttTubeArray->At(myHit->GetTubeID());

			if (0 == myTube) {
				std::cout << "-E- PndSttIsochroneDraw Tube does not exist! "
						<< myHit->GetTubeID() << std::endl;
				continue;
			}
			Double_t tubeLengthHalf = myTube->GetHalfLength();
			TVector3 wireDirection = myTube->GetWireDirection();
			TVector3 tubePosition = myTube->GetPosition();

			TVector3 startPoint = tubePosition - (tubeLengthHalf * wireDirection);
			TVector3 endPoint = tubePosition + (tubeLengthHalf * wireDirection);

			Double_t length1 = circle.Mod() + myHit->GetIsochrone();
			Double_t length2 = circle.Mod() - myHit->GetIsochrone();

			TVector2 sP(startPoint.X(), startPoint.Y());
			TVector2 wDir(wireDirection.X(), wireDirection.Y());
			//TVector2 wD = wDir.Unit();
			Double_t p_half = (wDir.X()*(sP.X() - circle.X())) + (wDir.Y()*(sP.Y() - circle.Y()));
			p_half /= wDir.Mod2();
			Double_t q1 = (((sP.X() - circle.X()) * (sP.X() - circle.X()) + ((sP.Y() - circle.Y()) * (sP.Y() - circle.Y())) - length1 * length1))/wDir.Mod2();
			Double_t q2 = (((sP.X() - circle.X()) * (sP.X() - circle.X()) + ((sP.Y() - circle.Y()) * (sP.Y() - circle.Y())) - length2 * length2))/wDir.Mod2();

			std::vector<Double_t> t;
			//Double_t t1 = -1;
			//Double_t t2 = -1;
			//Double_t t3 = -1;
			//Double_t t4 = -1;

			if (p_half * p_half > q1){
				t.push_back((-1) * p_half + TMath::Sqrt(p_half * p_half - q1));
				t.push_back((-1) * p_half - TMath::Sqrt(p_half * p_half - q1));
			}
			if (p_half * p_half > q2){
				t.push_back((-1) * p_half + TMath::Sqrt(p_half * p_half - q2));
				t.push_back((-1) * p_half - TMath::Sqrt(p_half * p_half - q2));
			}
			std::cout << "FindSkewedHits: TubePos: " << tubePosition.X() << "/" << tubePosition.Y() << "/" << tubePosition.Z() <<  " halflength: " << tubeLengthHalf << std::endl;
			std::cout << "FindSkewedHits: startPos: " << startPoint.X() << "/" << startPoint.Y() << "/" << startPoint.Z() << std::endl;
			std::cout << "FindSkewedHits: endPos: " << endPoint.X() << "/" << endPoint.Y() << "/" << endPoint.Z() << std::endl;
			std::cout << "FindSkewedHits: dirVec: " << wireDirection.X() << "/" << wireDirection.Y() << "/" << wireDirection.Z() << std::endl;
	//		std::cout << "AssignSkewedHits: dirVec2D: " << wD.X() << "/" << wD.Y() << std::endl;

			std::cout << "Circle: " << circle.X() << "/" << circle.Y() << std::endl;
			std::cout << "P_half " << p_half << " q1 " << q1 << " q2 " << q2 << std::endl;
			//std::cout << "t1: " << t1 << " t2 " << t2 << " t3 " << t3 << " t4 " << t4 << std::endl;
			for (int j = 0; j < t.size(); j++){
				if (TMath::Abs(t[j]) < 2 * tubeLengthHalf){
					TVector3 posError(0.2,0.2,2);
					TVector3 pos(startPoint + t[j]*wireDirection);
					TVector2 pos2D(pos.X(), pos.Y());
					TVector2 corrPos = (pos2D - circle).Unit();
					std::cout << "FindSkewedHits: Pos before fix: " << pos.X() << "/" << pos.Y() << "/" << pos.Z() << std::endl;
					corrPos *= circle.Mod();
					corrPos += circle;
					pos.SetX(corrPos.X());
					pos.SetY(corrPos.Y());


					std::cout << "FindSkewedHits: Pos after fix: " << pos.X() << "/" << pos.Y() << "/" << pos.Z() << std::endl;
					FairHit hit(-1, pos, posError, -1);
					hit.AddLink(myHit->GetEntryNr());
					tempResult.push_back(hit);
					hit.SetEntryNr(FairLink(-1, FairRootManager::Instance()->GetEntryNr(), FairRootManager::Instance()->GetBranchId(fSkewedBranchName), -1));
	//				FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit(hit);
	//				newSkewedHit->SetEntryNr(FairLink(-1, FairRootManager::Instance()->GetEntryNr(), FairRootManager::Instance()->GetBranchId(fSkewedBranchName), fSttSkewedArray->GetEntriesFast() - 1));

					std::cout << "FindSkewedHits: Hit " << j << " : " << t[j] << " " << hit.GetX() << "/" << hit.GetY() << "/" << hit.GetZ() << std::endl;
				}
			}
			std::cout << std::endl;
			result.push_back(tempResult);
		}
	}
	return result;
}



void PndIsochroneTrackFinder::AssignSkewedHits(PndTrackCand& cand, std::vector<std::vector<FairHit> >skewedHits, TVector2 circle)
{
	UInt_t nDetHits = cand.GetNHitsDet(fMvdHitsPixelBranchId);
	nDetHits += cand.GetNHitsDet(fMvdHitsStripBranchId);
	nDetHits += cand.GetNHitsDet(fGemHitBranchId);

	std::vector<Int_t> bestHits;

	if (nDetHits > 0) {
		TGraph g(nDetHits + 1);
		Int_t j = 0;
		g.SetPoint(j, 0, 0);
		j++;
		for (Int_t i = 0; i < cand.GetNHits(); i++){
			PndTrackCandHit myHit = cand.GetSortedHit(i);
			if(myHit.GetType() == fMvdHitsPixelBranchId ||
					myHit.GetType() == fMvdHitsStripBranchId ||
					myHit.GetType() == fGemHitBranchId) {
				std::cout << "PndIsochroneTrackFinder::AssignSkewedHits myHit " << myHit << std::endl;
				FairHit* myFairHit = &fMapFairLinkHit[myHit];
				std::cout << "PndIsochroneTrackFinder::AssignSkewedHits: SetPoint " << myHit << " Rho: " << myHit.GetRho() << " Z: " << myFairHit->GetZ() << std::endl;

				g.SetPoint(j, myHit.GetRho(), myFairHit->GetZ());
				j++;
			}
		}
		g.Fit("pol1","Q0"); // << std::endl;
		TF1* f = g.GetFunction("pol1");
		//std::cout << "f: " << f << std::endl;
		Double_t t = f->GetParameter(0);
		Double_t m = f->GetParameter(1);
		Double_t tError = f->GetParError(0);
		Double_t mError = f->GetParError(1);
		Double_t Chi2   = f->GetChisquare()/f->GetNDF();

		std::cout << "PndIsochroneTrackFinder::AssignSkewedHits: FitResult: t " << t << " +/- " << tError << " m  " << m << " +/- " << mError << " Chi2: " << Chi2 << std::endl;
		for(Int_t sttHitIndex = 0; sttHitIndex < skewedHits.size(); sttHitIndex++){
			Int_t bestSkewIndex = -1;
			Double_t bestArcLength = -1;
			Double_t dist = 100000;
			for (Int_t k = 0; k < skewedHits[sttHitIndex].size(); k++){
				FairHit myHit = skewedHits[sttHitIndex][k];
				Double_t s = CalcArcLength(myHit, circle);
				TVector2 p(s, myHit.GetZ());
				TVector2 n(m, 1);
				n /= n.Mod();
				Double_t tempDist = TMath::Abs(p * n);
				std::cout << "PndIsochroneTrackFinder::AssignSkewedHits: skewedHit " << p.X() << "/" << p.Y() << " dist: " << tempDist << std::endl;
				if (tempDist < dist)
				{
					dist = tempDist;
					bestSkewIndex = k;
					bestArcLength = s;
				}

			}
			bestHits.push_back(bestSkewIndex);


//			FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit(skewedHits[sttHitIndex][bestSkewIndex]);
//			newSkewedHit->SetEntryNr(FairLink(-1, FairRootManager::Instance()->GetEntryNr(), FairRootManager::Instance()->GetBranchId(fSkewedBranchName), fSttSkewedArray->GetEntriesFast() - 1));
//			std::cout << "PndIsochroneTrackFinder::AssignSkewedHits: newHit " << newSkewedHit->GetX() << "/" << newSkewedHit->GetY() << "/" << newSkewedHit->GetZ() << std::endl;
//			std::cout << newSkewedHit->GetEntryNr() << std::endl;
//			cand.AddHit(newSkewedHit->GetEntryNr(), bestArcLength);

		}
		std::map<Double_t, Int_t> rhoHits;
		for (int candIndex = 0; candIndex < cand.GetNHits(); candIndex++){
			rhoHits[cand.GetSortedHit(candIndex).GetRho()] = -1 * cand.GetSortedHit(candIndex).GetType();
		}
		for (int bestIndex = 0; bestIndex < bestHits.size(); bestIndex++){
			std::cout << "AssignSkewedHits bestIndex: " << bestIndex << " bestHits " << bestHits[bestIndex] << std::endl;
			if (bestHits[bestIndex] > -1){
				Double_t arcLength = CalcArcLength(skewedHits[bestIndex][bestHits[bestIndex]], circle);
				rhoHits[arcLength] = bestIndex;
			}
		}

		Double_t oldRho = -1;
		std::set<Int_t> fittingIndizes;
		std::cout << "AssignSkewedHits TurningDirection: " << fClockwise << std::endl;
		for (std::map<Double_t, Int_t>::iterator iter = rhoHits.begin(); iter != rhoHits.end(); iter++){
			std::cout << "PndIsochroneTrackFinder::AssignSkewedHits hitIndex " << iter->second << " rho: " << iter->first * circle.Mod() << std::endl;
			if (-1 * iter->second < 1 || -1 * iter->second == FairRootManager::Instance()->GetBranchId("STTHit")){
				if (oldRho == -1){
					oldRho = iter->first * circle.Mod();
				}
				std::cout << "PndIsochroneTrackFinder::AssignSkewedHits hitIndex " << iter->second << " oldRho " << oldRho << " newRho " <<  iter->first * circle.Mod() << std::endl;
				if (TMath::Abs(iter->first * circle.Mod() - oldRho) < 5.0){
					oldRho = iter->first * circle.Mod();
					fittingIndizes.insert(iter->second);
				} else {
					break;
				}
			}
		}

		for (std::set<Int_t>::iterator iter = fittingIndizes.begin(); iter != fittingIndizes.end(); iter++){
			if (*iter > -1){
				FairHit bestFairHit = skewedHits[*iter][bestHits[*iter]];
				//FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit();
				FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit();
				newSkewedHit->SetXYZ(bestFairHit.GetX(), bestFairHit.GetY(), bestFairHit.GetZ());
				newSkewedHit->SetDxyz(bestFairHit.GetDx(), bestFairHit.GetDy(), bestFairHit.GetDz());
				newSkewedHit->SetEntryNr(FairLink(-1, FairRootManager::Instance()->GetEntryNr(), FairRootManager::Instance()->GetBranchId(fSkewedBranchName), fSttSkewedArray->GetEntriesFast() - 1));
				std::cout << "PndIsochroneTrackFinder::AssignSkewedHits: newHit " << newSkewedHit->GetX() << "/" << newSkewedHit->GetY() << "/" << newSkewedHit->GetZ() << std::endl;
				std::cout << newSkewedHit->GetEntryNr() << std::endl;
				cand.AddHit(newSkewedHit->GetEntryNr(), CalcArcLength(bestFairHit, circle));
				fMapFairLinkHit[newSkewedHit->GetEntryNr()] =  bestFairHit;
			}
		}
	} else {
		std::cout << "PndIsochroneTrackFinder::AssignSkewedHits no MVD/GEM hits!" << std::endl;
		if (skewedHits.size() > 1){
			std::vector<Double_t> phiValues;
			std::vector<Double_t> phiDiffs;
			std::vector< std::vector<Int_t> > bestHitsVector;
			for (int i = 0; i < skewedHits[0].size(); i++){
				FairHit actualHit = skewedHits[0][i];
				Double_t arcLength = CalcArcLength(actualHit, circle);
				TVector2 phiVec(arcLength, actualHit.GetZ());
				phiValues.push_back(phiVec.Phi());
				phiDiffs.push_back(0);
				std::vector<Int_t> tempHits;
				tempHits.push_back(i);
				bestHitsVector.push_back(tempHits);
				std::cout << "PndIsochroneTrackFinder::AssignSkewedHits startValues: " << i << " : " << phiVec.X() << "/" << phiVec.Y() << "/" << phiVec.Phi() << std::endl;
			}
			for (int skewedHitsIndex = 1; skewedHitsIndex < skewedHits.size(); skewedHitsIndex++){
				for (int oldPhiIndex = 0; oldPhiIndex < phiValues.size(); oldPhiIndex++){
					Int_t bestChoice = -1;
					Double_t bestPhiDiff = 100000;
					Double_t bestPhi = 0;
					for (int skewedChoiceIndex = 0; skewedChoiceIndex < skewedHits[skewedHitsIndex].size(); skewedChoiceIndex++){
						FairHit actualHit = skewedHits[skewedHitsIndex][skewedChoiceIndex];
						Double_t arcLength = CalcArcLength(actualHit, circle);
						TVector2 phiVec(arcLength, actualHit.GetZ());
						Double_t actualPhi = phiVec.Phi();
						std::cout << "PndIsochroneTrackFinder::AssignSkewedHits actualValues: " << skewedHitsIndex << "/" << skewedChoiceIndex << " : " << phiVec.X() << "/" << phiVec.Y() << "/" << phiVec.Phi()
								<< " PhiDiff: " << TMath::Abs(phiValues[oldPhiIndex] - actualPhi) << std::endl;

						if (TMath::Abs(phiValues[oldPhiIndex] - actualPhi) < bestPhiDiff){
							bestPhiDiff = TMath::Abs(phiValues[oldPhiIndex] - actualPhi);
							bestChoice = skewedChoiceIndex;
							bestPhi = actualPhi;
						}
					}
					if (bestPhiDiff < 1){
						phiDiffs[oldPhiIndex] += bestPhiDiff;
						bestHitsVector[oldPhiIndex].push_back(bestChoice);
						phiValues[oldPhiIndex] = (phiValues[oldPhiIndex] + bestPhi)/2;
						std::cout << "BestValues: bestPhiDiff " << bestPhiDiff << " bestChoice: " << bestChoice << " AverageValue: " << phiValues[oldPhiIndex] << std::endl;
					} else {
						bestHitsVector[oldPhiIndex].push_back(-1);
					}
				}
			}
			Int_t bestPhiDiffsIndex = -1;
			Double_t bestPhiDiffsValue = 100000;
			for(int phiDiffsIndex = 0; phiDiffsIndex < phiDiffs.size(); phiDiffsIndex++){
				std::cout << "PhiDiffs: " << phiDiffsIndex << " : " << phiDiffs[phiDiffsIndex] << std::endl;
				if (phiDiffs[phiDiffsIndex] < bestPhiDiffsValue){
					bestPhiDiffsIndex = phiDiffsIndex;
					bestPhiDiffsValue =phiDiffs[phiDiffsIndex];
				}
			}
			if (bestPhiDiffsIndex > -1) {
				for (Int_t selectionIndex = 0; selectionIndex < bestHitsVector[bestPhiDiffsIndex].size(); selectionIndex++){
					std::cout << "Create SkewedHits: " << selectionIndex << " : " << bestHitsVector[bestPhiDiffsIndex][selectionIndex] << std::endl;
					if (bestHitsVector[bestPhiDiffsIndex][selectionIndex] > -1){
						FairHit bestFairHit = skewedHits[bestPhiDiffsIndex][bestHitsVector[bestPhiDiffsIndex][selectionIndex]];
						//FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit();
						FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit();
						newSkewedHit->SetXYZ(bestFairHit.GetX(), bestFairHit.GetY(), bestFairHit.GetZ());
						newSkewedHit->SetDxyz(bestFairHit.GetDx(), bestFairHit.GetDy(), bestFairHit.GetDz());
						newSkewedHit->SetEntryNr(FairLink(-1, FairRootManager::Instance()->GetEntryNr(), FairRootManager::Instance()->GetBranchId(fSkewedBranchName), fSttSkewedArray->GetEntriesFast() - 1));
						std::cout << "PndIsochroneTrackFinder::AssignSkewedHits: newHit " << newSkewedHit->GetX() << "/" << newSkewedHit->GetY() << "/" << newSkewedHit->GetZ() << std::endl;
						std::cout << newSkewedHit->GetEntryNr() << std::endl;
						cand.AddHit(newSkewedHit->GetEntryNr(), CalcArcLength(bestFairHit, circle));
						fMapFairLinkHit[newSkewedHit->GetEntryNr()] =  bestFairHit;
					}
				}
			}

		}
	}

}

FairTrackParP PndIsochroneTrackFinder::CalcTrackParP(FairHit hit, TVector2 circleCenter)
{
	TVector3 hitPos;
	TVector3 hitPosError(0.1, 0.1, 0.1);
	TVector3 momError(2, 2, 2);
	TVector3 dj(1,0,0);
	TVector3 dk(0,1,0);
	TVector3 origin(0, 0, 1);

	hitPos = CalcHitPosInTrack(hit, circleCenter);
	Double_t pt = CalcPt(circleCenter);
	TVector2 hit2D(hit.GetX(), hit.GetY());
	TVector2 ptDir = CalcPtDir(hit2D, circleCenter);
	TVector2 ptVec2D = pt * ptDir;
	TVector3 ptVec(ptVec2D.X(), ptVec2D.Y(), 0);
	Double_t q = 0;
	std::cout << "CalcTrackParP TurningDirection " << fClockwise << std::endl;
	if (fClockwise)
		q = 1;
	else
		q = -1;
	FairTrackParP result(hitPos, ptVec, hitPosError, momError, q, origin, dj, dk);
	return result;
}

PndTrack PndIsochroneTrackFinder::CalcPndTrack(PndTrackCand& cand, TVector2 circle) {

//	AssignSkewedHits(cand, skewedHits, circle);

	FairLink first = (FairLink)cand.GetSortedHit(0);
	FairLink last = (FairLink)cand.GetSortedHit(cand.GetNHits()-1);

//	for (int i = 0; i < fHitsToFit.size(); i++){
//		PndGeneralHoughHit myHit = fHitsToFit.at(i);
//		std::cout << "PndIsochroneTrackFinder::CalcPndTrack: FairHit list: " << myHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)myHit << std::endl;
//	}
	FairHit firstHit = (FairHit) fMapFairLinkHit[first]; //FairRootManager::Instance()->GetCloneOfLinkData(first); //(FairHit)fHitsToFit.at(first.GetIndex());
	FairHit lastHit =  (FairHit) fMapFairLinkHit[last]; //FairRootManager::Instance()->GetCloneOfLinkData(last); //(FairHit)fHitsToFit.at(last.GetIndex());

//	std::cout << "CalcPndTrack firstHit " << firstHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)firstHit << std::endl;
//	std::cout << "CalcPndTrack lastHit " << lastHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)lastHit << std::endl;
//	for (int i = 0; i < fHitsToFit.size(); i++){
//		PndGeneralHoughHit myHit = fHitsToFit.at(i);
//		std::cout << "PndIsochroneTrackFinder::CalcPndTrack2: FairHit list: " << myHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)myHit << std::endl;
//	}
	std::cout << "firstHit: " << first << " " <<  firstHit << " lastHit: " << last << " " << lastHit << std::endl;

	FairTrackParP firstPar = CalcTrackParP(firstHit, circle);
	FairTrackParP lastPar  = CalcTrackParP(lastHit, circle);
	PndTrack result(firstPar, lastPar, cand);
	return result;
}

Double_t PndIsochroneTrackFinder::CalcPt(TVector2 circleCenter) {
	double result = 0.3 * fBField * circleCenter.Mod() * 0.01;
	std::cout << "PndIsochroneTrackFinder::CalcPt " << result << std::endl;
	return result;
}

TVector2 PndIsochroneTrackFinder::CalcPtDir(TVector2 hit, TVector2 circleCenter) {
	TVector2 hitDirUnit = (hit - circleCenter).Unit();
	TVector2 result;
	result = hitDirUnit.Rotate(90.0 * TMath::DegToRad());
	if (fClockwise)
		result = result.Rotate(180.0 * TMath::DegToRad());

	return result;
}

TVector3 PndIsochroneTrackFinder::CalcHitPosInTrack(FairHit hit, TVector2 circleCenter)
{
	Double_t s = CalcArcLength(hit, circleCenter);
	TVector2 negOrigin = (-1)*circleCenter;

	Double_t startPhi = negOrigin.Phi();
	TVector2 radVec(circleCenter.Mod(), 0);
	TVector2 rotVec;
	if (fClockwise == kTRUE)
		rotVec = radVec.Rotate(startPhi - s);
	else
		rotVec = radVec.Rotate(startPhi + s);
	rotVec += circleCenter;
	TVector3 result(rotVec.X(), rotVec.Y(), hit.GetZ());

	std::cout << "PndIsochroneTrackFinder::CalcHitPosInTrack s: " << s * TMath::RadToDeg() << " startPhi: " << startPhi * TMath::RadToDeg()
		<< " RadVec: " << radVec.X() << "/" << radVec.Y() << " RotVec: " << rotVec.X() << "/" << rotVec.Y() << std::endl;

	std::cout << "PndIsochroneTrackFinder::CalcHitPosInTrack Hit " << hit << " Circle: " << circleCenter.X() << "/" << circleCenter.Y()
			<< " s " << s << " startPhi: " << startPhi << " Result " << result.X() << "/" << result.Y() << "/" << result.Z() << std::endl;
	return result;
}

Double_t PndIsochroneTrackFinder::CalcArcLength(FairHit hit, TVector2 circleCenter){
	TVector2 hitVec(hit.GetX(), hit.GetY());
	TVector2 opCircleCenter = (-1)*circleCenter;
	TVector2 dirVec = opCircleCenter;

	dirVec += hitVec;
//	std::cout << "PndIsochroneTrackFinder::CalcArcLength: Hit: " << hitVec.X() << "/" << hitVec.Y()
//			<< " circleCenter " << circleCenter.X() << "/" << circleCenter.Y()
//			<< " arcLength: " << TVector2::Phi_0_2pi(dirVec.DeltaPhi(opCircleCenter)) * TMath::RadToDeg() << std::endl;
	if (fClockwise == kFALSE){
		return TVector2::Phi_0_2pi(dirVec.DeltaPhi(opCircleCenter));
	} else {
		return 2 * TMath::Pi() - TVector2::Phi_0_2pi(dirVec.DeltaPhi(opCircleCenter));
	}

}

PndTrackCand PndIsochroneTrackFinder::FillTrackCand(Int_t histoBin) {
	PndTrackCand result;
	Int_t binX, binY, binZ;
	fClockwise = kFALSE;  //check the orientation of your circle. Standard is counterclockwise.
	fHoughHisto->GetBinXYZ(histoBin, binX, binY, binZ);
	TVector2 circleCenter(fHoughHisto->GetXaxis()->GetBinCenter(binX), fHoughHisto->GetYaxis()->GetBinCenter(binY));

	std::set<Int_t> hitsInBin = fBinToHit[histoBin];
	std::map<Double_t, FairHit> trackCandHits;
	PndTrackCand tempTrackCand;
	std::set<Int_t> tempHitsUsed;
	for (std::set<Int_t>::iterator iter = hitsInBin.begin(); iter!= hitsInBin.end(); ++iter){

		if (fHitsUsed.find(*iter) != fHitsUsed.end()) {
		//	std::cout << "PndIsochroneTrackFinder::FillTrackCand hit already used: " << (*iter) << std::endl;
			continue;
		}
		else tempHitsUsed.insert(*iter);

		std::cout << "PndIsochroneTrackFinder::FillTrackCand " << (*iter) << " Entries: " << fHitsToFit.size() << std::endl;

		FairHit myHit = (FairHit)fHitsToFit.at(*iter);
		// std::cout << "PndIsochroneTrackFinder::FillTrackCand: FairHit before " << myHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)myHit << std::endl;
		Double_t rho = CalcArcLength(myHit, circleCenter);
		// std::cout << "PndIsochroneTrackFinder::FillTrackCand: FairHit after " << myHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)myHit << std::endl;

		trackCandHits[rho] = myHit;
		tempTrackCand.AddHit(myHit.GetEntryNr(), rho);
	}

	std::map<Double_t, FairHit>::iterator candIter;
	Int_t nHitsBeforePi = 0;
	Bool_t bestBeforePi;
	std::map<Double_t, FairHit> cuttedTrack;
	std::cout << "PndIsochroneTrackFinder::FillTrackCand cut Track" << std::endl;
	for ( candIter = trackCandHits.begin(); candIter != trackCandHits.end(); candIter++){
		std::cout << "Rho: " << candIter->first << std::endl;
		if (candIter->first < TMath::Pi()){
			nHitsBeforePi++;
		} else {
			if (2 * nHitsBeforePi > trackCandHits.size()){
				std::cout << "NHitsBeforePi: " << nHitsBeforePi << " / " << trackCandHits.size() << std::endl;
				if (candIter != trackCandHits.begin()){
					cuttedTrack.insert(trackCandHits.begin(), candIter);
					bestBeforePi = kTRUE;
					break;
				}
			} else {
				cuttedTrack.insert(candIter, trackCandHits.end());
				bestBeforePi = kFALSE;
				break;
			}
		}
	}
	if (candIter == trackCandHits.end()){
		cuttedTrack = trackCandHits;
	}


	if (cuttedTrack.size() > 0 && cuttedTrack.rbegin()->first > TMath::Pi()){
		if ((2 * TMath::Pi()) - cuttedTrack.rbegin()->first < cuttedTrack.begin()->first){
			fClockwise = kTRUE;
		}
	}

	std::map<Double_t, FairHit> correctedTrackCandHits;
	std::cout << "FillTrackCand: TurningDirection set " << fClockwise << std::endl;

	for (std::map<Double_t, FairHit>::iterator iter = cuttedTrack.begin(); iter != cuttedTrack.end(); iter++){
			Double_t correctedRho;
			if (iter->second.GetEntryNr().GetType() != FairRootManager::Instance()->GetBranchId(GetSkewedBranchName())){
				if (fClockwise == kTRUE){
					correctedRho = (2 * TMath::Pi() - iter->first);
				} else {
					correctedRho = iter->first;
				}
				correctedTrackCandHits[correctedRho] = iter->second;
			}
	}

	std::map<Double_t, FairHit>::iterator endOfContinuity = CheckContinuity(correctedTrackCandHits, circleCenter.Mod());
	if (endOfContinuity != correctedTrackCandHits.begin()){
		std::map<Double_t, FairHit> continuousTrackCandHits(correctedTrackCandHits.begin(), endOfContinuity);
		if (continuousTrackCandHits.size() > 2){
			fHitsUsed.insert(tempHitsUsed.begin(), tempHitsUsed.end());

			for (std::map<Double_t, FairHit>::iterator iter = continuousTrackCandHits.begin(); iter != continuousTrackCandHits.end(); iter++)
			{
				std::cout << "PndIsochroneTrackFinder::FillTrackCand trackCandHits: " << iter->first << " " << iter->second << std::endl;
			}


			for (std::map<Double_t, FairHit>::iterator iter = continuousTrackCandHits.begin(); iter != continuousTrackCandHits.end(); iter++){
				result.AddHit(iter->second.GetEntryNr(), iter->first);
			}
		}
	}
	return result;
}

PndTrackCand PndIsochroneTrackFinder::FillTrackCand(PndPeakContent& peakContent) {
	PndTrackCand result;

	TVector2 circleCenter(peakContent.fCircleCenter);

	std::set<Int_t> hitsInBin = peakContent.fHitsInPeaks;
	std::map<Double_t, FairHit> trackCandHits;
	PndTrackCand tempTrackCand;
	std::set<Int_t> tempHitsUsed;
	for (std::set<Int_t>::iterator iter = hitsInBin.begin(); iter!= hitsInBin.end(); ++iter){

//		if (fHitsUsed.find(*iter) != fHitsUsed.end()) {
//		//	std::cout << "PndIsochroneTrackFinder::FillTrackCand hit already used: " << (*iter) << std::endl;
//			continue;
//		}
//		else tempHitsUsed.insert(*iter);

		FairHit myHit = (FairHit)fHitsToFit.at(*iter);
		std::cout << "PndIsochroneTrackFinder::FillTrackCand " << (*iter) << " EntryNr: " << myHit.GetEntryNr() << std::endl;

		// std::cout << "PndIsochroneTrackFinder::FillTrackCand: FairHit before " << myHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)myHit << std::endl;
		fClockwise = kFALSE;
		Double_t rho = CalcArcLength(myHit, circleCenter);
		// std::cout << "PndIsochroneTrackFinder::FillTrackCand: FairHit after " << myHit.GetPointerToLinks() << " : " << (FairMultiLinkedData_Interface)myHit << std::endl;

		trackCandHits[rho] = myHit;
		tempTrackCand.AddHit(myHit.GetEntryNr(), rho);
	}
	std::map<Double_t, FairHit>::iterator candIter;
	Int_t nHitsBeforePi = 0;
	Bool_t bestBeforePi;
	std::map<Double_t, FairHit> cuttedTrack;
	std::cout << "PndIsochroneTrackFinder::FillTrackCand cut Track" << std::endl;
	for ( candIter = trackCandHits.begin(); candIter != trackCandHits.end(); candIter++){
		std::cout << "Rho: " << candIter->first <<  " : "  << candIter->second.GetEntryNr() << std::endl;
		if (candIter->first < TMath::Pi()){
			nHitsBeforePi++;
		} else {
			if (2 * nHitsBeforePi > trackCandHits.size()){
				std::cout << "NHitsBeforePi: " << nHitsBeforePi << " / " << trackCandHits.size() << std::endl;
				if (candIter != trackCandHits.begin()){
					cuttedTrack.insert(trackCandHits.begin(), candIter);
					bestBeforePi = kTRUE;
					break;
				}
			} else {
				cuttedTrack.insert(candIter, trackCandHits.end());
				bestBeforePi = kFALSE;
				break;
			}
		}
	}
	if (candIter == trackCandHits.end()){
		cuttedTrack = trackCandHits;
	}

	if (cuttedTrack.size() > 0 && cuttedTrack.rbegin()->first > TMath::Pi()){
		if ((2 * TMath::Pi()) - cuttedTrack.rbegin()->first < cuttedTrack.begin()->first){
			fClockwise = kTRUE;
		}
	}

	std::map<Double_t, FairHit> correctedTrackCandHits;
	std::cout << "FillTrackCand: TurningDirection set " << fClockwise << std::endl;

	for (std::map<Double_t, FairHit>::iterator iter = cuttedTrack.begin(); iter != cuttedTrack.end(); iter++){
			Double_t correctedRho;
			if (iter->second.GetEntryNr().GetType() != FairRootManager::Instance()->GetBranchId(GetSkewedBranchName())){
				if (fClockwise == kTRUE){
					correctedRho = (2 * TMath::Pi() - iter->first);

					std::cout << "FillTrackCand: " << iter->second.GetEntryNr() << " rho: " << iter->first << " correctedRho: " << correctedRho << std::endl;
				} else {
					correctedRho = iter->first;
				}
				correctedTrackCandHits[correctedRho] = iter->second;
			}
	}

	std::map<Double_t, FairHit>::iterator endOfContinuity = CheckContinuity(correctedTrackCandHits, circleCenter.Mod());
	if (endOfContinuity != correctedTrackCandHits.begin()){
		std::map<Double_t, FairHit> continuousTrackCandHits(correctedTrackCandHits.begin(), endOfContinuity);
		if (continuousTrackCandHits.size() > 2){
			fHitsUsed.insert(tempHitsUsed.begin(), tempHitsUsed.end());

			for (std::map<Double_t, FairHit>::iterator iter = continuousTrackCandHits.begin(); iter != continuousTrackCandHits.end(); iter++)
			{
				std::cout << "PndIsochroneTrackFinder::FillTrackCand trackCandHits: " << iter->first << " " << iter->second << std::endl;
			}


			for (std::map<Double_t, FairHit>::iterator iter = continuousTrackCandHits.begin(); iter != continuousTrackCandHits.end(); iter++){
				result.AddHit(iter->second.GetEntryNr(), iter->first);
			}
		} else {
			std::cout << "FillTrackCand CheckContinuity failed!" << std::endl;
		}
	}
	std::cout << "FillTrackCand: result ";
	result.Print();
	return result;
}

std::map<Double_t, FairHit>::iterator PndIsochroneTrackFinder::CheckContinuity(std::map<Double_t, FairHit>& cand, Double_t circleMag)
{
	Double_t oldRho = 0;
	Int_t oldType = -1;
	Bool_t stopCheck = kFALSE;
	Int_t posOfBreak = -1;

	if (cand.size() > 0){
		oldRho = cand.begin()->first * circleMag;
		oldType = cand.begin()->second.GetEntryNr().GetType();
		std::cout << "PndIsochroneTrackFinder::CheckContinuity ArcLength: " << cand.begin()->first << " circleMag " << circleMag << " type " << oldType << std::endl;
//		if (oldType != FairRootManager::Instance()->GetBranchId("MVDHitsPixel") || oldType != FairRootManager::Instance()->GetBranchId("MVDHitsStrip"))
//			return kFALSE;
	}
	for (std::map<Double_t, FairHit>::iterator iter = cand.begin(); iter != cand.end(); ++iter){
			Double_t newRho = iter->first * circleMag;
			std::cout << "PndIsochroneTrackFinder::CheckContinuity rho: " << newRho << " type: " << iter->second.GetEntryNr() << std::endl;
	}
	std::cout << "PndIsochroneTrackFinder::CheckContinuity oldRho " << oldRho << " oldType " << oldType << " clockwise " << fClockwise << std::endl;

	std::vector<Double_t> arcLengthRegions = CalcArcLengthRegions(circleMag);
	if (oldRho > arcLengthRegions[0]){ 		//first hit is not an MVD Hit
		stopCheck = kTRUE;
		return cand.begin();
	}

	std::map<Double_t, FairHit>::iterator iter = cand.begin(); // first element already in oldRho!
//	iter++;
	for (; iter != cand.end(); ++iter){
		Double_t newRho = iter->first * circleMag;
		Double_t distance = TMath::Abs(oldRho - newRho);
		Int_t cut = 12;

		std::cout << "PndIsochroneTrackFinder::CheckContinuity newRho " << newRho << " type " << iter->second.GetEntryNr().GetType() << std::endl;
		if (iter->second.GetEntryNr().GetType() == FairRootManager::Instance()->GetBranchId("GEMHit")){
			std::cout << "GEMHit found!" << std::endl;
			cut = 30;
		} else if (oldRho < arcLengthRegions[0]){			//oldHit in MVD
			if (newRho < arcLengthRegions[0]){
				cut = 10;
			} else if (newRho < arcLengthRegions[1]){ // newHit in Stt inner Layer
				cut = 30;
			} else {
				return iter;
			}
		} else if (oldRho < arcLengthRegions[1]){	//oldHit in Stt inner Layer
			if (newRho < arcLengthRegions[1]){		//newHit in Stt inner Layer
				cut = 4;
			} else if (newRho < arcLengthRegions[2]){
				cut = arcLengthRegions[2] - arcLengthRegions[1];
			} else if (newRho < arcLengthRegions[3]) {
				cut = arcLengthRegions[2] - arcLengthRegions[1] + 5;
			} else {
				return iter;
			}
		} else if (oldRho < arcLengthRegions[2]){	//
			if (newRho < arcLengthRegions[3]){
				cut = arcLengthRegions[2] - arcLengthRegions[1] + 5;
			} else {
				return  iter;
			}
		} else if (oldRho < arcLengthRegions[3]){	//
			if (newRho < arcLengthRegions[3]){
				cut = 2;
			} else {
				return  iter;
			}
		}

		std::cout << "PndIsochroneTrackFinder::CheckContinuity cut " << cut << " distance " << distance << std::endl;
		if (distance > cut){
			return iter;
		}
		else {
			oldRho = iter->first * circleMag;
			oldType = iter->second.GetEntryNr().GetType();
		}
	}
	return iter;
}

std::vector<Double_t> PndIsochroneTrackFinder::CalcArcLengthRegions(Double_t radius)
{
	std::vector<Double_t> result;
	std::vector<Double_t> distances;
	distances.push_back(16);	//outer radius MVD
	distances.push_back(23.2);	//minimum outer radius parallel tubes
	distances.push_back(30.8 * TMath::ACos(30*TMath::DegToRad()));  //maximum outer radius skewed tubes
	distances.push_back(41.0);  //outer radius STT

	std::cout << "PndIsochroneTrackFinder::CalcArcLengthRegions: radius: " << radius;
	for (int i = 0; i < distances.size(); i++){
		Double_t arcLength = 2 * radius * TMath::ASin(distances[i]/(2*radius));
		result.push_back(arcLength);
		std::cout << " " << arcLength;
	}
	std::cout << std::endl;
	return result;
}
/**
 * Calculates a point lying on a isochrone around a point, at given angle
 * @param x0 x coordinate of center point
 * @param y0 y coordinate of center point
 * @param isoR isochrone radius around center point
 * @param phi position around center point where isochrone should be evaluated, in rad
 * @return TVector2 coordinate of calculated point
 */
TVector2 PndIsochroneTrackFinder::IsoCalc(PndGeneralHoughHit& houghHit, Double_t phi) {  // Calculate a point lying on a isochrone of radius isoR around (x0,y0) at angle phi
	Double_t x0 = houghHit.GetX();
	Double_t y0 = houghHit.GetY();
	Double_t isoR = houghHit.GetIsochroneRadius();
	Double_t s = (isoR*isoR - x0 * x0 - y0 * y0) /(2 * (x0 * TMath::Cos(phi) + y0 * TMath::Sin(phi) + isoR));
	TVector2 result(x0 + s * TMath::Cos(phi), y0 + s * TMath::Sin(phi));
	return result;
}

/**
 * Gets the global bin index in a two-dimensional histogram of a given (x,y) value pair
 * @param histo Histogram (filled)
 * @param xVal Some point's x value
 * @param yVal Some point's y value
 * @return Int_t Global index of bin where (x,y) value is found
 */
Int_t PndIsochroneTrackFinder::GetHistoBin(TH2* histo, Double_t xVal, Double_t yVal) {
	Int_t binx = histo->GetXaxis()->FindBin(xVal);
	Int_t biny = histo->GetYaxis()->FindBin(yVal);
	return histo->GetBin(binx, biny);
}

void PndIsochroneTrackFinder::InterpolateHisto(TH2* histo, TVector2& in, TVector2& out, Int_t& hitId, Double_t weight) {//, std::set<Int_t>& binsWithContent){
//	std::cout << "InterpolateBetween: " << in.X() << "/" << in.Y() << " and " << out.X() << "/" << out.Y() << std::endl;
	Double_t binWidthX = histo->GetXaxis()->GetBinWidth(1);
	Double_t binWidthY = histo->GetYaxis()->GetBinWidth(1);
	Double_t binWidth = TMath::Min(binWidthX, binWidthY);

	TVector2 dir = (out - in);
	Double_t dist = dir.Mod();
	dir = dir.Unit();

	Double_t s = 0;
	Double_t stepLength = binWidth/20;
	TVector2 runVec = in;
	TVector2 addVec = stepLength * dir;
	Int_t currentHistoIndex = GetHistoBin(histo, in.X(), in.Y());
//	std::cout << "currentHistoIndex: " << currentHistoIndex << " for " << in.X() << "/" << in.Y() << std::endl;
	s += stepLength;
	while (s < dist){
		runVec += addVec;
		if (GetHistoBin(histo, runVec.X(), runVec.Y()) == 997473 || currentHistoIndex == 997473){
			std::cout << "currentHistoIndex: " << currentHistoIndex << " FutureHistoIndex " << GetHistoBin(histo, runVec.X(), runVec.Y()) << " for " << runVec.X() << "/" << runVec.Y() << std::endl;
			std::cout << "StepLength: " << s << " Dist: " << dist << " addVec " << addVec.X() << "/" << addVec.Y() << std::endl;
			std::cout << "InVector: " << in.X() << "/" << in.Y() << " OutVector: " << out.X() << "/" << out.Y() << std::endl;
		}
		if (currentHistoIndex != GetHistoBin(histo, runVec.X(), runVec.Y())){
			currentHistoIndex = GetHistoBin(histo, runVec.X(), runVec.Y());
			if (fBinToHit[currentHistoIndex].find(hitId) == fBinToHit[currentHistoIndex].end()){
				Int_t bin = histo->Fill(runVec.X(), runVec.Y(), weight);
				fBinsWithContent.insert(bin);
				fBinToHit[bin].insert(hitId);
			}
//			std::cout << "Written into Bin: " << bin << std::endl;
		}
		s += stepLength;
	}
}

/**
 * Simple threshold peakfinder
 */
std::multimap<Int_t, Int_t> PndIsochroneTrackFinder::FindPeaks(std::set<Int_t>& binsWithContent, TH2* histo, Int_t threshold) {
	std::multimap<Int_t, Int_t> result;
//	for (std::set<Int_t>::iterator binIter = binsWithContent.begin(); binIter != binsWithContent.end(); binIter++){
//		if ((*binIter) > -1 && histo->GetBinContent(*binIter) > threshold){
//			result.insert(std::pair<Int_t, Int_t>(histo->GetBinContent(*binIter), *binIter));
//		}
//	}
	for (std::map<Int_t, std::set<Int_t> >::iterator peakIter = fBinToHit.begin(); peakIter != fBinToHit.end(); peakIter++){
		if (peakIter->second.size() > threshold){
			if (peakIter->first > 0)
				result.insert(std::pair<Int_t, Int_t>(peakIter->second.size(), peakIter->first));
		}
	}
	std::cout << "PndIsochroneTrackFinder::FindPeaks " << result.size() << " peaks found!" << std::endl;
	return result;
}

std::vector<PndPeakContent> PndIsochroneTrackFinder::MergePeaks(std::multimap<Int_t, Int_t> peaks, Double_t trackSimilarityCut, Int_t lowerHitLimit, Bool_t separateTracks)
{
	std::vector<PndPeakContent> result;
	if (peaks.size() == 0)
		return result;

	std::multimap<Int_t, Int_t>::reverse_iterator peakIter = peaks.rbegin();
	Int_t maxHits = peakIter->first;
	std::cout << "PndIsochroneTrackFinder::MergePeaks maxHits: " << maxHits << std::endl;

	std::set<Int_t> hitsUsed;
	for (Int_t nHits = maxHits; nHits > lowerHitLimit; nHits--) {
		std::pair<std::multimap<Int_t, Int_t>::iterator, std::multimap<Int_t, Int_t>::iterator > nHitsRange;
		nHitsRange = peaks.equal_range(nHits);
		std::cout << "PndIsochroneTrackFinder::MergePeaks nHits: " << nHits << " Peaks with nHits: " << std::endl;
		std::vector<PndPeakContent> tempResult;
		for (std::multimap<Int_t, Int_t>::iterator peakIterator = nHitsRange.first; peakIterator != nHitsRange.second; peakIterator++){
			std::set<Int_t> hitsInBin= fBinToHit[peakIterator->second];
			std::cout << "PndIsochroneTrackFinder::MergePeaks nHits: " << nHits << " /";
			for (std::set<Int_t>::iterator i = hitsInBin.begin(); i != hitsInBin.end(); i++)
				std::cout << *i << "/";
			Int_t xVal, yVal, zVal;
			fHoughHisto->GetBinXYZ(peakIterator->second, xVal, yVal, zVal);
			std::cout << " inBin: " << peakIterator->second << " (" << xVal << "/" << yVal << ")" << std::endl;
			TVector2 peakCircle(fHoughHisto->GetXaxis()->GetBinCenter(xVal), fHoughHisto->GetYaxis()->GetBinCenter(yVal));

			std::vector<PndPeakContent> separatedTracks;
			if (separateTracks){
				separatedTracks = SeparateTracks(hitsInBin, peakCircle, peakIter->second);
			} else {
				PndPeakContent myPeak;
				myPeak.fHitsInPeaks = hitsInBin;
				myPeak.AddPeak(peakIter->second);
				myPeak.fCircleCenter = peakCircle;
				separatedTracks.push_back(myPeak);
			}
			std::cout << "GeneratedSeparatedTracks: " << std::endl;
			for (Int_t sTrackIter = 0; sTrackIter < separatedTracks.size(); sTrackIter++){
				std::cout << sTrackIter << " : /";
				for(std::set<Int_t>::iterator iter = separatedTracks[sTrackIter].fHitsInPeaks.begin(); iter != separatedTracks[sTrackIter].fHitsInPeaks.end(); iter++){
					std::cout << *iter << "/";
				}
				std::cout << std::endl;
			}
			Bool_t alreadySeen = kFALSE;
			for (Int_t sTrackIter = 0; sTrackIter < separatedTracks.size(); sTrackIter++){
				for (int resultIter = 0; resultIter < result.size(); resultIter++){
					if (CheckTrackSimilarity(separatedTracks[sTrackIter].fHitsInPeaks, result[resultIter].fHitsInPeaks, trackSimilarityCut))
						//|| (CheckTrackSimilarity(separatedTracks[sTrackIter].fHitsInPeaks, result[resultIter].fHitsInPeaks, 0.6)
						//	&& CheckPeakDistance(result[resultIter].fPeakBins, *(separatedTracks[sTrackIter].fPeakBins.begin()) , 1.5)))
					{		//if nPercent of the new Hits are already in an old hit
						if ((result[resultIter].GetMeanPeakPos() - peakCircle).Mod() < fCutMergePeakDistance){
							result[resultIter].AddHits(separatedTracks[sTrackIter].fHitsInPeaks);
							if (CheckTrackSimilarity(result[resultIter].fHitsInPeaks, separatedTracks[sTrackIter].fHitsInPeaks,  0.9)){	//if nPercent of the old Hits are in the new hit
								result[resultIter].AddPeak(peakIterator->second);
								result[resultIter].FillMeanPeakPos(fHoughHisto);
								result[resultIter].FillPeaksRangeRPhi(fHoughHisto);
							}

							std::cout << "PndIsochroneTrackFinder::MergePeaks peaks merged " << result[resultIter] << std::endl;
						} else {
							std::cout << "PndIsochroneTrackFinder::MergePeaks hits too far apart " << (result[resultIter].GetMeanPeakPos() - peakCircle).Mod() << " > " << fCutMergePeakDistance << std::endl;
						}
						alreadySeen = kTRUE;
						break;
					}
				}
				if (alreadySeen == kFALSE){
					PndPeakContent myContent;
					myContent.AddHits(separatedTracks[sTrackIter].fHitsInPeaks);
					myContent.AddPeak(peakIterator->second);
					myContent.FillMeanPeakPos(fHoughHisto);
					myContent.FillPeaksRangeRPhi(fHoughHisto);
					result.push_back(myContent);
					std::cout << "PndIsochroneTrackFinder::MergePeaks new Peak added to results " << result.size() -1 << std::endl;
				}
			}
		}
	}
	return result;
}


std::vector<PndPeakContent> PndIsochroneTrackFinder::SeparateTracks(std::set<Int_t> hitsInBin, TVector2 circle, Int_t peakBin)
{
	std::map<Double_t, Int_t> arcLengthMap;
	Double_t gapSize = 10;
	std::vector<PndPeakContent> result;

	for (std::set<Int_t>::iterator hitIter = hitsInBin.begin(); hitIter != hitsInBin.end(); hitIter++){
		FairHit myHit = fHitsToFit[*hitIter];
		arcLengthMap[CalcArcLength(myHit, circle) * circle.Mod()] = *hitIter;
		std::cout << "PndIsochroneTrackFinder::SeparateTracks Hit: " << *hitIter << " ArcLength: " << CalcArcLength(myHit, circle) * circle.Mod() << std::endl;
	}
	std::map<Double_t, Int_t>::iterator separationPoint;
	Int_t nGaps = 0;
	Bool_t firstGap = kTRUE;
	Double_t oldArcLength = arcLengthMap.begin()->first;
	for (std::map<Double_t, Int_t>::iterator mapIter = arcLengthMap.begin(); mapIter != arcLengthMap.end(); mapIter++){
		if (TMath::Abs(oldArcLength - mapIter->first) > gapSize)
		{
			nGaps++;
			if (firstGap){
				separationPoint = mapIter;
				firstGap = kFALSE;
			}
		}
		oldArcLength = mapIter->first;
	}
	std::cout << "PndIsochroneTrackFinder::SeparateTracks " << nGaps << " Gaps found " << std::endl;
	if (nGaps == 1){

		PndPeakContent firstPart, secondPart;
		for(std::map<Double_t, Int_t>::iterator mapIter = arcLengthMap.begin(); mapIter != separationPoint; mapIter++){
			firstPart.fHitsInPeaks.insert(mapIter->second);
		}
		for(std::map<Double_t, Int_t>::iterator mapIter = separationPoint; mapIter != arcLengthMap.end(); mapIter++){
			secondPart.fHitsInPeaks.insert(mapIter->second);
		}
		if (firstPart.fHitsInPeaks.size() > 2){
			result.push_back(firstPart);
		}
		if (secondPart.fHitsInPeaks.size() > 2){
			result.push_back(secondPart);
		}

	} else {
		PndPeakContent myPeak;
		myPeak.fHitsInPeaks = hitsInBin;
		result.push_back(myPeak);
	}
	for (Int_t resultIter = 0; resultIter < result.size(); resultIter++){
		result[resultIter].fCircleCenter = circle;
		result[resultIter].AddPeak(peakBin);
	}
	return result;

}

Bool_t PndIsochroneTrackFinder::ContainsAB(std::set<Int_t> a, std::set<Int_t> b)
{
	std::set<Int_t>::iterator aIter = a.begin();
	std::set<Int_t>::iterator bIter = b.begin();
	std::cout << "PndIsochroneTrackFinder::ContainsAB a: /";
	for (; aIter != a.end(); aIter++){
		std::cout << *aIter << "/";
	}
	std::cout << " b: /";
	for (; bIter != b.end(); bIter++){
		std::cout << *bIter << "/";
	}
	aIter = a.begin();
	bIter = b.begin();

	for (; bIter != b.end() && aIter != a.end(); bIter++){
		while (aIter != a.end()){
			if (*bIter == *aIter){
				aIter++;
				break;
			}
			else aIter++;
		}
	}
	Bool_t result = kFALSE;
	if (aIter == a.end() && bIter == b.end())
		result = kTRUE;
	else
		result = kFALSE;
	std::cout << " Result: " << result << std::endl;
	return result;
}

Bool_t PndIsochroneTrackFinder::CheckTrackSimilarity(std::set<Int_t> first, std::set<Int_t> second, Double_t threshold)
{
	Int_t similarPoints = 0;
	for (std::set<Int_t>::iterator firstIter = first.begin(); firstIter != first.end(); firstIter++){
				similarPoints += second.count(*firstIter);
	}
	std::cout << "PndIsochroneTrackFinder::CheckTrackSimilarity similarPoints/nPoints " << similarPoints << "/" << first.size() << std::endl;
	if ((Double_t)similarPoints/first.size() > threshold)
		return kTRUE;
	else
		return kFALSE;
}

Bool_t PndIsochroneTrackFinder::CheckPeakDistance(std::set<Int_t> peakBins, Int_t binToCheck, Double_t threshold)
{
	Int_t xBin, yBin, zBin;
	Int_t xCheck, yCheck, zCheck;

	fHoughHisto->GetBinXYZ(binToCheck, xCheck, yCheck, zCheck);
	for (std::set<Int_t>::iterator peakIter = peakBins.begin(); peakIter != peakBins.end(); peakIter++){
		fHoughHisto->GetBinXYZ(*peakIter, xBin, yBin, zBin);
		std::cout << "PndIsochroneTrackFinder::CheckPeakDistance " << xBin << " + " << xCheck << " " << yBin << " - " << yCheck
				<< " : " << TMath::Sqrt(TMath::Power(xBin - xCheck, 2) + TMath::Power(yBin - yCheck, 2)) << std::endl;
		if (TMath::Sqrt(TMath::Power((Double_t)xBin - xCheck, 2) + TMath::Power((Double_t)yBin - yCheck, 2)) < threshold){
			std::cout << "Below threshold: " << threshold << std::endl;
			return true;
		}
	}
	return false;
}

/**
 * Markers for all found peaks
 * @return TGraph Graph with all peak points marked
 */
TGraph PndIsochroneTrackFinder::GetPeakCoordinateGraph() {
	TGraph peakGraph;
	for (unsigned int iPeak = 0; iPeak < fPeakCoordinates.size(); iPeak++) {
		peakGraph.SetPoint(iPeak, fPeakCoordinates[iPeak].X(), fPeakCoordinates[iPeak].Y());
	}
	peakGraph.SetMarkerStyle(kFullTriangleDown);
	peakGraph.SetMarkerColor(kMagenta);
	return peakGraph;
}

TGraph PndIsochroneTrackFinder::GetPeakCoordinateGraphMvd() {
	TGraph peakGraph;
	for (unsigned int iPeak = 0; iPeak < fPeakCoordinatesMvd.size(); iPeak++) {
		peakGraph.SetPoint(iPeak, fPeakCoordinatesMvd[iPeak].X(), fPeakCoordinatesMvd[iPeak].Y());
	}
	peakGraph.SetMarkerStyle(kFullTriangleDown);
	peakGraph.SetMarkerColor(kBlack);
	return peakGraph;
}