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

#ifndef PNDISOCHRONETRACKFINDER_H_
#define PNDISOCHRONETRACKFINDER_H_

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

#include "TObject.h"
#include "TH2D.h"
#include "TGraph.h"

#include "FairHit.h"

#include "PndTrack.h"
#include "PndTrackCand.h"
#include "PndTrackFromCircle.h"
#include "PndSttHit.h"


#include "TClonesArray.h"
class TVector2;

class PndGeneralHoughHit : public FairHit
{
public:
	PndGeneralHoughHit(){};
	~PndGeneralHoughHit(){};
	PndGeneralHoughHit(const FairHit& hit) : FairHit(hit), isochroneRadius(0){};

	void SetIsochroneRadius(Double_t val) {isochroneRadius = val;}
	Double_t GetIsochroneRadius() const {return isochroneRadius;}
	TVector2 GetXYVector(){return TVector2(GetX(), GetY());}
private:
	Double_t isochroneRadius;
};

struct PndCircleTime {
	TVector2 fCircle;
	Double_t fTime;
	Double_t fRRange;
	Double_t fPhiRange;
};

struct PndPeakContent {
	std::set<Int_t> fHitsInPeaks;
	std::set<Int_t> fPeakBins;
	TVector2 fCircleCenter;
	std::pair<Double_t, Double_t> fRPhiRange;
	void AddHits(std::set<Int_t> hits);
	void AddPeak(Int_t peak);
	void AddPeaks(std::set<Int_t> peaks);
	void AddPeakContent(const PndPeakContent& peak);
	TVector2 GetMeanPeakPos() const {return fCircleCenter;}
	std::pair<Double_t, Double_t> GetPeaksRangeRPhi() const {return fRPhiRange;}
	Int_t GetNHits()const {return fHitsInPeaks.size();}
	TVector2 FillMeanPeakPos(TH2* h2);
	std::pair<Double_t, Double_t> FillPeaksRangeRPhi(TH2* h2);
	virtual std::ostream& Print(std::ostream& out = std::cout) const {
		out << "Hits: /";
		for (std::set<Int_t>::iterator hitIter = fHitsInPeaks.begin(); hitIter != fHitsInPeaks.end(); hitIter++){
			out << *hitIter << "/";
		}
		out << " Peaks: /";
		for (std::set<Int_t>::iterator peakIter = fPeakBins.begin(); peakIter != fPeakBins.end(); peakIter++){
			std::cout << *peakIter << "/";
		}
		out << " CircleCenter: " << fCircleCenter.X() << "/" << fCircleCenter.Y() << " Mod: " << fCircleCenter.Mod() << " Phi: " << fCircleCenter.Phi()
				<< " RRange: " << fRPhiRange.first << " PhiRange " << fRPhiRange.second << std::endl;
		return out;
	}

	friend std::ostream& operator<< (std::ostream& out, const PndPeakContent& peak){
		peak.Print(out);
		return out;
	}
};

class PndCircleSZ : public FairMultiLinkedData {
	public:
		PndCircleSZ():fNPairs(0), fZoverS(0), fMaxR(0), fMinR(100000), fClockwise(kFALSE){
			SetInsertHistory(kFALSE);
		}
		PndCircleSZ(FairLink link1, FairLink link2, TVector2 circle, Double_t ZoverS, Bool_t clockwise = kFALSE)
		: fNPairs(0), fZoverS(0), fMaxR(0), fMinR(100000), fClockwise(clockwise){
			SetInsertHistory(kFALSE);
			AddPair(link1, link2, circle, ZoverS);
		}
		void AddPair(FairLink link1, FairLink link2, TVector2 circle, Double_t ZoverS){
			AddLink(link1);
			AddLink(link2);
			AddCircleCenter(circle);
			AddZoverS(ZoverS);
			fNPairs++;
		}

		Bool_t Contains(PndCircleSZ& circle){
			for (Int_t i = 0; i< circle.GetNLinks(); i++){
				if (IsLinkInList(circle.GetLink(i)) != kTRUE)
					return false;
			}
			return true;
		}

		void AddCircleSZ(PndCircleSZ& circle){
			fCircleCenter = (fCircleCenter * fNPairs + circle.GetCircleCenter() * circle.GetNPairs())/(fNPairs + circle.GetNPairs());
			fZoverS = (fZoverS * fNPairs + circle.GetZoverS() * circle.GetNPairs())/(fNPairs + circle.GetNPairs());
			if (circle.GetMinR() < GetMinR())
				fMinR = circle.GetMinR();
			if (circle.GetMaxR() > GetMaxR())
				fMaxR = circle.GetMaxR();

			for (int i = 0; i < circle.GetNLinks(); i++){
				AddLink(circle.GetLink(i));
				fNPairs++;
			}

		}

