#include "PndSttCellTrackFinder.h"
#include "PndSttHit.h"
#include "TString.h"
#include "PndSttHit.h"
#include "FairHit.h"

#include "PndSttStrawMap.h"
#include "PndSttGeometryMap.h"
#include "PndSttTube.h"
#include "PndRiemannHit.h"
#include "FairEventHeader.h"
#include "FairRootManager.h"

#include "TMath.h"

#include <iostream>
#include <math.h>
#include <vector>
#include <set>
#include <map>
#include <algorithm>

using namespace std;

ClassImp(PndSttCellTrackFinder)
;

void PndSttCellTrackFinder::SetSttTubeArray(TClonesArray* sttTubeArray) {
	if (fGeometryMap == 0) {
		fStrawMap.GenerateStrawMap(sttTubeArray);
		fGeometryMap = new PndSttGeometryMap(sttTubeArray, 1);
	}

	PndSttTube* tube;
	TVector3 pos;

	for (int i = 1; i < sttTubeArray->GetEntriesFast(); ++i) {
		tube = (PndSttTube*) sttTubeArray->At(i);
		pos = tube->GetPosition();
		fMapTubeIdToPos[i] = pos;
	}

}

void PndSttCellTrackFinder::AddHits(TClonesArray* hits, Int_t branchId) {
	TString geoPath;
	PndSttHit* myHit;
	FairLink myID;

	if (branchId == FairRootManager::Instance()->GetBranchId("STTHit")) {
		for (int i = 0; i < hits->GetEntries(); i++) {
			myHit = (PndSttHit*) (hits->At(i));

			if (myHit->GetEntryNr().GetIndex() < 0) {
				myID = FairLink(branchId, i);
				myHit->SetEntryNr(FairLink(branchId, i));
			} else
				myID = myHit->GetEntryNr();
			myHit->SetDxyz(myHit->GetIsochrone(), myHit->GetIsochrone(), 100);

			//fMapHitToFairLink[fHits.size() - 1] = myID;
			fMapHitToFairLink[i] = myID;
			fHits.push_back((FairHit*)myHit);

		}
	} else if (branchId == FairRootManager::Instance()->GetBranchId("STTCombinedSkewedHits")){
		for (int i = 0; i < hits->GetEntries(); i++) {
			PndSttSkewedHit* skewedHit = (PndSttSkewedHit*)(hits->At(i));
			int tubeId = skewedHit->GetTubeIDs().first;
			fCombinedSkewedHits.insert(std::pair<int, PndSttSkewedHit*>(tubeId, skewedHit));
			if (skewedHit->GetEntryNr().GetIndex() < 0) {
				myID = FairLink(branchId, i);
				skewedHit->SetEntryNr(FairLink(branchId, i));
			} else
				myID = skewedHit->GetEntryNr();
		}
	}

}

void PndSttCellTrackFinder::FindTracks() {

	if (fVerbose > 0) {
		cout << "PndSttCellTrackFinder::FindTracks()" << endl;
	}

	GenerateTracklets();

	//CombineTracklets();

	//CreateFurtherCombinations();

	CombineTrackletsMultiStages();

	if (fVerbose > 0) {

		cout << "#Found Combinations (recursive): " << fStateCombinations.size()
				<< endl;

		cout << "#Accepted Combination of Tracklets: " << fCombinedData.size() << endl;
		for (int i = 0; i < fCombinedData.size(); ++i) {
			cout << "Combination #" << i << endl;
			cout << "state of tracklets: ";
			for (set<int>::iterator iter = fCombinedData[i].tracklets.begin(); iter != fCombinedData[i].tracklets.end(); iter++) {
				cout << *iter << ", ";
			}
			cout << endl;
			/*cout << " radius: " << fCombinedData[i].trackletInf.riemannTrack.r()
			 << ", center: "
			 << fCombinedData[i].trackletInf.riemannTrack.orig()[0]
			 << ", "
			 << fCombinedData[i].trackletInf.riemannTrack.orig()[1]
			 << " , error: " << fCombinedData[i].trackletInf.error
			 << ", #wrong hits: "
			 << fCombinedData[i].trackletInf.numErrHits << endl;

			 if (fStartTracklets[fCombinedData[i].tracklets[0]].riemannTrack.getNumHits()
			 != 0) {
			 cout << "riemannTrack before:" << " , radius: "
			 << fStartTracklets[fCombinedData[i].tracklets[0]].riemannTrack.r()
			 << ", center: "
			 << fStartTracklets[fCombinedData[i].tracklets[0]].riemannTrack.orig()[0]
			 << ", "
			 << fStartTracklets[fCombinedData[i].tracklets[0]].riemannTrack.orig()[1]
			 << " , error: "
			 << fStartTracklets[fCombinedData[i].tracklets[0]].error
			 << ", #wrong hits: "
			 << fStartTracklets[fCombinedData[i].tracklets[0]].numErrHits
			 << endl;

			 }*/
		}

	}

	FindTrackletsWithoutCombi();

	AssignAmbiguousHits();

	AddMissingHits();

	CreatePndTrackCands();

}

void PndSttCellTrackFinder::CreatePndTrackCands() {

	int state, numHits;

	// for each combination
	for (int i = 0; i < fCombinedData.size(); ++i) {
		PndTrackCand trackCand;

		// add all hits of combined tracklet to trackCand
		for (int j = 0; j < fCombinedData[i].trackletInf.hitIDs.size(); ++j) {
			trackCand.AddHit(fMapHitToFairLink[fCombinedData[i].trackletInf.hitIDs[j]],	j);
		}

		fCombiTrackCand.push_back(trackCand);
		fCombiTrack.push_back(fCombinedData[i].trackletInf.riemannTrack.getPndTrack(2.0)); //todo replace fixed magnetic field with info from simulation
	}

	// add tracklets without combi and more than 2 hits
	for (int i = 0; i < fTrackletsWithoutCombi.size(); ++i) {
		PndTrackCand trackCand;
		numHits = fStartTracklets[fTrackletsWithoutCombi[i]].hitIDs.size();

		if (numHits > 2) {
			std::cout << "Tracklet: " << fTrackletsWithoutCombi[i] << " : ";
			for (int j = 0; j < numHits; ++j) {
				trackCand.AddHit(fMapHitToFairLink[fStartTracklets[fTrackletsWithoutCombi[i]].hitIDs[j]], j);
				std::cout << fMapHitToFairLink[fStartTracklets[fTrackletsWithoutCombi[i]].hitIDs[j]] << " ";
			}

			PndRiemannTrack trackRefit = CreateRiemannTrack(fStartTracklets[fTrackletsWithoutCombi[i]].hitIDs);
			trackRefit.refit(kFALSE);

			std::cout << std::endl << "RiemannTrack: " << trackRefit <<std::endl;
			fCombiTrackCand.push_back(trackCand);
			fCombiTrack.push_back(trackRefit.getPndTrack(2.0));
		}
	}
}

