/*
 * sgeisselsoeder
 *
 * Date June 2016
 *
 */

#include <vector>
#include <cassert>
#include <fstream>
#include <iostream>
#include <numeric>
#include <limits>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <boost/lexical_cast.hpp>
#include "candidate.hpp"
#include "vtkWriter.h"
#include "allScales.h"
#include <omp.h>
#include <boost/foreach.hpp>
#include <sys/types.h>
#include <dirent.h>
#include <sys/stat.h>
#include <cmath>
#include "sgcu/sgcu.hpp"
#include "rememberBoostHistogram.h"

#include <cmath>	// for isnan

using std::cout;
using std::cerr;
using std::endl;
using std::string;
using std::vector;
using std::ofstream;
using std::ifstream;
using std::stringstream;

// implementation of struct aroundRatios

namespace SG{

//double computeSegmentationThreshold(std::vector<double>& valuesToSegment, unsigned int numberOfSmearings = 50, double useOnlyUpperFraction = 0.5);

// NOTE this function can be kept for now to segment the individual scales as it worked well,
// but it could be replaced by a more deterministic segmentation as will be the case for the cluster search in the end

// This function looks at the distribution and tries to separate the majority of random non clustering gridpoints
// from the parts where high clustering might occur
// it fits the falling flank of the distribution linearly, computes the intersection with y=0
// and treats everything below this as background
double computeSegmentationThreshold(std::vector<double>& valuesToSegment, unsigned int numberOfSmearings = 50, double upperFractionToUse = 0.5)
{
	static unsigned int counter = 0;
//	if (counter == 0)
//	{
//		// build a histogram for the densities of all gridpoints
//		// create an accumulator
//		// 100 bins, approx. the same range as the actual distribution
//		acc myAccumulator( tag::density::num_bins = 100, tag::density::cache_size = valuesToSegment.size()-2);
//		// fill accumulator
//		for (unsigned int j = 0; j < valuesToSegment.size(); ++j){
//			myAccumulator(valuesToSegment[j]);
//		}
//		// do the boost histogram
//		histogram_type hist = density(myAccumulator);
//		ofstream raus(std::string(outputFolder_+"segmentationHistBeforeFull" + boost::lexical_cast<std::string>(counter) + ".txt").c_str());
//		for( unsigned int i = 0; i < (unsigned int) hist.size(); i++ ) {
//			raus << hist[i].first << " " << hist[i].second << endl;
//		}
//		raus.close();
//	}

	std::vector<double> valuesCopy = valuesToSegment;
	std::sort(valuesCopy.begin(), valuesCopy.end());
	valuesToSegment.clear();
	int numToUse = upperFractionToUse*valuesCopy.size();
	valuesToSegment.insert(valuesToSegment.begin(), valuesCopy.end()-numToUse, valuesCopy.end());

	// the threshold for background against signal in the end
	double xThres = 0.0;

	double mean = 0.0;
	double stdDev = 0.0;
	for (unsigned int j = 0; j < valuesToSegment.size(); ++j)
		mean += valuesToSegment[j];
	mean /= valuesToSegment.size();

	// build a histogram for the densities of all gridpoints
	// create an accumulator
	// 100 bins, approx. the same range as the actual distribution
	acc myAccumulator( boost::accumulators::tag::density::num_bins = 100, boost::accumulators::tag::density::cache_size = valuesToSegment.size()-2);
	// fill accumulator
	for (unsigned int j = 0; j < valuesToSegment.size(); ++j){
		myAccumulator(valuesToSegment[j]);
		double temp = valuesToSegment[j] - mean;
		stdDev += temp*temp;
	}
	stdDev /= valuesToSegment.size();
	stdDev = sqrt(stdDev);

// 	cout << "mean +- stddev: " << mean << " " << stdDev << endl;
// 	cout << "1,2,3,4,5 sigma borders: " << mean+stdDev << " , " << mean+2*stdDev << " , " << mean+3*stdDev << " , " << mean+4*stdDev << " , " << mean+5*stdDev << endl;
// 	xThres = mean+5*stdDev;
// 	return xThres;

	// do the boost histogram
	histogram_type hist = boost::accumulators::density(myAccumulator);
//	histogram_type histOrg = hist;
//	if (counter == 0)
//	{
//		ofstream raus(std::string(outputFolder_+"segmentationHistBefore" + boost::lexical_cast<std::string>(counter) + ".txt").c_str());
//		for( unsigned int i = 0; i < (unsigned int) hist.size(); i++ ) {
//			raus << hist[i].first << " " << hist[i].second << endl;
//		}
//		raus.close();
//	}

	for (unsigned int i = 0; i < numberOfSmearings; ++i){
		vector<double> vals(hist.size());
		for (unsigned int j = 0; j < (unsigned int) hist.size(); ++j){
			vals[j] = hist[j].second;
		}
		for (unsigned int j = 1; j < (unsigned int) hist.size()-1; ++j){
			hist[j].second = (vals[j-1] + vals[j] + vals[j+1]) / 3.0;
		}
		hist[0].second = (vals[0] + vals[1]) / 2.0;
		hist[hist.size()-1].second = (vals[hist.size()-2] + vals[hist.size()-1]) / 2.0;
	}

//	if (counter == 0)
//	{
//	 	ofstream raus(std::string(outputFolder_+"segmentationHist" + boost::lexical_cast<std::string>(counter) + ".txt").c_str());
//		for( unsigned int i = 0; i < (unsigned int) hist.size(); i++ ) {
//			raus << hist[i].first << " " << hist[i].second << " " << histOrg[i].second << endl;
//		}
//		raus.close();
//	}

	// find the maximum of the distribution and the point where only half of the maximum is
	std::vector<double> vals(hist.size());
	double max = 0.0;
	unsigned int maxIndex = 0;
//	double halfMax = 0.0;
	unsigned int halfMaxIndex = 0;
	for( unsigned int i = 0; i < (unsigned int) hist.size(); i++ ) {
		vals[i] = hist[i].second;
		if (hist[i].second > max){
			max = hist[i].second;
			maxIndex = i;
		}
		if (hist[i].second > 0.5*max){
//			halfMax = hist[i].second;
			halfMaxIndex = i;
		}
	}

	// now average the next broadness bins from the maximum on
	// to get a more robust point for (approx.) the maximum and the half maximum
	const unsigned int broadness = 3;
	double topX = 0.0;
	double topY = 0.0;
	for (unsigned int i = 0; ((i < broadness) && (i < (unsigned int) hist.size())); ++i){
		topX += hist[maxIndex+i].first;
		topY += hist[maxIndex+i].second;
	}
	topX /= broadness;
	topY /= broadness;

	double middleX = 0.0;
	double middleY = 0.0;
	for (unsigned int i = 0; i < (i < broadness) && (i < (unsigned int) hist.size()); ++i){
		middleX += hist[halfMaxIndex+i].first;
		middleY += hist[halfMaxIndex+i].second;
	}
	middleX /= broadness;
	middleY /= broadness;

	// compute the slope between the two points
	double diffX = middleX - topX;
	double diffY = middleY - topY;
	double slope = diffY/diffX;

	// compute the intersection of a line from the maximum to through the half maximum with the x axis
	double dx = fabs(topY/slope);
	xThres = topX+dx;

	++counter;
	return xThres;
}

void allScales::assertNoNansAndInfs() const
{
	unsigned int counter = 0;
	// cross check if there is a nan or inf value somewhere in the grid
	for (unsigned int i = 0; i < aroundRatioAll.size(); ++i)
	{
		for (unsigned int j = 0; j < aroundRatioAll[i].size(); ++j)
		{
			if (std::isnan(aroundRatioAll[i][j]) || std::isinf(aroundRatioAll[i][j]))
			{
				++counter;
			}
		}
	}
	if (counter > 0)
	{
		std::cerr << counter << " ratios have a nan or inf value! This is not how it works!" << std::endl;
		throw -1;
	}
}

unsigned int countNumberOfNonZeros(struct allScales const & allScales)
{
	unsigned int counter  = 0;
	for (unsigned int i = 0; i < allScales.aroundRatioAll.size(); ++i)
	{
		for (unsigned int j = 0; j < allScales.aroundRatioAll[i].size(); ++j)
		{
			if (allScales.aroundRatioAll[i][j] != 0.0)
				++counter;
		}
	}
	return counter;
}

void segmentAllRatiosScalewise(allScales & scales)
{
	unsigned int numPoints = scales.aroundRatioAll.size();
	unsigned int numDistanceBins = scales.aroundRatioAll[0].size();
	if (numPoints == 0)
	{
		cerr << "There is nothing to segment in an empty set of ratios." << endl;
		return;
	}
	if (numDistanceBins == 0)
	{
		cerr << "There is nothing to segment in a set of ratios without distance bins." << endl;
		return;
	}

	for (unsigned int j = 0; j < numDistanceBins; ++j)
	{
		std::vector<double> allRatioValues(numPoints);
		//cout << "######### scale " << j << endl;
		for (unsigned int i = 0; i < numPoints; ++i)
		{
			allRatioValues[i] = scales.aroundRatioAll[i][j];
		}
		double threshold = computeSegmentationThreshold(allRatioValues);
		if (threshold < sgcu::minValue(allRatioValues))
			throw -1;
		if (threshold > sgcu::maxValue(allRatioValues))
			throw -2;

		for (unsigned int i = 0; i < numPoints; ++i)
		{
			if (scales.aroundRatioAll[i][j] < threshold)
			{
				scales.aroundRatioAll[i][j] = 0.0;
			}
		}
	}
}

unsigned int allScales::size() const
{
	return aroundRatioAll.size();
}

void allScales::divideRatios(double value)
{
	if (value == 1.0)
		return;
	if (value == 0.0)
	{
		std::cerr << "Division by zero destroys all ratios and the universe!" << std::endl;
		throw (-1);
	}
	double invValue = 1.0/value;
	for (unsigned int i = 0; i < aroundRatioAll.size(); ++i)
	{
		for (unsigned int n = 0; n < aroundRatioAll[0].size(); ++n)
		{
			aroundRatioAll[i][n] *= invValue;
		}
	}
}

allScales::allScales(){
	clear();
}

void allScales::clear(){
	numBins_ = 0;
	stepDegree_ = 0.0;
	stepSize_ = 0.0;

	aroundRatioAll.clear();
}

void allScales::set(double stepDeg, unsigned int numGridpoints)
{
	stepDegree_ = stepDeg;
	numBins_ = static_cast<unsigned int>(90.0/stepDegree_)+1;
	const double oneDegree = M_PI/180.0;
	stepSize_ = stepDegree_*oneDegree;

	if (numGridpoints != 0)
	{
		if (aroundRatioAll.size() != numGridpoints){
			aroundRatioAll.resize(numGridpoints);
			for (unsigned int i = 0; i < aroundRatioAll.size(); ++i){
				aroundRatioAll[i].resize(numBins_);
			}
		}
	}
}

void allScales::init(double stepDeg, unsigned int numGridpoints)
{
 	clear();
 	set(stepDeg, numGridpoints);
}

void setToZero(allScales & scales)
{
	for (unsigned int i = 0; i < scales.aroundRatioAll.size(); ++i)
	{
		for (unsigned int j = 0; j < scales.aroundRatioAll[i].size(); ++j)
		{
			scales.aroundRatioAll[i][j] = 0.0;
		}
	}
}

long double allScales::computePValue(long double mean, long double found){
	long double p = 0.0;
	const long double changeThreshold = 120.0;
	if ((found <= changeThreshold) && (mean <= changeThreshold)){
		// plain poisson way:
		long double exponent = expl(-mean);
		long double power = powl(mean, found);
		long double fac = sgcu::faculty(found);
		int expExp = log10l(exponent);
		int expPow = log10l(power);
		int expFac = log10l(fac);
		int finalExp = expExp+expPow-expFac;
		exponent /= powl(10, expExp);
		power /= powl(10, expPow);
		fac /= powl(10, expFac);
		p = exponent*power/fac;
		p *= powl(10, finalExp);
	} else {
		if (found == 0.0) {
			p = 0.0;
		} else {
			// poissonian with stirling formula:
			long double frac = mean/found;
			long double logarithm = logl(frac);
			long double exponent2 = found*(1.0+logarithm)-mean;
			long double efunc = expl(exponent2);
			long double denominator = sqrtl(2.0*M_PI*(found+1.0/6.0));
			p = efunc / denominator;
		}
	}
	return p;
}

void allScales::computeAllRatios(const SG::sphericalGrid<double>& grid, const std::vector<SG::candidate_t>& events)
{
	timeval starttTotal, endtTotal;
	timeval startt, endt;
	gettimeofday(&starttTotal, 0);
	gettimeofday(&startt, 0);
 	cout << "DEBUG allScales::computeAllRatios started" << endl;

	if (aroundRatioAll.size() != grid.size())
	{
		aroundRatioAll.resize(grid.size());
		for (unsigned int i = 0; i < aroundRatioAll.size(); ++i)
		{
			aroundRatioAll[i].resize(numBins_);
		}
	}

	unsigned int eventsSize = events.size();
	int gridSize = static_cast<int>(grid.size());
	double evtSize = static_cast<double>(events.size());

	gettimeofday(&endt, 0); cout << "Runtime of ratio resize: " << endt.tv_sec - startt.tv_sec << endl; gettimeofday(&startt, 0);

	double distanceLimit = static_cast<double>(numBins_)*stepSize_;

//	double addValue = 1.0/static_cast<double>(events.size());
//	omp_set_num_threads(4);
//	#pragma omp parallel for // schedule(dynamic,1)	// parallelization even slows this step down if many events are used - prob. because each thread requires a distances vector and they don't all fit into a faster cache level (sorting becomes the extreme bottle neck), anyways, it doesn't get faster even without distance vector, so maybe the cache misses when writing
	for (int j = 0; j < gridSize; ++j)
	{
// 		if ((j % 99) == 0)
// 		{
// 			gettimeofday(&endt, 0); cout << j << " ... Runtime of last 100 loops : " << endt.tv_sec - startt.tv_sec << endl; gettimeofday(&startt, 0);
// 		}
// 		cout << j << " / " << grid.size() << endl;

		// evaluate all distances
		vector<double> distances;
		for (unsigned int k = 0; k < eventsSize; ++k)
		{
// 			if (quickDist(grid[j], events[k]) < distanceLimit)	// TODO: test if this actually speeds up the computation!
// 			{
		// TODO: speed up by continue if the conditions are not me// TODO: speed up by continue if the conditions are not mett
				double d = distanceRad(grid[j], events[k]);
				if (d < distanceLimit)
				{
					distances.push_back(d);
	//				aroundRatioAll[j][static_cast<unsigned int>(d/stepSize_)] += addValue;
				}
// 			}
		}
//		for (unsigned int i = 0; i < distances.size(); ++i)
//		{
//			++aroundRatioAll[j][distances[i]/stepSize_];
//		}
//
//		for (unsigned int i = 0; i < numBins_; ++i)
//		{
//			aroundRatioAll[j][i] /= evtSize;
//		}

		// sort them ascending
		std::sort(distances.begin(), distances.end());

		// fill the 180 ratios for this gridpoint accordingly
		vector<double>::const_iterator it = distances.begin();
		unsigned int counter = 0;
		for (unsigned int i = 0; i < numBins_; ++i){
			counter = 0; // resetting the counter for each bin means no cumulative search
			while ( ((*it) < (i+1)*stepSize_) && (it != distances.end()) ){
				++counter;
				++it;
			}
			aroundRatioAll[j][i] = static_cast<double>(counter)/static_cast<double>(evtSize);
		}
// 		cout << "distances size = " << distances.size() << endl;
// 		if ( (j == 10) || (j == 10000) )
// 		{
// 			for (unsigned int i = 0; i < numBins_; ++i)
// 				std::cout << "DEBUG " << i << " " << j << " : " << aroundRatioAll[j][i] << std::endl;
// 		}
	}

	gettimeofday(&endtTotal, 0); cout << "Total runtime of computeAllRatios: " << endtTotal.tv_sec - starttTotal.tv_sec << endl;
// 	cout << "DEBUG allScales::computeAllRatios finished" << endl;
}

void allScales::computePoissonProbabilityForAllRatios(SG::allScales const & normalization, unsigned int eventSize)
{
	timeval startt, endt;
	gettimeofday(&startt, 0);

	double evtSize = static_cast<double>(eventSize);

	//omp_set_num_threads(16);
	#pragma omp parallel for
	for (int j = 0; j < (int)aroundRatioAll.size(); ++j){
		for (unsigned int i = 0; i < numBins_; ++i){
			long double mean = normalization.aroundRatioAll[j][i]*evtSize;
			double found = aroundRatioAll[j][i]*evtSize;

			// Sum the p values for all values lower than the found number of events
			long double pSum = 0.0;
			for (unsigned int x = 0; x < static_cast<unsigned int>(found+0.5); ++x){
				long double thisFound = static_cast<long double>(x);
				pSum += computePValue(mean, thisFound);
			}

			const long double maxPSum = 1.0 - 1.0/100000000000;
			if (pSum > (maxPSum)){
				pSum = maxPSum;
// 				cerr << "No Probability " << pSum << " greater one allowed!" << endl;
			}
			aroundRatioAll[j][i] = 1.0-pSum;
		}
	}

	gettimeofday(&endt, 0); cout << "Runtime of computePoissonProbabilityForAllRatios: " << endt.tv_sec - startt.tv_sec << endl;
}

// load every .norm file in folder folder (that matches size and stepsize) and try to treat it as a valid normalization
int allScales::loadNormalization2(unsigned int gridsize, unsigned int numBins, unsigned int numEvents, std::string folder)
{
	if (folder == "")
	{
		cerr << "No or empty directory specified for data reading.";
		return -1;
	}
	string searchedEnding = ".nres2";
	string searchSize = "size" + boost::lexical_cast<string>(gridsize) + "_";
	string searchNumBins = "bins" + boost::lexical_cast<string>(numBins) + "_";
	string searchNumEvents = "events" + boost::lexical_cast<string>(numEvents) + "_";

	vector<string> inputFiles;
	int len = folder.length();
	if (folder.at(len-1) != '/')
		folder += "/";

	DIR* myDir;
	char* dirName = (char*) folder.c_str();
	struct dirent* entry;
	if (!(myDir = opendir(dirName))) {
		cerr << "Failed to open directory " << dirName << endl;
		return -1;
	}

	while ((entry = readdir(myDir)))
	{
		string temp = string(entry->d_name);
		string lowerFileName = temp;	// Assign lowerFileName to temp to have the right size reserved (transform does not allocate memory)
		std::transform(temp.begin(), temp.end(), lowerFileName.begin(), ::tolower);

		std::string::size_type found = lowerFileName.rfind(searchedEnding);
		if (found == (lowerFileName.length()-searchedEnding.length())){	// find it at the end
			if (lowerFileName.find(searchSize) != string::npos){	// only with the right gridSize
				if (lowerFileName.find(searchNumBins) != string::npos){	// only with the right stepSize
					if (lowerFileName.find(searchNumEvents) != string::npos){	// only for the right number of events
						inputFiles.push_back( temp );
					}
				}
			}
		}
	}
	closedir(myDir);
	if (inputFiles.empty()){
		cerr << "No " << searchedEnding << " file found in directory " << folder << endl;
		return -4;
	}

	int ret = 0;
	ret = getNormalizationResults(folder + *inputFiles.begin(), gridsize, numBins);
	if (ret != 0){
		cout << "Failed loading normalization file " << *inputFiles.begin() << " from folder " << folder << endl;
		throw -5;
	}

	double stepDegree = maximumDistanceToEvaluateInDegree/(numBins-1);
	set(stepDegree);
	return 0;
}

int allScales::loadFileToData(std::string file, unsigned int gridsize, unsigned int numBins, std::string searchStringEnding, std::vector<std::vector<double> >& data){
	// cerr << "DEBUG: loadFileToData starts" << endl;
	if (file == ""){
		cerr << "No file specified for data reading.";
		return -1;
	}

	ifstream rein;
	rein.open(file.c_str());
	if (! rein.is_open() ){
		cerr << file << " could not be opened!" << endl;
		return -2;
	}

	string lowerFileName = file;	// Assign lowerFileName to temp to have the right size reserved (transform does not allocate memory)
	std::transform(file.begin(), file.end(), lowerFileName.begin(), ::tolower);
	std::string::size_type found2 = lowerFileName.rfind(searchStringEnding);
	if (found2 == std::string::npos){
		cerr << "Dealing with a " << searchStringEnding << " file (" << file << ") that does not contain \"" << searchStringEnding << "\"! This should not have happend!" << endl;
		return -3;
	}

	data.clear();

	const int anz = 64000;
	char temp2[anz];
	rein.getline(temp2, anz);
	while (rein.good()){
		string thisLineAsString = string(temp2);
		size_t wo = thisLineAsString.find("#");
		string currentPart = thisLineAsString.substr(0, wo);	// get the line from start to the #

		if (currentPart.length() == 0){
			rein.getline(temp2, anz);
			continue;
		}

		stringstream sstr2;
		sstr2 << currentPart;
		if (!sstr2.good()){
			cerr << "Stringstream failed to hold the following data: " << currentPart << endl;
			return -4;
		}

		vector<double> thisLine;
		while (sstr2.good()){
			double entry;

			string temp = "";
			sstr2 >> temp;

			if (temp == "" || temp == " "){
				continue;
			}
			if (temp == "nan" || temp == "-nan"){
				cerr << "WARNING: Read " << temp << " in file " << file << " ! Substituting this mean value with 0.0." << endl;
				entry = 0.0;
			} else {
				entry = boost::lexical_cast<double>(temp);
			}

			thisLine.push_back(entry);
		}

		// Event was accepted
		data.push_back(thisLine);

		rein.getline(temp2, anz);
	}

	// Here we have read the entire file and parsed it to data

	// Now check for consistency with what was desired
	if (data.size() != gridsize){
		cerr << "The read number of lines (" << data.size() << ") in file " << file << " does not match the desired size of the normalization grid (" << gridsize << "). Dropping this file." << endl;
		return -6;
	}
	for (unsigned int i = 0; i < gridsize; ++i){
		if (data[i].size() != numBins){
			cerr << "addNormalizationResults: The read number of entries (" << data[i].size() << ") in line " << i << " in file " << file << " does not match the desired number of bins " << numBins << " (and therefore angular resolution) of the normalization grid. Dropping this file." << endl;
			return -7;
		}
	}

	return 0;
}

int allScales::getNormalizationResults(std::string file, unsigned int gridsize, unsigned int numBins)
{
  	cout << "Normalization adding file " << file << endl;

//	std::vector<std::vector<double> > data;

	string searchStringEnding = ".nres2";
	int ret = loadFileToData(file, gridsize, numBins, searchStringEnding, aroundRatioAll);
	if (ret){
		cerr << "loadFileToData failed with error code " << ret << endl;
		return ret;
	}
//	aroundRatioAll = data;

	return 0;
}

int allScales::dumpRatiosAsNormalization(std::string folder, unsigned int numEvents){
	folder += "/";
	string ofFileName;
	ofstream raus;

	static unsigned int numOfNormalizationsCounter = 0;
	++numOfNormalizationsCounter;

	unsigned int numBins = 0;
	if ( aroundRatioAll.size() ){
		numBins = aroundRatioAll[0].size();
	}

	// Output the results to file
	ofFileName = folder + "normalizationResults_events" + boost::lexical_cast<std::string>(numEvents) + "_Size" + boost::lexical_cast<string>(aroundRatioAll.size()) + "_bins" + boost::lexical_cast<string>(numBins) +  "_nr" + boost::lexical_cast<string>(numOfNormalizationsCounter) + ".nres2";
//	ofFileName = folder + "normalizationResults_Size" + boost::lexical_cast<string>(aroundRatioAll.size()) + "_bins" + boost::lexical_cast<string>(numBins) +  "_nr" + boost::lexical_cast<string>(numOfNormalizationsCounter) + ".nres2";
	cout << "Dumping normalization results to file " << ofFileName << endl;
	raus.open(ofFileName.c_str());
	if (raus.is_open() ){
		for (unsigned int i = 0; i < aroundRatioAll.size(); ++i){
			for (unsigned int j = 0; j < aroundRatioAll[i].size(); ++j)
				raus << aroundRatioAll[i][j] << " ";
			raus << endl;
		}
	} else {
		cerr << "Failed to open file " << ofFileName << endl;
		return -1;
	}
	raus.close();
	return 0;
}

void allScales::addRatios(struct allScales const & b)
{
       if (aroundRatioAll.size() != b.aroundRatioAll.size())
       {
               std::cerr << "The two allScales do not have the same size!" << std::endl;
               throw (-1);
       }
       for (unsigned int i = 0; i < aroundRatioAll.size(); ++i)
       {
               if (aroundRatioAll[i].size() != b.aroundRatioAll[i].size())
               {
                       std::cerr << "The allScales at index " << i << " do not have the same size!" << std::endl;
                       throw (-2);
               }
       }
       for (unsigned int i = 0; i < aroundRatioAll.size(); ++i)
       {
               for (unsigned int n = 0; n < aroundRatioAll[0].size(); ++n)
               {
                       aroundRatioAll[i][n] += b.aroundRatioAll[i][n];
               }
       }
}

}	// end of namespace SG


