//#define BZ_DEBUG

#ifdef BZ_DEBUG
#warning BZ_DEBUG aktiviert!
#define DEBUG_MSG "BZ_DEBUG aktiviert! "
#else
#warning ohne BZ_DEBUG
#define DEBUG_MSG "ohne BZ_DEBUG: "
#endif

#include <cstdio>
#include <cmath>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>
#include <typeinfo>
#include <stdexcept>
#include <complex>
#include <algorithm>
#include <Magick++.h>
#include <fftw3.h>
#include <sys/time.h>
#include <boost/any.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/limits.hpp>
#include <blitz/array.h>
#include "fm3.h"
#include "sourcecode.h"

 template <class T>
 inline std::string stringify (const T& t)
 {
	  std::stringstream ss;
	  ss << t;
	  return ss.str();
 }

using namespace std;
using namespace Magick;
using namespace blitz;
using namespace boost::program_options;
using namespace boost::algorithm;

template<typename T> inline T sqr(T x) { return x*x; };
template<typename T> inline T identity(T x) { return x; }
inline double deg2rad(double x) { return x*M_PI/180.0; };
inline double rad2deg(double x) { return x*180.0/M_PI; };

double y_mxPlusB(double x, double x1, double y1, double x2, double y2) {
	double m=(y2-y1)/(x2-x1);
	double b=y1-m*x1;
	return m*x+b;
}

template<typename T1, typename T2> void checkBound(Array<T1, 2> &a, Array<T2, 2> &b) {
	for(int i=0; i<2; i++) {
		FIF(a.lbound(i)!=b.lbound(i));
		FIF(a.ubound(i)!=b.ubound(i));
	}
};

void imageCacheToArray(unsigned char *ic, Array<int, 2> &a) {
	for(int y=a.ubound(1); y>=a.lbound(1); y--) {
		for(int x=a.lbound(0); x<=a.ubound(0); x++) {
			a(x, y)=*(ic++);
		}
	}
}

void plotAsciiPixel(double v) {
	int x=int(v*10.999);
	if(x>10) { printf("^"); return; }
	if(x<0) { printf("v"); return; }
	switch(x) {
		case 10: printf(" "); break;
		case 9: printf("·"); break;
		case 8: printf(":"); break;
		case 7: printf("¬"); break;
		case 6: printf("÷"); break;
		case 5: printf("Y"); break;
		case 4: printf("I"); break;
		case 3: printf("U"); break;
		case 2: printf("D"); break;
		case 1: printf("N"); break;
		case 0: printf("Ø"); break;
	}
}

void printArrayAscii(Array<int, 2> &a) {
	printf("%d\n", a.ubound(1)-a.lbound(1));
	for(int y=a.ubound(1); y>=a.lbound(1)+1; y-=2) {
		printf("%5d:", y);
		for(int x=a.lbound(0); x<=a.ubound(0); x++) {
			plotAsciiPixel((a(x, y)+a(x, y-1))/2.0/255.0);
		}
		printf(" :%d\n", y);
	}
}

inline int intImageCenterMax(int x) { return (x-1)/2; }
inline int intImageCenterMin(int x) { return intImageCenterMax(x)-(x-1); }

void derivateTo(
		int (*diffTransform)(int),
		Array<int, 2> &from,
		Array<int, 2> &deri,
		double rxD, double ryD) {

	checkBound(from, deri);

	int rx=int(round(rxD));
	int ry=int(round(ryD));
	for(int x=from.lbound(0); x<=from.ubound(0); x++) {
		for(int y=from.lbound(1); y<=from.ubound(1); y++) {
			Array<int, 2> window(from,
				Range(max(from.lbound(0), x-rx),
					min(from.ubound(0), x+rx)),
				Range(max(from.lbound(1), y-ry),
					min(from.ubound(1), y+ry))
					); // Achtung: Konstruktor -> Referenz, NICHT Kopie!
			int m=int(round(mean(window)));
			deri(x, y)=diffTransform(from(x, y)-m);
		}
	}
}

class dRange: public Range {
public:
	int iMin, iMax;
	double dMin, dMax;

	dRange(int iMin, int iMax, double dMin, double dMax):
		Range(iMin, iMax),
		iMin(iMin), iMax(iMax),
		dMin(dMin), dMax(dMax) {}

	double indexToReal(double x) {
		return y_mxPlusB(x, iMin, dMin, iMax, dMax);
		//return (x-iMin)/(iMax-iMin)*(dMax-dMin)+dMin;
	}

	int realToIndex(double x) {
		return int(round(
				y_mxPlusB(x, dMin, iMin, dMax, iMax)
				));
		/*return int(round(
				(x-dMin)/(dMax-dMin)*(iMax-iMin)+iMin
				));*/
	}
};

int theoreticMaxDForImage(Array<int, 2> &a) {
	return int(ceil(sqrt(
			sqr(a.ubound(0))+sqr(a.ubound(1))
			)));
}

int theoreticMinDForImage(Array<int, 2> &a) {
	return int(floor(-sqrt(
			sqr(a.lbound(0))+sqr(a.lbound(1))
			)));
}

inline double sin90(double x) {	return sin(x+M_PI/2); }
inline double cos90(double x) { return cos(x+M_PI/2); }

void imageToDrt(Array<int, 2> &img, Array<int, 2> &drt, dRange &thetaRange) {
	for(int thetaIndex=drt.lbound(0); thetaIndex<=drt.ubound(0); thetaIndex++) {
		double theta=thetaRange.indexToReal(thetaIndex);

		Array<int, 1> summandYA(Range(img.lbound(1), img.ubound(1)));
		for(int y=img.lbound(1); y<=img.ubound(1); y++)
			summandYA(y)=int(round(65536.0*(sin90(theta)*y +0.5)));

		for(int x=img.lbound(0); x<=img.ubound(0); x++) {
			int summandX=int(round(65536.0*cos90(theta)*x));
			for(int y=img.lbound(1); y<=img.ubound(1); y++) {
				drt(thetaIndex, (summandYA(y)+summandX)>>16)
					+=img(x, y);
			}
		}

	}
}

void normalize(Array<int, 2> &a, int toMaxVal) {
	int aMin=min(a);
	int aMax=max(a);
	a=((a-aMin)*toMaxVal+(aMax-aMin)/2)/(aMax-aMin);
}

void normalize(Array<double, 2> &a) {
	double aMin=min(a);
	double aMax=max(a);
	a=(a-aMin)/(aMax-aMin);
}

void buildAxis(Array<int, 2> &drt, int t1, int d1, int t2, int d2, Array<int, 1> sharpDrtAxis) {
	double tStep=double(t2-t1)/(d2-d1);
	double t=t1+0.5;
	for(int d=d1; d<=d2; d++) {
		int t_int=int(floor(t));
		if(t_int<drt.lbound(firstDim)) t_int=drt.lbound(firstDim);
		if(t_int>drt.ubound(firstDim)) t_int=drt.ubound(firstDim);
		sharpDrtAxis(d)=drt(t_int, d);
		t+=tStep;
	}
}

void findMaxDrtAxis(Array<int, 2> &drt, dRange &thetaRange, double tStart, double tStop, Array<int, 2> &img, int *pT1, int *pT2, int *pT1GleichT2) {
	int indexTStart=thetaRange.realToIndex(tStart);
	int indexTStop =thetaRange.realToIndex(tStop);
	//!*!*!*!*!*!*! double Max=std::numeric_limits<double>::min(); // min()>0!!!
	double Max=boost::numeric::bounds<double>::lowest(); // -1E308
	double t1GleichT2Max=Max;
	for(int t1=indexTStart; t1<=indexTStop; t1++) {
		int d1=img.lbound(1);
		for(int t2=indexTStart; t2<=indexTStop; t2++) {
			int d2=img.ubound(1);
			Array<int, 1> sharpDrtAxis(Range(d1, d2));
			buildAxis(drt, t1, d1, t2, d2, sharpDrtAxis);
			double m=mean(sharpDrtAxis);
			if(m>=Max) { // *pT1+*pT2 werden sonst ggf. nie gesetzt - z. B. bei ganz weißem Bild.
				Max=m;
				*pT1=t1;
				*pT2=t2;
			}
			if(t1==t2 && m>=t1GleichT2Max) { // dito
				t1GleichT2Max=m;
				*pT1GleichT2=t1;
			}
		}
	}
}

void writeToImage(Array<double, 2> &a, Image &i) {
	for(int y=a.ubound(1), yImage=0; y>=a.lbound(1); y--, yImage++) {
		for(int x=a.lbound(0), xImage=0; x<=a.ubound(0); x++, xImage++) {
			if(a(x, y)<0) i.pixelColor(xImage, yImage, "blue");
			else
				if(a(x, y)>1) i.pixelColor(xImage, yImage, "red");
				else i.pixelColor(xImage, yImage, Magick::ColorGray(a(x, y)));
		}
	}
}

double calcSchwerpunktD(Array<int, 1> &sharpDrtAxis) {
	double sumD=0;
	double n=0;
	for(int d=sharpDrtAxis.lbound(0); d<=sharpDrtAxis.ubound(0); d++) {
		sumD+=d*sharpDrtAxis(d);
		n+=sharpDrtAxis(d);
	}
	return sumD/n;
}

class fftPoint {
public:
	double abs;
	double halfWavelength; /* nur historische Gründe */
	double lines;
	fftPoint() {}
	fftPoint(double a, double b, double c): abs(a), halfWavelength(b), lines(c) {}

};
bool operator<(const fftPoint& a, const fftPoint& b) {
    return a.abs < b.abs;
}

double sd(double sumX, double sumXX, double length) {
	if(length==0.0) { return 0.0; }
	return sqrt((sumXX-sqr(sumX)/length)/length);
}

class drtParams {
public:
	double zoomFactor;
	double graphW2;
	double graphH2;
	double graphW;
	double radT1;
	double radT2;
	int drtAxisFftLength;
	vector<fftPoint> drtAxisFftOrderedByAbsDesc;
	vector<double> drtAxis;
};