void PndSttCellTrackFinder::GenerateTracklets() {

	//cout << "================= Generate Tracklets ===============" << endl;

	// init states of cells with the id of the tube
	PndSttHit* sttHit;
	for (int i = 0; i < fHits.size(); ++i) {
		sttHit = (PndSttHit*) fHits[i];
		fStates[sttHit->GetTubeID()] = sttHit->GetTubeID();
		fMapTubeIdToHit[sttHit->GetTubeID()] = i;
	}

	FindHitNeighbors();
	SeparateNeighbors();
	CorrectIsochrones();
	EvaluateState();

	if (fVerbose > 1) {
		std::cout << "States: " << std::endl;
		for (map<int, int>::iterator iter = fStates.begin(); iter != fStates.end(); iter++) {
			std::cout << iter->first << " : " << iter->second << std::endl;
		}
	}


	InitStartTracklets();

	if (fVerbose > 1) {

		cout << "Start-Tracklets#: " << fStartTracklets.size() << endl;

		cout << "Tracklet-Information: " << endl;
		map<int, TrackletInf_t>::iterator it;
		for (it = fStartTracklets.begin(); it != fStartTracklets.end(); ++it) {
			int state = it->first;
			TrackletInf_t inf = it->second;

			cout << "State: " << state << " ";
			inf.Print();
		}

	}

	SetVerbose(0);
	EvaluateMultiState();
	SetVerbose(3);

	if (fVerbose > 1) {
		std::cout << "MultiStates: " << std::endl;
		for (map<int, set<int> >::iterator iter = fMultiStates.begin();
				iter != fMultiStates.end(); iter++) {
			std::cout << iter->first << " : ";
			for (set<int>::iterator iter2 = iter->second.begin();
					iter2 != iter->second.end(); iter2++) {
				std::cout << *iter2 << " ";
			}
			std::cout << std::endl;
		}
	}

}

void PndSttCellTrackFinder::FindHitNeighbors() {

	/* Approach: At first create a set of the tubeIDs of all hits.
	 * Then get the neighbors of each hit/tube and store only those
	 * that are included in the set.*/

	PndSttHit* sttHit;
	int tubeId;
	set<int> hitIds;

	// init set with straw-ids of hits
	for (int i = 0; i < fHits.size(); ++i) {
		sttHit = (PndSttHit*) fHits[i];
		hitIds.insert(sttHit->GetTubeID());
	}

	// fill fHitNeighbors
	for (int i = 0; i < fHits.size(); ++i) {
		sttHit = (PndSttHit*) fHits[i];
		tubeId = sttHit->GetTubeID();
		//get neighbors
//		TArrayI tmp = fGeometryMap->FindNeighborings(tubeId);
		TArrayI tmp = fGeometryMap->GetNeighboringsByMap(tubeId);

		for (int j = 0; j < tmp.GetSize(); ++j) {

			// set contains neighbor?
			if (hitIds.find(tmp[j]) != hitIds.end()) {

				if (fStrawMap.IsEdgeStraw(tubeId)) {
					if (!fStrawMap.IsEdgeStraw(tmp[j]))
						fHitNeighbors[tubeId].push_back(tmp[j]);

				} else {
					fHitNeighbors[tubeId].push_back(tmp[j]);
				}

			}

		}
	}
}

void PndSttCellTrackFinder::SeparateNeighbors() {

	/* Separate the hits/active cells concerning the number of neighbors that made a signal, too.*/

	map<int, vector<int> >::iterator it;

	for (int i = 0; i < 8; ++i) {
		fSeparations[i].clear();
	}

	for (it = fHitNeighbors.begin(); it != fHitNeighbors.end(); ++it) {

		if (it->second.size() > 6) {
			// store cells with more than 6 neighbors in the same entry
			fSeparations[7].push_back(it->first);
		} else {
			// separation concerning 0-6 hit-neighbors
			fSeparations[it->second.size()].push_back(it->first);
		}
	}
}


void PndSttCellTrackFinder::CorrectIsochrones()
{
	for (int i = 0; i < fSeparations[2].size(); i++){
		int actualTubeId = fSeparations[2][i];
		std::vector<std::vector<double> > angles;

		if (fStrawMap.IsSkewedStraw(actualTubeId)) continue;


		std::cout << "PndSttCellTrackFinder::CorrectIsochrones : " << actualTubeId << std::endl;

		for (int j = 0; j < fHitNeighbors[actualTubeId].size(); j++){
			int neighborTubeId = fHitNeighbors[actualTubeId][j];
			std::cout << "NeighborTube: " << neighborTubeId << " : " << std::endl;
			angles.push_back(CalculateTangentAngles((PndSttHit*)fHits[fMapTubeIdToHit[actualTubeId]], (PndSttHit*)fHits[fMapTubeIdToHit[neighborTubeId]]));
		}

		if (angles.size() == 2) {
			std::vector<std::vector<std::vector<double> > > differences;
			std::vector<double> classification;
			Double_t bestPhi = 0;

			for (std::vector<std::vector<double> >::iterator iterFirst = angles.begin(); iterFirst != angles.end() - 1; iterFirst++){
				for (std::vector<std::vector<double> >::iterator iterSecond = iterFirst + 1; iterSecond != angles.end(); iterSecond++){
					std::vector<std::vector<double> > tmp2D;
					differences.push_back(tmp2D);
					for(int k = 0; k < iterFirst->size(); k++){
						Double_t smallestDifference = 1000;
						Double_t secondSmallestDifference = 1000;
						std::vector<double> tmp;
						differences.back().push_back(tmp);
						for (int j = 0; j < iterSecond->size(); j++){
							std::vector<double> first = *iterFirst;
							std::vector<double> second = *iterSecond;

							std::cout << "Phi 0: " << k << " : " << TMath::RadToDeg() * first[k] << " Phi 1: " << j << " : " << TMath::RadToDeg() * second[j] << std::endl;
							differences.back().at(k).push_back(fabs(first[k] - second[j]));

							if (fabs(first[k] - second[j]) < smallestDifference){
								secondSmallestDifference = smallestDifference;
								smallestDifference = fabs(first[k] - second[j]);
								bestPhi = (first[k] + second[j])/2;
								std::cout << "SmallestDifference: " << smallestDifference << " at: " << TMath::RadToDeg() * bestPhi << " SecondSmallestDifference: " << secondSmallestDifference << std::endl;
							}
						}
					}
					std::cout << std::endl;

				}

			}

//			for(int k = 0; k < angles[0].size(); k++){
//					std::vector<double> tmp;
//					differences.push_back(tmp);
//					for (int j = 0; j < angles[1].size(); j++){
//
//						std::cout << "Phi0: " << k << " : " << angles[0][k] << " Phi1 " << j << " : " << angles[1][j] << std::endl;
//						differences[k].push_back(fabs(angles[0][k] - angles[1][j]));
//
//						if (fabs(angles[0][k] - angles[1][j]) < smallestDifference){
//							secondSmallestDifference = smallestDifference;
//							smallestDifference = fabs(angles[0][k] - angles[1][j]);
//							bestPhi = (angles[0][k] + angles[1][j])/2;
//							std::cout << "SmallestDifference: " << smallestDifference << " at: " << bestPhi << " SecondSmallestDifference: " << secondSmallestDifference << std::endl;
//						}
//					}
//				}
//				std::cout << std::endl;


			CalcClassification(differences);

			PndSttHit* actualHit = (PndSttHit*)fHits[fMapTubeIdToHit[actualTubeId]];
			TVector3 origin(actualHit->GetX(), actualHit->GetY(), actualHit->GetZ());
			TVector3 correctedPosition(origin.x() + actualHit->GetIsochrone() * TMath::Cos(bestPhi),
					origin.y() + actualHit->GetIsochrone() * TMath::Sin(bestPhi), origin.z());
			TVector3 hitError(0.1, 0.1, 100);
			std::cout << "Corrected Position: "; correctedPosition.Print(); std::cout << std::endl;
			fCorrectedIsochrones[actualTubeId] = FairHit(-1, correctedPosition, hitError, -1);
		}
	}
}