		void AddPeakContent(PndPeakContent peakContent, std::vector<PndGeneralHoughHit> translation){
			fCircleCenter = (fCircleCenter * fNPairs + peakContent.GetMeanPeakPos() * peakContent.GetNHits())/(fNPairs + peakContent.GetNHits());
			std::set<Int_t> hitsInPeak =  peakContent.fHitsInPeaks;
			for (std::set<Int_t>::iterator hitIter = hitsInPeak.begin(); hitIter != hitsInPeak.end(); hitIter++){
				AddLink(translation[*hitIter].GetEntryNr());
				fNPairs++;
			}
		}

		void SetClockwise(Bool_t clockwise){ fClockwise = clockwise;}

		TVector2 GetCircleCenter() const {return fCircleCenter;};
		Double_t GetZoverS() const {return fZoverS;};
		Int_t GetNPairs() const {return fNPairs;};
		Double_t GetMinR() const {return fMinR;}
		Double_t GetMaxR() const {return fMaxR;}
		Double_t GetDR() const {
			Double_t result = (fMaxR - fMinR)/2;
			if (result < 2){
				result = fCircleCenter.Mod() * 0.1;
			}
			return result;
		}
		Bool_t GetClockwise() const{return fClockwise;}

		virtual std::ostream& Print(std::ostream& out = std::cout) const{
			out << "PndCircleSZ: (" << GetCircleCenter().X() << "/" << GetCircleCenter().Y() << ")" << " clockwise " << GetClockwise();
			out << " min/max R " << GetMinR() << "/" << GetMaxR();
			out << " Z/S: " << GetZoverS();
			out << " NPairs: " << GetNPairs() << " ";
			FairMultiLinkedData::Print(out);
			return out;
		}

		friend std::ostream& operator<< (std::ostream& out, const PndCircleSZ& circle){
			circle.Print(out);
			return out;
		}

	private:
		void AddCircleCenter(TVector2& circle){
			TVector2 tempCircle = fCircleCenter * fNPairs;
			tempCircle += circle;
			tempCircle /= fNPairs + 1;
			fCircleCenter = tempCircle;
			if (circle.Mod() < fMinR)
				fMinR = circle.Mod();
			if (circle.Mod() > fMaxR)
				fMaxR = circle.Mod();
		}
		void AddZoverS(Double_t& ZoverS){
			Double_t tempZS = fZoverS * fNPairs;
			tempZS += ZoverS;
			tempZS /= fNPairs + 1;
			fZoverS = tempZS;
		}
		TVector2 fCircleCenter;		///< average position of circleCenters
		Double_t fMaxR, fMinR;	///< maximum and minimum Pt
		Double_t fZoverS;			///< average ZoverS
		Int_t fNPairs;				///< number of HitPairs added
		Bool_t fClockwise;			///< turin
};

class PndIsochroneTrackFinder : public TObject {
public:
	PndIsochroneTrackFinder();
	virtual ~PndIsochroneTrackFinder();

	void SetVerbose(Int_t val = 1) {fVerbose = val;}
	void SetBField(Double_t field) {fBField = field;}
	void SetTubeArray(TClonesArray* val){fSttTubeArray = val;}
	void SetAngleRange(std::pair<Double_t, Double_t> range) {fAngleRange = range;}
	void SetAngleSteps(Double_t stepSize) {fAngleSteps = stepSize;}
	void SetPtRange(std::pair<Double_t, Double_t> range) {fPtRange = range;}
	void SetPtStepSize(Double_t stepSize) {fPtStepSize = stepSize;}

	void AddHits(TClonesArray* hits, Int_t branchId);
	std::vector<PndTrack> FindTracks();

	std::vector<PndTrack> FindMvdBasedTracks();

	std::vector<PndTrack> FindMvdTracks();

	TH2D* GetHoughHisto(){return fHoughHisto;}
	void SetSkewedBranchName(TString branchName){fSkewedBranchName = branchName;}
	TString GetSkewedBranchName() const {return fSkewedBranchName;}
	void SetSkewedArray (TClonesArray* val){ fSttSkewedArray = val; }
	TClonesArray* GetSkewed() const {return fSttSkewedArray;}
	std::vector<PndTrackCand> GetTrackCands(){return fTrackCands;}
	std::vector<TVector2> GetPeakCoordinates(){return fPeakCoordinates;};
	TGraph GetPeakCoordinateGraph();