void getDrtParamsFromImage(variables_map &cmdLine, Image &graphImg, drtParams &drtParam) {
	double pmMaxTheta=deg2rad(cmdLine["angle"].as<double>());
	double thetaCompare=deg2rad(cmdLine["compareAngle"].as<double>());

	graphImg.type( GrayscaleType );

	graphImg.normalize();
	graphImg.negate(true);

	if (cmdLine.count("zoom")) {
		cout << "zooming image to " << cmdLine["zoom"].as<string>() << "\n";
		graphImg.scale(cmdLine["zoom"].as<string>());
	}
	drtParam.zoomFactor=graphImg.size().height()/(drtParam.graphH2*2);

	unsigned char *imageCache=new unsigned char[graphImg.size().width()*graphImg.size().height()];
	graphImg.write(0, 0, graphImg.size().width(), graphImg.size().height(), "R", CharPixel, imageCache);

	Range imageRangeX(intImageCenterMin(graphImg.size().width() ), intImageCenterMax(graphImg.size().width() ));
	Range imageRangeY(intImageCenterMin(graphImg.size().height()), intImageCenterMax(graphImg.size().height()));

	Array<int, 2> img(imageRangeX, imageRangeY);
	imageCacheToArray(imageCache, img);
	delete imageCache;

	Array<int, 2> sharpImg(imageRangeX, imageRangeY);

	printf("sharpening image..."); fflush(stdout);
	derivateTo(identity, img, sharpImg,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100);
	printf("done.\n");

	double maxWinkelRad=pmMaxTheta +thetaCompare;
	dRange drtThetaRange(
			int(round(-maxWinkelRad*graphImg.size().height())),
			int(round( maxWinkelRad*graphImg.size().height())),
			-maxWinkelRad,
			 maxWinkelRad
			);

	dRange drtDRange(theoreticMinDForImage(sharpImg), theoreticMaxDForImage(sharpImg),
			-double(theoreticMinDForImage(sharpImg))/sharpImg.lbound(1)/2,
			double(theoreticMaxDForImage(sharpImg))/sharpImg.ubound(1)/2);

	Array<int, 2> drt(drtThetaRange, drtDRange);
	drt=0; // Initialisierung

	printf("imageToDrt..."); fflush(stdout);
	imageToDrt(sharpImg, drt, drtThetaRange);
	printf("done.\n");

	Array<int, 2> raiseDrt(drtThetaRange, drtDRange);
	printf("raising drt..."); fflush(stdout);
	derivateTo(abs, drt, raiseDrt,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100);

	printf("sharpening drt..."); fflush(stdout);
	Array<int, 2> sharpDrt(drtThetaRange, drtDRange);
	derivateTo(identity, drt, sharpDrt,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100);
	printf("done.\n");

	int t1, t2, t1GleichT2;
	int d1=sharpImg.lbound(1);
	int d2=sharpImg.ubound(1);
	printf("find max(raiseDrt)..."); fflush(stdout);

	findMaxDrtAxis(raiseDrt, drtThetaRange,
		-pmMaxTheta,
		 pmMaxTheta,
		sharpImg, &t1, &t2, &t1GleichT2);
	printf("done.\n");

	drtParam.radT1=drtThetaRange.indexToReal(t1);
	drtParam.radT2=drtThetaRange.indexToReal(t2);

	printf("angle at 0%%: %f; angle at 100%%: %f degrees\n", rad2deg(drtParam.radT1), rad2deg(drtParam.radT2));
	printf("max. var. at constant angle: %lf degrees\n", rad2deg(drtThetaRange.indexToReal(t1GleichT2)));

	//

	Array<int, 1> sharpDrtAxis(Range(d1, d2));
	buildAxis(sharpDrt, t1, d1, t2, d2, sharpDrtAxis);

	double doubleDrtAxis[d2-d1+1];
	printf("iDrtAxisVal:");
	for(int d=d1; d<=d2; d++) {
		doubleDrtAxis[d-d1]=double(sharpDrtAxis(d))/(sharpImg.ubound(0)-sharpImg.lbound(0)+1);
		printf(" %f", doubleDrtAxis[d-d1]);
		drtParam.drtAxis.push_back(doubleDrtAxis[d-d1]);
	}
	printf("\n");

	fftw_plan fftPlan;
	drtParam.drtAxisFftLength=(d2-d1+1)/2+1;
	complex<double>* fft = new complex<double>[drtParam.drtAxisFftLength];
	drtParam.drtAxisFftOrderedByAbsDesc=vector<fftPoint>(drtParam.drtAxisFftLength-1);
	//vector<fftPoint> drtParam.drtParam.drtAxisFftOrderedByAbsDesc(drtAxisFftLength-1);
/*  C++ has its own complex<T> template class, defined in the standard <complex> header file.
	  Reportedly, the C++ standards committee has recently agreed to mandate that the storage
	format used for this type be binary-compatible with the C99 type, i.e. an array T[2] with consecutive
	real [0] and imaginary [1] parts. (See report WG21/N1388.)
	  Although not part of the official standard as of this writing, the proposal stated that:
	“This solution has been tested with all current major implementations of the standard library and
	shown to be working.”
	  To the extent that this is true, if you have a variable complex<double> *x, you can pass it directly
	to FFTW via reinterpret_cast<fftw_complex*>(x).*/
	fftPlan = fftw_plan_dft_r2c_1d(d2-d1+1, doubleDrtAxis, reinterpret_cast<fftw_complex*>(fft), FFTW_ESTIMATE);
	fftw_execute(fftPlan);
	printf("iDrtAxisFFT(n=%d):", d2-d1+1);
	for(int i=1; i<drtParam.drtAxisFftLength; i++) {
		drtParam.drtAxisFftOrderedByAbsDesc[i-1].abs=abs(fft[i])/drtParam.drtAxisFftLength;
		// nächste Zeile: 33% (=0.33) war der default-Zoomfaktor der alten drt-Version.
		// damit die Zahlen vergleichbar sind, wird umgerechnet.
		drtParam.drtAxisFftOrderedByAbsDesc[i-1].halfWavelength=double(drtParam.drtAxisFftLength)*0.33/drtParam.zoomFactor/i;
		drtParam.drtAxisFftOrderedByAbsDesc[i-1].lines=i;
		printf(" %f", drtParam.drtAxisFftOrderedByAbsDesc[i-1].abs);
	}
	printf("\n");
	fftw_destroy_plan(fftPlan);
	delete[] fft;

	//

	Array<int, 1> raiseDrtAxis(Range(d1, d2));
	buildAxis(raiseDrt, t1, d1, t2, d2, raiseDrtAxis);
	double dSchwerRel=(calcSchwerpunktD(raiseDrtAxis)-d1)/(d2-d1);

	Array<int, 1> raiseDrtAxisVar1(Range(d1, d2));
	buildAxis(raiseDrt,
		t1-drtThetaRange.realToIndex(thetaCompare*   dSchwerRel ), d1,
		t2+drtThetaRange.realToIndex(thetaCompare*(1-dSchwerRel)), d2, raiseDrtAxisVar1);

	Array<int, 1> raiseDrtAxisVar2(Range(d1, d2));
	buildAxis(raiseDrt,
		t1+drtThetaRange.realToIndex(thetaCompare*   dSchwerRel ), d1,
		t2-drtThetaRange.realToIndex(thetaCompare*(1-dSchwerRel)), d2, raiseDrtAxisVar2);

	double var1qual=mean(raiseDrtAxisVar1)/mean(raiseDrtAxis);
	double var2qual=mean(raiseDrtAxisVar2)/mean(raiseDrtAxis);
	printf("drtAxis Var1:%f%% Var2:%f%% => %f\n",
			var1qual*100, var2qual*100, 100*(1-max(var1qual, var2qual)));

	if(cmdLine.count("drt")) {
		string sizeStr=stringify(drtThetaRange.iMax-drtThetaRange.iMin+1)
							+"x"+stringify(drtDRange.iMax-drtDRange.iMin+1);
		Image drtOut(sizeStr.c_str(), "black");
		drtOut.quality(100);

		Array<double, 2> tmpDrt(drtThetaRange, drtDRange);
		tmpDrt=raiseDrt*1.0;
		normalize(tmpDrt);

		double tStep=double(t2-t1)/(d2-d1);
		double t=t1+0.5;
		for(int d=d1; d<=d2; d++) {
			tmpDrt(int(floor(t)), d)=-1.0;
			t+=tStep;
		}

		writeToImage(tmpDrt, drtOut);

		try {
			drtOut.write(cmdLine["drt"].as<string>());
		}
		catch( Magick::WarningCoder &warning ) {
			cerr << "Coder Warning: " << warning.what() << endl;
		}
	    catch( Magick::Warning &warning ) {
	    	cerr << "Warning: " << warning.what() << endl;
	    }
	    catch( Magick::ErrorBlob &error) {
	    	cerr << "Error: " << error.what() << endl;
	    }
	}
}

string &after(string &haystack, const char *key, string &val) {
	if(haystack.find(key, 0)!=0) {
	    val=string("");
	    return val;
	}
	val=haystack.substr(strlen(key));
	return val;
}

int getDrtParamsFromDrtlog(variables_map &cmdLine, Image &graphImg, drtParams &drtParam) {
	drtParam.zoomFactor=0.33;

	ifstream drtLog;
	drtLog.open(cmdLine["import"].as<string>().c_str());
	if(drtLog.fail()) return 0;

	int paramcount=0;
	while(! drtLog.eof()) {
		string line;
		getline(drtLog, line);
		string val;

//			graphW/H wird nicht mehr angefasst, da bereits beim Laden des Bildes bestimmt.

		if((after(line, "size: ", val)).size()>0) {
			vector<string> size;
			split(size, val, is_any_of("x"));
			if(strtoul(size[0].c_str(), NULL, 10) != graphImg.size().width()
			|| strtoul(size[1].c_str(), NULL, 10) != graphImg.size().height()) {
				printf("FEHLER: Bildgröße aus %s stimmt nicht mit tatsächlicher Bildgröße überein.\n", cmdLine["import"].as<string>().c_str());
				return 0;
			}
		}

		if((after(line, "angle at ", val)).size()>0) {
			vector<string> angleParam;
			split(angleParam, val, is_any_of("%:; ")); // trennzeichen zusammenfassen

			double perD1=atof(angleParam[0].c_str());
			double radT1=deg2rad(atof(angleParam[3].c_str()));
			double perD2=atof(angleParam[7].c_str());
			double radT2=deg2rad(atof(angleParam[10].c_str()));

			drtParam.radT1=y_mxPlusB(  0, perD1, radT1, perD2, radT2);
			drtParam.radT2=y_mxPlusB(100, perD1, radT1, perD2, radT2);
			printf("radT1=%f\n", drtParam.radT1);
			printf("radT2=%f\n", drtParam.radT2);
			paramcount++;
		}

		if((after(line, "iDrtAxisFFT(n=", val)).size()>0) {
			vector<string> sfft;
			split(sfft, val, is_any_of(" "));
			vector<string> n;
			split(n, sfft[0], is_any_of(")"));
			int fftInputLength=atoi(n[0].c_str());
			drtParam.drtAxisFftLength=(fftInputLength-1)/2;
			// ja, war wohl in altem drt-Programm so;
			// die paar % machen den Kohl nicht fett.
			printf("drtParam.drtAxisFftLength=%d\n", drtParam.drtAxisFftLength);
			for(unsigned i=1; i<sfft.size(); i++) {
				drtParam.drtAxisFftOrderedByAbsDesc.push_back(fftPoint(
						atof(sfft[i].c_str()),
						double(drtParam.drtAxisFftLength)/i,
						double(i)
						));
			}
			paramcount++;
		}

		if((after(line, "iDrtAxisVal: ", val)).size()>0) {
			vector<string> string_drtAxis;
			split(string_drtAxis, val, is_any_of(" "));
			for(unsigned i=0; i<string_drtAxis.size(); i++) {
				drtParam.drtAxis.push_back(atof(string_drtAxis[i].c_str()));
			}
			paramcount++;
		}

		// "shrinking"="zooming"
		if((after(line, "shrinking image to ", val)).size()>0) {
			vector<string> zoom;
			split(zoom, val, is_any_of("%"));
			drtParam.zoomFactor=atof(zoom[0].c_str())/100;
			printf("zoomFactor=%f\n", drtParam.zoomFactor);
			paramcount++;
		}
		if((after(line, "zooming image to ", val)).size()>0) {
			vector<string> zoom;
			split(zoom, val, is_any_of("%"));
			drtParam.zoomFactor=atof(zoom[0].c_str())/100;
			printf("zoomFactor=%f\n", drtParam.zoomFactor);
			paramcount++;
		}
	}

	if(paramcount!=4) return 0;
	return 1;
}