#ifndef _PIVOT_CALIBRATION_VREP_PLUGIN_HPP_
#define _PIVOT_CALIBRATION_VREP_PLUGIN_HPP_

#include <iostream>
#include <memory>

#include <boost/exception/all.hpp>
#include <boost/algorithm/string.hpp>
#include <TRTK/PivotCalibration.hpp>

#include <spdlog/spdlog.h>

#include "grl/vrep/Eigen.hpp"
#include "grl/vrep/Vrep.hpp"

#include "v_repLib.h"

#include "grl/vector_ostream.hpp"

namespace grl {

/// Creates a complete vrep plugin object
/// usage:
/// @code
///    auto pivotCalib = std::make_shared<grl::PivotCalibrationVrepPlugin>();
///    pivotCalib->construct();
///    // move objects to new position
/// @endcode
///
/// @todo the rotation component may be different between the
class PivotCalibrationVrepPlugin : public std::enable_shared_from_this<PivotCalibrationVrepPlugin> {
public:

  static const bool debug = false;

    enum ParamIndex {
        ToolTipToModifyName,       ///<  V-REP handle string for the tip of the tool that will be *modified*
        ToolTipToMeasureName,      ///<  V-REP handle string for the tip of the tool that will be *measured*
        ToolBaseToModifyName,      ///<  V-REP handle string for the base of the tool that will be *modified*
        ToolBaseToMeasureName,     ///<  V-REP handle string for the base of the tool that will be *measured*
    };

