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

#include "boost/tuple/tuple.hpp"

#include "TH2D.h"

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

PndAhMultipeakAnalysis::PndAhMultipeakAnalysis() : 
	fVerbose(0)
{}
PndAhMultipeakAnalysis::PndAhMultipeakAnalysis(TH2D * baseHist) : 
	fBaseHist(baseHist),
	isFirstFilter(true),
	fVerbose(10)
{}

PndAhMultipeakAnalysis::~PndAhMultipeakAnalysis()
{}

void PndAhMultipeakAnalysis::initializeNewHistogram() {
	fReturnHist = new TH2D(*fBaseHist);
	fReturnHist->Reset();
	isFirstFilter = false;
}

void PndAhMultipeakAnalysis::copyOriginalToReturn() {
	fReturnHist = fBaseHist;
}

void PndAhMultipeakAnalysis::addArbitraryFiltermask(double (*weightingFuntion)(double[])) {
	if (isFirstFilter) initializeNewHistogram();

	double data[9];
	unsigned int nBinsX = fBaseHist->GetNbinsX(), nBinsY = fBaseHist->GetNbinsY();

	std::cout << "nBinsX = " << nBinsX << ", nBinsY = " << nBinsY << std::endl;

	for (int x = 2; x <= nBinsX-1; ++x) {  // histograms range from 1 to n, 0 is underflow, n+1 overflow bin; x = 2 is lazy start since I dont want to take care about the edges (TODO)
		for (int y = 2; y <= nBinsY-1; ++y) {
			data[kW] = fBaseHist->GetBinContent(x-1, y);
			std::cout << "x = " << x << ", y = " << y << ", data[kW] = " << data[kW] << ", binContent at (x-1, y) " <<  fBaseHist->GetBinContent(x-1, y) << std::endl;
			data[kSW] = fBaseHist->GetBinContent(x-1, y-1);
			data[kS] = fBaseHist->GetBinContent(x, y-1);
			data[kSE] = fBaseHist->GetBinContent(x+1, y-1);
			data[kE] = fBaseHist->GetBinContent(x+1, y);
			data[kNE] = fBaseHist->GetBinContent(x+1, y+1);
			data[kN] = fBaseHist->GetBinContent(x, y+1);
			data[kNW] = fBaseHist->GetBinContent(x-1, y+1);
			data[kC] = fBaseHist->GetBinContent(x, y);
			if (data[kC] > 0) std::cout << "data_center = " << data[kC] << std::endl;
			double newCenter = weightingFuntion(data);
			if (data[kC] > 0) std::cout << "data_center_new = " << newCenter << std::endl;

			fReturnHist->SetBinContent(x, y, newCenter);
		}
	}
}

boost::tuple<double, double> PndAhMultipeakAnalysis::getMeansTwoOrders(int xIndexCenter, int yIndexCenter) {
	double orderOne = 0;
	double orderTwo = 0;
	double currentBinValue = 0;
	int level = 0;
	for (int i = -2; i <= 2; i++) {
		for (int j = -2; j <= 2; j++) {
			currentBinValue = fBaseHist->GetBinContent(xIndexCenter+i, yIndexCenter+j);
			level = std::max(abs(i), abs(j));
			if (level == 1) {
				orderOne += currentBinValue;
			} else {  // level == 2
				orderTwo += currentBinValue;
			}
		}
	}
	orderOne /= 8;  // mean
	orderTwo /= 16;  // mean
	return boost::make_tuple(orderOne, orderTwo);
}

void PndAhMultipeakAnalysis::secondOrderMeanFilter(double orderOneWeight, double orderTwoWeight) {
	if (isFirstFilter) initializeNewHistogram();

	double center;

	unsigned int nBinsX = fBaseHist->GetNbinsX(), nBinsY = fBaseHist->GetNbinsY();
	for (int x = 2; x < nBinsX - 1; ++x) {
		for (int y = 2; y < nBinsY - 1; ++y) {
			center = fBaseHist->GetBinContent(x, y);
			boost::tuple<double, double> means = getMeansTwoOrders(x, y);
			double orderOneMean = means.get<0>();
			double orderTwoMean = means.get<1>();
			if (!(orderOneMean < (orderOneWeight*center) && orderTwoMean < (orderTwoWeight*orderOneMean))) {
				center = 0;
			}
			fReturnHist->SetBinContent(x, y, center);
		}
	}
}

void PndAhMultipeakAnalysis::addThresholdCut(int threshold = 1) {
	if (isFirstFilter) initializeNewHistogram();

	unsigned int nBinsX = fBaseHist->GetNbinsX(), nBinsY = fBaseHist->GetNbinsY();
	for (int i = 0; i < nBinsX; ++i) {
		for (int j = 0; j < nBinsY; ++j) {
			if (fBaseHist->GetBinContent(i, j) < threshold) fReturnHist->SetBinContent(i, j, 0);
		}
	}
}