int main(int argc, char **argv)
{
	FIF(sizeof(int)!=4);
	FIF((3>>1)!=1);
	FIF((-3>>1)!=-2);

	options_description optDesc(string(DEBUG_MSG)+string(__FILE__)+" @ "+string(__DATE__)+" "+string(__TIME__)+"\nAllowed options");
	optDesc.add_options()
	    ("help", "produce help message")
	    ("source", "print program source code")
	    ("if",  value<string>(), "input image file")
	    ("import", value<string>(), "import drtLog file")
	    ("drt", value<string>(), "drt image file (visualization of drt)")
	    ("tmpfile",  value<string>(), "temporary image file (format conversion only); will not be deleted")
	    ("of",  value<string>(), "output image file (after spherical correction)")
	    ("python", value<string>()->default_value("python"), "python command (requires PIL)")
	    ("doTrans", "does format conversion with transCmd, if applicable (sd<maxSd)")
	    ("noTransDeskew", "format conversion&statistics only")
	    ("maxSd", value<double>()->default_value(0.8), "max. sd(halfWavelength) allowing transformation")
	    ("sr",  value<double>()->default_value(1.0), "shapen radius [image height %]")
	    ("zoom",  value<string>()->default_value("33%"), "zoom +'%'")
	    ("angle",  value<double>()->default_value(5.0), "drt angle probe range [Grad]")
	    ("compareAngle",  value<double>()->default_value(1.0), "+/- angle variation to compare")
	    ("halfWavelengthVarianceAbsFactor",  value<double>()->default_value(50), "variance(halfWavelength(fft where abs(fft)>=max(abs(fft))*hWVAF))")

	    ("minPageHeight",  value<double>()->default_value(150.0), "page height minus border[mm]")
	    ("maxPageHeight",  value<double>()->default_value(600.0), "page height minus border[mm]")
	    ("minLineSpaceSize", value<double>()->default_value(10.0), "[pt] (1/72 inch = 0.35277mm)")
	    ("maxLineSpaceSize", value<double>()->default_value(16.0), "[pt] (1/72 inch = 0.35277mm)")
	    ("minLineSpaceRatio", value<double>()->default_value(0.7))
		("maxLineSpaceRatio", value<double>()->default_value(2.0))
		("minLineLike", value<double>()->default_value(26.664827), "%")
		("hysteresis", value<double>()->default_value(4.278268))
		("maxSdLineSpace", value<double>()->default_value(6.272757))
		("gamma", value<double>()->default_value(2.2), "sRGB gamma correction before(1/x)&after(x) transform")
	;

	variables_map cmdLine;
	store(parse_command_line(argc, argv, optDesc), cmdLine);
	notify(cmdLine);

	if (cmdLine.count("help")) {
	    cout << optDesc << "\n";
	    return 1;
	}
	if (cmdLine.count("source")) {
		cerr << SOURCECODE_TIMESTAMP << endl;
	    fwrite(SOURCECODE, 1, SOURCECODE_LENGTH, stdout);
	    return 1;
	}

	FIF(!cmdLine.count("if"));

	Image graphImg;
	printf("reading image..."); fflush(stdout);
	try {
		graphImg.read(cmdLine["if"].as<string>()); printf("done.\n");
	}
	catch( Magick::WarningCoder &warning ) {
		cerr << "Coder Warning: " << warning.what() << endl;
	}
	catch( Magick::Warning &warning ) {
		cerr << "Warning: " << warning.what() << endl;
	}
	catch( Magick::ErrorBlob &error) {
		cerr << "Error: " << error.what() << endl;
	}

	Image transImg=graphImg; // wird unten gebraucht

	graphImg.modifyImage();
	//SetImageVirtualPixelMethod( graphImg.image(), MagickCore::WhiteVirtualPixelMethod);
	//TODO: will in aktueller Version noch nicht
	// graphImg.options()->virtualPixelMethod( MagickCore::WhiteVirtualPixelMethod );

	graphImg.virtualPixelMethod(MagickCore::WhiteVirtualPixelMethod); // aber auch das will in Magick++ unter (ubuntu) karmic koala nicht

	drtParams drtParam;
	printf("size: %lux%lu\n", graphImg.size().width(), graphImg.size().height());
	drtParam.graphW2=double(graphImg.size().width())/2;
	drtParam.graphH2=double(graphImg.size().height())/2;
	drtParam.graphW=drtParam.graphW2*2;

	if(cmdLine.count("tmpfile")) {
		// ist ja nur ein tmpfile: graphImg.density(Geometry(300, 300)); // 300 dpi setzen
		graphImg.quality(100);
		printf("saving image..."); fflush(stdout);
		//graphImg.write(cmdLine["tmpfile"].as<string>());
		printf("done.\n");
	}

	if(!cmdLine.count("import")) {
		getDrtParamsFromImage(cmdLine, graphImg, drtParam);
	} else {
		if(!getDrtParamsFromDrtlog(cmdLine, graphImg, drtParam)) {
			fprintf(stderr, "WARNUNG: drtLog-(import)-Datei %s fehlerhaft. DRT-Analyse wird vom Bild durchgefuehrt.\n", cmdLine["import"].as<string>().c_str());
			getDrtParamsFromImage(cmdLine, graphImg, drtParam);
		}
	}

	double radT12avg=(drtParam.radT1+drtParam.radT2)/2;
	double radT1rel=drtParam.radT1-radT12avg;
	double radT2rel=drtParam.radT2-radT12avg;

	// 0 3
	// 1 2

	vector<complex<double> > transKoord(4);

	if(radT2rel<0) {
		transKoord[0]=complex<double>(-drtParam.graphW2, drtParam.graphH2);
		transKoord[3]=transKoord[0]+polar(drtParam.graphW, radT2rel);
	} else {
		transKoord[3]=complex<double>(drtParam.graphW2, drtParam.graphH2);
		transKoord[0]=transKoord[3]-polar(drtParam.graphW, radT2rel);
	}

	if(radT1rel>0) {
		transKoord[1]=complex<double>(-drtParam.graphW2, -drtParam.graphH2);
		transKoord[2]=transKoord[1]+polar(drtParam.graphW, radT1rel);
	} else {
		transKoord[2]=complex<double>(drtParam.graphW2, -drtParam.graphH2);
		transKoord[1]=transKoord[2]-polar(drtParam.graphW, radT1rel);
	}

	double distortParam[16];
	for(int i=0; i<4; i++) {
		int x, y;
		switch(i) {
			case 0: x=0; y=0; break;
			case 1: x=0; y=drtParam.graphH2*2-1; break;
			case 2: x=drtParam.graphW2*2-1; y=drtParam.graphH2*2-1; break;
			case 3: x=drtParam.graphW2*2-1; y=0; break;
		}
		double r=abs(transKoord[i]);
		double phi=std::arg(transKoord[i])+radT12avg;
		distortParam[i*4+0]=r*cos(phi)+drtParam.graphW2; //+pilXoff;
		distortParam[i*4+1]=(drtParam.graphH2*2-1)-(r*sin(phi)+drtParam.graphH2); //+pilYoff;
		distortParam[i*4+2]=x;
		distortParam[i*4+3]=y;

		printf("%d: %f,%f %f,%f\n", i, distortParam[i*4+0], distortParam[i*4+1], distortParam[i*4+2], distortParam[i*4+3]);
	}

	sort(drtParam.drtAxisFftOrderedByAbsDesc.begin(), drtParam.drtAxisFftOrderedByAbsDesc.end());
	reverse(drtParam.drtAxisFftOrderedByAbsDesc.begin(), drtParam.drtAxisFftOrderedByAbsDesc.end());
	printf("iDrtAxisFFT ordered by abs: ");
	double sumW=0;
	double sumL=0;
	double sumWW=0;
	int n=0;
	for(int i=0; i<drtParam.drtAxisFftLength-1
		&& drtParam.drtAxisFftOrderedByAbsDesc[i].abs>=
			drtParam.drtAxisFftOrderedByAbsDesc[0].abs
				*cmdLine["halfWavelengthVarianceAbsFactor"].as<double>()/100;
		i++) {
		sumW+=drtParam.drtAxisFftOrderedByAbsDesc[i].halfWavelength;
		sumWW+=pow(drtParam.drtAxisFftOrderedByAbsDesc[i].halfWavelength, 2);
		sumL+=drtParam.drtAxisFftOrderedByAbsDesc[i].lines;
		n++;
		printf(" (%.3lf:%.3lf)", drtParam.drtAxisFftOrderedByAbsDesc[i].abs,
				drtParam.drtAxisFftOrderedByAbsDesc[i].halfWavelength);
	}
	printf("\n");
	printf("sd=%lf halfWl=%lf lines=%lf\n", sd(sumW, sumWW, n), sumW/n, sumL/n);

	// neue Evaluierung der Drt-Achse:

	double minLines=cmdLine["minPageHeight"].as<double>() / (cmdLine["maxLineSpaceSize"].as<double>()*0.35277);
	double maxLines=cmdLine["maxPageHeight"].as<double>() / (cmdLine["minLineSpaceSize"].as<double>()*0.35277);
	int minLineSpace=int(drtParam.drtAxis.size()/maxLines);
	int maxLineSpace=int(drtParam.drtAxis.size()/minLines+0.999);
	printf("minLineSpace:%d\n", minLineSpace);
	printf("maxLineSpace:%d\n", maxLineSpace);
	double minRatio=cmdLine["minLineSpaceRatio"].as<double>();
	double maxRatio=cmdLine["maxLineSpaceRatio"].as<double>();

	double line=0;
	double space=0;
	double sumLine=0;
	double sumLine2=0;
	double sumSpace=0;
	double sumSpace2=0;
	double sumN=0;

	int state=0;
	double hyst=cmdLine["hysteresis"].as<double>();

	double insgesamt=0;
	double zeilenartig=0;

	if(drtParam.drtAxis[0]>0) { state=1; space++; } else { state=0; line++; }

	printf("iDrtAxis-LineSpacePattern:");
	for(unsigned i=1; i<drtParam.drtAxis.size(); i++) {
		if(state==0) {
			if(drtParam.drtAxis[i]>hyst) {
				printf("(l:%.0lf;s:%.0lf)s", line, space);
				if(space==0.0) { space+=0.000001; }
				double ratio=line/space;
				insgesamt+=line+space;
				if(line+space>=minLineSpace && line+space<=maxLineSpace)	{
					if(ratio>=minRatio/1.55 && ratio<=maxRatio*1.55) {
						sumLine+=line;
						sumLine2+=line*line;
						sumSpace+=space;
						sumSpace2+=space*space;
						sumN++;
					}
					if(ratio>=minRatio && ratio<=maxRatio) {
						zeilenartig+=line+space;
					}
				}
				line=0; space=1;
				state=1;
			} else { printf("l"); line++; } // hyst. n. überschr.
		} else { // status=1
			if(drtParam.drtAxis[i]<(-hyst)) {
				line++;
				printf("l");
				state=0;
			} else { printf("s"); space++; }
		}
	}
	printf("\n");
	double sdLine=sd(sumLine, sumLine2, sumN);
	double sdSpace=sd(sumSpace, sumSpace2, sumN);
	double sdLineSpace=sqrt(sdLine*sdLine+sdSpace*sdSpace);

	double proz_zeilenartig=double(zeilenartig)*100.0/(insgesamt+boost::numeric::bounds<double>::smallest()); // sicherheitshalber, falls weißes Blatt etc.
	printf("line-like: %.2f%%; sdLineSpace=%.3f\n", proz_zeilenartig, sdLineSpace);

	if(cmdLine.count("doTrans") && cmdLine.count("of")) {
		//if(sd(sumW, sumWW, n) < cmdLine["maxSd"].as<double>() && cmdLine.count("noTransDeskew")==0)
		// wird nun in doUnzip...sh erledigt: transImg.density(Geometry(300, 300)); // 300 dpi setzen
		if(proz_zeilenartig >= cmdLine["minLineLike"].as<double>()
				&& sdLineSpace <= cmdLine["maxSdLineSpace"].as<double>()
				&& cmdLine.count("noTransDeskew")==0) {
			transImg.compressType(NoCompression);
			transImg.depth(16);
			transImg.gamma(1/cmdLine["gamma"].as<double>());
			transImg.distort(MagickCore::PerspectiveDistortion,  16, distortParam);
			transImg.gamma(cmdLine["gamma"].as<double>());
			transImg.depth(8);
		}
		try {
			transImg.write(cmdLine["of"].as<string>());
		}
		catch( Magick::WarningCoder &warning ) {
			cerr << "Coder Warning: " << warning.what() << endl;
		}
		catch( Magick::Warning &warning ) {
			cerr << "Warning: " << warning.what() << endl;
		}
		catch( Magick::ErrorBlob &error) {
			cerr << "Error: " << error.what() << endl;
		}
	}
}