    typedef std::tuple<
        std::string,
        std::string,
        std::string,
        std::string
        > Params;

    static const Params defaultParams(){
        return std::make_tuple(
                    "RobotMillTip"              , // ToolTipToModifyHandle,
                    "Fiducial#55"               , // ToolTipToMeasureHandle,
                    "RobotFlangeTip"            , // ToolTipHandle,
                    "Fiducial#22"                 // ToolBaseHandle,
                );
    }

/// @todo allow KukaFRIThreadSeparator parameters to be updated
PivotCalibrationVrepPlugin (Params params = defaultParams())
      :
      isFirstFrame(true),
      frameCount(0),
      params_(params)
{
/// @todo figure out how to re-enable when .so isn't loaded
 // initHandles();

}

/// construct() function completes initialization of the plugin
/// @todo move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests.
void construct(){
  logger_ = spdlog::get("console");
  initHandles();

  // set the current transformEstimate to the initial estimate already in vrep
  if(allHandlesSet) transformEstimate = getObjectTransform(toolTip,toolBase);
}

/// Possible algorithms are:
///        TWO_STEP_PROCEDURE,         //!< First, estimate the pivot point in global coordinates. Second, estimate the sought translation by computing the difference vector between the pivot point and the center of the sensor system; then take the mean.\n This algorithm is advantageous in the case of zero-mean noise.
///        COMBINATORICAL_APPROACH     //!< Setup n equations that yield the pivot point from a single measurement using the sought (but unknown) translation. Eliminate the pivot point in the system of equations and finally solve for the sought translation. \n This algorithm is advantageous in the case of non-zero-mean noise (e.g. a systematic error).
///
/// If the algorithm string doesn't match an existing algorithm, this API will assume TWO_STEP_PROCEDURE.
void setAlgorithm(std::string algorithm_){
  algorithm = algorithm_;
  // if (boost::iequals(algorithm, std::string("COMBINATORICAL_APPROACH"))) algorithm = TRTK::PivotCalibration<double>::Algorithm::COMBINATORICAL_APPROACH;
  // else algorithm = TRTK::PivotCalibration<double>::Algorithm::TWO_STEP_PROCEDURE;
}

void addFrame() {
   logger_->info("Adding pivot calibration frame #", ++frameCount );

    auto toolTipInToolBase    = getObjectTransform(toolTip,toolBase);

    if(isFirstFrame){
      firstToolTipInToolBaseInverse             = toolTipInToolBase.inverse();
      isFirstFrame = false;
    }

    auto toolTipInFirstTipBase      = firstToolTipInToolBaseInverse * toolTipInToolBase;        // A_0_Inv * A_i

    rvecsArm.push_back(     toolTipInFirstTipBase.rotation().matrix());
    tvecsArm.push_back(     toolTipInFirstTipBase.translation()     );

   if(debug){

     logger_->info("\ntoolTipInToolBase:\n", poseString(toolTipInToolBase));

     logger_->info("\ntoolTipInFirstTipBase:\n", poseString(toolTipInFirstTipBase));

     // print simulation transfrom from tip to fiducial
     Eigen::Affine3d ToolTipToFiducial = getObjectTransform(toolTip,toolBase);
     logger_->info( "\n", poseString(ToolTipToFiducial,"expected ToolBaseToToolTip (simulation only): "));
   }
}

/// @brief run solver to estimate the unknown transform and set the simulation to the estimated value
///
/// after calling addFrame a number of times in different positions and orientations
///
/// @todo evaluate if applyEstimate should not be called by this
void estimatePivotOffset(){

   logger_->info("Running Pivot Calibration Estimate with the following numbers of entries in each category:  rvecsFiducial", " rvecsArm: ", rvecsArm.size(), " tvecsArm: ", tvecsArm.size() );

   BOOST_VERIFY(allHandlesSet);

   /// TODO(ahundt) use Ransac pivot calibration wrapper because there will likely be noise
   TRTK::PivotCalibrationTwoStep<double> pivotCalib;
   pivotCalib.setLocations(TRTK::make_range(tvecsArm));
   pivotCalib.setRotations(TRTK::make_range(rvecsArm));

   /// TODO(ahundt) Allow selection of "TWO_STEP_PROECDURE" and "COMBINATORIAL_APPROACH" from V-REP LUA API
   //pivotCalib.setAlgorithm("TWO_STEP_PROCEDURE");

   pivotCalib.compute();

   transformEstimate.translation() = pivotCalib.getPivotPoint();

    // get fiducial in optical tracker base frame
    int ret = simGetObjectPosition(toolTip, toolBase, detectedObjectPosition.begin());
    if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("PivotCalibrationVrepPlugin: Could not get position"));
    ret = simGetObjectQuaternion(toolTip, toolBase, detectedObjectQuaternion.begin());
    if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("PivotCalibrationVrepPlugin: Could not get quaternion"));

