#include <vector>
#include <algorithm>
#include <utility>
#include <numeric>
#include <map>
#include <iostream>
#include <cmath>

// #include "boost/tuple/tuple.hpp"
#include <tuple>

#include "TH2D.h"
#include "TApplication.h"
#include "TCanvas.h"
#include "TString.h"
#include "TGraph.h"

#include "PndAhTools.h"
#include "PndAhHoughTransform.h"
#include "PndAhMultipeakAnalysis.h"


std::vector<std::tuple<double, double, double> > peakOfHill(TH2D * hist, std::vector<double> angles, std::vector<double> rs) {
	// std::cout << " --- peakOfHill --- BEGIN ---" << std::endl;
	std::vector<std::tuple<double, double, double> > peaks;

	for (int i = 0; i < angles.size(); ++i) {
		// std::cout << " --- getHighestBinOfHit --- " << "angles[i]: " << angles[i] << ", rs[i] " << rs[i] << " -- GetBin(angles[i], rs[i]): " << hist->GetBin(angles[i], rs[i]) << " -- FindFixBin(angles[i], rs[i]): " <<  hist->FindFixBin(angles[i], rs[i]) << std::endl;
		int currentBinId = hist->FindFixBin(angles[i], rs[i]);
		double currentBinContent = hist->GetBinContent(currentBinId);
		// std::cout << " --- getHighestBinOfHit --- " << " currentBinContent: " << currentBinContent << std::endl;
		// if (currentBinContent > highZ) {
		// 	highZ = currentBinContent;
		// 	highX = angles[i];
		// 	highY = rs[i];
		// }
		// std::cout << currentBinContent << " " << angles[i] << " " << rs[i] << std::endl;
		peaks.push_back(std::make_tuple(currentBinContent, angles[i], rs[i]));
	}
	// std::cout << " --- peakOfHill --- END ---" << std::endl;
	return peaks;
}

bool compareTupleFirstElement(const std::tuple<double, double, double> &lhs, const std::tuple<double, double, double> &rhs) {
  return std::get<0>(lhs) > std::get<0>(rhs);
}

double weightedMean(const std::vector<double> & values, const std::vector<double> & weights) {
	double mean = 0;
	double meanWeight = 0;
	for (int i = 0; i < values.size(); i++) {
		mean += values[i] * weights[i];
		meanWeight += weights[i];
	}
	return mean/meanWeight;
}

std::tuple<double, double, double> findOneCog(TH2D * hist, double x, double y, int numberOfNeighboringBins = 3) {
	int currentBinId = hist->FindFixBin(x, y);
	int binIdX, binIdY, binIdZ;
	hist->GetBinXYZ(currentBinId, binIdX, binIdY, binIdZ);
	// std::cout << " --- findOneCoG --- bin numbers -- currentBinId: " << currentBinId << ", binIdX: " << binIdX << ", binIdY: " << binIdY << ", binIdZ: " << binIdZ << std::endl;
		// hist->SetBinContent(binIdX, binIdY, 30);
		// hist->SetBinContent(binIdX + 1, binIdY + 1, 20);
		// std::cout << " --- findOneCoG --- new bin content: " << hist->GetBinContent(binIdX + 1, binIdY + 1) << std::endl;

	// generate indices of neighbors
	std::vector<int> indices;
	for (int i = -numberOfNeighboringBins; i <= numberOfNeighboringBins; i++) {
		// if (i != 0) {
			indices.push_back(i);
		// }
	}
	
	// gather information about neighbors
	std::vector<double> valuesX, valuesY, weights;
	for (int indexX = 0; indexX < indices.size(); ++indexX) {
		for (int indexY = 0; indexY < indices.size(); ++indexY) {
			int tempBinIdX = binIdX + indices[indexX];
			int tempBinIdY = binIdY + indices[indexY];
			// std::cout << " --- findOneCog --- neighbor gather loop -- tempBinIdX: " << tempBinIdX << ", tempBinIdY: " << tempBinIdY << std::endl;
			double weight = hist->GetBinContent(tempBinIdX, tempBinIdY);
			double coordinateX = hist->GetXaxis()->GetBinCenter(tempBinIdX);
			double coordinateY = hist->GetYaxis()->GetBinCenter(tempBinIdY);
			// std::cout << " --- findOneCog --- neighbor gather loop -- weight: " << weight << ", coordinateX: " << coordinateX << ", coordinateY: " << coordinateY << std::endl;

			valuesX.push_back(coordinateX);
			valuesY.push_back(coordinateY);
			weights.push_back(weight);
		}
	}

	// actually calculate the cogs
	double meanX = weightedMean(valuesX, weights);
	double meanY = weightedMean(valuesY, weights);
	// we want to amplify single bins with great multiplicity over lots of bins with small multiplicity each - hence we take an n dimensional pythagoras of the weights
	double sumOfWeights = 0;
	for (std::vector<double>::iterator it = weights.begin(); it != weights.end(); it++) sumOfWeights += (*it)*(*it);
	sumOfWeights = sqrt(sumOfWeights);

	return std::make_tuple(sumOfWeights, meanX, meanY);
}