	std::vector<TVector2> GetPeakCoordinatesMvd(){return fPeakCoordinatesMvd;};
	TGraph GetPeakCoordinateGraphMvd();
	std::map<Double_t, FairHit>::iterator CheckContinuity(std::map<Double_t, FairHit>& cand, Double_t circleMag);  ///< checks if the hits of a track a continuous to reduce ghost tracks
	Bool_t CheckTrackSimilarity(std::set<Int_t> first, std::set<Int_t> second, Double_t threshold); ///< checks if the percentage of identical hits between the two tracks is above the threshold
	Bool_t CheckPeakDistance(std::set<Int_t> peakBins, Int_t binToCheck, Double_t threshold);	///< checks if the distance between the circle center of the binToCheck and at least one out of the peakBins is less than threshold in cm

	void AssignSkewedHits(PndTrackCand& cand, std::vector<std::vector<FairHit> > skewedHits, TVector2 circle);
	Double_t CalcDerivation(PndCircleSZ circleSZ, TVector2 circle); ///< Calculates the variance of the hits in circleSZ based on the circle given by "circle"

	void Reset(){
		if (fHoughHisto != 0)
			fHoughHisto->Reset();
		fBinsWithContent.clear();
		for (std::map<Int_t, std::set<Int_t> >::iterator iter = fBinToHit.begin(); iter != fBinToHit.end(); ++iter){
			iter->second.clear();
		}
		fBinToHit.clear();
		fHitsUsed.clear();
		fTrackCands.clear();
		fHitsToFit.clear();
		fSkewedHits.clear();
		if (fSttSkewedArray != 0)
			fSttSkewedArray->Delete();
		fPeakCoordinates.clear();
		fPeakCoordinatesMvd.clear();

	}

protected:
	void AddHitsToFit(TClonesArray* hits, Int_t branchId);
	TVector2 IsoCalc(PndGeneralHoughHit& houghHit, Double_t phi);
	std::vector<Double_t> CalcPhiForPt(PndGeneralHoughHit& hit, Double_t pt);
	std::pair<Double_t, Double_t> CalcPhiForT(Double_t t, TVector2 hit);
	std::vector<Double_t> CalcPhiForCircleTimes(TVector2& hit, std::vector<PndCircleTime>& circleTimes);
	std::vector<Double_t> CalcPhiForPeakContents(TVector2& hit, std::vector<PndPeakContent>& peakContents);
	std::vector<Double_t> CalcPhiForCircleSZ(TVector2& hit, PndCircleSZ& circleSZ);

	TVector2 CalcIntersectionNonIso(FairHit& hit1, FairHit& hit2);

	Int_t GetHistoBin(TH2* histo, Double_t xVal, Double_t yVal);
	void InterpolateHisto(TH2* histo, TVector2& in, TVector2& out, Int_t& hitId, Double_t weight = 1.);
	std::multimap<Int_t, Int_t> FindPeaks(std::set<Int_t>& binsWithContent, TH2* histo, Int_t threshold); ///< returns multimap with number of hits in bin and bin number
	std::vector<PndPeakContent> MergePeaks(std::multimap<Int_t, Int_t> peaks, Double_t trackSimilarityCut,  Int_t lowerHitLimit, Bool_t separateTracks = kFALSE);
	PndTrackCand FillTrackCand(Int_t histoBin, Int_t lowerHitLimit);
	PndTrackCand FillTrackCand(PndPeakContent& peakContent);
	Double_t CalcArcLength(FairHit hit, TVector2 circleCenter, Bool_t clockwise);
	PndTrack CalcPndTrack(PndTrackCand& cand, TVector2 circle);
	FairTrackParP CalcTrackParP(FairHit hit, TVector2 circleCenter);
	TVector3 CalcHitPosInTrack(FairHit hit, TVector2 circleCenter);
	Double_t CalcPt(TVector2 circleCenter);
	TVector2 CalcPtDir(TVector2 hit, TVector2 circleCenter);
	std::vector<TVector2> CalcCircleIntersections(TVector2 circleCenter1, Double_t radius1, TVector2 circleCenter2, Double_t radius2);
	//Bool_t IsEqual(std::set<Int_t> a, std::set<Int_t> b);
	Bool_t ContainsAB(std::set<Int_t> a, std::set<Int_t> b);
	std::vector<PndPeakContent> SeparateTracks(std::set<Int_t> hitsInBin, TVector2 circle, Int_t peakBin);

	std::vector<Double_t> CalcArcLengthRegions(Double_t radius);

