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

#include "PndIsochroneTrackFinder.h"
#include "cuda/PndIsochroneTrackFinderGpuInterfacer.h"
#include "PndIsoTimer.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"
#include <fstream>

ClassImp(PndIsochroneTrackFinder);

void writeTofile(float nEvent, float nHit, float nAngles, float cpuAngle, float cpuHough, float gpuAngle, float gpuHough, float nThreads, float nBlocks, TString fileName = "timings") {
	fileName += "-";
	fileName += nThreads;
	fileName += "-";
	fileName += nAngles;
	fileName += ".csv";
	std::ofstream myfile(fileName, std::ios_base::app);
	myfile << 
		nEvent << "," <<
		nHit << "," <<
		nAngles << "," <<
		cpuAngle << "," <<
		cpuHough << "," <<
		gpuAngle << "," <<
		gpuHough << "," <<
		nThreads << "," <<
		nBlocks << 
		"\n";
	myfile.close();
}

PndIsochroneTrackFinder::PndIsochroneTrackFinder(): fHoughHisto(0), fHitsToFit(0), fSttTubeArray(0), fSttSkewedArray(0), fClockwise(kFALSE), fAngleRange(0., 360.), fAngleStepSize(1.), fUseGpu(false) {
	// TODO Auto-generated constructor stub
	fHoughHisto = new TH2D("histo","histo", 1000, -300, 300, 1000, -300, 300);
	fWeightMap["STTHit"] = 1;
	fWeightMap["MVDHitsPixel"] = 1;
	fWeightMap["MVDHitsStrip"] = 1;
	fWeightMap["GemHit"] = 1;

}

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

void PndIsochroneTrackFinder::SetUseGpu(Bool_t gpu) {
    fUseGpu = gpu;
    
    std::cout << "GPU Usage is turned ";
    if (fUseGpu) {
        std::cout << "ON";
    } else {  // !fUseGpu
        std::cout << "OFF";
    }
    std::cout << std::endl;
}

void PndIsochroneTrackFinder::AddHits(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));
			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 " << 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));
			
//			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 " << myFairHit->GetEntryNr() << std::endl;
			std::cout << i << " : " << myFairHit->GetX() << "/" << myFairHit->GetY() << "/" << myFairHit->GetZ() << std::endl;

			fMapFairLinkHit[myFairHit->GetEntryNr()] = myHoughHit;
		}
	}
}

std::vector<float> PndIsochroneTrackFinder::GenerateAngles() {
	std::vector<float> resultVector;
	for (Double_t phi = fAngleRange.first; phi < fAngleRange.second; phi += fAngleStepSize) {
		if (360 < phi) {
			phi -= 360;
		}
		resultVector.push_back(phi * TMath::DegToRad());
	}
	return resultVector;
}