std::vector<std::vector<double> > PndSttCellTrackFinder::CalcClassification(std::vector<std::vector<std::vector<double> > > differences)
{
	std::vector<std::vector<double> > smallestValues;
	std::vector<std::vector<std::pair<int, int> > > pairsOfSmallestValues;
	std::vector<std::vector<double> > classification;

	std::cout << "PndSttCellTrackFinder::CalcClassification: classification" << std::endl;

	std::vector<double> sumOfSmallestValues;

	int out = 0;
	for (std::vector<std::vector<std::vector<double> > >::iterator outerIter = differences.begin(); outerIter != differences.end(); outerIter++)
	{
		sumOfSmallestValues.push_back(0);
		std::vector<double> tmp;
		smallestValues.push_back(tmp);
		classification.push_back(tmp);

		std::vector<std::pair<int, int> > tmp2;
		pairsOfSmallestValues.push_back(tmp2);

		for (int i = 0; i < outerIter->size(); i++){
			double smallestValue = 10000;
			std::pair<int, int> pairOfSmallest;

			for (int j = 0; j < outerIter->at(i).size(); j++) {
				std::cout << "Pairs: " << i << "/" << j << " : " << outerIter->at(i)[j] << std::endl;
				if (outerIter->at(i)[j] < smallestValue){
					smallestValue = outerIter->at(i)[j];
					pairOfSmallest = std::make_pair(i,j);
				}
			}

			std::cout << std::endl;

			smallestValues[out].push_back(smallestValue);
			pairsOfSmallestValues[out].push_back(pairOfSmallest);
			sumOfSmallestValues[out] += smallestValue;
		}
		for (int i = 0; i < smallestValues[out].size(); i++){
			classification[out].push_back(1- smallestValues[out][i]/sumOfSmallestValues[out]);
			std::cout << "Pair: " << pairsOfSmallestValues[out][i].first << "/" << pairsOfSmallestValues[out][i].second << " Value: " << TMath::RadToDeg() * smallestValues[out][i] << " Classification: " << classification[out][i] << std::endl;
		}
		out++;

	}

	return classification;
}


std::vector<double> PndSttCellTrackFinder::CalculateTangentAngles(PndSttHit* tube1, PndSttHit* tube2)
{
	Double_t radius1 = tube1->GetIsochrone();
	Double_t radius2 = tube2->GetIsochrone();

	TVector3 origin1, origin2;
	tube1->Position(origin1);
	tube2->Position(origin2);

	TVector3 directionBetweenTubes = origin2 - origin1;

	std::cout << "Origin1: "; origin1.Print(); std::cout << " Origin 2: "; origin2.Print(); std::cout << " Dir: "; directionBetweenTubes.Print(); std::cout << std::endl;
	std::cout << "Radius1: " << radius1 << " Radius2: " << radius2 << std::endl;

	Double_t R, r;
	Bool_t tube1Larger = kTRUE;
	if (radius1 > radius2){
		R = radius1;
		r = radius2;
		tube1Larger = kTRUE;
	} else {
		R = radius2;
		r = radius1;
		tube1Larger = kFALSE;
	}

	Double_t phi0 = directionBetweenTubes.Phi();
	Double_t phi1 = TMath::Sin((R-r)/directionBetweenTubes.Mag());
	Double_t phi2 = TMath::Sin((R+r)/directionBetweenTubes.Mag());

	std::cout << "Phi0: " << TMath::RadToDeg() * phi0 << " Phi1: " << TMath::RadToDeg() *phi1 << " Phi2: " << TMath::RadToDeg() * phi2 << std::endl << std::endl;

	std::vector<double> results;
	if (tube1Larger) {
		Double_t angle1 = phi0 + phi1 - TMath::Pi() / 2;
		if (angle1 < 0) angle1 += TMath::Pi() * 2;
		results.push_back(angle1);

		Double_t angle2 = phi0 - phi1 + TMath::Pi() / 2;
		if (angle2 < 0) angle2 += TMath::Pi() * 2;
		results.push_back(angle2);

		Double_t angle3 = phi0 + phi2 - TMath::Pi() / 2;
		if (angle3 < 0) angle3 += TMath::Pi() * 2;
		results.push_back(angle3);

		Double_t angle4 = phi0 - phi2 + TMath::Pi() / 2;
		if (angle4 < 0) angle4 += TMath::Pi() * 2;
		results.push_back(angle4);

		std::cout << "Phi0 + Phi1 - Pi/2 = " << TMath::RadToDeg() * angle1 << std::endl;
		std::cout << "Phi0 - Phi1 + Pi/2 = " << TMath::RadToDeg() * angle2 << std::endl;
		std::cout << "Phi0 + Phi2 - Pi/2 = " << TMath::RadToDeg() * angle3 << std::endl;
		std::cout << "Phi0 - Phi2 + Pi/2 = " << TMath::RadToDeg() * angle4 << std::endl << std::endl;
	} else {
		Double_t angle1 = phi0 + phi1 + TMath::Pi() / 2;
		if (angle1 < 0) angle1 += TMath::Pi() * 2;
		results.push_back(angle1);

		Double_t angle2 = phi0 - phi1 - TMath::Pi() / 2;
		if (angle2 < 0) angle2 += TMath::Pi() * 2;
		results.push_back(angle2);

		Double_t angle3 = phi0 + phi2 - TMath::Pi() / 2;
		if (angle3 < 0) angle3 += TMath::Pi() * 2;
		results.push_back(angle3);

		Double_t angle4 = phi0 - phi2 + TMath::Pi() / 2;
		if (angle4 < 0) angle4 += TMath::Pi() * 2;
		results.push_back(angle4);

		std::cout << "Phi0 + Phi1 + Pi/2 = " << TMath::RadToDeg() * angle1 << std::endl;
		std::cout << "Phi0 - Phi1 - Pi/2 = " << TMath::RadToDeg() * angle2 << std::endl;
		std::cout << "Phi0 + Phi2 - Pi/2 = " << TMath::RadToDeg() * angle3 << std::endl;
		std::cout << "Phi0 - Phi2 + Pi/2 = " << TMath::RadToDeg() * angle4 << std::endl << std::endl;

	}

	return results;

}

void PndSttCellTrackFinder::EvaluateState() {

	/* Approach: The state has to be calculated for cells with 1 or 2 hit-neighbors (no ambiguities).
	 * Each cell starts with an unique state-ID (tubeID). At each step the cells analyzes similar cells
	 * (1/2 hit-neighbors) and calculate the minimum of all states. The states will be changed to
	 * this minimum simultaneously after the calculation. If there are no changes at consecutive steps,
	 * the evaluation of the states is finished. Cells with the same state-ID belongs to the same track
	 * and form a tracklet.*/

	int currentSum = 0;
	int priorSum = -1;
	int newState;
	set<int> plainHitIds;
	vector<int>::iterator it;
	map<int, int>::iterator itStates;
	vector<int> neighbors;
	map<int, int> tmpStates;

	// set with tube-ids of hits without ambiguity
	plainHitIds.insert(fSeparations[1].begin(), fSeparations[1].end());
	plainHitIds.insert(fSeparations[2].begin(), fSeparations[2].end());

	// calculate for tubes with only one or two hits in neighborings until the sum of states does not change anymore
	while (currentSum != priorSum) {

		priorSum = currentSum;
		currentSum = 0;

		// calculate new state of tubes with one hit in neighboring
		for (it = fSeparations[1].begin(); it < fSeparations[1].end(); ++it) {
			neighbors = fHitNeighbors[(*it)];

			// include only state of hits without ambiguity
			if (plainHitIds.find(neighbors[0]) != plainHitIds.end()) {
				newState = min(fStates[(*it)], fStates[neighbors[0]]);
				tmpStates[(*it)] = newState;
				currentSum += newState;
			} else {
				// no hit neighboring to consider
				tmpStates[(*it)] = fStates[(*it)];
				currentSum += newState;
			}

		}
		// calculate new state of tubes with two hits in neighboring
		for (it = fSeparations[2].begin(); it < fSeparations[2].end(); ++it) {
			neighbors = fHitNeighbors[(*it)];

			// include only state of hits without ambiguity
			if (plainHitIds.find(neighbors[1]) == plainHitIds.end())
				neighbors.erase(neighbors.begin() + 1);
			if (plainHitIds.find(neighbors[0]) == plainHitIds.end())
				neighbors.erase(neighbors.begin());

			// calculate minimum
			switch (neighbors.size()) {
			case 2:
				newState = min(fStates[(*it)],
						min(fStates[neighbors[0]], fStates[neighbors[1]]));
				break;
			case 1:
				newState = min(fStates[(*it)], fStates[neighbors[0]]);
				break;
			case 0:
				newState = fStates[(*it)];
			}

			tmpStates[(*it)] = newState;
			currentSum += newState;
		}

		// update states simultaneously
		fStates.clear();
		fStates.insert(tmpStates.begin(), tmpStates.end());

	}
}