/**
 * Copyright (c) 2011-2017 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/bitcoin/formats/base_58.hpp>

#include <boost/algorithm/string.hpp>
#include <bitcoin/bitcoin/utility/assert.hpp>

namespace libbitcoin {

const std::string base58_chars =
    "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";

bool is_base58(const char ch)
{
    // This works because the base58 characters happen to be in sorted order
    return std::binary_search(base58_chars.begin(), base58_chars.end(), ch);
}

bool is_base58(const std::string& text)
{
    const auto test = [](const char ch)
    {
        return is_base58(ch);
    };

    return std::all_of(text.begin(), text.end(), test);
}

template <typename Data>
auto search_first_nonzero(const Data& data) -> decltype(data.cbegin())
{
    auto first_nonzero = data.cbegin();
    while (first_nonzero != data.end() && *first_nonzero == 0)
        ++first_nonzero;

    return first_nonzero;
}

size_t count_leading_zeros(data_slice unencoded)
{
    // Skip and count leading '1's.
    size_t leading_zeros = 0;
    for (const uint8_t byte: unencoded)
    {
        if (byte != 0)
            break;

        ++leading_zeros;
    }

    return leading_zeros;
}

void pack_value(data_chunk& indexes, size_t carry)
{
    // Apply "b58 = b58 * 256 + ch".
    for (auto it = indexes.rbegin(); it != indexes.rend(); ++it)
    {
        carry += 256 * (*it);
        *it = carry % 58;
        carry /= 58;
    }

    BITCOIN_ASSERT(carry == 0);
}

std::string encode_base58(data_slice unencoded)
{
    size_t leading_zeros = count_leading_zeros(unencoded);

    // size = log(256) / log(58), rounded up.
    const size_t number_nonzero = unencoded.size() - leading_zeros;
    const size_t indexes_size = number_nonzero * 138 / 100 + 1;

    // Allocate enough space in big-endian base58 representation.
    data_chunk indexes(indexes_size);

    // Process the bytes.
    for (auto it = unencoded.begin() + leading_zeros;
        it != unencoded.end(); ++it)
    {
        pack_value(indexes, *it);
    }

    // Skip leading zeroes in base58 result.
    auto first_nonzero = search_first_nonzero(indexes);

    // Translate the result into a string.
    std::string encoded;
    const size_t estimated_size = leading_zeros +
        (indexes.end() - first_nonzero);
    encoded.reserve(estimated_size);
    encoded.assign(leading_zeros, '1');

    // Set actual main bytes.
    for (auto it = first_nonzero; it != indexes.end(); ++it)
    {
        const size_t index = *it;
        encoded += base58_chars[index];
    }

    return encoded;
}

size_t count_leading_zeros(const std::string& encoded)
{
    // Skip and count leading '1's.
    size_t leading_zeros = 0;
    for (const uint8_t digit: encoded)
    {
        if (digit != base58_chars[0])
            break;

        ++leading_zeros;
    }

    return leading_zeros;
}

void unpack_char(data_chunk& data, size_t carry)
{
    for (auto it = data.rbegin(); it != data.rend(); it++)
    {
        carry += 58 * (*it);
        *it = carry % 256;
        carry /= 256;
    }

    BITCOIN_ASSERT(carry == 0);
}

bool decode_base58(data_chunk& out, const std::string& in)
{
    // Trim spaces and newlines around the string.
    const auto leading_zeros = count_leading_zeros(in);

    // log(58) / log(256), rounded up.
    const size_t data_size = in.size() * 733 / 1000 + 1;

    // Allocate enough space in big-endian base256 representation.
    data_chunk data(data_size);

    // Process the characters.
    for (auto it = in.begin() + leading_zeros; it != in.end(); ++it)
    {
        const auto carry = base58_chars.find(*it);
        if (carry == std::string::npos)
            return false;

        unpack_char(data, carry);
    }

    // Skip leading zeroes in data.
    auto first_nonzero = search_first_nonzero(data);

    // Copy result into output vector.
    data_chunk decoded;
    const size_t estimated_size = leading_zeros + (data.end() - first_nonzero);
    decoded.reserve(estimated_size);
    decoded.assign(leading_zeros, 0x00);
    decoded.insert(decoded.end(), first_nonzero, data.cend());

    out = decoded;
    return true;
}

// For support of template implementation only, do not call directly.
bool decode_base58_private(uint8_t* out, size_t out_size, const char* in)
{
    data_chunk buffer;
    if (!decode_base58(buffer, in) || buffer.size() != out_size)
        return false;

    for (size_t i = 0; i < out_size; ++i)
        out[i] = buffer[i];

    return true;
}

} // namespace libbitcoin


#pragma once

#pragma once

#include "openMVG/matching/svg_matches.hpp"

#include "colocParams.hpp"
#include "colocData.hpp"

#include <cstdlib>
#include <iostream>
#include <memory>
#include <string>
#include <dlib/optimization.h>
#include <algorithm>
#include <functional>

using namespace openMVG;
using namespace openMVG::features;
using namespace openMVG::matching;
using namespace openMVG::sfm;

using namespace dlib;

namespace coloc
{
	class Utils
	{
	public:
		double computeScaleDifference(Scene &scene1, std::vector <IndexT> &idx1, Scene &scene2, std::vector <IndexT> &idx2, std::vector<IndMatch> commonFeatures);
		bool matchSceneWithMap(Scene& scene);
		bool rescaleMap(Scene& scene, double scale);
		int drawFeaturePoints(std::string& imageName, features::PointFeatures points);
		bool drawFeatures(std::string& image, std::pair <int, int>& imageSize, features::SIOPointFeatures& features, std::string& outputFilename, const double feature_circle_radius,
			const double stroke_size);
		bool drawMatches(std::pair <int, int>& imageSize, std::string& outputFilename, std::string& image1, std::string& image2, Regions& regions1, Regions& regions2, std::vector <IndMatch>& matches);

		void loadPoseCovariance(Cov6& cov, matrix<double, 3, 3>& covNew)
		{
			covNew = cov[21], cov[22], cov[23],
				cov[27], cov[28], cov[29],
				cov[33], cov[34], cov[35];
		}

		void readPoseCovariance(matrix<double, 3, 3>& covNew, Cov6& cov)
		{
			cov[21] = covNew(0, 0);
			cov[22] = covNew(0, 1);
			cov[23] = covNew(0, 2);
			cov[27] = covNew(1, 0);
			cov[28] = covNew(1, 1);
			cov[29] = covNew(1, 2);
			cov[33] = covNew(2, 0);
			cov[34] = covNew(2, 1);
			cov[35] = covNew(2, 2);
		}

		static Pair_Set handlePairs(const uint8_t numImages)
		{
			return exhaustivePairs(numImages);
		}

		static cv::Mat rot2euler(const cv::Mat & rotationMatrix)
		{
			cv::Mat euler(3, 1, CV_64F);

			double m00 = rotationMatrix.at<double>(0, 0);
			double m02 = rotationMatrix.at<double>(0, 2);
			double m10 = rotationMatrix.at<double>(1, 0);
			double m11 = rotationMatrix.at<double>(1, 1);
			double m12 = rotationMatrix.at<double>(1, 2);
			double m20 = rotationMatrix.at<double>(2, 0);
			double m22 = rotationMatrix.at<double>(2, 2);

			double bank, attitude, heading;

			// Assuming the angles are in radians.
			if (m10 > 0.998) { // singularity at north pole
				bank = 0;
				attitude = CV_PI / 2;
				heading = atan2(m02, m22);
			}
			else if (m10 < -0.998) { // singularity at south pole
				bank = 0;
				attitude = -CV_PI / 2;
				heading = atan2(m02, m22);
			}
			else
			{
				bank = atan2(-m12, m11);
				attitude = asin(m10);
				heading = atan2(-m20, m00);
			}

			euler.at<double>(0) = bank;
			euler.at<double>(1) = attitude;
			euler.at<double>(2) = heading;

			return euler;
		}

		static cv::Mat euler2rot(const cv::Mat & euler)
		{
			cv::Mat rotationMatrix(3, 3, CV_64F);

			double bank = euler.at<double>(0);
			double attitude = euler.at<double>(1);
			double heading = euler.at<double>(2);

			// Assuming the angles are in radians.
			double ch = cos(heading);
			double sh = sin(heading);
			double ca = cos(attitude);
			double sa = sin(attitude);
			double cb = cos(bank);
			double sb = sin(bank);

			double m00, m01, m02, m10, m11, m12, m20, m21, m22;

			m00 = ch * ca;
			m01 = sh * sb - ch * sa*cb;
			m02 = ch * sa*sb + sh * cb;
			m10 = sa;
			m11 = ca * cb;
			m12 = -ca * sb;
			m20 = -sh * ca;
			m21 = sh * sa*cb + ch * sb;
			m22 = -sh * sa*sb + ch * cb;

			rotationMatrix.at<double>(0, 0) = m00;
			rotationMatrix.at<double>(0, 1) = m01;
			rotationMatrix.at<double>(0, 2) = m02;
			rotationMatrix.at<double>(1, 0) = m10;
			rotationMatrix.at<double>(1, 1) = m11;
			rotationMatrix.at<double>(1, 2) = m12;
			rotationMatrix.at<double>(2, 0) = m20;
			rotationMatrix.at<double>(2, 1) = m21;
			rotationMatrix.at<double>(2, 2) = m22;

			return rotationMatrix;
		}
	private:

		std::shared_ptr<Regions_Provider> mapFeatures;
		EMatcherType matchingType;
	};

	bool Utils::drawMatches(std::pair <int, int>& imageSize, std::string& outputFilename, std::string& image1, std::string& image2, Regions& regions1, Regions& regions2, std::vector <IndMatch>& matches)
	{
		Matches2SVG(
			image1, { imageSize.first, imageSize.second }, regions1.GetRegionsPositions(),
			image2, { imageSize.first, imageSize.second }, regions2.GetRegionsPositions(),
			matches, outputFilename, true);
		return EXIT_SUCCESS;
	}

	bool Utils::drawFeatures(std::string& image, std::pair <int, int>& imageSize, features::SIOPointFeatures& features, std::string& outputFilename, const double feature_circle_radius = 3.0,
		const double stroke_size = 3.0)
	{
		svg::svgDrawer svgStream(imageSize.first, imageSize.second);

		// Draw image
		svgStream.drawImage(image, imageSize.first, imageSize.second);

		// Draw features
		for (const features::PointFeature & feat_it : features) {
			// Draw the feature (circle)
			svgStream.drawCircle(
				feat_it.x(), feat_it.y(), feature_circle_radius,
				svg::svgStyle().stroke("green", stroke_size));
		}

		// Save the SVG file
		std::ofstream svgFile(outputFilename.c_str());
		if (svgFile.is_open())
		{
			svgFile << svgStream.closeSvgFile().str();
			svgFile.close();
			return true;
		}
		return false;
	}

	double Utils::computeScaleDifference(Scene &scene1, std::vector <IndexT> &idx1, Scene &scene2, std::vector <IndexT> &idx2, std::vector<IndMatch> commonFeatures)
	{
		if (commonFeatures.empty()) {
			std::cout << "No common features between the maps." << std::endl;
			return 1.0;
		}

		std::cout << "Number of common features between the maps: " << commonFeatures.size() << std::endl;

		double scale = 0.0, scaleDiff = 1.0;
		for (size_t i = 0; i < commonFeatures.size() - 1; ++i) {
			Vec3 X11 = scene1.GetLandmarks().at(idx1[commonFeatures[i].i_]).X;
			Vec3 X12 = scene1.GetLandmarks().at(idx1[commonFeatures[i + 1].i_]).X;

			Vec3 X21 = scene2.GetLandmarks().at(idx2[commonFeatures[i].j_]).X;
			Vec3 X22 = scene2.GetLandmarks().at(idx2[commonFeatures[i + 1].j_]).X;

			float dist1 = (X12 - X11).norm();
			float dist2 = (X22 - X21).norm();

			scale += dist1 / dist2;

			//std::cout << "Distance ratio: " << dist1/dist2 << std::endl;
		}

		scaleDiff = scale / (commonFeatures.size() - 1);
		return scaleDiff;
	}

	bool Utils::rescaleMap(Scene& scene, double scale)
	{
		for (const auto & landmark : scene.GetLandmarks()) {
			scene.structure.at(landmark.first).X = scene.structure.at(landmark.first).X * scale;
		}

		for (unsigned int i = 0; i < scene.poses.size(); ++i) {
			scene.poses[i] = Pose3(scene.poses.at(i).rotation(), scene.poses.at(i).center() * scale);
		}
		return EXIT_SUCCESS;
	}

}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file sequence_test.cpp
/// \brief Add your file description here.

#include <numeric>
#include <vector>
#include <boost/test/minimal.hpp>
#include "mcrl2/utilities/sequence.h"

using namespace mcrl2;

struct f
{
  std::vector<int>& v_;
  int& sum_;

  f(std::vector<int>& v, int& sum)
    : v_(v), sum_(sum)
  {}

  // Adds the sum of the elements of v_ to sum_.
  void operator()() const
  {
    for (std::vector<int>::const_iterator i = v_.begin(); i != v_.end(); ++i)
    {
      std::cout << *i << " ";
    }
    std::cout << std::endl;
    sum_ += std::accumulate(v_.begin(), v_.end(), 0);
  }
};

void test_sequence()
{
  std::vector<int> a;
  std::vector<int> b;
  std::vector<int> c;

  a.push_back(1);
  a.push_back(2);
  b.push_back(3);
  c.push_back(2);
  c.push_back(5);

  std::vector<std::vector<int> > v;
  v.push_back(a);
  v.push_back(b);
  v.push_back(c);

  int sum = 0;

  // w will hold a sequence
  std::vector<int> w(v.size());

  utilities::foreach_sequence(v, w.begin(), f(w, sum));
  std::cout << "sum == " << sum << std::endl;
  BOOST_CHECK(sum == 32);
}

int test_main(int, char*[])
{
  test_sequence();

  return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2010 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "sparse.h"
#include <Eigen/SparseExtra>

#ifdef EIGEN_UMFPACK_SUPPORT
#include <Eigen/UmfPackSupport>
#endif

#ifdef EIGEN_SUPERLU_SUPPORT
#include <Eigen/SuperLUSupport>
#endif

template<typename Scalar> void sparse_lu(int rows, int cols)
{
  double density = (std::max)(8./(rows*cols), 0.01);
  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
  typedef Matrix<Scalar,Dynamic,1> DenseVector;

  DenseVector vec1 = DenseVector::Random(rows);

  std::vector<Vector2i> zeroCoords;
  std::vector<Vector2i> nonzeroCoords;

    SparseMatrix<Scalar> m2(rows, cols);
    DenseMatrix refMat2(rows, cols);

    DenseVector b = DenseVector::Random(cols);
    DenseVector refX(cols), x(cols);

    initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag, &zeroCoords, &nonzeroCoords);

    FullPivLU<DenseMatrix> refLu(refMat2);
    refX = refLu.solve(b);
    #if defined(EIGEN_SUPERLU_SUPPORT) || defined(EIGEN_UMFPACK_SUPPORT)
    Scalar refDet = refLu.determinant();
    #endif
    x.setZero();
    // // SparseLU<SparseMatrix<Scalar> > (m2).solve(b,&x);
    // // VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: default");

    #ifdef EIGEN_UMFPACK_SUPPORT
    {
      // check solve
      x.setZero();
      SparseLU<SparseMatrix<Scalar>,UmfPack> lu(m2);
      VERIFY(lu.succeeded() && "umfpack LU decomposition failed");
      VERIFY(lu.solve(b,&x) && "umfpack LU solving failed");
      VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: umfpack");
      VERIFY_IS_APPROX(refDet,lu.determinant());
        // TODO check the extracted data
        //std::cerr << slu.matrixL() << "\n";
    }
    #endif

    #ifdef EIGEN_SUPERLU_SUPPORT
    {
      x.setZero();
      SparseLU<SparseMatrix<Scalar>,SuperLU> slu(m2);
      if (slu.succeeded())
      {
        if (slu.solve(b,&x)) {
          VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LU: SuperLU");
        }
        // std::cerr << refDet << " == " << slu.determinant() << "\n";
        if (slu.solve(b, &x, SvTranspose)) {
          VERIFY(b.isApprox(m2.transpose() * x, test_precision<Scalar>()));
        }

        if (slu.solve(b, &x, SvAdjoint)) {
         VERIFY(b.isApprox(m2.adjoint() * x, test_precision<Scalar>()));
        }

        if (!NumTraits<Scalar>::IsComplex) {
          VERIFY_IS_APPROX(refDet,slu.determinant()); // FIXME det is not very stable for complex
        }
      }
    }
    #endif

}

void test_sparse_lu()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(sparse_lu<double>(8, 8) );
    int s = internal::random<int>(1,300);
    CALL_SUBTEST_2(sparse_lu<std::complex<double> >(s,s) );
    CALL_SUBTEST_1(sparse_lu<double>(s,s) );
  }
}


/**
 * Copyright (c) 2018, University Osnabrück
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University Osnabrück nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * EigenSVDPointAlign.hpp
 *
 *  @date Feb 21, 2014
 *  @author Thomas Wiemann
 */