   if(debug){
     // print simulation transfrom from tip to fiducial
     Eigen::Affine3d ToolTipToFiducial = getObjectTransform(toolTip,toolBase);
     logger_->info( "\n", poseString(ToolTipToFiducial,"expected ToolBaseToToolTip (simulation only): "));
   }

   logger_->info( "\n", poseString(transformEstimate,"estimated toolBaseMeasured to toolTipMeasured:"));

   // print results
   Eigen::Quaterniond eigenQuat(transformEstimate.rotation());
   logger_->info( "Pivot Calibration Estimate quat wxyz\n: ", eigenQuat.w(), " ", eigenQuat.x(), " ", eigenQuat.y(), " ", eigenQuat.z(), " ", " translation xyz: ", transformEstimate.translation().x(), " ", transformEstimate.translation().y(), " ", transformEstimate.translation().z(), " ");

    logger_->info( "Optical Tracker Base Measured quat wxyz\n: ", detectedObjectQuaternion[0], " ", detectedObjectQuaternion[1], " ", detectedObjectQuaternion[2], " ", detectedObjectQuaternion[3], " ", " translation xyz: ", detectedObjectPosition[0], " ", detectedObjectPosition[1], " ", detectedObjectPosition[2], " ");

}

/// @brief  Will apply the stored estimate to the v-rep simulation value
///
/// A default transform is saved when construct() was called
/// so if no estimate has been found that will be used.
void applyEstimate(){

   // set transform between end effector and fiducial
   setObjectTransform(toolTipToModify, toolBaseToModify, transformEstimate);
}

Eigen::Affine3d getEstimate(){
   return transformEstimate;
}

/// @brief Correct modified transform for measured optical tracker data
///
/// If real time transform measurements are happening applyEstimate will move the measured object
/// relative to the sensor and this puts the transform measurement back so the object
/// appears in its real position. However, if running in simulation only
/// the sensor will be in the correct position, so you don't want to call this.
void restoreSensorPosition(){
   // set real optical tracker position and orientation relative to the robot fiducial
   simSetObjectPosition  (toolTip, toolBase, detectedObjectPosition.begin());
   simSetObjectQuaternion(toolTip, toolBase, detectedObjectQuaternion.begin());
}

private:

/// @todo create a function that calls simGetObjectHandle and throws an exception when it fails
/// @todo throw an exception if any of the handles is -1
void initHandles() {

	toolTip                         = grl::vrep::getHandleFromParam<ToolTipToMeasureName>                   (params_);					//Obtain ToolTip handle
	toolBase                        = grl::vrep::getHandleFromParam<ToolBaseToMeasureName>                  (params_);
	toolTipToModify                 = grl::vrep::getHandleFromParam<ToolTipToModifyName>                    (params_);					//Obtain ToolTip handle
	toolBaseToModify                = grl::vrep::getHandleFromParam<ToolBaseToModifyName>                   (params_);

	allHandlesSet  = true;
}

std::shared_ptr<spdlog::logger> logger_;
// TRTK::PivotCalibration<double>::Algorithm algorithm = TRTK::PivotCalibration<double>::Algorithm::TWO_STEP_PROCEDURE;
std::string algorithm = "TWO_STEP_PROCEDURE";

std::vector<typename TRTK::PivotCalibrationTwoStep<double>::Matrix3T > rvecsArm;
std::vector<typename TRTK::PivotCalibrationTwoStep<double>::Vector3T > tvecsArm;

bool isFirstFrame;
std::size_t frameCount;
Eigen::Affine3d firstToolTipInToolBaseInverse;

///
std::array<float,3> detectedObjectPosition;
std::array<float,4> detectedObjectQuaternion;
/// estimate transform between end effector and fiducial
Eigen::Affine3d transformEstimate;

int toolTip = -1;					//Obtain ToolTip handle
int toolBase = -1;
int toolTipToModify = -1;
int toolBaseToModify = -1;
//int ImplantCutPath = -1;
//int BallJointPath = -1;
//int bone = -1;

bool allHandlesSet = false;

private:
Params params_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

}

#endif


#include <QApplication>
#include <QMessageBox>
#include <QMainWindow>
#include "Kernel_type.h"
#ifdef USE_SURFACE_MESH
#include "Scene_surface_mesh_item.h"
#include <CGAL/Mesh_3/properties_Surface_mesh.h>
#else
#include "Polyhedron_type.h"
#include "Scene_polyhedron_item.h"
#include <CGAL/Mesh_3/properties_Polyhedron_3.h>
#endif
#include "Scene_polyhedron_selection_item.h"
#include "Scene_polylines_item.h"
#include "Messages_interface.h"

#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>

#include <CGAL/Polyhedron_copy_3.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>

#include <CGAL/boost/graph/copy_face_graph.h>

#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/property_map.h>

#include <boost/foreach.hpp>
#include <boost/function_output_iterator.hpp>
#include <boost/unordered_map.hpp>
#include "Color_map.h"

#ifdef USE_SURFACE_MESH
typedef Scene_surface_mesh_item Scene_facegraph_item;
#else
typedef Scene_polyhedron_item Scene_facegraph_item;
#endif
typedef Scene_facegraph_item::Face_graph FaceGraph;
using namespace CGAL::Three;
class Polyhedron_demo_join_and_split_polyhedra_plugin:
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  QAction* actionJoinPolyhedra, *actionSplitPolyhedra, *actionColorConnectedComponents;
  Messages_interface* msg_interface;
public:
  QList<QAction*> actions() const { return QList<QAction*>() << actionJoinPolyhedra << actionSplitPolyhedra << actionColorConnectedComponents; }

  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface* m)
  {
    mw = mainWindow;
    scene = scene_interface;
    msg_interface = m;

    actionJoinPolyhedra= new QAction(tr("Join Selected Polyhedra"), mainWindow);
    actionJoinPolyhedra->setProperty("subMenuName", "Operations on Polyhedra");
    actionJoinPolyhedra->setObjectName("actionJoinPolyhedra");

    actionSplitPolyhedra= new QAction(tr("Split Selected Polyhedra"), mainWindow);
    actionSplitPolyhedra->setProperty("subMenuName", "Operations on Polyhedra");
    actionSplitPolyhedra->setObjectName("actionSplitPolyhedra");

    actionColorConnectedComponents = new QAction(tr("Color Each Connected Component"), mainWindow);
    actionColorConnectedComponents ->setProperty("subMenuName", "Polygon Mesh Processing");
    actionColorConnectedComponents->setObjectName("actionColorConnectedComponents");

    autoConnectActions();
  }

  bool applicable(QAction* a) const
  {
    Q_FOREACH(int index, scene->selectionIndices())
    {
      if (qobject_cast<Scene_facegraph_item*>(scene->item(index)))
        return true;
      else if (a == actionColorConnectedComponents
            && qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index)))
        return true;
    }
    return false;
  }

public Q_SLOTS:
  void on_actionJoinPolyhedra_triggered();
  void on_actionSplitPolyhedra_triggered();
  void on_actionColorConnectedComponents_triggered();

private :
  CGAL::Three::Scene_interface* scene;
  std::vector<QColor> colors_;
}; // end Polyhedron_demo_polyhedron_stitching_plugin