bool peakIsEqual(double x1, double y1, double x2, double y2, double uncertaintyX = 0.02, double uncertaintyY = 0.01) {
	// uncertainty: allowed uncertainty
	bool upperLimitX = x1 < x2 + uncertaintyX;
	bool lowerLimitX = x1 > x2 - uncertaintyX;
	bool upperLimitY = y1 < y2 + uncertaintyY;
	bool lowerLimitY = y1 > y2 - uncertaintyY;

	bool isInBoundsX = upperLimitX && lowerLimitX;
	bool isInBoundsY = upperLimitY && lowerLimitY;

	if (isInBoundsX) {
		if (isInBoundsY) {
			return true;
		}
	} else {
		return false;
	}
	// std::cout << " --- peakIsEqual --- y1: " << y1 << ", (1 + uncertainty) * y2: " << (1 + uncertainty) * y2 << " -- y1 < ... y2: " << (y1 < (1 + uncertainty) * y2) << std::endl;
	// if (abs(x1) > (1 - uncertainty) * abs(x2) && abs(x1) < (1 + uncertainty) * abs(x2)) {
	// 	std::cout << " --- peakIsEqual --- x is within uncertainty -- " << std::endl;
	// 	if (abs(y1) > (1 - uncertainty) * abs(y2) && abs(y1) < (1 + uncertainty) * abs(y2)) {
	// 		std::cout << " --- peakIsEqual --- y is within uncertainty --" << std::endl;
	// 		return true;
	// 	}
	// } else {
	// 	return false;
	// }
}
std::vector<std::tuple<double, double, double> > mergeEqualPeaks(std::vector<std::tuple<double, double, double> > peaks) {
	std::vector<std::tuple<double, double, double> > mergedPeaks;

	unsigned int nOfPeaks = peaks.size();

	mergedPeaks.push_back(peaks[0]);

	for (int i = 1; i < nOfPeaks; ++i) {
		double currentHeight = std::get<0>(peaks[i]);
		double currentXCoordinate = std::get<1>(peaks[i]);
		double currentYCoordinate = std::get<2>(peaks[i]);

		bool merged = false;

		for (int j = 0; j < mergedPeaks.size(); ++j) {
			double mergedHeight = std::get<0>(mergedPeaks[j]);
			double mergedXCoordinate = std::get<1>(mergedPeaks[j]);
			double mergedYCoordinate = std::get<2>(mergedPeaks[j]);

			// std::cout << " --- mergeEqualPeaks --- test_peak (x, y): (" << currentXCoordinate << ", " << currentYCoordinate << ")" << " -- merged_peak (x, y): " << mergedXCoordinate << ", " << mergedYCoordinate << ")" << std::endl;

			// std::cout << " --- mergeEqualPeaks --- peak is equal? -- " << peakIsEqual(currentXCoordinate, currentYCoordinate, mergedXCoordinate, mergedYCoordinate) << std::endl;

			if (peakIsEqual(currentXCoordinate, currentYCoordinate, mergedXCoordinate, mergedYCoordinate)) {
				// merge peaks
				double newHeight = (currentHeight + mergedHeight) / 2;
				double newXCoordinate = (currentXCoordinate + mergedXCoordinate) / 2;
				double newYCoordinate = (currentYCoordinate + mergedYCoordinate) / 2;
				mergedPeaks[j] = std::make_tuple(newHeight, newXCoordinate, newYCoordinate);
				merged = true;
				// std::cout << " --- mergeEqualPeaks --- merged a peak! -- " << std::endl;
				break;
			}
		}
		// no equal peak could be found - we have a new one
		if (!merged) {
			// std::cout << " --- mergeEqualPeaks --- merged no peak! -- " << std::endl;
			mergedPeaks.push_back(peaks[i]);
		}
	}

	return mergedPeaks;
}

TGraph peaksToMarkers(std::vector<std::tuple<double, double, double> > means) {
	TGraph markerGraph;
	for (int i = 0; i < means.size(); ++i) {
		markerGraph.SetPoint(i, std::get<1>(means[i]), std::get<2>(means[i]));
	}
	markerGraph.SetMarkerStyle(kFullTriangleDown);
	return markerGraph;
}