void PndSttCellTrackFinder::EvaluateMultiState() {

	/* Approach: For 3 and 4 hit-neighbors the cell value is a copy of all indices of the neighboring cells.
	 * The copying is repeated until the values do not change anymore. In this way the cell content is a collection
	 * of all unique tracklets touching the cluster of ambiguous cells.*/

	int currentSum = 0;
	int priorSum = -1;
	set<int> newState;
	set<int> plainHitIds;
	set<int>::iterator it;
	map<int, int>::iterator itStates;
	vector<int> neighbors;
	map<int, set<int> > tmpStates;
	map<int, TrackletInf_t>::iterator trackletIt;

	// set with tube-ids of hits without ambiguity
	plainHitIds.insert(fSeparations[3].begin(), fSeparations[3].end());
	plainHitIds.insert(fSeparations[4].begin(), fSeparations[4].end());
	plainHitIds.insert(fSeparations[5].begin(), fSeparations[5].end());
	plainHitIds.insert(fSeparations[6].begin(), fSeparations[6].end());
	plainHitIds.insert(fSeparations[7].begin(), fSeparations[7].end());

	// generate multistate for short tracklets too
	for (trackletIt = fShortTracklets.begin();
			trackletIt != fShortTracklets.end(); ++trackletIt) {

		for (int i = 0; i < trackletIt->second.hitIDs.size(); ++i) {
			PndSttHit* sttHit = (PndSttHit*) fHits[trackletIt->second.hitIDs[i]];
			int tubeID = sttHit->GetTubeID();
			plainHitIds.insert(tubeID);
		}
	}

	if (fVerbose > 2) {
		std::cout << "EvaluateMultiState: PlainHitIds: ";
		for (it = plainHitIds.begin(); it != plainHitIds.end(); it++) {
			std::cout << *it << " ";
		}
		std::cout << std::endl;
		std::cout << "fStates: " << std::endl;
		for (map<int, int>::iterator mapIt = fStates.begin();
				mapIt != fStates.end(); mapIt++) {
			if (fVerbose > 0)
				std::cout << mapIt->first << " : " << mapIt->second
						<< std::endl;
		}

	}

	int loopcount = 0;
	while (currentSum != priorSum) {
		if (fVerbose > 2) {
			std::cout << std::endl;
			std::cout << "LoopCount: " << loopcount++ << " currentSum: "
					<< currentSum << " priorSum: " << priorSum << std::endl;
		}

		priorSum = currentSum;
		currentSum = 0;

		for (it = plainHitIds.begin(); it != plainHitIds.end(); ++it) {
			neighbors = fHitNeighbors[(*it)];
			newState.clear();
			if (fVerbose > 2) {
				std::cout << "PlainHitId: " << *it << std::endl;
				std::cout << "Neighbors: " << std::endl;
			}

			for (int i = 0; i < neighbors.size(); i++) {
				if (fVerbose > 2)
					std::cout << neighbors[i] << " : ";
				if (fMultiStates.count(neighbors[i]) > 0) {
					newState.insert(fMultiStates[neighbors[i]].begin(),
							fMultiStates[neighbors[i]].end());
					if (fVerbose > 2) {
						for (std::set<int>::iterator iter =
								fMultiStates[neighbors[i]].begin();
								iter != fMultiStates[neighbors[i]].end();
								iter++) {
							std::cout << *iter << "/";
						}
					}
				} else if (fStates.count(neighbors[i]) > 0) {
					newState.insert(fStates[neighbors[i]]);
					if (fVerbose > 2)
						std::cout << fStates[neighbors[i]];
				}
				if (fVerbose > 2)
					std::cout << " | ";
			}
			if (fVerbose > 2)
				std::cout << std::endl;

			if (newState.size() > tmpStates[(*it)].size())
				tmpStates[(*it)] = newState;

			if (fVerbose > 2)
				std::cout << "NewState for " << *it << " : ";
			for (std::set<int>::iterator stateIter = newState.begin();
					stateIter != newState.end(); stateIter++) {
				if (fVerbose > 2)
					std::cout << *stateIter << " | ";
				currentSum += *stateIter;
			}
			if (fVerbose > 2)
				std::cout << std::endl;
		}

		// update states simultaneously
		fMultiStates.clear();
		fMultiStates.insert(tmpStates.begin(), tmpStates.end());
	}

}

void PndSttCellTrackFinder::InitStartTracklets() {

	vector<int>::iterator it;
	map<int, vector<int> > tmpStartTracklets;
	map<int, vector<int> >::iterator trackIt;

	// get states + hitIDs of tracklets with 1 or 2 hit-neighbors
	for (it = fSeparations[1].begin(); it < fSeparations[1].end(); ++it) {
		tmpStartTracklets[fStates[(*it)]].push_back(fMapTubeIdToHit[(*it)]);

	}

	for (it = fSeparations[2].begin(); it < fSeparations[2].end(); ++it) {
		tmpStartTracklets[fStates[(*it)]].push_back(fMapTubeIdToHit[(*it)]);
	}

	vector<int> hitIDs;
	int state;
	PndSttHit* sttHit;
	int tubeID;

	std::cout << "Calculation of StartTracklets: " << std::endl;
	for (trackIt = tmpStartTracklets.begin();
			trackIt != tmpStartTracklets.end(); ++trackIt) {

		state = trackIt->first;
		TrackletInf_t trackletInf;
		trackletInf.hitIDs.insert(trackletInf.hitIDs.begin(),
				trackIt->second.begin(), trackIt->second.end());

		hitIDs = trackletInf.hitIDs;
		PndTrackCand trackCand;
		trackletInf.startID = state;
		trackletInf.maxID = 0;
		trackletInf.endID = state;
		trackletInf.straight = false;
		trackletInf.numSkewed = 0;
		bool endID;

		for (int i = 0; i < hitIDs.size(); ++i) {

			sttHit = (PndSttHit*) fHits[hitIDs[i]];
			tubeID = sttHit->GetTubeID();

			if (fStrawMap.IsSkewedStraw(tubeID)) {
				++trackletInf.numSkewed;
			}

			endID = IsEndTubeOfTracklet(tubeID);

			// found tubeID that's bigger than maxID of tracklet?
			if (tubeID > trackletInf.maxID) {

				// if tube is end of tracklet --> straight tracklet
				if (fStrawMap.GetRow(tubeID)
						== fStrawMap.GetRow(trackletInf.maxID)) {
					// Hits in same row --> not a straight track
					trackletInf.straight = false;
				}
				if (endID) {
					// no hits in same row and hit in end-tube
					trackletInf.straight = true;
				} else {
					// hit was not in end-tube
					trackletInf.straight = false;
				}

				trackletInf.maxID = tubeID;
			}

			// safe ID of end-tube
			if (endID && tubeID != state) {
				trackletInf.endID = tubeID;
			}

			if (fCalcFirstTrackletInf) {
				// add hits to TrackCand
				trackCand.AddHit(fMapHitToFairLink[hitIDs[i]], 0);
			}

			// erase state-entries for hits of short tracklets --> that's important for generating multistates
			if (hitIDs.size() < 3) {
				fStates.erase(tubeID);
			}
		}

		if (fCalcFirstTrackletInf) {
			// calc riemannTrack
			PndRiemannTrack riemannTrack = CreateRiemannTrack(hitIDs);

			// add riemannTrack to trackletInf if possible
			if (riemannTrack.getNumHits() > 2) {
				riemannTrack.refit(false);
				trackletInf.riemannTrack = riemannTrack;
				trackletInf.error = CalcDeviationOfRiemannTrack(riemannTrack);
				trackletInf.numErrHits = GetDeviationCount(riemannTrack);
				fFirstRiemannTrack.push_back(riemannTrack);

			}
			fFirstTrackCand.push_back(trackCand);
		}

		std::cout << state << " : " << trackletInf.riemannTrack << std::endl;

		if (trackIt->second.size() < 3) {
			// save as short tracklet
			fShortTracklets[state] = trackletInf;

		} else {
			// tracklet with enough hits
			fStartTracklets[state] = trackletInf;

		}

	}
}