void Polyhedron_demo_join_and_split_polyhedra_plugin::on_actionJoinPolyhedra_triggered()
{
  CGAL::Three::Scene_interface::Item_id mainSelectionIndex
    = scene->selectionIndices().first();
  Scene_facegraph_item* mainSelectionItem
    = qobject_cast<Scene_facegraph_item*>(scene->item(mainSelectionIndex));
  if(!mainSelectionItem)
  {
    std::cerr<<"No selected polyhedron_item"<<std::endl;
    return;
  }
  QList<int> indices_to_remove;
  Q_FOREACH(int index, scene->selectionIndices()) {
    if (index == mainSelectionIndex)
      continue;

    Scene_facegraph_item* item =
      qobject_cast<Scene_facegraph_item*>(scene->item(index));
    if(item)
    {
      indices_to_remove.push_front(index);
      CGAL::copy_face_graph(*item->polyhedron(), *mainSelectionItem->polyhedron());
    }
    else
      std::cerr<<"No selected polyhedron_item"<<std::endl;
  }
  mainSelectionItem->invalidateOpenGLBuffers();
  scene->itemChanged(mainSelectionIndex);

  //remove the other items
  Q_FOREACH(int index, indices_to_remove)
  {
    scene->erase(index);
  }
}

struct Polyhedron_appender{
  Polyhedron_appender(std::list<FaceGraph*>& new_polyhedra):
    m_new_polyhedra(new_polyhedra) {}
  void operator()(const FaceGraph& p){
    m_new_polyhedra.push_back( new FaceGraph(p) );
  }
  std::list<FaceGraph*>& m_new_polyhedra;
};

void Polyhedron_demo_join_and_split_polyhedra_plugin::on_actionSplitPolyhedra_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices()) {
    colors_.clear();
    Scene_facegraph_item* item =
      qobject_cast<Scene_facegraph_item*>(scene->item(index));
    if(item)
    {
      QApplication::setOverrideCursor(Qt::WaitCursor);
      std::list<FaceGraph*> new_polyhedra;
      typedef boost::property_map<FaceGraph,CGAL::face_patch_id_t<int> >::type PatchIDMap;
      PatchIDMap pidmap = get(CGAL::face_patch_id_t<int>(), *item->face_graph());
      int nb_patches = CGAL::Polygon_mesh_processing::connected_components(*item->face_graph(),
                                                          pidmap);

      for(int i=0; i<nb_patches; ++i)
      {
        //std::vector<int> pids;
        //pids.push_back(i);
        CGAL::Face_filtered_graph<FaceGraph> filter_graph(*item->face_graph(), i, pidmap);
        FaceGraph* new_graph = new FaceGraph();
        CGAL::copy_face_graph(filter_graph, *new_graph);
        new_polyhedra.push_back(new_graph);
      }

      if (new_polyhedra.size()==1)
      {
        delete new_polyhedra.front();
        msg_interface->information( tr("%1 has only one connected component").arg(item->name()) );
        QApplication::restoreOverrideCursor();
        continue;
      }

      int cc=0;

      compute_color_map(item->color(), item->isItemMulticolor() ? static_cast<unsigned int>(new_polyhedra.size()) : 1,
                        std::back_inserter(colors_));

      Scene_group_item *group = new Scene_group_item("CC");
       scene->addItem(group);
      BOOST_FOREACH(FaceGraph* polyhedron_ptr, new_polyhedra)
      {
        Scene_facegraph_item* new_item=new Scene_facegraph_item(polyhedron_ptr);
        new_item->setName(tr("%1 - CC %2").arg(item->name()).arg(cc));
        new_item->setColor(colors_[item->isItemMulticolor()? cc : 0]);
        ++cc;
        scene->addItem(new_item);
        scene->changeGroup(new_item, group);
      }
      item->setVisible(false);
      QApplication::restoreOverrideCursor();
    }
  }
}

struct Polyhedron_cc_marker{
  int cc_index;
  Polyhedron_cc_marker() : cc_index(0) {}
  void start_new_connected_component(){
    ++cc_index;
  }

  template <class Facet_iterator>
  void mark(Facet_iterator begin, Facet_iterator end)
  {
    for(;begin!=end; ++begin)
      (*begin)->set_patch_id(cc_index-1);
  }
};

void Polyhedron_demo_join_and_split_polyhedra_plugin::on_actionColorConnectedComponents_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);

  std::set<Scene_facegraph_item*> to_skip;

  Q_FOREACH(int index, scene->selectionIndices())
  {
    Scene_facegraph_item* item =
      qobject_cast<Scene_facegraph_item*>(scene->item(index));
    if(item && to_skip.find(item) == to_skip.end())
    {
      item->setItemIsMulticolor(true);
      typedef boost::property_map<FaceGraph,CGAL::face_patch_id_t<int> >::type PatchIDMap;
      PatchIDMap pidmap = get(CGAL::face_patch_id_t<int>(), *item->face_graph());
      CGAL::Polygon_mesh_processing::connected_components(*item->face_graph(),
                                                          pidmap);

      item->invalidateOpenGLBuffers();
      scene->itemChanged(item);
    }
    else
    {
      Scene_polyhedron_selection_item* selection_item =
        qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index));

      if (selection_item)
      {
        namespace PMP = CGAL::Polygon_mesh_processing;
        typedef boost::graph_traits<FaceGraph>::face_descriptor   face_descriptor;

        selection_item->polyhedron_item()->setItemIsMulticolor(true);
#ifndef USE_SURFACE_MESH
        selection_item->polyhedron_item()->set_color_vector_read_only(false);
#endif
        FaceGraph& pmesh = *(selection_item->polyhedron());

        boost::property_map<FaceGraph, boost::face_index_t>::type fim
          = get(boost::face_index, pmesh);
        boost::vector_property_map<int,
          boost::property_map<FaceGraph, boost::face_index_t>::type>
          fccmap(fim);
        boost::property_map<FaceGraph, CGAL::face_patch_id_t<int> >::type pid
          = get(CGAL::face_patch_id_t<int>(), pmesh);

        std::cout << "color CC" << std::endl;

        PMP::connected_components(pmesh
          , fccmap
          , PMP::parameters::edge_is_constrained_map(selection_item->constrained_edges_pmap())
          .face_index_map(fim));

        BOOST_FOREACH(face_descriptor f, faces(pmesh))
        {
          put(pid, f, fccmap[f]);
        }

        to_skip.insert(selection_item->polyhedron_item());

        selection_item->changed_with_poly_item();
      }
    }

    // default cursor
    QApplication::restoreOverrideCursor();
  }
}

#include "Join_and_split_polyhedra_plugin.moc"


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Stanford University
*  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 Rice University 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 THE
*  COPYRIGHT OWNER OR CONTRIBUTORS 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.
*********************************************************************/

/* Author: Javier V. Gomez */

#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/fmt/FMT.h>
#include <ompl/util/PPM.h>
#include <ompl/base/PredefinedStateSampler.h>

#include <ompl/config.h>
#include <../tests/resources/config.h>

#include <boost/filesystem.hpp>
#include <iostream>
#include <fstream>

namespace ob = ompl::base;
namespace og = ompl::geometric;

bool heuristics = false;
bool nearestK = true;
bool cacheCC = false;

struct Path2D
{
    std::vector<ob::State*> spath_;
    double time_, cost_;
};

struct Config
{
    double max_time_;
    std::size_t exp_;
    std::vector<std::size_t> n_samples_;
};

void preSampleFree(const ob::SpaceInformationPtr si, const size_t n, std::vector<const ob::State*> &states)
{
    size_t statesSampled = 0;
    ob::StateSamplerPtr sampler = si->allocStateSampler();
    states.reserve(n);
    ob::State* s = si->allocState();

    while (statesSampled < n)
    {
        sampler->sampleUniform(s);
        bool collision_free = si->isValid(s);
        if (collision_free)
        {
            ++statesSampled;
            states.push_back(s);
            s = si->allocState();
        } // If collision free
    } // While statesSampled < n
}

struct PredefinedStateSamplerAllocator
{
    PredefinedStateSamplerAllocator(std::vector<const ob::State*> states) : states_(states) {}

    PredefinedStateSamplerAllocator(const char *filename)
    {
        ob::RealVectorStateSpace ss(2);
        std::ifstream ifs;
        ifs.open(filename);

        std::cout << std::fixed;

        if (ifs.is_open())
        {
            double x,y;
            while(ifs.good())
            {
                ifs >> x >> y;
                ob::State* s = ss.allocState();
                s->as<ob::RealVectorStateSpace::StateType>()->values[0] = x;
                s->as<ob::RealVectorStateSpace::StateType>()->values[1] = y;
                states_.push_back(s);
            }
        }
        else
            std::cout << "Error opening file " << filename << "!" << std::endl;
        ifs.close();
    }

    ob::StateSamplerPtr operator()(const ob::StateSpace* space)
    {
        return ob::StateSamplerPtr(new ob::PredefinedStateSampler(space, states_));
    }

    void saveSamples(const char *filename)
    {
        std::ofstream ofs;
        ofs.open(filename, std::ofstream::trunc);
        ofs << std::fixed;

        for (std::size_t i = 0; i < states_.size(); ++i)
        {
            ofs << states_[i]->as<ob::RealVectorStateSpace::StateType>()->values[0] << "\t"
                << states_[i]->as<ob::RealVectorStateSpace::StateType>()->values[1];

            if (i != states_.size()-1)
                ofs << std::endl;
        }
        ofs.close();
    }

    std::vector<const ob::State*> states_;
};

