/*
 * PndAhHoughTransform.cpp
 *
 *  Created on: May 14, 2013
 *      Author: Andreas Herten
 */

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

#include <iostream>
#include <vector>

#include "TMath.h"
#include "TH2D.h"

#define SUPERSMALLVALUE 0.001 // has to be smaller than the smallest possible grid cell (/granularity)

PndAhHoughTransform::PndAhHoughTransform()
 {}

PndAhHoughTransform::~PndAhHoughTransform()
 {}

PndAhHoughTransform::PndAhHoughTransform(std::vector<double> __x, std::vector<double> __y, std::vector<double> __r) : 
  fVX(__x),
  fVY(__y),
  fVR(__r)
 {}

PndAhHoughTransform::PndAhHoughTransform(std::vector<double> __x, std::vector<double> __y, std::vector<double> __r, double __maxAngle, double __everyXDegrees, bool __verbose, bool __keepHitOriginInfo) :
  fVX(__x),
  fVY(__y),
  fVR(__r),
  fMaxAngle(__maxAngle),
  fEveryXDegrees(__everyXDegrees),
  fHistLimitsAreSet(false),
  fVerbose(__verbose),
  fKeepHitOriginInfo(__keepHitOriginInfo) {
	DoEverything();
}

void PndAhHoughTransform::DoEverything() {
	// DoConformalMapping
	if (fVerbose) std::cout << "-- PndAhHoughTransform::DoEverything() -- Generating Angles" << std::endl;
	DoGenerateAngles();

	if (fVerbose) std::cout << "-- PndAhHoughTransform::DoEverything() -- Hough Transforming" << std::endl;
	DoHoughTransform();
}

void PndAhHoughTransform::DoGenerateAngles() {
	if (fVerbose) std::cout << "-- PndAhHoughTransform::DoGenerateAngles() -- fVangles size before resize: " << fVAngles.size() << std::endl;
	fVAngles.resize(fMaxAngle/fEveryXDegrees);
	if (fVerbose) std::cout << "-- PndAhHoughTransform::DoGenerateAngles() -- fVangles size after resizing: " << fVAngles.size() << std::endl;

	if (fVerbose) std::cout << "-- PndAhHoughTransform::DoGenerateAngles() -- Generate angles by means of PndAhTools:generate()" << std::endl;
	PndAhTools::generate(
		fVAngles.begin(), 
		fVAngles.end(), 
		(int)0,
		fEveryXDegrees
	);
	if (fVerbose) std::cout << "-- PndAhHoughTransform::DoGenerateAngles() -- First two angles are: " << fVAngles[0] << " and " << fVAngles[1] << std::endl;
}

void PndAhHoughTransform::DoHoughTransform() {
	if (fVerbose) std::cout << "-- PndAhHoughTransform::DoHoughTransform() -- Generating all rs for all (x,y,r)s" << std::endl;
	fVVrs = generateAllrsForAllXYRs(fVX, fVY, fVR, fVAngles);
	if (fKeepHitOriginInfo) fVpVrs = generateAllrsForAllXYRsWithOrigin(fVX, fVY, fVR, fVAngles);
}

template<typename _InputType>
std::vector<std::vector<_InputType> > PndAhHoughTransform::generateAllrsForAllXYRs (std::vector<_InputType> __x, std::vector<_InputType> __y, std::vector<_InputType> __r, std::vector<_InputType> __alpha) {
	if (fVerbose) std::cout << "-- PndAhHoughTransform::generateAllrsForAllXYRs() -- " << std::endl;
	std::vector<std::vector<_InputType> > tOutput;
	for (int i = 0; i < __x.size(); ++i) {
		std::vector<_InputType> rForCurrentXYR = generateAllrsForOneXYR(__alpha.begin(), __alpha.end(), __x[i], __y[i], __r[i]);
		tOutput.push_back(rForCurrentXYR);
	}

	/* The following, unfortunately, gives some strange errors
		// for (typename _InputType::iterator itX = __x.begin(), 
		// 	itY = __y.begin(), 
		// 	itR = __r.begin(); 
		// 	itX != __x.end(); 
		// 	++itX, ++itY, ++itR) {
		// 	std::vector<_InputType> rForCurrentXYR = generateAllrsForOneXYR(__alpha.begin(), __alpha.end(), *itX, *itY, *itR);
		// 	tOutput.push_back(rForCurrentXYR);
		// }
	*/
	return tOutput;
}

template<typename _InputType>
std::vector<std::pair<std::vector<_InputType> > > PndAhHoughTransform::generateAllrsForAllXYRsWithOrigin (std::vector<_InputType> __x, std::vector<_InputType> __y, std::vector<_InputType> __r, std::vector<_InputType> __alpha) {
	if (fVerbose) std::cout << "-- PndAhHoughTransform::generateAllrsForAllXYRs() -- " << std::endl;
	std::vector<std::pair<std::vector<_InputType> > > tOutput;
	for (int i = 0; i < __x.size(); ++i) {
		std::vector<_InputType> rForCurrentXYR = generateAllrsForOneXYR(__alpha.begin(), __alpha.end(), __x[i], __y[i], __r[i]);
		tOutput.push_back(std::make_pair(rForCurrentXYR, i));
	}
	return tOutput;
}