//void PndSttCellTrackFinder::CombineTracklets() {

//	vector<int> leftTrackCands; // State of tracklets on the left side of stt
//	vector<int> rightTrackCands; // State of tracklets on the right side of stt
//
//	map<int, TrackletInf_t>::iterator trackletsIt;
//
//	int tubeId;
//	vector<int> hits;
//
//	// devide tracklets into tracklets of left and right half of the stt to get less combination possibilities
//	for (trackletsIt = fStartTracklets.begin();
//			trackletsIt != fStartTracklets.end(); ++trackletsIt) {
//
//		// state is equal to tubeID of on hit in tracklet
//		tubeId = trackletsIt->first;
//		hits = trackletsIt->second.hitIDs;
//
//		// add only tracklets with more than two hits of not skewed tubes
//		if ((hits.size() - trackletsIt->second.numSkewed) > 2) {
//
//			// check the sector of the tube
//			if (fStrawMap.GetSector(tubeId) <= 2) {
//				// tracklet is located in the left half of stt
//				leftTrackCands.push_back(tubeId);
//			} else {
//				// tracklet is located in the right half of stt
//				rightTrackCands.push_back(tubeId);
//			}
//
//		}
//
//	}
//
//	// sort tracklets by state
//	sort(leftTrackCands.begin(), leftTrackCands.end());
//	sort(rightTrackCands.begin(), rightTrackCands.end());
//
//	int firstState, secondState, sector;
//	vector<int> tracklets;
//	set<int> sectorNeighbors;
//
//	// combine the leftTrackCands
//	for (int i = 0; i < leftTrackCands.size(); ++i) {
//
//		firstState = leftTrackCands[i];
//
//		// combine straight and "unfinished" tracklets only
//		if (fStartTracklets[firstState].straight
//				&& fHitNeighbors[fStartTracklets[firstState].maxID].size()
//						> 1) {
//
//			sector = fStrawMap.GetSector(fStartTracklets[firstState].maxID);
//			sectorNeighbors.clear();
//			sectorNeighbors.insert(sector);
//			sectorNeighbors.insert(fStrawMap.GetLeftSector(sector));
//			sectorNeighbors.insert(fStrawMap.GetRightSector(sector));
//
//			for (int j = i + 1; j < leftTrackCands.size(); ++j) {
//				secondState = leftTrackCands[j];
//
//				// combine with tracklets with a higher state than maxID of first tracklet
//				if (secondState > fStartTracklets[firstState].maxID) {
//
//					// is second tracklet in nearby sector?
//					if (sectorNeighbors.find(fStrawMap.GetSector(secondState))
//							!= sectorNeighbors.end()) {
//
//						tracklets.clear();
//						tracklets.push_back(firstState);
//						tracklets.push_back(secondState);
//
//						TrackletInf_t trackletInf = GetTrackletInf(tracklets);
//
//						/* it is not necessary to check riemannTrack because only tracklet with
//						 * more than 2 hits of unskewed tubes were combined.*/
//						if (trackletInf.numErrHits == 0) {
//							Combination_t combi;
//							combi.tracklets = tracklets;
//							combi.trackletInf = trackletInf;
//
//							fCombinedData.push_back(combi);
//						}
//
//						/*	old code
//						 *
//						 * // get hit-ids of both tracklets
//						 hitIDs = fStartTracklets[firstState].hitIDs;
//						 hitIDs.insert(hitIDs.end(),
//						 fStartTracklets[secondState].hitIDs.begin(),
//						 fStartTracklets[secondState].hitIDs.end());
//
//						 PndRiemannTrack riemannTrack = CreateRiemannTrack(hitIDs);
//
//						 // at least 5 riemannHits? --> fit riemannTrack and save in map
//						 if (riemannTrack.getNumHits() > 4) {
//						 riemannTrack.refit(false);
//
//						 numWrongHits = GetDeviationCount(riemannTrack);
//
//						 if (numWrongHits < 2) {
//						 Combination combi;
//
//						 vector<int> tracklets;
//						 tracklets.push_back(firstState);
//						 tracklets.push_back(secondState);
//
//						 error = CalcDeviationOfRiemannTrack(riemannTrack);
//
//						 combi.riemannTrack = riemannTrack;
//						 combi.tracklets = tracklets;
//						 combi.errSum = error;
//						 combi.numErrHits = numWrongHits;
//
//						 fCombinedData.push_back(combi);
//						 }*/
//
//					}
//
//				}
//
//			}
//		}
//	}
//
//	// combine the rightTrackCands
//	for (int i = 0; i < rightTrackCands.size(); ++i) {
//		firstState = rightTrackCands[i];
//
//		// combine straight and "unfinished" tracklets only
//		if (fStartTracklets[firstState].straight
//				&& fHitNeighbors[fStartTracklets[firstState].maxID].size()
//						> 1) {
//
//			sector = fStrawMap.GetSector(fStartTracklets[firstState].maxID);
//			sectorNeighbors.clear();
//			sectorNeighbors.insert(sector);
//			sectorNeighbors.insert(fStrawMap.GetLeftSector(sector));
//			sectorNeighbors.insert(fStrawMap.GetRightSector(sector));
//
//			for (int j = i + 1; j < rightTrackCands.size(); ++j) {
//				secondState = rightTrackCands[j];
//
//				// combine with tracklets with a higher state than maxID of first tracklet
//				if (secondState > fStartTracklets[firstState].maxID) {
//
//					// is second tracklet in nearby sector?
//					if (sectorNeighbors.find(fStrawMap.GetSector(secondState))
//							!= sectorNeighbors.end()) {
//
//						tracklets.clear();
//						tracklets.push_back(firstState);
//						tracklets.push_back(secondState);
//
//						TrackletInf_t trackletInf = GetTrackletInf(tracklets);
//
//						/* it is not necessary to check riemannTrack because only tracklet with
//						 * more than 2 hits of unskewed tubes were combined.*/
//						if (trackletInf.numErrHits == 0) {
//							Combination_t combi;
//							combi.tracklets = tracklets;
//							combi.trackletInf = trackletInf;
//
//							fCombinedData.push_back(combi);
//						}
//
//					}
//				}
//			}
//		}
//
//	}
//
//}