#ifndef EIGENSVDPOINTALIGN_HPP_
#define EIGENSVDPOINTALIGN_HPP_

#include "SLAMScanWrapper.hpp"
#include "lvr2/types/MatrixTypes.hpp"

#include <Eigen/Dense>

namespace lvr2
{

template<typename T, typename PointT = float>
class EigenSVDPointAlign
{
public:
    using Vec3 = Vector3<T>;
    using Mat4 = Transform<T>;
    using Mat3 = Eigen::Matrix<T, 3, 3>;
    using Point3 = Vector3<PointT>;
    using PointPairVector = std::vector<std::pair<Point3, Point3>>;

    EigenSVDPointAlign() {};

    /**
     * @brief Calculates the estimated Transformation to match a Data Pointcloud to a Model
     *        Pointcloud
     *
     * Apply the resulting Transform to the Data Pointcloud.
     *
     * @param scan       The Data Pointcloud
     * @param neighbors  An array containing a Pointer to a neighbor in the Model Pointcloud for
     *                   each Point in `scan`, or nullptr if there is no neighbor for a Point
     * @param centroid_m The center of the Model Pointcloud
     * @param centroid_d The center of the Data Pointcloud
     * @param align      Will be set to the Transformation
     *
     * @return The average Point-to-Point error of the Scans
     */
    T alignPoints(
        SLAMScanPtr scan,
        Point3** neighbors,
        const Vec3& centroid_m,
        const Vec3& centroid_d,
        Mat4& align) const;