void loadData(const char *filename, std::vector<Path2D> &paths, Config &config)
{
    ob::RealVectorStateSpace ss(2);

    std::ifstream ifs;
    ifs.open(filename);
    if (ifs.is_open())
    {
        // Reading header of the results file.
        ifs >> config.exp_ >> config.max_time_;
        double val;
        for (unsigned int i = 0; i < config.exp_; ++i)
        {
            ifs >> val;
            config.n_samples_.push_back(val);
        }

        // Reading paths.
        double x,y;
        for (unsigned int i = 0; i < config.exp_; ++i)
        {
            Path2D path;
            std::size_t n_states;
            ifs >> path.cost_ >> path.time_ >> n_states;
            // Reading states of each path.
            for (std::size_t j = 0; j < n_states; ++j)
            {
                ob::State *s = ss.allocState();
                ifs >> x >> y;
                s->as<ob::RealVectorStateSpace::StateType>()->values[0] = x;
                s->as<ob::RealVectorStateSpace::StateType>()->values[1] = y;
                path.spath_.push_back(s);
            }
            paths.push_back(path);
        }
    }
    else
        std::cout << "Error opening file " << filename << "!" << std::endl;
}

std::size_t differentPaths(const std::vector<ob::State*> p1, const std::vector<ob::State*> p2)
{
    std::size_t diff = 0;

    if (p1.size() != p2.size())
        return abs(p1.size() - p2.size());
    else
    {
        ob::RealVectorStateSpace ss(2);
        for (std::size_t i = 0; i < p1.size(); ++i)
        {
            if (!ss.equalStates(p1[i], p2[i]))
                ++diff;
        }
    }

    return diff;
}

template<typename planner_t>
class Test2D
{
public:

    Test2D(const char *ppm_file)
    {
        bool ok = false;
        try
        {
            ppm_.loadFile(ppm_file);
            ok = true;
        }
        catch(ompl::Exception &ex)
        {
            OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
        }
        if (ok)
        {
            ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace();
            space->addDimension(0.0, ppm_.getWidth());
            space->addDimension(0.0, ppm_.getHeight());
            maxWidth_ = ppm_.getWidth() - 1;
            maxHeight_ = ppm_.getHeight() - 1;
            ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));

            // set state validity checking for this space
            ss_->setStateValidityChecker(boost::bind(&Test2D::isStateValid, this, _1));
            space->setup();
            ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());

            // configuring underlying planner
            ss_->setPlanner(ob::PlannerPtr( new planner_t(ss_->getSpaceInformation()) ));
        }
    }

    bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col, double max_time)
    {
        if (!ss_)
            return false;
        ob::ScopedState<> start(ss_->getStateSpace());
        start[0] = start_row;
        start[1] = start_col;
        ob::ScopedState<> goal(ss_->getStateSpace());
        goal[0] = goal_row;
        goal[1] = goal_col;
        ss_->setStartAndGoalStates(start, goal);
        ss_->solve(max_time);

        if (ss_->haveSolutionPath())
            return true;
        else
            return false;
    }

    const ob::SpaceInformationPtr getSpaceInformation() const
   {
       return ss_->getSpaceInformation();
   }

    const og::SimpleSetupPtr getSetup() const
   {
       return ss_;
   }

    const ob::PlannerPtr getPlanner() const
    {
        return ss_->getPlanner();
    }

    void setStateSamplerAllocator(const ob::StateSamplerAllocator ssa)
    {
        ss_->getStateSpace()->setStateSamplerAllocator(ssa);
    }

private:

    bool isStateValid(const ob::State *state) const
    {
        const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_);
        const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_);

        const ompl::PPM::Color &c = ppm_.getPixel(h, w);
        return c.red > 127 && c.green > 127 && c.blue > 127;
    }

    og::SimpleSetupPtr ss_;
    int maxWidth_;
    int maxHeight_;
    ompl::PPM ppm_;
};

int main(int argc, char ** argv)
{
    boost::filesystem::path path(TEST_RESOURCES_DIR);

    // Reading states to use in the test.
    PredefinedStateSamplerAllocator pssa("fmttest_samples_k.txt");

    if (pssa.states_.empty())
        return 1;

    // Reading paths and configuration.
    Config config;
    std::vector<Path2D> paths;
    loadData("fmttest_paths_k.txt", paths, config);

    if (paths.empty())
        return 1;

    // Executing sampler to test and caching results.
    std::vector<Path2D> result_paths;
    result_paths.reserve(config.exp_);
    for (size_t i = 0; i < config.exp_; ++i)
    {
        Test2D<og::FMT> test_planner((path / "ppm/floor.ppm").string().c_str());

        test_planner.getPlanner()->as<og::FMT>()->setNumSamples(config.n_samples_[i]);
        test_planner.getPlanner()->as<og::FMT>()->setNearestK(nearestK);
        //test_planner.getPlanner()->as<og::FMT>()->setHeuristics(heuristics);
        //test_planner.getPlanner()->as<og::FMT>()->setCacheCC(cacheCC);
        test_planner.setStateSamplerAllocator(pssa);

        if (test_planner.plan(0,0, 1859, 1568, config.max_time_)
            && test_planner.getSetup()->haveExactSolutionPath())
        {
            Path2D path;
            std::vector<ob::State*> spath = test_planner.getSetup()->getSolutionPath().getStates();
            // A deep copy of the states is required.
            for(std::size_t j = 0; j < spath.size(); ++j)
                path.spath_.push_back(test_planner.getSpaceInformation()->cloneState(spath[j]));

            path.cost_ = test_planner.getSetup()->getSolutionPath().length();
            path.time_ = test_planner.getSetup()->getLastPlanComputationTime();
            result_paths.push_back(path);
        }
    }

    // Analyzing results.
    std::cout << std::endl << "***** ANALYZING RESULTS *****" << std::endl << std::endl;
    bool warn = false;
    for (size_t i = 0; i < config.exp_; ++i)
    {
        bool local_warn = false;
        std::cout << "[TEST " << i << "] ------------------- "<< std::endl;
        if (abs(paths[i].cost_ - result_paths[i].cost_) > std::numeric_limits<double>::epsilon())
        {
            warn = true;
            local_warn = true;
            std::cout << "[WARNING]: Path costs are different:" << std::endl;
            std::cout << "\t" << "Exp: " << i << std::endl;
            std::cout << "\t" << "N: " << config.n_samples_[i] << std::endl;
            std::cout << "\t" << "Real cost: " << paths[i].cost_ << std::endl;
            std::cout << "\t" << "Test cost: " << result_paths[i].cost_ << std::endl;
            std::cout << "\t" << "Ratio test/real: " << (double)result_paths[i].cost_ / paths[i].cost_ << std::endl << std::endl;
        }

        if ((paths[i].time_/result_paths[i].time_) < 0.5 || (result_paths[i].time_/paths[i].time_) < 0.5 )
        {
            warn = true;
            local_warn = true;
            std::cout << "[WARNING]: Computation times differ too much:" << std::endl;
            std::cout << "\t" << "Exp: " << i << std::endl;
            std::cout << "\t" << "N: " << config.n_samples_[i] << std::endl;
            std::cout << "\t" << "Real time: " << paths[i].time_ << std::endl;
            std::cout << "\t" << "Test time: " << result_paths[i].time_ << std::endl;
            std::cout << "\t" << "Ratio test/real: " << (double)result_paths[i].time_ / paths[i].time_ << std::endl << std::endl;
        }

        std::size_t diff_states = differentPaths(paths[i].spath_, result_paths[i].spath_);
        if (diff_states)
        {
            warn = true;
            local_warn = true;
            std::cout << "[WARNING]: Paths have " <<diff_states << " different state." << std::endl;
            std::cout << "\t" << "Exp: " << i << std::endl;
            std::cout << "\t" << "N: " << config.n_samples_[i] << std::endl << std::endl;
        }

        if (!local_warn)
            std::cout << "[TEST OK]" << std::endl << std::endl;
    }

    if (warn)
    {
        std::cout << "THERE WERE WARNINGS! Test NOT successfull." << std::endl;
        return 1;
    }
    else
    {
        std::cout << "*** Test OK! ***" << std::endl;
        return 0;
    }
}


#pragma once

#include <type_traits>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>

#include "../util/assert.hh"

namespace bold
{
  typedef unsigned char uchar;

  /** Models a integral image, also known as a summed area table.
   *
   * Used in some kinds of object recognition.
   *
   * See https://en.wikipedia.org/wiki/Integral_image
   */
  class IntegralImage
  {
  public:
    /** Creates an IntegralImage instance from the specified image.
     *
     * Linear time.
     *
     * @param image Must be a CV_8UC1 image (8bpp)
     */
    static IntegralImage create(cv::Mat const& image);

    IntegralImage(cv::Mat data)
    : d_mat(data)
    {
      ASSERT(data.type() == CV_32SC1 && "Image must be of type CV_32SC1");
    }

    /** Lookup the sum of intensities between the min/max coordinates, inclusive.
     *
     * Constant time.
     */
    int getSummedArea(Eigen::Vector2i min, Eigen::Vector2i max) const;

    /** Return the sum of intensities from (0,0) to the specified coordinates, inclusive.
     */
    int at(unsigned x, unsigned y) const { return d_mat.at<int>(y, x); }

  private:
    cv::Mat d_mat;
  };
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file GaussNewtonSolverBase.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_GAUSS_NEWTON_SOLVER_HPP
#define STEAM_GAUSS_NEWTON_SOLVER_HPP

#include <Eigen/Core>

#include <steam/solver/SolverBase.hpp>

#include <steam/state/StateVector.hpp>

#include <steam/blockmat/BlockSparseMatrix.hpp>
#include <steam/blockmat/BlockMatrix.hpp>
#include <steam/blockmat/BlockVector.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Reports that the decomposition failed. This is due to poor conditioning, or
///        possibly that the matrix is not positive definite.
//////////////////////////////////////////////////////////////////////////////////////////////
class decomp_failure : public solver_failure
{
public:
  decomp_failure(const std::string& s) : solver_failure(s) {}
};

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Intermediate solver interface that implements Gauss-Newton related solves
//////////////////////////////////////////////////////////////////////////////////////////////
class GaussNewtonSolverBase : public SolverBase
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  GaussNewtonSolverBase(OptimizationProblem* problem);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Query the covariance related to a single state variable
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::MatrixXd queryCovariance(const steam::StateKey& key);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Query the covariance relating two state variables
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::MatrixXd queryCovariance(const steam::StateKey& rowKey, const steam::StateKey& colKey);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Query a block of covariances
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrix queryCovarianceBlock(const std::vector<steam::StateKey>& keys);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Query a block of covariances
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrix queryCovarianceBlock(const std::vector<steam::StateKey>& rowKeys,
                                   const std::vector<steam::StateKey>& colKeys);