std::set<std::pair<int, int> > PndSttCellTrackFinder::CreatePairCombis(int firstState, std::set<int> values) {
	std::set < std::pair<int, int> > result;

	if (values.count(firstState) > 0) {
		for (std::set<int>::iterator iter = values.begin(); iter != values.end(); iter++){
			std::pair<int, int> actualPair;
			actualPair.first = firstState;
			if (firstState != *iter){
				actualPair.second = *iter;
				result.insert(actualPair);
			}
		}
	} else {
		std::cout << "-E- PndSttCellTrackFinder::CreatePairCombis Value " << firstState << " is not in set: ";
		for (std::set<int>::iterator iter = values.begin(); iter != values.end(); iter++){
			std::cout << *iter << " ";
		}
		std::cout << std::endl;
	}

	return result;
//
//
//
//	if (values.size() > 2) {
//		std::set<int>::iterator iterForward = values.begin();
//		std::set<int>::iterator iterBackward = values.end();
//		std::set<int>::iterator iterOneBeforeEnd = values.end();
//		iterOneBeforeEnd--;
//
////		SetVerbose(3);
//		if (fVerbose > 2) {
//			std::cout << "-I- PndSttCellTrackFinder::CreatePairCombis values: ";
//			for (std::set<int>::iterator iter = values.begin();
//					iter != values.end(); iter++) {
//				std::cout << *iter << " ";
//			}
//			std::cout << std::endl;
//			std::cout << "Combinations: ";
//		}
//
//		for (iterForward = values.begin(); iterForward != iterOneBeforeEnd;
//				iterForward++) {
//			iterBackward = values.end();
//			iterBackward--;
//			for (; iterBackward != iterForward; iterBackward--) {
//				if (fVerbose > 2)
//					std::cout << "(" << *iterForward << "/" << *iterBackward
//							<< ") ";
//				std::pair<int, int> createdPair(*iterForward, *iterBackward);
//				result.insert(createdPair);
//
//			}
//		}
//		if (fVerbose > 2)
//			std::cout << std::endl;
////		SetVerbose(0);
//	} else {
//		std::set<int>::iterator iter = values.begin();
//		std::pair<int, int> createdPair;
//		if (values.size() == 1) {
//			createdPair = std::make_pair(-1, *iter);
//		} else if (values.size() == 2) {
//			createdPair = std::make_pair(*iter, *iter++);
//		}
//		result.insert(createdPair);
//	}
//
//	return result;
}

//void PndSttCellTrackFinder::CombineTrackletsMultiStagesOld() {

//	map<int, TrackletInf_t>::iterator trackletIt;
//	vector<int> combinedTracklets;
//
//	int curEndID, curState, ambiguousID, numUnskewed;
//	set < pair<int, int> > pairCombinations;
//	set<pair<int, int> >::iterator pairIt;
//
//	for (trackletIt = fStartTracklets.begin();
//			trackletIt != fStartTracklets.end(); ++trackletIt) {
//
//		// there should at least be one hit of an unskewed tube
//		if ((trackletIt->second.hitIDs.size() - trackletIt->second.numSkewed)
//				> 2) {
//
//			curState = trackletIt->first;
//			curEndID = trackletIt->second.endID;
//
//			if (fVerbose > 2) {
//				cout << "Search for a combination for " << trackletIt->first
//						<< ", endID: " << curEndID << endl;
//			}
//			// find neighbor with ambiguities
//
//			if (fHitNeighbors[curEndID].size() == 2) {
//				if (fMultiStates.count(fHitNeighbors[curEndID].at(0))) {
//					ambiguousID = fHitNeighbors[curEndID].at(0);
//				} else if (fMultiStates.count(fHitNeighbors[curEndID].at(1))) {
//					ambiguousID = fHitNeighbors[curEndID].at(1);
//				} else {
//					continue;
//				}
//
//				if (fVerbose > 2) {
//					cout << "Found neighbor with multiStates: " << ambiguousID
//							<< endl;
//				}
//
//				pairCombinations = CreatePairCombis(fMultiStates[ambiguousID]);
//
//				// find pair with curState
//				for (pairIt = pairCombinations.begin();
//						pairIt != pairCombinations.end(); ++pairIt) {
//
////					if(pairIt->first == curState){
////
////					}else if(pairIt->second == curState){
////
////					}else{ continue;};
//
//					numUnskewed = fStartTracklets[pairIt->first].hitIDs.size()
//							- fStartTracklets[pairIt->first].numSkewed
//							+ fStartTracklets[pairIt->second].hitIDs.size()
//							- fStartTracklets[pairIt->second].numSkewed;
//					if (fVerbose > 2) {
//						cout << "Pair <" << pairIt->first << ","
//								<< pairIt->second << ">, # unskewed: "
//								<< numUnskewed << endl;
//					}
//
//					if ((pairIt->first == curState || pairIt->second == curState)
//							&& (numUnskewed > 2)) {
//						// found combination with curState  --> create combination
//
//						if (fVerbose > 2) {
//							cout << "try to combine tracklets" << endl;
//						}
//
//						combinedTracklets.clear();
//						combinedTracklets.push_back(pairIt->first);
//						combinedTracklets.push_back(pairIt->second);
//
//						TrackletInf_t trackletInf = GetTrackletInf(
//								combinedTracklets);
//
//						if (trackletInf.riemannTrack.getNumHits()
//								> fStartTracklets[curState].riemannTrack.getNumHits()
//								&& (trackletInf.numErrHits == 0)) {
//							Combination_t combi;
//							combi.tracklets = combinedTracklets;
//							combi.trackletInf = trackletInf;
//
//							if (fVerbose > 2) {
//								cout << "successful approximation!" << endl;
//							}
//							fCombinedData.push_back(combi);
//						}
//
//					}
//				}
//			}
//		}
//	}

//}

void PndSttCellTrackFinder::CombineTrackletsMultiStages() {

	if (fVerbose > 0)
		cout
				<< "========= void PndSttCellTrackFinder::CombineTrackletsMultiStages() ======="
				<< endl;

	map<int, TrackletInf_t>::iterator trackletIt;

	for (trackletIt = fStartTracklets.begin(); trackletIt != fStartTracklets.end(); ++trackletIt) {
		set<int> combi;
		if (trackletIt->first < 104)
			CombineTrackletsMultiStagesRecursive(trackletIt->first, combi);
	}

	vector<set<int> >::iterator combiIt;
	set<int>::iterator setIt;

	if (fVerbose > 0) {
		for (combiIt = fStateCombinations.begin();
				combiIt != fStateCombinations.end(); ++combiIt) {
			cout << "Found combinations: ";
			for (setIt = (*combiIt).begin(); setIt != (*combiIt).end();
					++setIt) {
				cout << (*setIt) << " ";
			}
			cout << endl;
		}
	}



	// fit combinations
	for (combiIt = fStateCombinations.begin(); combiIt != fStateCombinations.end(); ++combiIt) {
//		vector<int> tracklets;
		TrackletInf_t trackletInf;

//		for (setIt = (*combiIt).begin(); setIt != (*combiIt).end(); ++setIt) {
//			tracklets.push_back(*setIt);
//		}

		trackletInf = GetTrackletInf(*combiIt);

		if (fVerbose > 2) {
			std::cout << "CombiTrackletsMultiStrange: trackletInf: ";
			trackletInf.Print();
		}

		if (trackletInf.riemannTrack.getNumHits() > 2
				&& trackletInf.numErrHits == 0) {
			Combination_t combi;
			combi.tracklets = *combiIt;
			combi.trackletInf = trackletInf;

			fCombinedData.push_back(combi);
		}
	}

}