std::vector<TVector2> PndIsochroneTrackFinder::GenerateHoughValues(std::vector<float> angles, Double_t xVal, Double_t yVal, Double_t isoChrone) {
	std::vector<TVector2> resultVector;
//	std::cout << " -- PndIsochroneTrackFinder::GenerateHoughValues -- Printing Hough values" << std::endl;
	for (std::vector<float>::iterator it = angles.begin(); it != angles.end(); it++) {
		resultVector.push_back(IsoCalc(xVal, yVal, isoChrone, *it));
//		TVector2 temp = IsoCalc(xVal, yVal, isoChrone, *it);
//		std::cout << temp.X() << ", " << temp.Y() << std::endl;
	}
	return resultVector;
}

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

	for (Double_t iAllDegrees = 0; iAllDegrees < circleCenters.size(); iAllDegrees++){
		//Todo implement check for skewed layers
		TVector2 circleCenter = circleCenters[iAllDegrees];
		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()) < 1000 && TMath::Abs(circleCenter.Mod()) < 1000){
				//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) {
	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() > 4) {
			std::vector<std::vector<FairHit> > correctedSkewedHits = FindSkewedHits(circle);
			PndTrack myTrack = CalcPndTrack(myTrackCand, correctedSkewedHits, 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::FindTracks() {
	std::cout << "Nthreads = " << fNumberOfThreads << ", angle step siez = " << fAngleStepSize << std::endl;
	PndIsochroneTrackFinderGpuInterfacer * gpuInterfacer;
	// if (fUseGpu) {
		gpuInterfacer = new PndIsochroneTrackFinderGpuInterfacer();
		gpuInterfacer->SetNThreads(fNumberOfThreads);
		gpuInterfacer->GenerateAndSetAngles(fAngleRange, fAngleStepSize);
	// }
	PndIsoTimer * angleTimer = new PndIsoTimer();
	angleTimer->Start();
	std::vector<float> angles = GenerateAngles();
	angleTimer->Stop();

	// make std vectors for x, y, r
	std::vector<float> vX, vY, vR, vWeight;
	std::vector<Bool_t> vIsSkewed;
	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();
		Bool_t isSkewed = false;
		if (myHit.GetEntryNr().GetType() == FairRootManager::Instance()->GetBranchId("STTHit")){
			if (myHit.GetZ() > 35.5 || myHit.GetZ() < 34.5){
				std::cout << "PndIsochroneTrackFinder::FindTracks() -- skewed hit" << std::endl;
				isSkewed = true;
			}
		}
		Double_t hitWeight = fWeightMap[FairRootManager::Instance()->GetBranchName(myHit.GetEntryNr().GetType())];
		vX.push_back(xVal);
		vY.push_back(yVal);
		vR.push_back(isoChrone);
		vIsSkewed.push_back(isSkewed);
		vWeight.push_back(hitWeight);
	}

	PndIsoTimer * timerHoughGpu = new PndIsoTimer();
	timerHoughGpu->Start();
	std::vector<std::vector<TVector2> > vCircleCenters = gpuInterfacer->IsoCalc(vX, vY, vR);
	timerHoughGpu->Stop();

	PndIsoTimer * timerHoughCpu = new PndIsoTimer();
	timerHoughCpu->Start();
	std::vector<std::vector<TVector2> > vCircleCentersCpu;
	for (int i = 0; i < vX.size(); ++i) {
		vCircleCentersCpu.push_back(GenerateHoughValues(angles, vX[i], vY[i], vR[i]));
	}
	timerHoughCpu->Stop();

	std::cout << "PndIsochroneTrackFinder::FindTracks() -- Timings. CPU = " << timerHoughCpu->GetDiffMs() << " ms, GPU = " << timerHoughGpu->GetDiffMs() << " ms." <<  std::endl;

	for (int i = 0; i < vCircleCenters.size(); i++){
        // 	PndIsoTimer * circleTimer = new PndIsoTimer();
        // 	circleTimer->Start();
        //     circleCenters = GenerateHoughValues(angles, xVal, yVal, isoChrone);
        //     circleTimer->Stop();
        // // }
        // writeTofile(fEventNumber, i, gpuInterfacer->GetAnglesLength(), angleTimer->GetDiffMs(), circleTimer->GetDiffMs(), gpuInterfacer->GetTimeAngles(), gpuInterfacer->GetTimeHough(), gpuInterfacer->GetNThreads(), gpuInterfacer->GetNBlocks());
		if (!vIsSkewed[i]) FillHoughHist(vCircleCenters[i], i, vWeight[i]);
	}

	std::multimap<Int_t, Int_t> peakBins = FindPeaks(fBinsWithContent, fHoughHisto, 6);
	std::cout << "PeakFinder: " << std::endl;
	std::vector<PndTrack> result = PeaksToTracks(peakBins);
	return result;
}

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);
					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(FairRootManager::Instance()->GetBranchId("MVDHitsPixel"));
	nDetHits += cand.GetNHitsDet(FairRootManager::Instance()->GetBranchId("MVDHitsStrip"));
	nDetHits += cand.GetNHitsDet(FairRootManager::Instance()->GetBranchId("GemHit"));

	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() == FairRootManager::Instance()->GetBranchId("MVDHitsPixel") ||
					myHit.GetType() == FairRootManager::Instance()->GetBranchId("MVDHitsStrip") ||
					myHit.GetType() == FairRootManager::Instance()->GetBranchId("GemHit")) {
				std::cout << "PndIsochroneTrackFinder::AssignSkewedHits myHit " << myHit << std::endl;
				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;
				}

			}
			if (bestSkewIndex > -1){
				FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit(skewedHits[sttHitIndex][bestSkewIndex]);
//				FairHit* newSkewedHit = new ((*fSttSkewedArray)[fSttSkewedArray->GetEntriesFast()]) FairHit();
//				newSkewedHit->SetXYZ(skewedHits[sttHitIndex][bestSkewIndex].GetX(), skewedHits[sttHitIndex][bestSkewIndex].GetY(), skewedHits[sttHitIndex][bestSkewIndex].GetZ());
//				newSkewedHit->SetDxyz(skewedHits[sttHitIndex][bestSkewIndex].GetDx(), skewedHits[sttHitIndex][bestSkewIndex].GetDy(), skewedHits[sttHitIndex][bestSkewIndex].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(), bestArcLength);
			}