	std::vector< std::vector <FairHit> > FindSkewedHits(TVector2 circle, Bool_t clockwise);
	std::vector<TVector2> GenerateHoughValues(PndGeneralHoughHit& houghHit);
	std::vector<TVector2> GenerateHoughValuesPhi(PndGeneralHoughHit& houghHit, std::set<Double_t> phiValues);
	std::vector<Double_t> GeneratePtValues();
	std::set<Double_t> GeneratePhiValues(PndGeneralHoughHit& myHit);
	void FillHoughHist(std::vector<TVector2>, Int_t, Double_t);
	void CalcHoughHist(PndGeneralHoughHit& hit, Int_t hitIndex, Bool_t plusIsochrone);
	std::vector<PndTrack> PeaksToTracks(std::multimap<Int_t, Int_t>, Int_t lowerHitLimit);
	std::vector<PndTrack> PeaksToTracks(std::vector<PndPeakContent>& mergedPeaks, Int_t lowerHitLimit);
	PndCircleTime FindCircleTime(Int_t peakBins);
	Double_t CalcTimeStampInBin(Int_t bin);
	std::map<Double_t, FairHit> CutTrackAtPi(std::map<Double_t, FairHit> &trackCandHits);	///< Cuts a track at 180° and keeps the part of the track with the most points.

	std::vector<PndPeakContent> MergePeaksByPos(std::vector<PndPeakContent>& peaks, Double_t peakDistanceCut);

	PndCircleSZ MergeTracks(PndCircleSZ circleSZ, PndPeakContent peakContent);

	PndTrack CircleSZToTracks(PndCircleSZ& circleSZ);
	PndTrackCand CircleSZToTrackCands(PndCircleSZ& circleSZ);

	Double_t CircleDistance(FairHit hit, TVector2 circleCenter){
		TVector2 temp(hit.GetX(), hit.GetY());
		temp -= circleCenter;
		return TMath::Abs(temp.Mod() - circleCenter.Mod());
	}

	Double_t ZDistance(FairHit hit, TVector2 circleCenter, Double_t zs, Double_t clockwise){
		Double_t s = CalcArcLength(hit, circleCenter, clockwise) * circleCenter.Mod();
		return TMath::Abs(s * zs - hit.GetZ());
	}

	Double_t RToPt(Double_t r) {				//Pt in GeV and r in cm
		return 0.3 * fBField * r * 0.01;
	}

	Double_t PtToR(Double_t pt){
		return pt * 100/(0.3 * fBField);
	}


	//void GetStartEndPointOfTube(PndSttHit*, TVector3& startPoint, TVector3& endPoint);

//	void FillHistogram();

private:
	Int_t fVerbose;
	Double_t fBField;
	std::pair<Double_t, Double_t> fAngleRange;
	Double_t fAngleSteps;
	std::pair<Double_t, Double_t> fPtRange;  // range of investigated pt momentum, usually 0 to 15 GeV/c (all possible momenta a track can have)
	Double_t fPtStepSize;  // the step width with which the momentum range is rastered through
	TH2D* fHoughHisto;
	std::set<Int_t> fBinsWithContent; //!
	std::vector<PndGeneralHoughHit> fHitsToFit;
	std::map<Int_t, std::set<Int_t> > fBinToHit; //Map of BinInHisto(key) to index in fHitsToFit
	std::set<Int_t> fHitsUsed;
	Bool_t fClockwise;
	std::vector<PndTrackCand> fTrackCands;
	std::map<TString, Double_t> fWeightMap;
	TClonesArray* fSttTubeArray;
	std::vector<PndSttHit*> fSkewedHits;
	std::map<FairLink, PndGeneralHoughHit> fMapFairLinkHit;
	std::vector<TVector2> fPeakCoordinates;
	std::vector<TVector2> fPeakCoordinatesMvd;
	std::multimap<Int_t, std::pair<PndTrackCand, TVector2> > fSeparatedTracks; ///< NumberOfHitsInTrack, <TrackCand, Circle> >

	PndTrackFromCircle fTrackCalc;
	Double_t fM;


	TString fSkewedBranchName;				//<Output branchName for the calculated hits from the skewed layers
	TClonesArray* fSttSkewedArray;

	TClonesArray* fMvdHitsPixelArray;
	TClonesArray* fMvdHitsStripArray;
	TClonesArray* fSttHitArray;
	TClonesArray* fGemHitArray;

	Int_t fMvdHitsPixelBranchId;
	Int_t fMvdHitsStripBranchId;
	Int_t fSttHitBranchId;
	Int_t fGemHitBranchId;

	Int_t fHitsToFitAfterMvd;	//< first hit after MVD hits at the beginning of the fHitsToFit vector

	Double_t fCutMergePeakDistance;

public:
	ClassDef(PndIsochroneTrackFinder, 1);
};

#endif /* PNDISOCHRONETRACKFINDER_H_ */