void PndSttCellTrackFinder::CombineTrackletsMultiStagesRecursive(
		int stateToCombine, set<int> currentCombi) {

	if (fVerbose > 2)
		cout << "CombineTrackletsMultiStagesRecursive, tracklet: "
				<< stateToCombine << endl;

	set<int> newCombi;
	newCombi.insert(currentCombi.begin(), currentCombi.end());
	newCombi.insert(stateToCombine);

	int ambiguousID;
	int endID = fStartTracklets[stateToCombine].endID;
	set<int>::iterator it;

	if (fVerbose > 3) {
		cout << "neighbors of endtube: ";
		for (int i = 0; i < fHitNeighbors[endID].size(); ++i) {
			cout << fHitNeighbors[endID].at(i) << " ";
		}
		cout << endl;
	}

	// check neighbors of end-tube
	if (fHitNeighbors[endID].size() == 2) {
		if (fMultiStates.count(fHitNeighbors[endID].at(0))) {
			ambiguousID = fHitNeighbors[endID].at(0);

			//way was aleready chosen?
			for (it = currentCombi.begin(); it != currentCombi.end(); ++it) {
				if (fMultiStates[ambiguousID].count(*it)) {
					ambiguousID = 0;
					break;
				}
			}

		} else if (fMultiStates.count(fHitNeighbors[endID].at(1))) {
			ambiguousID = fHitNeighbors[endID].at(1);

			//way was aleready chosen?
			for (it = currentCombi.begin(); it != currentCombi.end(); ++it) {
				if (fMultiStates[ambiguousID].count(*it)) {
					ambiguousID = 0;
					break;
				}
			}
		} else {
			ambiguousID = 0;
		}

	} else {
		// there are not two neighbors
		if (fVerbose > 2) {
			cout << "there are less than two neighbors. finish combi: ";
			for (it = newCombi.begin(); it != newCombi.end(); ++it) {
				cout << *it << ", ";
			}
			cout << endl;
		}

		if (newCombi.size() > 1)
			InsertCombination(newCombi);

		return;
	}

	if (ambiguousID == 0) {
		// no neighbor found
		if (fVerbose > 2) {
			cout << "ambiguous id was not found. finish combi: ";
			for (it = newCombi.begin(); it != newCombi.end(); ++it) {
				cout << *it << ", ";
			}
			cout << endl;
		}

		if (newCombi.size() > 1)
			InsertCombination(newCombi);

		return;
	}

	if (fVerbose > 2)
		cout << "found ambiguous tube: " << ambiguousID << endl;

	set < pair<int, int> > pairCombinations;
	set<pair<int, int> >::iterator pairIt;
	int combiPartner;

	// get possible combination


	pairCombinations = CreatePairCombis(stateToCombine, fMultiStates[ambiguousID]);

	if (fVerbose > 2) {
		std::cout << "StateToCombine " << stateToCombine << " with: ";
		for (std::set<int>::iterator iter = fMultiStates[ambiguousID].begin(); iter != fMultiStates[ambiguousID].end(); iter++){
			std::cout << *iter << " ";
		}
		std::cout << std::endl;
		std::cout << "Gives PairCombinations: ";
		for (std::set<std::pair<int,int> >::iterator iter = pairCombinations.begin(); iter != pairCombinations.end(); iter++){
			std::cout << iter->first << "/" << iter->second << " ";
		}
		std::cout << std::endl;
	}

	// find pair with curState
	for (pairIt = pairCombinations.begin(); pairIt != pairCombinations.end();++pairIt) {
		combiPartner = pairIt->second;

		if (fVerbose > 2)
			cout << "found combipartner: " << combiPartner << endl;

		if (!newCombi.count(combiPartner) && (combiPartner != -1))
			CombineTrackletsMultiStagesRecursive(combiPartner, newCombi);
	}

}

void PndSttCellTrackFinder::InsertCombination(set<int> combination) {

	bool found = false;
	set<int>::iterator oldIt;
	set<int>::iterator newIt;

	for (int i = 0; i < fStateCombinations.size(); ++i) {

		if (fStateCombinations[i] == combination) {
			found = true;
			break;
		}
	}

	if (!found)
		fStateCombinations.push_back(combination);
}

void PndSttCellTrackFinder::FindTrackletsWithoutCombi() {

	set<int> combinedTracklets;

// fill set with states of combined tracklets
	for (int i = 0; i < fCombinedData.size(); ++i) {
		for (set<int>::iterator iter = fCombinedData[i].tracklets.begin(); iter != fCombinedData[i].tracklets.end(); iter++) {
			combinedTracklets.insert(*iter);
		}
	}

	map<int, TrackletInf_t>::iterator it;
	for (it = fStartTracklets.begin(); it != fStartTracklets.end(); ++it) {

		if (combinedTracklets.find(it->first) == combinedTracklets.end()) {
			fTrackletsWithoutCombi.push_back(it->first);
		}
	}

}

//void PndSttCellTrackFinder::CreateFurtherCombinations() {
//
//	int curSize = fCombinedData.size();
//	Combination_t curCombi, nextCombi;
//
//	for (int i = 0; i < curSize; ++i) {
//
//		curCombi = fCombinedData[i];
//
////search for another combi that starts with the state of the second tracklet of the current combi
//		for (int j = i + 1; j < curSize; ++j) {
//
//			nextCombi = fCombinedData[j];
//
//			// is there a chain?
//			if (curCombi.tracklets[1] == nextCombi.tracklets[0]) {
//
//				//combine 2 two-part-combinations to 1 three-part-combination
//				Combination_t newCombi;
//				newCombi.tracklets.push_back(curCombi.tracklets[0]);
//				newCombi.tracklets.push_back(curCombi.tracklets[1]);
//				newCombi.tracklets.push_back(nextCombi.tracklets[1]);
//				newCombi.trackletInf = GetTrackletInf(newCombi.tracklets);
//
//				if (newCombi.trackletInf.numErrHits == 0) {
//					fCombinedData.push_back(newCombi);
//					if (fVerbose > 1) {
//						cout << "Found Combination of 3 tracklets!" << endl;
//					}
//				}
//			}
//		}
//	}
//
//}

void PndSttCellTrackFinder::AssignAmbiguousHits()
{
	for (std::map<int, std::set<int> >::iterator iter = fMultiStates.begin(); iter != fMultiStates.end(); iter++){
		if (iter->second.size() == 1){
			if (fStartTracklets.count(*(iter->second.begin())) > 0){
				fStartTracklets[*(iter->second.begin())].hitIDs.push_back(fMapTubeIdToHit[iter->first]);
			}
		}
		if (iter->second.size() == 2){
			if (fStartTracklets.count(*(iter->second.begin())) > 0){
				PndRiemannHit hit(fHits[fMapTubeIdToHit[iter->first]]);
				if (fStartTracklets[*(iter->second.begin())].riemannTrack.dist(&hit) < 1)
					fStartTracklets[*(iter->second.begin())].hitIDs.push_back(fMapTubeIdToHit[iter->first]);
			}
		}
	}
	for (int i = 0; i < fCombinedData.size(); i++){
		fCombinedData[i].trackletInf = GetTrackletInf(fCombinedData[i].tracklets);
	}
}

void PndSttCellTrackFinder::AddMissingHits() {

	// add hits with ambiguities to the best combination
	int curTubeID;
	vector<int> index;
	index.push_back(3);
	index.push_back(4);

	for (int i = 0; i < index.size(); ++i) {

		for (int k = 0; k < fSeparations[index[i]].size(); ++k) {
			curTubeID = fSeparations[index[i]][k];

			if (!fStrawMap.IsSkewedStraw(curTubeID)) {
				AddHitToBestCombi(fMapTubeIdToHit[curTubeID]);
			}
		}
	}

	// add hits of short tracklets to the best combination
	int state;
	PndSttHit* sttHit;
	map<int, TrackletInf_t>::iterator it;
	for (it = fShortTracklets.begin(); it != fShortTracklets.end(); ++it) {
		state = it->first;

		for (int j = 0; j < fShortTracklets[state].hitIDs.size(); ++j) {
			sttHit = (PndSttHit*) fHits[fShortTracklets[state].hitIDs[j]];
			if (!fStrawMap.IsSkewedStraw(sttHit->GetTubeID())) {
				AddHitToBestCombi(fShortTracklets[state].hitIDs[j]);

			}
		}

	}
}