void PndAhMultipeakAnalysis::addThresholdCut(TH2D * hist, int threshold = 1) {
	unsigned int nBinsX = hist->GetNbinsX(), nBinsY = hist->GetNbinsY();
	for (int i = 0; i < nBinsX; ++i) {
		for (int j = 0; j < nBinsY; ++j) {
			if (hist->GetBinContent(i, j) < threshold) hist->SetBinContent(i, j, 0);
		}
	}
}

double PndAhMultipeakAnalysis::func_wednesdaysIdea(double data[]) {
	/**
	Function, intended to be used in a function pointer.

	Scalar-multiplies a 3x3 grid (with kC at center) with filterMask, weighting the diagonal elements a bit more.
	The new center is aboves scalar product multiplied with (old center**6).
	@return 0, if the old center is 0, the new center otherwise
	*/ 
	double filterMask[] = { 1, sqrt(2), 1, sqrt(2), 1, sqrt(2), 1, sqrt(2), 0};
	double newCenter = PndAhTools::scalarProduct(data, filterMask, 9);
	if (data[kC] > 0) {
		return (pow(data[kC], 6) * newCenter);
		std::cout << "data[kC] = " << data[kC] << " newcenter = " << newCenter << std::endl;
		std::cout << "YIPPI" << std::endl;
	} else {
		return 0;
	}
	// return data[kC] / ( data[kW] + data[kSW] * sqrt(2) + data[kS] + data[kSE] * sqrt(2) + data[kE] + data[kNE] * sqrt(2) + data[kN] + data[kNW] * sqrt(2) );
}

double PndAhMultipeakAnalysis::func_diagonallyEnabledLaplacianFilter(double data[]) {
	// for (int i = 0; i < 9; ++i) {
	// 	std::cout << "data[" << i << "] = " << data[i] << std::endl;
	// }

	// int filterMask[] = {1, 1, 1, 1, 1, 1, 1, 1, -8};
	double filterMask[] = {-1, -1, -1, -1, -1, -1, -1, -1, 8};
	// int filterMask[] = {-1, -2, -1, -2, -1, -2, -1, -2, 10};
	// int filterMask[] = {-2, -1, -2, -1, -2, -1, -2, -1, 12};//

	return PndAhTools::scalarProduct(data, filterMask, 9);
	// return (data[kW] + data[kSW] + data[kS] + data[kSE] + data[kE] + data[kNE] + data[kN] + data[kNW] - (8 * data[kC]));
}