    /**
     * @brief Calculates the estimated Transformation to match a Data Pointcloud to a Model
     *        Pointcloud
     *
     * Apply the resulting Transform to the Data Pointcloud.
     *
     * @param points     A vector of pairs with (model, data) Points
     * @param centroid_m The center of the Model Pointcloud
     * @param centroid_d The center of the Data Pointcloud
     * @param align      Will be set to the Transformation
     *
     * @return The average Point-to-Point error of the Scans
     */
    T alignPoints(
        PointPairVector& points,
        const Vec3& centroid_m,
        const Vec3& centroid_d,
        Mat4& align) const;
};

} /* namespace lvr2 */

#include "EigenSVDPointAlign.tcc"

#endif /* EIGENSVDPOINTALIGN_HPP_ */


/**
 * Copyright (c) 2013 Jonan Cruz-Martin
 *
 * Distributed under the terms of the MIT license, see the accompanying
 * file COPYING or http://opensource.org/licenses/MIT.
 *
 * @file
 * @brief Implement calculation of the surface of a cell.
 */

#ifndef ALGORHITHMS_CELL_SURFACE_HPP
#define ALGORHITHMS_CELL_SURFACE_HPP

#include <boost/python.hpp>
using namespace boost::python;

#include "../points/cartesian.hpp"
#include "../points/cylindrical.hpp"
#include "../points/polar.hpp"
#include "../points/spherical.hpp"

#include "../cells/linear.hpp"
#include "../cells/triangular.hpp"
#include "../cells/quadrilateral.hpp"
#include "../cells/tetrahedral.hpp"

////////////
// Linear //
////////////

/**
 * Calculate the surface of a linear cartesian 1D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCartesian1D_Cell_surface(LinearCartesian1D_Cell cell);

/**
 * Calculate the surface of a linear cartesian 2D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCartesian2D_Cell_surface(LinearCartesian2D_Cell cell);

/**
 * Calculate the surface of a linear cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCartesian3D_Cell_surface(LinearCartesian3D_Cell cell);

/**
 * Calculate the surface of a linear cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCylindrical3D_Cell_surface(LinearCylindrical3D_Cell cell);

/**
 * Calculate the surface of a linear polar cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearPolar2D_Cell_surface(LinearPolar2D_Cell cell);

/**
 * Calculate the surface of a linear spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearSpherical3D_Cell_surface(LinearSpherical3D_Cell cell);

////////////////
// Triangular //
////////////////

/**
 * Calculate the surface of a triangular cartesian 2D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularCartesian2D_Cell_surface(TriangularCartesian2D_Cell cell);

/**
 * Calculate the surface of a triangular cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularCartesian3D_Cell_surface(TriangularCartesian3D_Cell cell);

/**
 * Calculate the surface of a triangular cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularCylindrical3D_Cell_surface(TriangularCylindrical3D_Cell cell);

/**
 * Calculate the surface of a triangular polar cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularPolar2D_Cell_surface(TriangularPolar2D_Cell cell);

/**
 * Calculate the surface of a triangular spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularSpherical3D_Cell_surface(TriangularSpherical3D_Cell cell);

///////////////////
// Quadrilateral //
///////////////////

/**
 * Calculate the surface of a quadrilateral cartesian 2D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralCartesian2D_Cell_surface(QuadrilateralCartesian2D_Cell cell);

/**
 * Calculate the surface of a quadrilateral cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralCartesian3D_Cell_surface(QuadrilateralCartesian3D_Cell cell);

/**
 * Calculate the surface of a quadrilateral cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralCylindrical3D_Cell_surface(QuadrilateralCylindrical3D_Cell cell);

/**
 * Calculate the surface of a quadrilateral polar cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralPolar2D_Cell_surface(QuadrilateralPolar2D_Cell cell);

/**
 * Calculate the surface of a quadrilateral spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralSpherical3D_Cell_surface(QuadrilateralSpherical3D_Cell cell);

/////////////////
// Tetrahedral //
/////////////////

/**
 * Calculate the surface of a tetrahedral cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TetrahedralCartesian3D_Cell_surface(TetrahedralCartesian3D_Cell cell);

/**
 * Calculate the surface of a tetrahedral cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TetrahedralCylindrical3D_Cell_surface(TetrahedralCylindrical3D_Cell cell);

/**
 * Calculate the surface of a tetrahedral spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TetrahedralSpherical3D_Cell_surface(TetrahedralSpherical3D_Cell cell);

#endif /* end of include guard: ALGORHITHMS_CELL_SURFACE_HPP */


#include <shared_mutex>
#include <dlib/dnn.h>
#include <dlib/image_loader/image_loader.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/graph_utils.h>
#include "facerec.h"

using namespace dlib;

template <template <int, template <typename> class, int, typename> class block, int N, template <typename> class BN, typename SUBNET>
using residual = add_prev1<block<N, BN, 1, tag1<SUBNET>>>;

template <template <int, template <typename> class, int, typename> class block, int N, template <typename> class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2, 2, 2, 2, skip1<tag2<block<N, BN, 2, tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block = BN<con<N, 3, 3, 1, 1, relu<BN<con<N, 3, 3, stride, stride, SUBNET>>>>>;

template <int N, typename SUBNET>
using ares = relu<residual<block, N, affine, SUBNET>>;
template <int N, typename SUBNET>
using ares_down = relu<residual_down<block, N, affine, SUBNET>>;

template <typename SUBNET>
using alevel0 = ares_down<256, SUBNET>;
template <typename SUBNET>
using alevel1 = ares<256, ares<256, ares_down<256, SUBNET>>>;
template <typename SUBNET>
using alevel2 = ares<128, ares<128, ares_down<128, SUBNET>>>;
template <typename SUBNET>
using alevel3 = ares<64, ares<64, ares<64, ares_down<64, SUBNET>>>>;
template <typename SUBNET>
using alevel4 = ares<32, ares<32, ares<32, SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<128, avg_pool_everything<
																									alevel0<
																											alevel1<
																													alevel2<
																															alevel3<
																																	alevel4<
																																			max_pool<3, 3, 2, 2, relu<affine<con<32, 7, 7, 2, 2, input_rgb_image_sized<150>>>>>>>>>>>>>;

typedef matrix<float, 0, 1> descriptor;

static const size_t RECT_LEN = 4;
static const size_t DESCR_LEN = 128;
static const size_t RECT_SIZE = RECT_LEN * sizeof(long);
static const size_t DESCR_SIZE = DESCR_LEN * sizeof(float);

class FaceRec
{
public:
	FaceRec(const char *model_dir)
	{
		detector_ = get_frontal_face_detector();

		std::string dir = model_dir;
		std::string shape_predictor_path = dir + "/shape_predictor_5_face_landmarks.dat";
		std::string resnet_path = dir + "/dlib_face_recognition_resnet_model_v1.dat";

		deserialize(shape_predictor_path) >> sp_;
		deserialize(resnet_path) >> net_;
	}

	std::vector<rectangle>
	Detect(const matrix<rgb_pixel> &img)
	{
		std::lock_guard<std::mutex> lock(detector_mutex_);
		return detector_(img);
	}

	std::vector<descriptor>
	GetDescriptor(const matrix<rgb_pixel> &img, std::vector<rectangle> rects)
	{
		std::vector<descriptor> descrs;
		std::vector<matrix<rgb_pixel>> face_imgs;

		for (const auto &rect : rects)
		{
			auto shape = sp_(img, rect);
			matrix<rgb_pixel> face_chip;
			extract_image_chip(img, get_face_chip_details(shape, 150, 0.25), face_chip);
			face_imgs.push_back(std::move(face_chip));
		}

		{
			std::lock_guard<std::mutex> lock(net_mutex_);
			descrs = net_(face_imgs);
		}
		return descrs;
	}

	// TODO(Kagami): Jittering?
	std::pair<std::vector<rectangle>, std::vector<descriptor>>
	Recognize(const matrix<rgb_pixel> &img, int max_faces)
	{
		std::vector<rectangle> rects;
		std::vector<descriptor> descrs;

		{
			std::lock_guard<std::mutex> lock(detector_mutex_);
			rects = detector_(img);
		}

		// Short circuit.
		if (rects.size() == 0 || (max_faces > 0 && rects.size() > (size_t)max_faces))
		{
			return {std::move(rects), std::move(descrs)};
		}
		std::sort(rects.begin(), rects.end());

		std::vector<matrix<rgb_pixel>> face_imgs;
		for (const auto &rect : rects)
		{
			auto shape = sp_(img, rect);
			matrix<rgb_pixel> face_chip;
			extract_image_chip(img, get_face_chip_details(shape, 150, 0.25), face_chip);
			face_imgs.push_back(std::move(face_chip));
		}

		{
			std::lock_guard<std::mutex> lock(net_mutex_);
			descrs = net_(face_imgs);
		}

		return {std::move(rects), std::move(descrs)};
	}

	// int Classify(const descriptor& test_sample) {
	// 	std::shared_lock<std::shared_mutex> lock(samples_mutex_);
	// 	if (samples_.size() == 0)
	// 		return -1;
	// 	return classify(samples_, cats_, test_sample);
	// }
private:
	std::mutex detector_mutex_;
	std::mutex net_mutex_;
	// std::shared_mutex samples_mutex_;
	frontal_face_detector detector_;
	shape_predictor sp_;
	anet_type net_;
	// std::vector<descriptor> samples_;
	// std::unordered_map<int, int> cats_;
};

// Plain C interface for Go.

facerec *facerec_init(const char *model_dir)
{
	facerec *rec = (facerec *)calloc(1, sizeof(facerec));
	try
	{
		FaceRec *cls = new FaceRec(model_dir);
		rec->cls = (void *)cls;
	}
	catch (serialization_error &e)
	{
		rec->err_str = strdup(e.what());
		rec->err_code = SERIALIZATION_ERROR;
	}
	catch (std::exception &e)
	{
		rec->err_str = strdup(e.what());
		rec->err_code = UNKNOWN_ERROR;
	}
	return rec;
}

faceret *facerec_recognize(facerec *rec, const uint8_t *img_data, int width, int height, int max_faces)
{
	faceret *ret = (faceret *)calloc(1, sizeof(faceret));
	FaceRec *cls = (FaceRec *)(rec->cls);
	std::vector<rectangle> rects;
	std::vector<descriptor> descrs;

	matrix<rgb_pixel> img(height, width);
	for (int y = 0; y < height; y++)
	{
		for (int x = 0; x < width; x++)
		{
			int offset = 3 * ((y * width) + x);
			img(y, x) = rgb_pixel(img_data[offset + 0], img_data[offset + 1], img_data[offset + 2]);
		}
	}
	try
	{
		std::tie(rects, descrs) = cls->Recognize(img, max_faces);
	}
	catch (std::exception &e)
	{
		ret->err_str = strdup(e.what());
		ret->err_code = UNKNOWN_ERROR;
		return ret;
	}
	ret->num_faces = descrs.size();
	if (ret->num_faces == 0)
		return ret;
	ret->rectangles = (long *)malloc(ret->num_faces * RECT_SIZE);
	for (int i = 0; i < ret->num_faces; i++)
	{
		long *dst = ret->rectangles + i * 4;
		dst[0] = rects[i].left();
		dst[1] = rects[i].top();
		dst[2] = rects[i].right();
		dst[3] = rects[i].bottom();
	}
	ret->descriptors = (float *)malloc(ret->num_faces * DESCR_SIZE);
	for (int i = 0; i < ret->num_faces; i++)
	{
		void *dst = (uint8_t *)(ret->descriptors) + i * DESCR_SIZE;
		void *src = (void *)&descrs[i](0, 0);
		memcpy(dst, src, DESCR_SIZE);
	}
	return ret;
}