template<typename _InputIterator, typename _DataType>
std::vector<_DataType> PndAhHoughTransform::generateAllrsForOneXYR(_InputIterator __AlphaFirst, _InputIterator __AlphaLast, _DataType __x, _DataType __y, _DataType __r) {
	std::vector<_DataType> rsForOneXYR;
	for (; __AlphaFirst != __AlphaLast; ++__AlphaFirst) {
		rsForOneXYR.push_back(function_houghTrafo(function_generic_houghTrafo, __x, __y, __r, *__AlphaFirst));
	}

	return rsForOneXYR;
}

template<typename _DataType>
_DataType PndAhHoughTransform::function_houghTrafo(_DataType (*trafoFunction)(_DataType, _DataType, _DataType, _DataType), _DataType __x, _DataType __y, _DataType __r, _DataType __alpha) {
	std::cout << "-- PndAhHoughTransform::function_houghTrafo() -- x = " << __x << ", y = " << __y << ", r = " << __r << ", alpha = " << __alpha << std::endl;
	return trafoFunction(__x, __y, __r, __alpha);
	// return sin(__alpha * TMath::DegToRad()) * __y + cos(__alpha * TMath::DegToRad()) * __x + __r;
}

template<typename _DataType>
_DataType PndAhHoughTransform::function_generic_houghTrafo(_DataType __x, _DataType __y, _DataType __r, _DataType __alpha) {
	return sin(__alpha * TMath::DegToRad()) * __y + cos(__alpha * TMath::DegToRad()) * __x + __r;
}

void PndAhHoughTransform::DoSetAutoHistLimits() {
	fYMin = PndAhTools::findMinOfVectorOfVectors(fVVrs);
	fYMax = PndAhTools::findMaxOfVectorOfVectors(fVVrs);
	fHistLimitsAreSet = true;
}


void PndAhHoughTransform::SetManualHistLimits(double yMin, double yMax) {
	fYMin = yMin;
	fYMax = yMax;
	fHistLimitsAreSet = true;
}

TH2D * PndAhHoughTransform::GetHistogram(bool useAutoLimits, TString histName, TString histTitle) {
	//c are about limits
	if (false == fHistLimitsAreSet && true == useAutoLimits) DoSetAutoHistLimits();
	if (false == fHistLimitsAreSet && false == useAutoLimits) std::cout << " - E - PndAhHoughTransform::GetHistogram - Please set manual limits for histogram." << std::endl;

	// initialize histogram
	int nBins = fMaxAngle/fEveryXDegrees;
	unsigned int yRebinFactor = 3;
	TH2D * tempHist = new TH2D(histName, histTitle, nBins, 0, fMaxAngle, nBins/yRebinFactor, fYMin*0.95, fYMax*1.05);
	tempHist->GetXaxis()->SetTitle("#alpha/#circ");
	tempHist->GetYaxis()->SetTitle("r");

	// fill histogram
	for (int i = 0; i < fVVrs.size(); ++i) {
		for (int j = 0; j < fVVrs[i].size(); ++j) {
			double newTempAlpha = fVAngles[j] + SUPERSMALLVALUE;

			// if (tempHist->GetBinContent(newTempAlpha, fVVrs[i][j]) > 0 ) {
			// 	std::cout << "-- PndAhHoughTransform::GetHistogram() -- Cell (" << newTempAlpha << "," << fVVrs[i][j] << ") > 0 before filling! It is: " << tempHist->GetBinContent(newTempAlpha, fVVrs[i][j]) << "." << std::endl;
			// }

			// std::cout << "-- PndAhHoughTransform::GetHistogram() -- (i,j) = (" << i << "," << j << "); (alpha,r) = (" << newTempAlpha << "," << fVVrs[i][j] << "); Bin Content (before) = " << tempHist->GetBinContent(newTempAlpha, fVVrs[i][j]);

			tempHist->Fill(newTempAlpha, fVVrs[i][j]);

			// std::cout << "; Bin Content (after) = " << tempHist->GetBinContent(newTempAlpha, fVVrs[i][j]) << std::endl;

			// if (tempHist->GetBinContent(newTempAlpha, fVVrs[i][j]) > 1 ) {
			// 	std::cout << "-- PndAhHoughTransform::GetHistogram() -- Cell (" << newTempAlpha << "," << fVVrs[i][j] << ") > 1! It is: " << tempHist->GetBinContent(newTempAlpha, fVVrs[i][j]) << ". Resetting it!" << std::endl;
			// 	tempHist->SetBinContent(newTempAlpha, fVVrs[i][j], 1);
			// 	std::cout << "-- PndAhHoughTransform::GetHistogram() -- Now it's: " << tempHist->GetBinContent(newTempAlpha, fVVrs[i][j]) << std::endl;
			// }
		}
	}

	return tempHist;
}