void PndAhMultipeakAnalysis::addSurroundingEightThreshold() {
	// this filter manipulates fReturnHist!
	if (isFirstFilter) {
		initializeNewHistogram();
		copyOriginalToReturn();
	}

	unsigned int nBinsX = fReturnHist->GetNbinsX(), nBinsY = fReturnHist->GetNbinsY();

	bool deleteThisBin = false;
	for (int i = 1; i <= nBinsX; ++i) {  // histograms range from 1 to n, 0 is underflow, n+1 overflow bin
		for (int j = 1; j <= nBinsY; ++j) {
			double thisBinContent = fReturnHist->GetBinContent(i, j);

			if (fVerbose > 0) std::cout << "Checking bin(i,j) = bin(" << i << "," << j << ") = " << thisBinContent << std::endl;
			
			// Trivial case: our bin actually is 0
			if (thisBinContent < ZERO) {
				if (fVerbose > 0) std::cout << "This bin is a 0 content bin. Doing nothing." << std::endl; 
				continue;
			}

			// Start with west bin, go counter-clockwise (W, SW, S, SE, E, NE, N, NW)
			// W
			if (i-1 != 0) { 
				if (fVerbose > 0) std::cout << "  BinContent(i-1, j) = (" << i-1 << "," << j << ") = " << fReturnHist->GetBinContent(i-1, j) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i-1, j) > thisBinContent) deleteThisBin = true;
			}
			// SW
			if (!deleteThisBin && (i-1 != 0 && j-1 != 0))  { 
				if (fVerbose > 0) std::cout << "  BinContent(i-1, j-1) = (" << i-1 << "," << j-1 << ") = " << fReturnHist->GetBinContent(i-1, j-1) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i-1, j-1) > thisBinContent) deleteThisBin = true;
			}
			// S
			if (!deleteThisBin && (j-1 != 0)) { 
				if (fVerbose > 0) std::cout << "  BinContent(i, j-1) = (" << i << "," << j-1 << ") = " << fReturnHist->GetBinContent(i, j-1) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i, j-1) > thisBinContent) deleteThisBin = true;
			}
			// SE
			if (!deleteThisBin && (i+1 != nBinsX && j-1 != 0)) {
				if (fVerbose > 0) std::cout << "  BinContent(i+1, j-1) = (" << i+1 << "," << j-1 << ") = " << fReturnHist->GetBinContent(i+1, j-1) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i+1, j-1) > thisBinContent) deleteThisBin = true;
			} 
			// E
			if (!deleteThisBin && (i+1 != nBinsX)) {
				if (fVerbose > 0) std::cout << "  BinContent(i+1, j) = (" << i+1 << "," << j << ") = " << fReturnHist->GetBinContent(i+1, j) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i+1, j) > thisBinContent) deleteThisBin = true;
			}
			// NE
			if (!deleteThisBin && (i+1 != nBinsX && j+1 != nBinsY)) {
				if (fVerbose > 0) std::cout << "  BinContent(i+1, j+1) = (" << i+1 << "," << j+1 << ") = " << fReturnHist->GetBinContent(i+1, j+1) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i+1, j+1) > thisBinContent) deleteThisBin = true;
			}
			// N
			if (!deleteThisBin && (j+1 != nBinsY)) {
				if (fVerbose > 0) std::cout << "  BinContent(i, j+1) = (" << i << "," << j+1 << ") = " << fReturnHist->GetBinContent(i, j+1) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i, j+1) > thisBinContent) deleteThisBin = true;
			}
			// NW
			if (!deleteThisBin && (i-1 != 0 && j+1 != nBinsY)) {
				if (fVerbose > 0) std::cout << "  BinContent(i-1, j+1) = (" << i-1 << "," << j+1 << ") = " << fReturnHist->GetBinContent(i-1, j+1) << " -- currentBinContent = " << thisBinContent << std::endl;
				if (fReturnHist->GetBinContent(i-1, j+1) > thisBinContent) deleteThisBin = true;
			}


			if (deleteThisBin) {  // || fReturnHist->GetBinContent(i,j) < 6) {
				if (fVerbose > 0) std::cout << "Deleting bin(" << i << "," << j << ")!" << std::endl;
				fReturnHist->SetBinContent(i, j, 0);
			}
			if (!deleteThisBin) {
				if (fVerbose > 0) std::cout << "Found peak at (" << i << "," << j << ")!" << std::endl;
			}
			deleteThisBin = false;
		}
	}
	std::cout << "NbinsX: " << nBinsX << std::endl;
	std::cout << "NbinsY: " << nBinsY << std::endl;
}

double PndAhMultipeakAnalysis::getNormalizedContrast(const TH2D * hist, int bin1X, int bin1Y, int bin2X, int bin2Y) {
	double oContent = hist->GetBinContent(bin1X, bin1Y);
	double tContent = hist->GetBinContent(bin2X, bin2Y);
	double maxContent = hist->GetMaximum();
	double minContent = hist->GetMinimum();
	double contrast = abs(abs(oContent) - abs(tContent)) / abs(maxContent - minContent);
	return contrast;
} 

void PndAhMultipeakAnalysis::combineNeighbors() {
	if (isFirstFilter) initializeNewHistogram();
	unsigned int nBinsX = fBaseHist->GetNbinsX(), nBinsY = fBaseHist->GetNbinsY();
	for (int i = 2; i <= nBinsX-1; ++i) {  // histograms range from 1 to n, 0 is underflow, n+1 overflow bin
		for (int j = 2; j <= nBinsY-1; ++j) {
			double thisBinContent = fBaseHist->GetBinContent(i, j);
			// std::map<double, boost::tuple<int, int> > m;
			boost::tuple<double, int, int> highestBin = 0, secondHighestBin = 0, oldBin = 0;
			for (int x = -1; x <= 1; x++) {
				for (int y = -1; y <= 1; y++) {
					if (x != 0 && y !=0) {
						// m[fReturnHist->GetBinContent(i+x, j+y)] = boost::make_tuple(i+x, j+y);
						double currentBinContent = fBaseHist->GetBinContent(i+x, j+y);
						if (currentBinContent >= secondHighestBin.get<0>()) {
							oldBin = secondHighestBin;
							secondHighestBin = boost::make_tuple(currentBinContent, i+x, j+y);
							if (currentBinContent >= highestBin.get<0>()) {
								highestBin = secondHighestBin;
								secondHighestBin = oldBin;
							}
						}
					}
				}
			}
			double newBinContent = (thisBinContent + highestBin.get<0>() + secondHighestBin.get<0>())/3;
			fReturnHist->SetBinContent(i, j, newBinContent);
		}
	}
}

// double x, double y, double everyXDegrees, double (*trafoFunction)(double, double, double, double)

boost::tuple<int, int, double> PndAhMultipeakAnalysis::getHighestBinOfHit(std::vector<double> angles, std::vector<double> rs) {
	if (isFirstFilter) initializeNewHistogram();

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

	return boost::make_tuple(highX, highY, highZ);
}