void facerec_free(facerec *rec)
{
	if (rec)
	{
		if (rec->cls)
		{
			FaceRec *cls = (FaceRec *)(rec->cls);
			delete cls;
			rec->cls = NULL;
		}
		free(rec);
	}
}

double descriptor_distance(const float *a, const float *b)
{
	descriptor desc_a = mat(a, DESCR_LEN, 1);
	descriptor desc_b = mat(b, DESCR_LEN, 1);
	auto dist_func = dlib::squared_euclidean_distance();
	return dist_func(desc_a, desc_b);
}


//
// Visualization of Signed Distance Fields
//

//
// https://www.shadertoy.com/view/XsB3Rm
//

#include <Eigen/Dense>

#include <GL/freeglut.h>

#include <cuda_gl_interop.h>
#include <cuda_profiler_api.h>
#include <cuda_runtime.h>
#include <driver_functions.h>
#include <vector_functions.h>
#include <vector_types.h>

#include <cstdio>
#include <cstdlib>

#include <helper_cuda.h>
#include <helper_math.h>

#include <cuda_gl_interop.h>
#include <helper_gl.h>

using Vec3f = Eigen::Vector3f;
template <typename Derived>
using EigStdVector = std::vector<Derived, Eigen::aligned_allocator<Derived>>;

void render_kernel(dim3 gridSize,
                   dim3 blockSize,
                   uint *const d_output,
                   const uint imageW,
                   const uint imageH,
                   const Vec3f *const centers,
                   const float *const radii,
                   const int N,
                   const float normalization,
                   const float t);

int iDivUp(int a, int b) {
  return (a % b != 0) ? (a / b + 1) : (a / b);
}

struct State {
  float2 view_center = make_float2(0.0, 0.0);
  float2 prev_view_center = make_float2(0.0, 0.0);
  float2 prev_mouse_pos = make_float2(0.0, 0.0);

  // OpenGL pixel buffer object
  GLuint pbo = 0;

  // OpenGL texture object
  GLuint tex = 0;

  // CUDA Graphics Resource (to transfer PBO)
  struct cudaGraphicsResource *cuda_pbo_resource;

  // Image render dimensions
  uint width = 512;
  uint height = 512;

  float scale = 1.0;

  dim3 blockSize = dim3(16, 16);
  dim3 gridSize = dim3(iDivUp(width, blockSize.x), iDivUp(height, blockSize.y));

  //
  // Rendering paramters
  //
  float time_step = 0.0;
  float direction = 1.0;
  const float time_spacing = 0.005;
  const float min_time = -3.13;

  float normalization = 0.1;

  EigStdVector<Vec3f> centers;
  EigStdVector<float> radii;

  Vec3f *d_centers;
  float *d_radii;

} gstate;

void initGL(int *argc, char **argv) {
  // initialize GLUT callback functions
  glutInit(argc, argv);
  glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
  glutInitWindowSize(gstate.width, gstate.height);
  glutCreateWindow("CUDA volume rendering");

  if (!isGLVersionSupported(2, 0) ||
      !areGLExtensionsSupported("GL_ARB_pixel_buffer_object")) {
    printf("Required OpenGL extensions are missing.");
    exit(EXIT_SUCCESS);
  }
}

class ManagedTexture {
  ManagedTexture() {

  }

};

void initPixelBuffer() {
  if (gstate.pbo) {
    // unregister this buffer object from CUDA C
    checkCudaErrors(cudaGraphicsUnregisterResource(gstate.cuda_pbo_resource));

    // delete old buffer
    glDeleteBuffers(1, &(gstate.pbo));
    glDeleteTextures(1, &(gstate.tex));
  }

  // create pixel buffer object for display
  glGenBuffers(1, &(gstate.pbo));
  glBindBuffer(GL_PIXEL_UNPACK_BUFFER_ARB, gstate.pbo);
  glBufferData(GL_PIXEL_UNPACK_BUFFER_ARB,
               gstate.width * gstate.height * sizeof(GLubyte) * 4, 0,
               GL_STREAM_DRAW_ARB);
  glBindBuffer(GL_PIXEL_UNPACK_BUFFER_ARB, 0);

  // register this buffer object with CUDA
  checkCudaErrors(
      cudaGraphicsGLRegisterBuffer(&(gstate.cuda_pbo_resource), gstate.pbo,
                                   cudaGraphicsMapFlagsWriteDiscard));

  // create texture for display
  glGenTextures(1, &(gstate.tex));
  glBindTexture(GL_TEXTURE_2D, gstate.tex);
  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, gstate.width, gstate.height, 0,
               GL_RGBA, GL_UNSIGNED_BYTE, NULL);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glBindTexture(GL_TEXTURE_2D, 0);
}

void send_parameters(const EigStdVector<Vec3f> &centers,
                     const EigStdVector<float> &radii) {
  const int n = radii.size();

  checkCudaErrors(cudaMalloc((void **)&(gstate.d_centers), sizeof(Vec3f) * n));
  checkCudaErrors(cudaMalloc((void **)&(gstate.d_radii), sizeof(float) * n));

  checkCudaErrors(cudaMemcpy(gstate.d_centers, centers.data(),
                             sizeof(Vec3f) * n, cudaMemcpyHostToDevice));
  checkCudaErrors(cudaMemcpy(gstate.d_radii, radii.data(), sizeof(float) * n,
                             cudaMemcpyHostToDevice));
}

void render() {
  uint *d_output;
  checkCudaErrors(cudaGraphicsMapResources(1, &(gstate.cuda_pbo_resource), 0));

  size_t num_bytes;
  checkCudaErrors(cudaGraphicsResourceGetMappedPointer(
      (void **)&d_output, &num_bytes, gstate.cuda_pbo_resource));

  checkCudaErrors(cudaMemset(d_output, 0, gstate.width * gstate.height * 4));

  // Create performance metrics
  cudaEvent_t start, stop;
  cudaEventCreate(&start);
  cudaEventCreate(&stop);

  cudaEventRecord(start);

  if (gstate.time_step >= 1.0) {
    gstate.direction = -1.0;
  } else if (gstate.time_step <= gstate.min_time) {
    gstate.direction = 1.0;
  }
  gstate.time_step += gstate.time_spacing * gstate.direction;

  // Render the acutal image
  render_kernel(gstate.gridSize, gstate.blockSize, d_output, gstate.width,
                gstate.height, gstate.d_centers, gstate.d_radii,
                gstate.radii.size(), gstate.normalization, gstate.time_step);

  cudaEventRecord(stop);
  cudaEventSynchronize(stop);

  float milliseconds = 0;
  cudaEventElapsedTime(&milliseconds, start, stop);

  checkCudaErrors(
      cudaGraphicsUnmapResources(1, &(gstate.cuda_pbo_resource), 0));
}

// display results using OpenGL (called by GLUT)
void display() {
  // use OpenGL to build view matrix
  GLfloat modelView[16];
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();
  glGetFloatv(GL_MODELVIEW_MATRIX, modelView);
  glPopMatrix();
  render();

  // Display results
  glClear(GL_COLOR_BUFFER_BIT);
  glDisable(GL_DEPTH_TEST);
  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);

  //
  // Prepare render to texture
  //
  glBindBuffer(GL_PIXEL_UNPACK_BUFFER_ARB, gstate.pbo);
  glBindTexture(GL_TEXTURE_2D, gstate.tex);
  glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, gstate.width, gstate.height, GL_RGBA,
                  GL_UNSIGNED_BYTE, 0);
  glBindBuffer(GL_PIXEL_UNPACK_BUFFER_ARB, 0);

  //
  // Draw the texture on the simplest quad
  //
  // Draw textured quad
  glEnable(GL_TEXTURE_2D);
  glBegin(GL_QUADS);

  // This is just a quad that occupies the whole screen
  glTexCoord2f(0, 0);
  glVertex2f(-1, -1);
  glTexCoord2f(1, 0);
  glVertex2f(1, -1);
  glTexCoord2f(1, 1);
  glVertex2f(1, 1);
  glTexCoord2f(0, 1);
  glVertex2f(-1, 1);

  glEnd();

  glDisable(GL_TEXTURE_2D);
  glBindTexture(GL_TEXTURE_2D, 0);

  //
  // And it's over
  //

  glutSwapBuffers();
  glutReportErrors();
}

void idle() {
  glutPostRedisplay();
}

void motion(int x, int y) {
  float2 mouse_pos = make_float2(x, y);

  float2 delta_pos =
      (mouse_pos - gstate.prev_mouse_pos) / (gstate.height / 2.0);

  gstate.view_center =
      make_float2(-delta_pos.x, delta_pos.y) + gstate.prev_view_center;

  glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
  int mods;

  float2 mouse_pos = make_float2(x, y);
  if (state == GLUT_DOWN) {
    gstate.prev_mouse_pos = mouse_pos;
  } else if (state == GLUT_UP) {
    gstate.prev_view_center = gstate.view_center;
  }

  // It's a wheel event
  if ((button == 3) || (button == 4)) {
    if (state == GLUT_UP) return;

    gstate.scale *= (button == 3) ? 0.9 : 1.1;
  }

  std::cout << gstate.view_center.x << " , " << gstate.view_center.y
            << " :: " << gstate.scale << std::endl;
}

void keyboard(unsigned char key, int x, int y) {
  switch (key) {
    case 27:
      glutDestroyWindow(glutGetWindow());
      return;

    case 'f':
      break;

    case 'G':
      std::cout << "Resetting" << std::endl;
      gstate.view_center = make_float2(0.0, 0.0);
      gstate.scale = 1.0f;
      break;

    case 'q':
    case 'Q':
      std::cout << "Attempting to exit" << std::endl;
      glutDestroyWindow(glutGetWindow());
      return;

    default:
      break;
  }

  glutPostRedisplay();
}