bool PndSttCellTrackFinder::AddHitToBestCombi(int hitID) {

	double minDistance = 100, tmpDistance;
	PndSttHit* sttHit = (PndSttHit*) fHits[hitID];
	int tubeID = sttHit->GetTubeID();
	int sector = fStrawMap.GetSector(tubeID);
	int sectorOfCombi, foundCombi;

	for (int i = 0; i < fCombinedData.size(); ++i) {

		sectorOfCombi = fStrawMap.GetSector(*(fCombinedData[i].tracklets.begin()));

		if (sector / 3 == sectorOfCombi / 3) {
			// combi and hit are in the same half of the STT (integer division)
			tmpDistance = CalcDeviation(
					fCombinedData[i].trackletInf.riemannTrack, hitID);
			if (tmpDistance < minDistance) {
				minDistance = tmpDistance;
				foundCombi = i;
			}
		}
	}

// min distance smaller than radius of a tube?
	if (minDistance < 0.5005) {
// add hit to combi
		fCombinedData[foundCombi].trackletInf.hitIDs.push_back(hitID);
		if (fVerbose > 2) {
			cout << "AddHitToBestCombi(): add " << tubeID << " to Combi #"
					<< foundCombi << ", distance: " << minDistance << endl;
		}
		return true;
	} else {
		return false;
	}

}

// es wird davon ausgegangen, dass nur hintereinanderliegende tracklets kombiniert werden
TrackletInf_t PndSttCellTrackFinder::GetTrackletInf(set<int> tracklets) {

// sort by state of tracklets
//	sort(tracklets.begin(), tracklets.end());

	TrackletInf_t inf;
	int numTracklets = tracklets.size();
	int state = 0, maxID = 0, endID = 0;
	bool straight = false;

	if (numTracklets > 1) {
// update trackletInf

		state = 5000;
// state of combined tracklet equals min state of all
		for (set<int>::iterator iter = tracklets.begin(); iter != tracklets.end(); ++iter) {
			state = TMath::Min(state, *iter);
		}
		inf.startID = state;

		straight = true;
// combined tracklet is straight if each particular tracklets are straight
		for (set<int>::iterator iter = tracklets.begin(); iter != tracklets.end(); ++iter) {
			if (fStartTracklets[*iter].straight) {
				straight = false;
				break;
			}
		}

		if (straight) {
			// maxID equals the data of the last tracklet
			maxID = fStartTracklets[*(--tracklets.end())].maxID;

		} else {
			// maxID is the maximum of all maxID
			maxID = 0;
			for (set<int>::iterator iter = tracklets.begin(); iter != tracklets.end(); ++iter) {
				maxID = TMath::Max(maxID, fStartTracklets[*iter].maxID);
			}
		}
// endID equals the endID of last tracklet
		endID = fStartTracklets[*(--tracklets.end())].endID;

// update hitIDs
		for (set<int>::iterator iter = tracklets.begin(); iter != tracklets.end(); ++iter) {
			inf.hitIDs.insert(inf.hitIDs.end(),	fStartTracklets[*iter].hitIDs.begin(), fStartTracklets[*iter].hitIDs.end());
			inf.numSkewed += fStartTracklets[*iter].numSkewed;
		}

	}

	inf.riemannTrack = CreateRiemannTrack(inf.hitIDs);

	if (inf.riemannTrack.getNumHits() > 2) {
		inf.riemannTrack.refit(false);
		inf.error = CalcDeviationOfRiemannTrack(inf.riemannTrack);
		inf.numErrHits = GetDeviationCount(inf.riemannTrack);
	}

	return inf;

}

bool PndSttCellTrackFinder::IsEndTubeOfTracklet(int tubeID) {

// tube is end of tracklet if there's only one hit-neighbor or if it has neighbors with ambiguity hits
	if (fHitNeighbors[tubeID].size() == 1
			|| fHitNeighbors[fHitNeighbors[tubeID][0]].size() > 2
			|| fHitNeighbors[fHitNeighbors[tubeID][1]].size() > 2) {
		return true;
	}

	return false;
}

PndRiemannTrack PndSttCellTrackFinder::CreateRiemannTrack(vector<int> hitIDs) {

	PndRiemannTrack riemannTrack;
	PndSttHit* sttHit;
	std::set<int> skewedTubeIdsInTrack;

	for (int i = 0; i < hitIDs.size(); ++i) {

// check if hit was from skewed straw tube
		sttHit = (PndSttHit*) fHits[hitIDs[i]];
		if (!fStrawMap.IsSkewedStraw(sttHit->GetTubeID())) {

			// no skewed tube --> add hit to riemannTrack
			PndRiemannHit riemannHit(fHits[hitIDs[i]], hitIDs[i]);
			riemannTrack.addHit(riemannHit);

		} else {
			skewedTubeIdsInTrack.insert(sttHit->GetTubeID());
		}
	}
	if (riemannTrack.getNumHits() > 2) {
		riemannTrack.refit(false);
		for (std::set<int>::iterator tubeIter = skewedTubeIdsInTrack.begin(); tubeIter != skewedTubeIdsInTrack.end(); tubeIter++){
			std::multimap<int, PndSttSkewedHit*>::iterator it, itlow, itup, itbest;
			itlow = fCombinedSkewedHits.lower_bound(*tubeIter);
			itup = fCombinedSkewedHits.upper_bound(*tubeIter);
			itbest = fCombinedSkewedHits.end();
			Double_t minDist = 1000;
			for (it = itlow; it != itup; ++it){
				PndRiemannHit skewedRiemann(it->second);
				Double_t actDist = riemannTrack.dist(&skewedRiemann);
				if (actDist < minDist){
					itbest = it;
					minDist = actDist;
				}
			}
			if (itbest != fCombinedSkewedHits.end()){
				PndRiemannHit skewedRiemann(itbest->second);
				riemannTrack.addHit(skewedRiemann);
			}
		}
	}

	return riemannTrack;
}

double PndSttCellTrackFinder::CalcDeviationOfRiemannTrack(
		PndRiemannTrack& track) {

	vector<PndRiemannHit> hits = track.getHits();
	TVector2 pos;
	TVectorD orig(2);
	orig = track.orig();
	TVector2 diff;
	double r = track.r();
	double sum = 0;

	for (int i = 0; i < track.getNumHits(); ++i) {

// get x- and y-coordinate of the hit
		pos.Set(hits[i].x()[0], hits[i].x()[1]);
// calc diff-vector to origin
		diff.Set(pos.X() - orig[0], pos.Y() - orig[1]);
// calc distance to circle
		sum += (diff.Mod() - r) * (diff.Mod() - r);
	}

	return sum / track.getNumHits();

}

double PndSttCellTrackFinder::CalcDeviation(PndRiemannTrack& track, int hitID) {

	PndSttHit* sttHit = (PndSttHit*) fHits[hitID];
	TVector3 pos = fMapTubeIdToPos[sttHit->GetTubeID()];
	TVector2 diff;
	TVectorD orig(2);
	double r = track.r();
	orig = track.orig();

	diff.Set(pos.X() - orig[0], pos.Y() - orig[1]);

	return TMath::Abs(diff.Mod() - r);

}

int PndSttCellTrackFinder::GetDeviationCount(PndRiemannTrack& track) {
	int counter = 0;

	vector<PndRiemannHit> hits = track.getHits();
	TVector2 pos;
	TVectorD orig(2);
	orig = track.orig();
	TVector2 diff;
	double r = track.r();

	for (int i = 0; i < track.getNumHits(); ++i) {

// get x- and y-coordinate of the hit
		pos.Set(hits[i].x()[0], hits[i].x()[1]);
		diff.Set(pos.X() - orig[0], pos.Y() - orig[1]);

		if (TMath::Abs(diff.Mod() - r) > 0.5005) {
			++counter;
		}
	}

	return counter;
}