 protected:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Build the left-hand and right-hand sides of the Gauss-Newton system of equations
  //////////////////////////////////////////////////////////////////////////////////////////////
  void buildGaussNewtonTerms(Eigen::SparseMatrix<double>* approximateHessian,
                             Eigen::VectorXd* gradientVector);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Perform the LLT decomposition on the approx. Hessian matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  void factorizeHessian(const Eigen::SparseMatrix<double>& approximateHessian,
                        bool augmentedHessian = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Solve the Gauss-Newton system of equations: A*x = b
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::VectorXd solveGaussNewton(const Eigen::SparseMatrix<double>& approximateHessian,
                                   const Eigen::VectorXd& gradientVector,
                                   bool augmentedHessian = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Find the Cauchy point (used for the Dogleg method).
  ///        The cauchy point is the optimal step length in the gradient descent direction.
  //////////////////////////////////////////////////////////////////////////////////////////////
  static Eigen::VectorXd getCauchyPoint(const Eigen::SparseMatrix<double>& approximateHessian,
                                        const Eigen::VectorXd& gradientVector);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the predicted cost reduction based on the proposed step
  //////////////////////////////////////////////////////////////////////////////////////////////
  static double predictedReduction(const Eigen::SparseMatrix<double>& approximateHessian,
                                   const Eigen::VectorXd& gradientVector,
                                   const Eigen::VectorXd& step);

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Build the system, solve for a step size and direction, and update the state
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual bool linearizeSolveAndUpdate(double* newCost, double* gradNorm) = 0;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief The solver object (stored over iterations to reuse the same pattern)
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::SimplicialLLT<Eigen::SparseMatrix<double>, Eigen::Upper> hessianSolver_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Whether or not the pattern of the approx. Hessian has been analyzed by the solver
  //////////////////////////////////////////////////////////////////////////////////////////////
  bool patternInitialized_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Whether or not the last factorization was for the information matrix and if
  ///        if it was successful. *Note that solving LM does not solve the information matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  bool factorizedInformationSuccesfully_;
};

} // steam

#endif // STEAM_GAUSS_NEWTON_SOLVER_HPP


/**
 * @file BoostPolygonRegister.hpp
 * @author bwu
 * @brief Adaption of the generic geometry models to boost polygon concept
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_BOOSTPOLYGONREGISTER_HPP
#define GENERIC_GEOMETRY_BOOSTPOLYGONREGISTER_HPP
#include "Geometries.hpp"
#include <boost/polygon/polygon.hpp>
#include <type_traits>
namespace boost {
namespace polygon {

using namespace generic::geometry;
//point
template <typename num_type>
struct geometry_concept<Point2D<num_type> >
{
    using type = point_concept;
};

template <typename num_type>
struct point_traits<Point2D<num_type> >
{
    using coordinate_type = num_type ;
    static inline coordinate_type get(const Point2D<num_type> & point, orientation_2d orient)
    {
        if(orient == HORIZONTAL) return point[0];
        else return point[1];
    }
};

template <typename num_type>
struct point_mutable_traits<Point2D<num_type> >
{
    using coordinate_type = num_type ;
    static inline void set(Point2D<num_type> & point, orientation_2d orient, num_type value)
    {
        if(orient == HORIZONTAL) point[0] = value;
        else point[1] = value;
    }

    static inline Point2D<num_type> construct(num_type x, num_type y)
    {
        return Point2D<num_type>(x, y);
    }

    static inline Point2D<num_type> construct(const point_data<num_type> & p)
    {
        return Point2D<num_type>(p.get(HORIZONTAL), p.get(VERTICAL));
    }
};

//segment
template <typename num_type>
struct geometry_concept<Segment2D<num_type> >
{
    using type = segment_concept;
};

template <typename num_type>
struct segment_traits<Segment2D<num_type> >
{
    using coordinate_type = num_type ;
    using point_type = Point2D<num_type> ;

    static inline point_type get(const Segment2D<num_type> & segment, direction_1d dir)
    {
        if(dir == LOW) return segment[0];
        return segment[1];
    }
};

template <typename num_type>
struct segment_mutable_traits<Segment2D<num_type> > {
    using coordinate_type = num_type;
    using point_type = Point2D<num_type>;

    static inline void set(Segment2D<num_type> & segment, direction_1d dir, const point_type & point)
    {
        if(dir == LOW) segment[0] = point;
        else segment[1] = point;
    }

    static inline Segment2D<num_type> construct(const point_type & low, const point_type & high)
    {
        return Segment2D<num_type>(low, high);
    }

    static inline Segment2D<num_type> construct(const point_data<num_type> & low, const point_data<num_type> & high)
    {
        Point2D<num_type> p1(low.get(HORIZONTAL), low.get(VERTICAL));
        Point2D<num_type> p2(high.get(HORIZONTAL), high.get(VERTICAL));
        return Segment2D<num_type>(p1, p2);
    }
};

//triangle
template <typename num_type>
struct geometry_concept<Triangle2D<num_type> >
{
    using type = polygon_concept;
};

template <typename num_type>
struct polygon_traits_general<Triangle2D<num_type> >
{
    using coordinate_type = num_type;
    using point_type = Point2D<num_type> ;
    using iterator_type = typename Triangle2D<num_type>::const_iterator;

    static inline iterator_type begin_points(const Triangle2D<num_type> & triangle)
    {
        return triangle.ConstBegin();
    }

    static inline iterator_type end_points(const Triangle2D<num_type> & triangle)
    {
        return triangle.ConstEnd();
    }

    static inline std::size_t size(const Triangle2D<num_type> & triangle)
    {
        return 3;
    }

    static inline winding_direction winding(const Triangle2D<num_type> & triangle)
    {
        if(triangle.isCCW()) return counterclockwise_winding;
        else return clockwise_winding;
    }
};

//box
template <typename num_type>
struct geometry_concept<Box2D<num_type> >
{
    using type = rectangle_concept;
};

template <typename num_type>
struct rectangle_traits<Box2D<num_type> >
{
    using coordinate_type = num_type;
    using interval_type = interval_data<num_type>;
    static inline interval_type get(const Box2D<num_type> & box, orientation_2d orient)
    {
        if(orient == HORIZONTAL) return interval_type(box[0][0], box[1][0]);
        else return interval_type(box[0][1], box[1][1]);
    }
};

template <typename num_type>
struct rectangle_mutable_traits<Box2D<num_type> >
{
    using interval_type = interval_data<num_type>;
    static inline void set(Box2D<num_type> & box, orientation_2d orient, const interval_type & interval)
    {
        if (orient == HORIZONTAL) {
            box[0][0] = interval.low();
            box[1][0] = interval.high();
        }
        else {
            box[0][1] = interval.low();
            box[1][1] = interval.high();
        }
    }

    static inline Box2D<num_type> construct(const interval_type & interval_horizontal, const interval_type & interval_vertical)
    {
        return Box2D<num_type>(interval_horizontal.low(), interval_vertical.low(), interval_horizontal.high(), interval_vertical.high());
    }
};

//polygon
template <typename num_type>
struct geometry_concept<Polygon2D<num_type> >
{
    using type = polygon_concept ;
};

template <typename num_type>
struct polygon_traits_general<Polygon2D<num_type> >
{
    using coordinate_type = typename Polygon2D<num_type>::coor_t;
    using point_type = typename Polygon2D<num_type>::point_t;
    using iterator_type = typename Polygon2D<num_type>::const_point_iterator;
    static inline iterator_type begin_points(const Polygon2D<num_type> & polygon)
    {
        return polygon.ConstBegin();
    }

    static inline iterator_type end_points(const Polygon2D<num_type> & polygon)
    {
        return polygon.ConstEnd();
    }

    static inline std::size_t size(const Polygon2D<num_type> & polygon)
    {
        return polygon.Size();
    }

    static inline winding_direction winding(const Polygon2D<num_type> &)
    {
        return unknown_winding;
    }
};

template <typename num_type>
struct polygon_mutable_traits<Polygon2D<num_type> >
{
    template <typename iterator, typename std::enable_if<std::is_same<
              typename std::iterator_traits<iterator>::value_type, point_data<num_type> >::value, bool>::type = true>
    static inline Polygon2D<num_type> & set_points(Polygon2D<num_type> & polygon,
                                                    iterator input_begin, iterator input_end)
    {
        polygon.Clear();
        polygon.GetPoints().reserve(std::distance(input_begin, input_end));
        for(auto iter = input_begin; iter != input_end; ++iter)
            polygon << point_mutable_traits<Point2D<num_type> >::construct(*iter);
        return polygon;
    }

    template <typename iterator, typename std::enable_if<std::is_same<
              typename std::iterator_traits<iterator>::value_type, Point2D<num_type> >::value, bool>::type = true>
    static inline Polygon2D<num_type> & set_points(Polygon2D<num_type> & polygon,
                                                    iterator input_begin, iterator input_end)
    {
        polygon.Clear();
        polygon.Insert(polygon.End(), input_begin, input_end);
        return polygon;
    }
};

//polygon with holes
template <typename num_type>
struct geometry_concept<PolygonWithHoles2D<num_type> >
{
    using type = polygon_with_holes_concept ;
};

template <typename num_type>
struct polygon_traits_general<PolygonWithHoles2D<num_type> >
{
    using coordinate_type = typename PolygonWithHoles2D<num_type>::coor_t;
    using point_type = typename PolygonWithHoles2D<num_type>::point_t;
    using iterator_type = typename PolygonWithHoles2D<num_type>::const_point_iterator;
    static inline iterator_type begin_points(const PolygonWithHoles2D<num_type> & pwh)
    {
        return pwh.outline.ConstBegin();
    }

    static inline iterator_type end_points(const PolygonWithHoles2D<num_type> & pwh)
    {
        return pwh.outline.ConstEnd();
    }

    static inline std::size_t size(const PolygonWithHoles2D<num_type> & pwh)
    {
        return pwh.outline.Size();
    }

    static inline winding_direction winding(const PolygonWithHoles2D<num_type> &)
    {
        return unknown_winding;
    }
};

template <typename num_type>
struct polygon_mutable_traits<PolygonWithHoles2D<num_type> >
{
    template <typename iterator>
    static inline PolygonWithHoles2D<num_type> & set_points(PolygonWithHoles2D<num_type> & pwh,
                                                    iterator input_begin, iterator input_end)
    {
        pwh.outline.Clear();
        pwh.outline.Insert(pwh.outline.End(), input_begin, input_end);
        return pwh;
    }
};

template <typename num_type>
struct polygon_with_holes_traits<PolygonWithHoles2D<num_type> >
{
    using hole_type = typename PolygonWithHoles2D<num_type>::hole_type;
    using iterator_holes_type = typename PolygonWithHoles2D<num_type>::const_hole_iterator;

    static inline iterator_holes_type begin_holes(const PolygonWithHoles2D<num_type> & pwh)
    {
        return pwh.ConstBeginHoles();
    }

    static inline iterator_holes_type end_holes(const PolygonWithHoles2D<num_type> & pwh)
    {
        return pwh.ConstEndHoles();
    }

    static inline std::size_t size_holes(const PolygonWithHoles2D<num_type> & pwh)
    {
        return pwh.HolesSize();
    }
};

template <typename num_type>
struct polygon_with_holes_mutable_traits<PolygonWithHoles2D<num_type> >
{
    template <typename iterator>
    static inline PolygonWithHoles2D<num_type> & set_holes(PolygonWithHoles2D<num_type> & pwh,
                                                                iterator input_begin, iterator input_end)
    {
        pwh.holes.clear();
        pwh.holes.insert(pwh.holes.end(), input_begin, input_end);
        return pwh;
    }
};

}//namespace polygon
}//namespace boost

#endif//GENERIC_GEOMETRY_BOOSTPOLYGONREGISTER_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_IDENTITY2D_INCLUDE
#define MTL_MATRIX_IDENTITY2D_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/vector/mat_cvec_multiplier.hpp>

namespace mtl { namespace mat {

/// Matrix-free linear operator for identity
struct identity2D
{
    /// Constructor for \p m by \p m matrix
    identity2D(std::size_t m) : m(m), n(m) {}

    /// Constructor for \p m by \p n matrix
    identity2D(std::size_t m, std::size_t n) : m(m), n(n) {}

    /// Member function that realizes the multiplication
    template <typename VectorIn, typename VectorOut, typename Assign>
    void mult(const VectorIn& v, VectorOut& w, Assign) const
    {
	MTL_DEBUG_THROW_IF(std::size_t(size(v)) != n, incompatible_size());
	MTL_DEBUG_THROW_IF(size(w) != 0 && std::size_t(size(w)) != m, incompatible_size());

	if (size(w) == 0)
	    w.change_dim(m);

	if (m == n)
	    Assign::first_update(w, v);
	else if (m < n)
	    Assign::first_update(w, v[irange(m)]);
	else {
	    VectorOut w1(w[irange(n)]), w2(w[irange(n, imax)]);
	    Assign::first_update(w1, v);
	    Assign::init(w2);
	}
    }

    /// Multiplication is procastinated until we know where the product goes
    template <typename VectorIn>
    vec::mat_cvec_multiplier<identity2D, VectorIn> operator*(const VectorIn& v) const
    {	return vec::mat_cvec_multiplier<identity2D, VectorIn>(*this, v);    }

    std::size_t m, n;
};

inline std::size_t size(const identity2D& A) { return A.m * A.n; } ///< Matrix size
inline std::size_t num_rows(const identity2D& A) { return A.m; } ///< Number of rows
inline std::size_t num_cols(const identity2D& A) { return A.n; } ///< Number of columns

}} // namespace mtl::matrix

namespace mtl {

    template <>
    struct Collection<mat::identity2D>
    {
	typedef double         value_type;
	typedef std::size_t    size_type;
    };

    // namespace ashape {
    // 	template <> struct ashape_aux<mtl::mat::identity2D>
    // 	{	typedef nonscal type;    };
    // }
}

#endif // MTL_MATRIX_IDENTITY2D_INCLUDE


#include <phpcpp.h>

#include <iostream>
#include <Eigen/Dense>

#include "EigenPhp.h"

//using namespace std;

bool EigenPhp::set_matd(Eigen::MatrixXd mat)
{
	this->matd = mat;
	this->val_type = EigenPhp::MAT_DOUBLE;

	return true;
}

bool EigenPhp::set_mati(Eigen::MatrixXi mat)
{
	this->mati = mat;
	this->val_type = EigenPhp::MAT_INTEGER;

	return true;
}

Php::Value EigenPhp::matd_to_zarr()
{
	Php::Value arr;
	int j, k;

	for(j = 0; j < this->matd.rows(); j++)
	{
		arr[j] = Php::Array();
		for(k = 0; k < this->matd.cols(); k++)
		{
			arr[j][k] = (double) this->matd(j, k);
		}
	}

	return arr;
}

Php::Value EigenPhp::mati_to_zarr()
{
	Php::Value arr;
	int j, k;

	for(j = 0; j < this->mati.rows(); j++)
	{
		arr[j] = Php::Array();
		for(k = 0; k < this->mati.cols(); k++)
		{
			arr[j][k] = (int) this->mati(j, k);
		}
	}

	return arr;
}

Php::Value EigenPhp::__toString()
{
	std::stringstream buffer;
	if (val_type == EigenPhp::MAT_DOUBLE)
		buffer << this->matd << std::endl;
	else
		buffer << this->mati << std::endl;
	return buffer.str();
}

Php::Value EigenPhp::random_d(Php::Parameters &params)
{
	int rows = (int) params[0];
	int cols = (int) params[1];

	this->set_matd(Eigen::MatrixXd::Random(rows, cols));
	return this->matd_to_zarr();
}

Php::Value EigenPhp::random_i(Php::Parameters &params)
{
	int rows = (int) params[0];
	int cols = (int) params[1];

	this->set_mati(Eigen::MatrixXi::Random(rows, cols));
	return this->mati_to_zarr();
}

Php::Value EigenPhp::constant_i(Php::Parameters &params)
{
	int rows = (int) params[0];
	int cols = (int) params[1];
	int val = (int) params[2];

	this->set_mati(Eigen::MatrixXi::Constant(rows, cols, val));
	return this->mati_to_zarr();
}

/**
 *  tell the compiler that the get_module is a pure C function
 */
extern "C" {

    /**
     * Function that is called by PHP right after the PHP process
     * has started, and that returns an address of an internal PHP
     * strucure with all the details and features of your extension
     *
     * @return void* a pointer to an address that is understood by PHP
     */
    PHPCPP_EXPORT void *get_module()
    {
        // static(!) Php::Extension object that should stay in memory
        // for the entire duration of the process
        static Php::Extension extension("eigen_extension", "1.0");

        Php::Class<EigenPhp> eigen_php("EigenPhp");

        eigen_php.method<&EigenPhp::random_d> ("random_d", {
		Php::ByVal("a", Php::Type::Numeric),
		Php::ByVal("b", Php::Type::Numeric)
	});
        eigen_php.method<&EigenPhp::random_i> ("random_i", {
		Php::ByVal("a", Php::Type::Numeric),
		Php::ByVal("b", Php::Type::Numeric)
	});
        eigen_php.method<&EigenPhp::constant_i> ("constant_i", {
		Php::ByVal("a", Php::Type::Numeric),
		Php::ByVal("b", Php::Type::Numeric),
		Php::ByVal("c", Php::Type::Numeric)
	});

        extension.add(std::move(eigen_php));

        return(extension);
    }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2012 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file fdmtimedepdirichletboundary.hpp
    \brief time dependent Dirichlet boundary conditions
*/

#ifndef quantlib_fdm_time_dep_dirichlet_boundary_hpp
#define quantlib_fdm_time_dep_dirichlet_boundary_hpp

#include <ql/methods/finitedifferences/boundarycondition.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearop.hpp>

#include <boost/function.hpp>

namespace QuantLib {

    class FdmMesher;
    class FdmLinearOpLayout;

    class FdmTimeDepDirichletBoundary : public BoundaryCondition<FdmLinearOp> {
      public:
        // types and enumerations
        typedef FdmLinearOp operator_type;
        typedef FdmLinearOp::array_type array_type;
        typedef BoundaryCondition<FdmLinearOp>::Side Side;

        FdmTimeDepDirichletBoundary(
            const boost::shared_ptr<FdmMesher>& mesher,
            const boost::function<Real (Real)>& valueOnBoundary,
            Size direction, Side side);

        FdmTimeDepDirichletBoundary(
            const boost::shared_ptr<FdmMesher>& mesher,
            const boost::function<Disposable<Array> (Real)>& valueOnBoundary,
            Size direction, Side side);

        void setTime(Time);
        void applyBeforeApplying(operator_type&) const {}
        void applyBeforeSolving(operator_type&, array_type&) const {}
        void applyAfterApplying(array_type&) const;
        void applyAfterSolving(array_type&) const;

      private:
        const std::vector<Size> indices_;
        const boost::function<Real (Real)> valueOnBoundary_;
        const boost::function<Disposable<Array>(Real)> valuesOnBoundary_;

        Array values_;
    };
}

#endif