//			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);

		}
	}

}

FairTrackParP PndIsochroneTrackFinder::CalcTrackParP(FairHit hit, TVector2 circleCenter, Double_t mag)
{
	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(mag, 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;
	if (fClockwise)
		q = 1;
	else
		q = -1;
	FairTrackParP result(hitPos, ptVec, hitPosError, momError, q, origin, dj, dk);
	return result;
}

PndTrack PndIsochroneTrackFinder::CalcPndTrack(PndTrackCand& cand, std::vector<std::vector<FairHit> > skewedHits, TVector2 circle, Double_t mag) {

	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, mag);
	FairTrackParP lastPar  = CalcTrackParP(lastHit, circle, mag);
	PndTrack result(firstPar, lastPar, cand);
	return result;
}

Double_t PndIsochroneTrackFinder::CalcPt(Double_t mag, TVector2 circleCenter) {
	double result = 0.3 * mag * 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;
	fHoughHisto->GetBinXYZ(histoBin, binX, binY, binZ);
	TVector2 circleCenter(fHoughHisto->GetXaxis()->GetBinCenter(binX), fHoughHisto->GetYaxis()->GetBinCenter(binY));

	std::pair<std::multimap<Int_t, Int_t>::iterator, std::multimap<Int_t, Int_t>::iterator > hitIter;
	std::set<Int_t> hitsInBin= fBinToHit[histoBin];
	std::map<Double_t, FairLink> trackCandHits;
	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 fHitsUsed.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.GetEntryNr();
	}

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

	fClockwise = kFALSE;  //check the orientation of your circle. Standard is counterclockwise.
	if (trackCandHits.size() > 0 && trackCandHits.rbegin()->first > TMath::Pi()){
		if ((2 * TMath::Pi()) - trackCandHits.rbegin()->first < trackCandHits.begin()->first){
			fClockwise = kTRUE;
		}
	}

	for (std::map<Double_t, FairLink>::iterator iter = trackCandHits.begin(); iter != trackCandHits.end(); iter++){
			Double_t correctedRho;
			if (fClockwise == kTRUE){
				correctedRho = 2 * TMath::Pi() - iter->first;
			} else {
				correctedRho = iter->first;
			}
			result.AddHit(iter->second, correctedRho);
	}
	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(Double_t x0, Double_t y0, Double_t isoR, Double_t phi) {  // Calculate a point lying on a isochrone of radius isoR around (x0,y0) at angle phi
	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);
				if (bin == 997473){
					std::cout << "Bin 997473 " << hitId << " " << fHoughHisto->GetBinContent(997473) << std::endl;
					std::cout << "currentHistoIndex: " << currentHistoIndex << " GetHistoBin: " << GetHistoBin(histo, runVec.X(), runVec.Y()) << std::endl;
				}
			}
//			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));
		}
	}
	return result;
}

/**
 * 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;
}