int main(int argc, char** argv) {
	double everyXDegrees = 1;
	if (argc > 1) everyXDegrees = static_cast<double>(atof(argv[1]));
	int readInUpToLineNumber = 5;
	if (argc > 2) readInUpToLineNumber = static_cast<int>(atof(argv[2]));

	std::vector<double> x, y, r;

	PndAhTools::readPoints("real_data.txt", x , y, r, readInUpToLineNumber);

	std::vector<double> fakeR(x.size());
	std::fill(fakeR.begin(), fakeR.end(), 0.);

	PndAhHoughTransform * myHough = new PndAhHoughTransform(x, y, fakeR, 180, everyXDegrees, true);

	TH2D * houghHist = myHough->GetHistogram();

	std::vector<std::vector<double> > theRs = myHough->Getrs();
	std::vector<double> theAngles = myHough->GetAngles();
	std::vector<std::tuple<double, double, double> > theMeans;

	for (int i = 0; i < theRs.size(); ++i) {  // loop over every rho
		// std::cout << " --- peak finding loop --- for hit number i: " << i << std::endl;
		// get all heights of each rho-alpha value
		auto peaks = peakOfHill(houghHist, theAngles, theRs[i]);
		// sort list of heights by height
		std::sort(peaks.begin(), peaks.end(), compareTupleFirstElement);
		// for (std::vector<std::tuple<double, double, double> >::iterator it = peaks.begin(); it != peaks.end(); ++it) {
		// 	std::cout << "   -- hill height at (x, y): (" << std::get<1>(*it) << ", " << std::get<2>(*it) << ") with multiplicity: "  << std::get<0>(*it) << std::endl;
		// }

		// we're only interested in highest bin, so: get index in sorted peaks vector of start of second highest multiplicity
		int indexOfSecondHighest = peaks.size();
		for (int j = 0; j < peaks.size(); j++) {
			if (std::get<0>(peaks[j]) > std::get<0>(peaks[j+1])) {
				indexOfSecondHighest = j+1;
				break;
			}
		}
			// std::cout << " --- cog loop --- indexOfSecondHighest: " << indexOfSecondHighest << std::endl;
		// create a copy of the peaks list and cut as soon as second highest multiplicity starts
		auto peaksCut = peaks;
		peaksCut.resize(indexOfSecondHighest);

		// for every peak still existing: calculate the cog with the neighbors
		std::vector<std::tuple<double, double, double> > cogMeans;
		for (int j = 0; j < peaksCut.size(); j++) {
			std::tuple<double, double, double> cogMean = findOneCog(houghHist, std::get<1>(peaks[j]), std::get<2>(peaks[j]));
				// std::cout << "   -- cog peaks --- (x, y): " << std::get<1>(cogMean) << ", " << std::get<2>(cogMean) << ") with multiplicity: " << std::get<0>(cogMean) << std::endl;
			cogMeans.push_back(cogMean);
		}
		// sort list of cog-means by height
		std::sort(cogMeans.begin(), cogMeans.end(), compareTupleFirstElement);

		// std::cout << "   -- cog loop --- " << "the best peak is located at (x, y): (" << std::get<1>(cogMeans[0]) << ", " << std::get<2>(cogMeans[0]) << ") with multiplicity: " << std::get<0>(cogMeans[0]) << std::endl;
		

		// push highest cog-mean to master peak list
		theMeans.push_back(cogMeans[0]);
	}

	// merge similar means
	std::vector<std::tuple<double, double, double> > finalPeaks = mergeEqualPeaks(theMeans);
	for (int i = 0; i < finalPeaks.size(); ++i) {
		std::cout << " --- Final, cog-ed, neighbor-merged peaks ---  (x, y): (" << std::get<1>(finalPeaks[i]) << ", " << std::get<2>(finalPeaks[i]) << ") with multiplicity: " << std::get<0>(finalPeaks[i]) << std::endl;
	}
	TGraph prelimPeaks = peaksToMarkers(theMeans);
	prelimPeaks.SetMarkerColor(kMagenta);
	TGraph myPeaks = peaksToMarkers(finalPeaks);

	TApplication * theApp = new TApplication("app", &argc, argv, 0, -1);
	TCanvas * cOriginal = new TCanvas("cOriginal", "Original Histogram", 10, 10, 800, 600);
	houghHist->Draw("COLZ");
	prelimPeaksout.Draw("P");
	myPeaks.Draw("P");
	cOriginal->Update();

	theApp->Run();
	return 1;
}