int main(int argc, char **argv) {
#if defined(__linux__)
  setenv("DISPLAY", ":0", 0);
#endif
  std::cout << "Jake cuViewer starting\n" << std::endl;

  //
  // Initialize openGL
  //
  initGL(&argc, argv);

  //
  // Initialize glut
  //
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
  glutDisplayFunc(display);
  glutKeyboardFunc(keyboard);
  glutIdleFunc(idle);

  //
  // Zero the pbo
  //
  initPixelBuffer();

  const int count = 60;

  // std::vector<Vec3f> centers;
  // std::vector<float> radii;

  const auto add_circle = [&gstate](const Vec3f center, const float radius) {
    gstate.centers.push_back(center);
    gstate.radii.push_back(radius);
  };

  add_circle(Vec3f(0.0, 0.0, 1.0), 0.4);
  add_circle(Vec3f(0.0, 0.3, 1.2), 0.3);
  add_circle(Vec3f(0.3, 0.3, 0.5), 0.2);
  // add_circle(Vec3f(0.3, 0.3, 1.2), 0.8);

  send_parameters(gstate.centers, gstate.radii);
  glutMainLoop();
}


//
// Created by kellerberrin on 3/4/21.
//

#include <kol_library.h>
#include "kol_test.h"
#include "contrib/kol_GoGraphImpl.h"
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

namespace kellerberrin::ontology {

// This object is re-created for each test case; store the graph in a static pointer so that it is only created once.
class TestEnrichment {

public:

  TestEnrichment() = default;

  ~TestEnrichment() = default;

  [[nodiscard]] static const TermAnnotation &annotation() {

    if (not static_annotation_) {

      static_annotation_ = getAnnotation();

    }
    BOOST_REQUIRE(static_annotation_);
    return *static_annotation_;

  }

  [[nodiscard]] static const GoGraph &goGraph() {

    if (not static_graph_) {

      static_graph_ = getGoGraph();

    }
    BOOST_REQUIRE(static_graph_);
    return *static_graph_;

  }

private:

  inline static std::shared_ptr<const GoGraph> static_graph_;
  inline static std::shared_ptr<const TermAnnotation> static_annotation_;

  [[nodiscard]] static std::shared_ptr<const TermAnnotation> getAnnotation() {

    PolicyEvidence default_evidence;
    return ParserAnnotationGaf::parseAnnotationFile(default_evidence, UnitTestDefinitions::gafFileName());

  }

  [[nodiscard]] static std::shared_ptr<GoGraph> getGoGraph() {

    auto go_parser_ptr = ParserGoFactory::createGoParser(ParserGoType::PARSER_GO_OBO);
    BOOST_REQUIRE(go_parser_ptr);
    return go_parser_ptr->parseGoFile(UnitTestDefinitions::oboFileName());

  }

};

} // namespace

namespace kol = kellerberrin::ontology;
namespace kel = kellerberrin;

BOOST_FIXTURE_TEST_SUITE(TestEnrichmentSuite, kol::TestEnrichment)

///////////////////////////////////////////////
// Gene and GO Term count accessors
///////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(test_enrichment_tools_hypergeometric_test)
{

  kel::HypergeometricDistribution  hyper_dist(8,10,17);
  auto p_val = hyper_dist.upperSingleTailTest(7);
  BOOST_CHECK_CLOSE( p_val, 0.036404771, 0.0001);
  BOOST_TEST_MESSAGE( "test_enrichment_tools_hypergeometric_test ... OK" );

}

BOOST_AUTO_TEST_CASE(test_enrichment_tools_child_genes)
{

  auto term_genes = kol::EnrichmentTools::getDescendantGenes(goGraph(), annotation(), "GO:0002507");
  auto child_terms = goGraph().getGoGraphImpl().getDescendantTerms("GO:0002507");
  child_terms.insert("GO:0002507");

  for (auto const& gene : term_genes) {

    auto go_BP = annotation().getGoTermsForGeneBP(gene);
    if (kol::SetUtilities::setIntersection(go_BP, child_terms).empty()) BOOST_FAIL("Empty intersection of BP go terms and child go terms" );

  }
  BOOST_TEST_MESSAGE( "test_enrichment_tools_child_genes ... OK" );

}

BOOST_AUTO_TEST_CASE(test_enrichment_tools_enrichment_significance)
{

  auto term_genes = kol::EnrichmentTools::getDescendantGenes(goGraph(), annotation(), "GO:0002507");
  auto p_val = kol::EnrichmentTools::enrichmentSignificance(goGraph(), annotation(), term_genes, "GO:0002507");
  BOOST_CHECK_CLOSE( p_val, 0.0, 0.0001);
  BOOST_TEST_MESSAGE( "test_enrichment_tools_enrichment_significance ... OK" );

}

BOOST_AUTO_TEST_SUITE_END()


/* boost random/non_central_chi_squared_distribution.hpp header file
 *
 * Copyright Thijs van den Berg 2014
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_NON_CENTRAL_CHI_SQUARED_DISTRIBUTION_HPP
#define BOOST_RANDOM_NON_CENTRAL_CHI_SQUARED_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iosfwd>
#include <istream>
#include <boost/limits.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>

namespace boost {
namespace random {

/**
 * The noncentral chi-squared distribution is a real valued distribution with
 * two parameter, @c k and @c lambda.  The distribution produces values > 0.
 *
 * This is the distribution of the sum of squares of k Normal distributed
 * variates each with variance one and \f$\lambda\f$ the sum of squares of the
 * normal means.
 *
 * The distribution function is
 * \f$\displaystyle P(x) = \frac{1}{2} e^{-(x+\lambda)/2} \left( \frac{x}{\lambda} \right)^{k/4-1/2} I_{k/2-1}( \sqrt{\lambda x} )\f$.
 *  where  \f$\displaystyle I_\nu(z)\f$ is a modified Bessel function of the
 * first kind.
 *
 * The algorithm is taken from
 *
 *  @blockquote
 *  "Monte Carlo Methods in Financial Engineering", Paul Glasserman,
 *  2003, XIII, 596 p, Stochastic Modelling and Applied Probability, Vol. 53,
 *  ISBN 978-0-387-21617-1, p 124, Fig. 3.5.
 *  @endblockquote
 */
template <typename RealType = double>
class non_central_chi_squared_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef non_central_chi_squared_distribution distribution_type;

        /**
         * Constructs the parameters of a non_central_chi_squared_distribution.
         * @c k and @c lambda are the parameter of the distribution.
         *
         * Requires: k > 0 && lambda > 0
         */
        explicit
        param_type(RealType k_arg = RealType(1), RealType lambda_arg = RealType(1))
        : _k(k_arg), _lambda(lambda_arg)
        {
            BOOST_ASSERT(k_arg > RealType(0));
            BOOST_ASSERT(lambda_arg > RealType(0));
        }

        /** Returns the @c k parameter of the distribution */
        RealType k() const { return _k; }

        /** Returns the @c lambda parameter of the distribution */
        RealType lambda() const { return _lambda; }

        /** Writes the parameters of the distribution to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._k << ' ' << parm._lambda;
            return os;
        }

        /** Reads the parameters of the distribution from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            is >> parm._k >> std::ws >> parm._lambda;
            return is;
        }

        /** Returns true if the parameters have the same values. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._k == rhs._k && lhs._lambda == rhs._lambda; }

        /** Returns true if the parameters have different values. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _k;
        RealType _lambda;
    };

    /**
     * Construct a @c non_central_chi_squared_distribution object. @c k and
     * @c lambda are the parameter of the distribution.
     *
     * Requires: k > 0 && lambda > 0
     */
    explicit
    non_central_chi_squared_distribution(RealType k_arg = RealType(1), RealType lambda_arg = RealType(1))
      : _param(k_arg, lambda_arg)
    {
        BOOST_ASSERT(k_arg > RealType(0));
        BOOST_ASSERT(lambda_arg > RealType(0));
    }

    /**
     * Construct a @c non_central_chi_squared_distribution object from the parameter.
     */
    explicit
    non_central_chi_squared_distribution(const param_type& parm)
      : _param( parm )
    { }

    /**
     * Returns a random variate distributed according to the
     * non central chi squared distribution specified by @c param.
     */
    template<typename URNG>
    RealType operator()(URNG& eng, const param_type& parm) const
    { return non_central_chi_squared_distribution(parm)(eng); }

    /**
     * Returns a random variate distributed according to the
     * non central chi squared distribution.
     */
    template<typename URNG>
    RealType operator()(URNG& eng)
    {
        using std::sqrt;
        if (_param.k() > 1) {
            boost::random::normal_distribution<RealType> n_dist;
            boost::random::chi_squared_distribution<RealType> c_dist(_param.k() - RealType(1));
            RealType _z = n_dist(eng);
            RealType _x = c_dist(eng);
            RealType term1 = _z + sqrt(_param.lambda());
            return term1*term1 + _x;
        }
        else {
            boost::random::poisson_distribution<> p_dist(_param.lambda()/RealType(2));
            boost::random::poisson_distribution<>::result_type _p = p_dist(eng);
            boost::random::chi_squared_distribution<RealType> c_dist(_param.k() + RealType(2)*_p);
            return c_dist(eng);
        }
    }

    /** Returns the @c k parameter of the distribution. */
    RealType k() const { return _param.k(); }

    /** Returns the @c lambda parameter of the distribution. */
    RealType lambda() const { return _param.lambda(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return _param; }

    /** Sets parameters of the distribution. */
    void param(const param_type& parm) { _param = parm; }

    /** Resets the distribution, so that subsequent uses does not depend on values already produced by it.*/
    void reset() {}

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION() const
    { return RealType(0); }

    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION() const
    { return (std::numeric_limits<RealType>::infinity)(); }

    /** Writes the parameters of the distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, non_central_chi_squared_distribution, dist)
    {
        os << dist.param();
        return os;
    }

    /** reads the parameters of the distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, non_central_chi_squared_distribution, dist)
    {
        param_type parm;
        if(is >> parm) {
            dist.param(parm);
        }
        return is;
    }

    /** Returns true if two distributions have the same parameters and produce
        the same sequence of random numbers given equal generators.*/
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(non_central_chi_squared_distribution, lhs, rhs)
    { return lhs.param() == rhs.param(); }

    /** Returns true if two distributions have different parameters and/or can produce
       different sequences of random numbers given equal generators.*/
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(non_central_chi_squared_distribution)

private:

    /// @cond show_private
    param_type  _param;
    /// @endcond
};

} // namespace random
} // namespace boost

#endif


#ifndef PROGRAM_KEY_GENERATOR_HPP
#define PROGRAM_KEY_GENERATOR_HPP

#include <string>
#include <random>

#include <boost/date_time/posix_time/posix_time.hpp>

static inline std::string generate_key(int generate_len = 6, char start = ' ', char end = '~') {
    std::mt19937 generator{std::random_device{}()};
    std::uniform_int_distribution<int> distribution{start, end};

    std::string rand_str(generate_len, '\0');
    for (auto &dis: rand_str)
        dis = distribution(generator);
    return rand_str;
}

static inline boost::posix_time::ptime generate_expiration_timestamp() {
    static const int duration_in_days = 1;
    static const boost::posix_time::hours duration(duration_in_days * 24);
    boost::posix_time::ptime expiration_timestamp(boost::posix_time::second_clock::local_time() + duration);
    return expiration_timestamp;
}

#endif //PROGRAM_KEY_GENERATOR_HPP