#include <HMMlib/hmm_table.hpp>
#include <HMMlib/hmm_vector.hpp>
#include <HMMlib/hmm.hpp>

#include <iostream>
#include <stdio.h>
#include <string.h>
#include <vector>
#include <stdlib.h>

#include "KFoldCrossValidation.h"

#include <dlib/clustering.h>
#include <dlib/rand.h>

#include "Model/GestureFinger.h"
#include "Model/GestureFrame.h"
#include "Model/GestureHand.h"
#include "Model/Vertex.h"

#include "StorageDriver/BinaryFileStorageDriver.h"
#include "StorageDriver/GestureStorageDriver.h"

#include "PathUtil.h"

#include "preprocess/LMpre.h"

#include "HMM/HMMClass.h"

using namespace hmmlib;
using namespace std;

#define MAX_FINGER_COUNT 5

// Should return a list of centroids (multiple per each class)
void kmeans(std::vector<std::vector<double> > data, int classCounter = 13,
		int maxCentroidsPerOneClass = 8, double kernelParam = 0.1, double error = 0.01) {
	typedef dlib::matrix<double> sample_type;
	typedef dlib::radial_basis_kernel<sample_type> kernel_type;

	// The first argument to the constructor is the kernel we wish to
	// use.
	// The second is a parameter that determines the numerical accuracy with which
	// the object will perform part of the learning algorithm.  Generally, smaller values
	// give better results but cause the algorithm to attempt to use more dictionary vectors
	// (and thus run slower and use more memory).
	// The third argument, however, is the
	// maximum number of dictionary vectors a kcentroid is allowed to use.  So you can use
	// it to control the runtime complexity.
	dlib::kcentroid<kernel_type> kc(kernel_type(kernelParam), error, maxCentroidsPerOneClass);

	// Now we make an instance of the kkmeans object and tell it to use kcentroid objects
	// that are configured with the parameters from the kc object we defined above.
	dlib::kkmeans<kernel_type> kmeans(kc);

	std::vector<sample_type> samples;
	std::vector<sample_type> initial_centers;

	// Copying to compatible type
	for (int i = 0; i < data.size(); i++) {
		sample_type m;
		m.set_size(data[i].size(),1);

		for (int j = 0; j < data[i].size(); j++) {
			m(j) = data[i][j];
		}
		samples.push_back(m);
	}

	// tell the kkmeans object we made that we want to run k-means with k set to 13.
	// (i.e. we want 13 clusters)
	kmeans.set_number_of_centers(classCounter);

	// You need to pick some initial centers for the k-means algorithm.  So here
	// we will use the dlib::pick_initial_centers() function which tries to find
	// n points that are far apart (basically).
	pick_initial_centers(classCounter, initial_centers, samples, kmeans.get_kernel());

	// now run the k-means algorithm on our set of samples.
	kmeans.train(samples, initial_centers);

	// To test one example:
	// label = test(samples[i])

	// Now print out how many dictionary vectors each center used.  Note that
	// the maximum number of 8 was reached.  If you went back to the kcentroid
	// constructor and changed the 8 to some bigger number you would see that these
	// numbers would go up.  However, 8 is all we need to correctly cluster this dataset.
	for (int i = 0; i < classCounter; i++) {
		cout << "num dictionary vectors for center " << i<<" : "
				<< kmeans.get_kcentroid(i).dictionary_size() << endl;
	}
}

// Should return a list of centroids (1 per class)
void kmeansSimple(std::vector<std::vector<double> > *data, int count,
		vector< vector<double> > &centroids, int classCounter = 13) {

	typedef dlib::matrix<double> sample_type;

	std::vector<sample_type> samples;
	std::vector<sample_type> initial_centers;

	// Copying to compatible type
	for (int k=0;k<count;k++)
	{
		for (int i = 0; i < data[k].size(); i++) {
			sample_type m;
			m.set_size(data[k][i].size(),1);

			for (int j = 0; j < data[k][i].size(); j++) {
				m(j) = data[k][i][j];
			}
			samples.push_back(m);
		}
	}

	typedef dlib::radial_basis_kernel<sample_type> kernel_type;
	pick_initial_centers(classCounter, initial_centers, samples, kernel_type(0.1));
	dlib::find_clusters_using_kmeans(samples, initial_centers);

	ofstream kmeanscentroids;
	kmeanscentroids.open("centroids.dat");
	for (int i = 0; i < initial_centers.size(); i++) {
		kmeanscentroids << i <<" " << trans(initial_centers[i]);
		centroids.push_back(vector< double >(initial_centers[i].size()));
		for (int j = 0; j< initial_centers[i].size(); j++)
		{
		//	cout<<initial_centers[i].size()<<endl;
		//	cout<<initial_centers[i](j)<<endl;
			centroids[i][j] = initial_centers[i](j);
		}

	}
	kmeanscentroids.close();
}

void readCentroidsFromFile( vector< vector<double> > &centroids )
{
	ifstream kmeanscentroids;
	kmeanscentroids.open("centroids.dat");
	string line;
	double val;
	while(!kmeanscentroids.eof()) {
		kmeanscentroids>>val;
		if ( val != 0 )
		{
			vector<double> row;
			for (int i=0;i<19;i++)
			{
				kmeanscentroids>>val;
				row.push_back(val);
			}
			centroids.push_back(row);
		}
	}
	kmeanscentroids.close();
}

// Maybe define it differently ?
double similarity(std::vector<double> a, std::vector<double> b)
{
	if (a.size() != b.size())
		return 1.0/0.00000001;

	double dist = 0.0;

	for (int k = 0; k < a.size(); k++) {
		dist += pow(a[k] - b[k], 2);
	}
	if (dist < 0.0001) {
		dist = 0.0001;
	}
	return 10.0 / dist;
}

// Can return labeled data, but cannot label new examples
void newman_cluster(std::vector<std::vector<double> > data, double eps = 0.0001,
		unsigned long max_iteration = 2000) {

	// calculating the similarity measure between examples
	std::vector<dlib::ordered_sample_pair> ord_samples;
	for (int i = 0; i < data.size(); i++) {
		for (int j = i+1; j < data.size(); j++) {

			dlib::ordered_sample_pair t(i, j, similarity(data[i], data[j]));
			ord_samples.push_back(t);
		}
	}
	cout<<"Running newman clustering"<<endl;
	std::vector<unsigned long> labels;
	dlib::newman_cluster(ord_samples, labels, eps, max_iteration);

	sort(labels.begin(), labels.end());
	unique(labels.begin(), labels.end());
	cout<<"Newman found :" <<labels.size() <<" classes" << endl;
}

// Can return labeled, but cannot label new examples
void chinese_whispers(std::vector< std::vector<double> > data, unsigned long numberOfIteration = 100) {
	// calculating the similarity measure between examples
	std::vector<dlib::ordered_sample_pair> ord_samples;
	for (int i = 0; i < data.size(); i++) {
		for (int j = i + 1; j < data.size(); j++) {

			dlib::ordered_sample_pair t(i, j, similarity(data[i], data[j]));
			ord_samples.push_back(t);
		}
	}
	std::vector<unsigned long> labels;
	dlib::chinese_whispers(ord_samples, labels, numberOfIteration);

	sort(labels.begin(), labels.end());
	unique(labels.begin(), labels.end());
	cout<<"Chinese whispers found :" <<labels.size() <<" classes" << endl;
}

int getIndexOfClassName(vector<string> &classNames, string name)
{
	for (int i=0; i<classNames.size(); i++)
	{
		if (classNames[i].compare(name) == 0)
			return i;
	}

	return -1;
}

void addAttribute(float attributeValue, int& attributeCounter,
		vector<double> &attributes) {
	attributes.push_back(attributeValue);
	attributeCounter++;
}

void fingerCountAttribute(int& fingerCount, int& attributeCounter,
		vector<double>& result) {
	addAttribute(fingerCount, attributeCounter, result);
}

void anglesBetweenFingersAttribute(GestureHand* tempHand, int& fingerCount,
		int& attributeCounter, vector<double>& result) {

	// If Hand exists and we have fingers
	if (tempHand != NULL && fingerCount > 1) {

		vector<float> angles(4, 0.0);

		// For all combinations of finger configurations
		for (int i = 0; i < fingerCount; i++) {
			Vertex fingerDirection = tempHand->getFinger(i)->getDirection();

			for (int j= i+1 ;j < fingerCount; j++) {
				Vertex fingerDirection2 = tempHand->getFinger(j)->getDirection();
				float angle = fingerDirection.dotProduct(fingerDirection);
				angles.push_back(angle);// Can be acos if somebody want it
			}
		}

		sort(angles.begin(), angles.end(), std::greater<float>());

		for (int i = 0; i < 4; i++) {
			addAttribute(angles[i], attributeCounter, result);
		}
	}
	// There are no fingers in the captured data
	else
	{
		for (int i = 0; i < 4; i++) {
			addAttribute(0.0, attributeCounter, result);
		}
	}
}

void anglesFingersPalmAttribute(GestureHand* tempHand, int& fingerCount,
		int& attributeCounter, vector<double>& result) {

	// If Hand exists and we have fingers
	if (tempHand != NULL && fingerCount > 1) {

		vector<float> angles(4, 0.0);

		// For all combinations of finger configurations
		for (int i = 0; i < fingerCount; i++) {
			Vertex fingerDirection = tempHand->getFinger(i)->getDirection();
			float angle = fingerDirection.dotProduct(tempHand->getPalmNormal());
			angles.push_back(angle);	// Can be acos if somebody want it
		}

		sort(angles.begin(), angles.end(), std::greater<float>());

		for (int i = 0; i < 4; i++) {
			addAttribute(angles[i], attributeCounter, result);
		}
	}
	// There are no fingers in the captured data
	else {
		for (int i = 0; i < 4; i++) {
			addAttribute(0.0, attributeCounter, result);
		}
	}
}

void distancesBetweenFingersAttribute(GestureHand* tempHand, int& fingerCount,
		int& attributeCounter, vector<double>& result) {

	// If Hand exists and we have fingers
	if (tempHand != NULL && fingerCount > 1) {

		vector<float> distances(4, 0.0);

		// For all combinations of finger configurations
		for (int i = 0; i < fingerCount; i++) {
			Vertex tipPosition = tempHand->getFinger(i)->getStabilizedTipPosition();

			for (int j= i+1 ;j < fingerCount; j++) {

				Vertex tipPosition2 = tempHand->getFinger(j)->getStabilizedTipPosition();
				float dist = (tipPosition - tipPosition2).getMagnitude();
				distances.push_back(dist);
			}
		}

		sort(distances.begin(), distances.end(), std::greater<float>());

		for (int i = 0; i < 4; i++) {
			addAttribute(distances[i], attributeCounter, result);
		}
	}
	// There are no fingers in the captured data
	else
	{
		for (int i = 0; i < 4; i++) {
			addAttribute(0.0, attributeCounter, result);
		}
	}
}

void distancesBetweenFingersPalmAttribute(GestureHand* tempHand, int& fingerCount,
		int& attributeCounter, vector<double>& result) {

	// If Hand exists and we have fingers
	if (tempHand != NULL && fingerCount > 1) {

		vector<float> distances(4, 0.0);

		// For all combinations of finger configurations
		for (int i = 0; i < fingerCount; i++) {
			Vertex fingerDirection = tempHand->getFinger(i)->getStabilizedTipPosition();
			Vertex palmPosition = tempHand->getPalmPosition();
			distances.push_back((fingerDirection - palmPosition).getMagnitude());
		}

		sort(distances.begin(), distances.end(), std::greater<float>());

		for (int i = 0; i < 4; i++) {
			addAttribute(distances[i], attributeCounter, result);
		}
	}
	// There are no fingers in the captured data
	else {
		for (int i = 0; i < 4; i++) {
			addAttribute(0.0, attributeCounter, result);
		}
	}
}

void handMovementAttribute(GestureHand* tempHand, GestureHand* tempHand2,
		int& attributeCounter, vector<double>& result) {
	if (tempHand == NULL || tempHand2 == NULL) {
		addAttribute(0.0, attributeCounter,
				result);
		addAttribute(0.0, attributeCounter,
				result);
		addAttribute(0.0, attributeCounter,
				result);
	} else {
		//cout<< (tempHand == NULL) <<" " << (tempHand2 == NULL) <<endl;
		Vertex palmPos = tempHand->getPalmPosition();
		Vertex palmPos2 = tempHand2->getPalmPosition();

		//cout << palmPos2.getX() << " " << palmPos2.getY() << " "
		//		<< palmPos2.getZ() << endl;
		//cout << palmPos.getX() << " " << palmPos.getY() << " " << palmPos.getZ()
		//		<< endl;

		addAttribute(palmPos2.getX() - palmPos.getX(), attributeCounter,
				result);
		addAttribute(palmPos2.getY() - palmPos.getY(), attributeCounter,
				result);
		addAttribute(palmPos2.getZ() - palmPos.getZ(), attributeCounter,
				result);
	}
}

void handMovement2Attribute(GestureHand* tempHand, GestureHand* tempHand2,
		int& attributeCounter, vector<double>& result) {
	if (tempHand == NULL || tempHand2 == NULL) {
		addAttribute(0.0, attributeCounter,
				result);
		addAttribute(0.0, attributeCounter,
				result);
		addAttribute(0.0, attributeCounter,
				result);
	} else {
		Vertex palmPos = tempHand->getPalmPosition();
		Vertex palmPos2 = tempHand2->getPalmPosition();

		//cout << palmPos2.getX() << " " << palmPos2.getY() << " "
		//		<< palmPos2.getZ() << endl;
		//cout << palmPos.getX() << " " << palmPos.getY() << " " << palmPos.getZ()
		//		<< endl;
		Vertex directionOfMovement(palmPos2.getX() - palmPos.getX(),
				palmPos2.getY() - palmPos.getY(),
				palmPos2.getZ() - palmPos.getZ());

		Vertex normalPalm = tempHand->getPalmNormal();
		Vertex directionPalm = tempHand->getDirection();
		Vertex movementNormalized = directionOfMovement.getNormalized();

		addAttribute(directionOfMovement.getMagnitude(), attributeCounter,
				result);
		addAttribute(abs(normalPalm.dotProduct(movementNormalized)), attributeCounter,
				result);
		addAttribute(abs(directionPalm.dotProduct(movementNormalized)), attributeCounter,
				result);
	}
}

void handSpeedsAttribute(GestureHand* tempHand, GestureHand* tempHand2,
		int& attributeCounter, vector<double>& result) {
	if (tempHand == NULL || tempHand2 == NULL) {
		addAttribute(0.0, attributeCounter,
				result);
		addAttribute(0.0, attributeCounter,
				result);
		addAttribute(0.0, attributeCounter,
				result);
		addAttribute(0.0, attributeCounter, result);
	} else {
		int fingerCount = tempHand->getFingerCount();
		int fingerCount2 = tempHand2->getFingerCount();

		vector<float> speeds(4,0.0);
		for (int i=0;i< min(fingerCount, fingerCount2); i++)
		{
			Vertex tipPosition = tempHand->getFinger(i)->getStabilizedTipPosition();
			Vertex tipPosition2 = tempHand2->getFinger(i)->getStabilizedTipPosition();
			speeds[i] = (tipPosition2 - tipPosition).getMagnitude();
		}
		sort(speeds.begin(), speeds.end(), std::greater<float>());

		for (int i=0;i<4;i++)
			addAttribute(speeds[i], attributeCounter,
				result);
	}
}

// Computes the feature set
vector<double> computeFeatureSet(GestureFrame *gestureFrame, GestureFrame *gestureFrame2) {
	vector<double> result;

	// Get the numbe of fingers
	GestureHand *tempHand = gestureFrame->getHand(0);
	GestureHand *tempHand2 = gestureFrame2->getHand(0);
	int fingerCount =
			(tempHand != NULL) ?
					min(tempHand->getFingerCount(), MAX_FINGER_COUNT) : 0;
	int attributeCounter = 1;

	// Adding the finger count to the feature set
	fingerCountAttribute(fingerCount, attributeCounter, result);

	int fingerCount2 =
				(tempHand2 != NULL) ?
						min(tempHand2->getFingerCount(), MAX_FINGER_COUNT) : 0;

	// Adding the finger count to the feature set
	fingerCountAttribute(fingerCount2, attributeCounter, result);

	// Adding 4 angles to the palm normal
	distancesBetweenFingersPalmAttribute(tempHand2, fingerCount2, attributeCounter,
			result);

	// Adding 4 angles to the palm normal
	anglesFingersPalmAttribute(tempHand2, fingerCount2, attributeCounter,
			result);

	// Adding the 4 highest angles to the feature set
	anglesBetweenFingersAttribute(tempHand2, fingerCount2, attributeCounter,
			result);

	// Adding the 4 greatest distances to the feature set
	distancesBetweenFingersAttribute(tempHand2, fingerCount2, attributeCounter,
			result);
	//distancesBetweenFingersAttribute(tempHand, fingerCount2, attributeCounter,
	//			result);

	// Hand movement
	//handMovementAttribute(tempHand, tempHand2, attributeCounter, result);

	//
	handMovement2Attribute(tempHand, tempHand2, attributeCounter, result);

	//handSpeedsAttribute(tempHand, tempHand2, attributeCounter, result);

	return result;
}

void columnScaling(vector< vector<double > >& data ) {

	int trainSetSize = data.size();
	int featureSize = data[0].size();
	double ** scaling = new double*[2];
	scaling[0] = new double [ featureSize ];
	scaling[1] = new double [ featureSize ];

	for (int j = 0; j < featureSize; j++) {
		double min_val = data[0][j], max_val = data[0][j];
		for (int i = 0; i < trainSetSize; i++) {
			if (data[i][j] < min_val) {
				min_val = data[i][j];
			}
			if (data[i][j] > max_val) {
				max_val = data[i][j];
			}
		}
		scaling[0][j] = min_val;
		scaling[1][j] = max_val - min_val;

		if ( scaling[1][j] > 0.1)
		{
			for (int i = 0; i < trainSetSize; i++) {
				data[i][j] = (data[i][j] - scaling[0][j]) / (scaling[1][j]);
			}
		}
		}
	delete [] scaling[0];
	delete [] scaling[1];
	delete [] scaling;
}

void readingInputData(string fileName, vector<GestureFrame> &frames) {
	GestureStorageDriver* gestureStorageDriver = new BinaryFileStorageDriver();

	gestureStorageDriver->openConnection(fileName, false);

	GestureFrame currGestureFrame;
	while (gestureStorageDriver->loadGestureFrame(currGestureFrame)) {
		frames.push_back(currGestureFrame);
		currGestureFrame.clear();
	}
	gestureStorageDriver->closeConnection();
	delete gestureStorageDriver;
}

void dataPreparation(int count, vector<GestureFrame> *frames,
		const int preprocessingWidth,
		vector<vector<double> > *dataset) {
	for (int i = 0; i < count; i++) {
		LMpre::LMpre pre(frames[i], preprocessingWidth);
		frames[i] = pre.process();
		//cout<<i<<" "<< frames[i].size() << " " 	;
		for (int j = 0; j < frames[i].size(); j++) {
			vector<double> row;
			int k = (j - 10) >= 0 ? (j-10) : 0;

			//cout<<"("<<i<<", "<<j<<")"<<endl;
			row = computeFeatureSet(&frames[i][j], &frames[i][k]);
			dataset[i].push_back(row);
		}
		// Data normalizaion in columns
		//if ( frames[i].size() != 0)
			columnScaling(dataset[i]);

		frames[i].clear();
	}
}

//void saveToEstimateNumbeOfClasses(vector<vector<double> > *dataset, int argc) {
//	// Saving for tool
//	ofstream zapis("out.csv");
//	for (int i = 0; i < argc -1 ; i++) {
//		if (i == 0) {
//			for (int j = 0; j < dataset[i].size(); j++) {
//				zapis << '"' << j << '"' << ';';
//			}
//			zapis << endl;
//		}
//		zapis << '"' << i << '"' << ";";
//		for (int j = 0; j < dataset[i].size(); j++) {
//			zapis << dataset[i][j] << ";";
//		}
//		zapis << endl;
//	}
//	zapis.close();
//
//	std::fstream datasetFile;
//	datasetFile.open("DATASET_FILE", std::fstream::out | std::fstream::trunc);
//	for (int i = 0; i < argc -1; i++) {
//		int lastElement = dataset[i].size() - 1;
//		for (int j = 0; j < lastElement; j++) {
//			datasetFile << dataset[i][j] << " ";
//		}
//
//		datasetFile << dataset[i][lastElement] << "\n";
//	}
//	datasetFile.close();
//}

void readConfig(int &preprocessingWidth, int &crossValK, int &K, int &M,
		int &classNumber, int &readCentroids, double &learningRate,
		int &iteration_number, int &datasetSize)
{
	ifstream in;
	in.open("config.cfg");

	string tmp;
	string val;

	getline(in, tmp);
	getline(in, val);
	preprocessingWidth = atoi(val.c_str());

	getline(in, tmp);
	getline(in, val);
	crossValK = atoi(val.c_str());

	getline(in, tmp);
	getline(in, val);
	K = atoi(val.c_str());

	getline(in, tmp);
	getline(in, val);
	classNumber = atoi(val.c_str());
	M = classNumber;

	getline(in, tmp);
	getline(in, val);
	readCentroids = atoi(val.c_str());

	getline(in, tmp);
	getline(in, val);
	learningRate = atof(val.c_str());

	getline(in, tmp);
	getline(in, val);
	iteration_number = atoi(val.c_str());

	getline(in, tmp);
	getline(in, val);
	datasetSize = atoi(val.c_str());

	in.close();
}

int main(int argc, char **argv) {
	// Parameters to play with
	int preprocessingWidth = 10;
	int crossValK = 5;
	int K = 10; // number of states in one gesture
	int M = 15;
	int classNumber = 15; // number of possible observations
	int readCentroids = 1; // 0 - calculate using k-means, 1 - read them from file
	double learningRate = 0.2; // How much do we incorporate new training data into trained model
	int iteration_number = 100; // How many learning iterations
	int datasetSize = 720;

	// Read parameters
	readConfig(preprocessingWidth, crossValK, K, M, classNumber, readCentroids,
			learningRate, iteration_number, datasetSize);

	// Reading data in
	vector<GestureFrame> frames[1000];
	vector<int> label;

	for (int k=0;k<6;k++)
	{
		for (int i=0;i<4;i++)
		{
			string base = "dataset_new/";
			if (i == 0)
				base = base + "kasia/";
			else if (i == 1)
				base = base + "kuba/";
			else if (i == 2)
				base = base + "michal/";
			else
				base = base + "oli/";

//			if ( k == 0)
//				base = base + "cyfra8/";
//			else if (k == 1)
//				base = base + "kolko/";
//			else if (k == 2)
//				base = base + "opadanieCalejReki/";
//			else if (k == 3)
//				base = base + "pistolet/";
//			else if (k == 4)
//				base = base + "swipeOdPrawej/";
//			else
//				base = base + "czyszczenie/";
			if (k == 0)
				base = base + "123/";
			else if (k == 1)
				base = base + "drzwi/";
			else if (k == 2)
				base = base + "kolko/";
			else if (k == 3)
				base = base + "nozyce/";
			else if (k == 4)
				base = base + "pistolet/";
			else
				base = base + "przenoszenie/";
			for (int j=1;j<31;j++)
			{
				string name = base;
				stringstream ss2;
				ss2 << j;
				name = name + ss2.str() + ".lmr";
				//cout<<name<<" " << k*120 + i*30 + j-1 << endl;
				readingInputData(name, frames[k*120 + i*30 + j-1 ]);
				//readingInputData(name, frames[k*30 + j-1 ]);
				label.push_back(k);
			}

		}

	}
	cout<<"Ended loading data" <<endl;

	// Preprocessing &&
	// Saving all possible gestures as feature sets &&
	// Scaling
	vector<vector<double> > dataset[1000];
	dataPreparation(datasetSize, frames, preprocessingWidth, dataset);

	// Saving for tool to estimate number of classes
	//saveToEstimateNumbeOfClasses(dataset, argc);

	cout << " k-means " << endl;
	vector< vector<double> > centroids;
	if ( readCentroids )
		readCentroidsFromFile(centroids);
	else
		kmeansSimple(dataset, datasetSize, centroids, classNumber);

	//cout<<endl<<"!!!!!!! KMEANS !!!!!!" << endl;
	//kmeans(dataset, 11);
	//cout<<endl<<"!!!!!!! CHINESE WHISPERS !!!!!!" << endl;
	///chinese_whispers(dataset);
	//cout<<endl<<"!!!!!!! NEWMAN CLUSTERING !!!!!!" << endl;
	//newman_cluster(dataset);

	// Seperating into train/test dataset
	std::vector<sequence> trainDataset[6];
	std::vector<sequence> testDataset;

	// Finding the longest sequence of observations
	int n = 0;
	for (int p=0;p<datasetSize;p++)
	{
		n = max(n, (int)dataset[p].size());
	}

	cout <<"Seperating data"<<endl;
	// Determine the observation values for all sequences
	for (int p=0;p<datasetSize;p++)
	{
		vector<unsigned int> observationLabels(n, 0.0);
		for (int j=0; j <  n;j++)
		{
			// Fill the rest with the last label
			if ( j >= dataset[p].size() )
			{
				observationLabels[j] = observationLabels[dataset[p].size() - 1];
			}
			else
			{
				// Calculate the errors to the centroids
				double best_error = 0.0;
				int best_index = -2;
				for (int i = 0; i < centroids.size(); i++) {
					double error = 0.0;
					for (int k = 0; k < centroids[i].size(); k++) {
						error += (dataset[p][j][k] - centroids[i][k])
								* (dataset[p][j][k] - centroids[i][k]);
					}
					if (best_error > error || best_index < 0) {
						best_error = error;
						best_index = i;
					}
				}
				observationLabels[j] = (best_index);
			}
		}
		// First 20 used for training
		if ( p % 30 < 20)
			trainDataset[p/120].push_back(observationLabels);
			//trainDataset[p/30].push_back(observationLabels);

		testDataset.push_back(observationLabels);
	}
	for(int k=0;k<6;k++)
		cout<<"Train: " << trainDataset[k].size()<<endl;
	cout<<"Test: "<<testDataset.size()<< " " << datasetSize<<endl;
	cout<<"HMM start"<<endl;

	// HMM
	HMMClass *hmmGesture[6];
	HMMClass *bestHmmGesture[6];
	double bestRecognitionRate = 0;
	for (int k=0;k<10;k++)
	{
		for (int i=0;i<6;i++)
		{
			cout<<"Iteration k=" <<k<<"\tlearning model i=" << i << " on " <<trainDataset[i].size()<< " samples";
			hmmGesture[i] = new HMMClass(K, n, M);
			hmmGesture[i]->train(trainDataset[i],crossValK, iteration_number, learningRate);
			cout<<" --- Model learnt" << endl;
		}

		//HMMClass *hmmGesture = new HMMClass("hmmFirstModel.model");

		vector<int> predictedLabels[6];
		vector<int> trainLabels[6];
		for (int p =0;p<6;p++)
		{
			for (int i=0;i<trainDataset[p].size();i++)
			{
				double error;
				int index = -1;
				for (int j=0;j<6;j++)
				{
					double loglik = hmmGesture[j]->predict(trainDataset[p][i]);
					if ( index == -1 || loglik > error)
					{
						error = loglik;
						index = j;
					}
				}
				predictedLabels[p].push_back(index);
				trainLabels[p].push_back(p);
			}
		}

		cout<<"Counting percentage ..."<<endl;
		int counter = 0;
		int trainSize = 0;
		for (int j=0;j<6;j++)
		{
			trainSize += trainDataset[j].size();
			for (int i=0; i< trainDataset[j].size();i++)
				if ( predictedLabels[j][i] == trainLabels[j][i])
					counter++;
		}
		cout<<"Train recognition rate : " << counter * 100.0 / trainSize << " " << counter <<" " << trainSize <<endl;
		// Evaluate best model
		vector<int> predicted;
		for (int i = 0; i < datasetSize; i++) {
			double error;
			int index = -1;
			for (int j = 0; j < 6; j++) {
				double loglik = hmmGesture[j]->predict(testDataset[i]);
				if (index == -1 || loglik > error) {
					error = loglik;
					index = j;
				}
			}
			predicted.push_back(index);
		}

		int count = 0;
		for (int i = 0; i < datasetSize; i++) {
//			if (i % 30 < 20)
//				cout << predictedLabels[i / 30][i % 30] << " "
//						<< trainLabels[i / 30][i % 30] << " ";
//			else
//				cout << " -  - ";
//
//			cout << predicted[i] << " " << label[i] << endl;
//
			if (predicted[i] == label[i])
				count++;
		}
		cout << "On total recognition rate : " << count * 100.0 / datasetSize
				<< endl;

		if ( counter * 100.0 / trainSize > bestRecognitionRate)
		{
			bestRecognitionRate = counter * 100.0 / trainSize;
			for (int j=0;j<6;j++)
			{
				if (k!=0) delete bestHmmGesture[j];
				bestHmmGesture[j] = hmmGesture[j];
			}
		}
		else
			for (int j=0;j<6;j++) delete hmmGesture[j];
	}

	// Evaluate best model
	vector<int> predictedLabels;
	for (int i = 0; i < datasetSize; i++) {
		double error;
		int index = -1;
		for (int j = 0; j < 6; j++) {
			double loglik = bestHmmGesture[j]->predict(testDataset[i]);
			if (index == -1 || loglik > error) {
				error = loglik;
				index = j;
			}
		}
		predictedLabels.push_back(index);
	}

	cout << "Counting percentage ..." << endl;
	int counter = 0;
	for (int i = 0; i < datasetSize; i++)
		if (predictedLabels[i] == label[i])
			counter++;
	cout<<"Best total recognition rate : " << counter * 100.0 / datasetSize << endl;

	//hmmGesture->show();

	for (int j=0;j<6;j++)
	{
		string name = "hmmModel_";
		stringstream ss2;
		ss2 << (j+1);
		name = name + ss2.str() + ".model";
		bestHmmGesture[j]->saveModel(name);
		delete bestHmmGesture[j];
	}
}


#include <geometry.h>
#include <tiny_math_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(raycast_plane)
{

  using std::sqrt;

  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;
  typedef MT::value_traits          VT;

  V                  const normal = V::make(1.0,1.0,1.0);
  T                  const offset = norm( normal );
  geometry::Plane<V> const plane  = geometry::make_plane( unit(normal), offset);

  BOOST_CHECK(geometry::is_valid(plane));

  // Orthogonal hit from back-side
  {
    V                const p   = V::make( 0.0, 0.0, 0.0);
    V                const r   = V::make( 1.0, 1.0, 1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_plane(ray, plane, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, offset, 0.01);
    BOOST_CHECK_CLOSE( q(0),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(2),  1.0, 0.01);
  }

  // Orthogonal hit from back-side with front-face only
  {
    V                const p   = V::make( 0.0, 0.0, 0.0);
    V                const r   = V::make( 1.0, 1.0, 1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_plane(ray, plane, q, length, true);

    BOOST_CHECK( !hit );
  }

  // Orthogonal hit from front-side but with ray origin on back-side
  {
    V                const p   = V::make( 0.0, 0.0, 0.0);
    V                const r   = V::make( -1.0, -1.0, -1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_plane(ray, plane, q, length, true);

    BOOST_CHECK( !hit );
  }

  // Parallel ray and plane
  {
    V                const p   = V::make( 0.0, 0.0, 0.0);
    V                const r   = V::make( -1.0, 1.0, 0.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_plane(ray, plane, q, length);

    BOOST_CHECK( !hit );
  }

  // Oblique hit
  {
    V                const p   = V::make( 2.0, 1.0, 1.0);
    V                const r   = V::make( -1.0, 0.0, 0.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_plane(ray, plane, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 1.0, 0.01);
    BOOST_CHECK_CLOSE( q(0),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(2),  1.0, 0.01);
  }

}

BOOST_AUTO_TEST_SUITE_END();


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

/*
 Copyright (C) 2010 Liquidnet Holdings, Inc.

 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.
*/

#ifndef quantlib_test_autocovariances_hpp
#define quantlib_test_autocovariances_hpp

#include <boost/test/unit_test.hpp>

/* remember to document new and/or updated tests in the Doxygen
   comment block of the corresponding class */

class AutocovariancesTest {
  public:
    static void testConvolutions();
    static void testAutoCovariances();
    static void testAutoCorrelations();
    static boost::unit_test_framework::test_suite* suite();
};

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

/*
 Copyright (C) 2010 Liquidnet Holdings, Inc.

 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.
*/

#include "utilities.hpp"
#include <ql/math/autocovariance.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace std;

void AutocovariancesTest::testConvolutions() {
    BOOST_TEST_MESSAGE("Testing convolutions...");
    Array x(10, 1, 1);
    Array conv(6);
    convolutions(x.begin(), x.end(), conv.begin(), 5);
    Real expected[] = { 385, 330, 276, 224, 175, 130 };
    Array delta = conv - Array(expected, expected+6);
    if (DotProduct(delta, delta) > 1.0e-6)
        BOOST_ERROR("Convolution: \n"
                    << std::setprecision(4) << std::scientific
                    << "    calculated:   " << conv << "\n"
                    << "    expected:     " << Array(expected, expected+6));
}

void AutocovariancesTest::testAutoCovariances() {
    BOOST_TEST_MESSAGE("Testing auto-covariances...");
    Array x(10, 1, 1);
    Array acovf(6);
    Real mean = autocovariances(x.begin(), x.end(), acovf.begin(), 5, false);
    Real expected[] = { 8.25, 6.416667, 4.25, 1.75, -1.08333, -4.25 };
    if (std::fabs(mean-5.5) > 1.0e-6) {
        BOOST_ERROR("Mean: \n"
                    << "    calculated:   " << mean << "\n"
                    << "    expected:     " << 5.5);
    }
    Array delta = acovf - Array(expected, expected+6);
    if (DotProduct(delta, delta) > 1.0e-6)
        BOOST_ERROR("Autocovariances: \n"
                    << std::setprecision(4) << std::scientific
                    << "    calculated:   " << acovf << "\n"
                    << "    expected:     " << Array(expected, expected+6));
}

void AutocovariancesTest::testAutoCorrelations() {
    BOOST_TEST_MESSAGE("Testing auto-correlations...");
    Array x(10, 1, 1);
    Array acorf(6);
    Real mean = autocorrelations(x.begin(), x.end(), acorf.begin(), 5, true);
    Real expected[] = { 9.166667, 0.77777778, 0.51515152,
                        0.21212121, -0.13131313, -0.51515152 };
    if (std::fabs(mean-5.5) > 1.0e-6) {
        BOOST_ERROR("Mean: \n"
                    << "    calculated:   " << mean << "\n"
                    << "    expected:     " << 5.5);
    }
    Array delta = acorf - Array(expected, expected+6);
    if (DotProduct(delta, delta) > 1.0e-6)
        BOOST_ERROR("Autocovariances: \n"
                    << std::setprecision(4) << std::scientific
                    << "    calculated:   " << acorf << "\n"
                    << "    expected:     " << Array(expected, expected+6));
    delta = x - Array(10, -4.5, 1);
    if (DotProduct(delta, delta) > 1.0e-6)
        BOOST_ERROR("Centering: \n"
                    << std::setprecision(4) << std::scientific
                    << "    calculated:   " << x << "\n"
                    << "    expected:     " << Array(10, -4.5, 1));
}

test_suite* AutocovariancesTest::suite() {
    test_suite* suite = BOOST_TEST_SUITE("auto-covariance tests");
    suite->add(QUANTLIB_TEST_CASE(&AutocovariancesTest::testConvolutions));
    suite->add(QUANTLIB_TEST_CASE(&AutocovariancesTest::testAutoCovariances));
    suite->add(QUANTLIB_TEST_CASE(&AutocovariancesTest::testAutoCorrelations));
    return suite;
}

#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  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 MPI-IS 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: Andreas Orthey */

#include <ompl/base/spaces/special/KleinBottleStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>
#include <cmath>
#include <boost/math/constants/constants.hpp>

using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

KleinBottleStateSampler::KleinBottleStateSampler(const StateSpace *space) : StateSampler(space)
{
}

void KleinBottleStateSampler::sampleUniform(State *state)
{
    bool acceptedSampleFound = false;
    while (!acceptedSampleFound)
    {
        const double u = rng_.uniformReal(0, pi);
        const double v = rng_.uniformReal(-pi, pi);

        // NOTE: The idea here is that to compute the norm of the gradient at each
        // point of the surface (i.e. the gradient of the coordinate mapping from
        //(u,v) to (x,y,z)). To get the norm, we divide by the maximum norm of the
        // gradient over the whole surface. This gives a number between [0,1]. We
        // then do rejection sampling, by choosing a random number in [0,1] and
        // accept if the norm is larger than this random number. Surface elements
        // with a high curvature will have a small norm and will therefore be
        // penalized under this method (i.e. rejected more often).
        // See also: https://mathematica.stackexchange.com/questions/148693/generating-random-points-on-a-kleins-bottle

        // NOTE: Automatic differential via sympy script
        const double cu = std::cos(u);
        const double cv = std::cos(v);
        const double su = std::sin(u);
        const double sv = std::sin(v);
        const double cu3 = std::pow(cu, 3);
        const double cu5 = std::pow(cu, 5);
        const double cu6 = std::pow(cu, 6);
        const double cu7 = std::pow(cu, 7);
        const double cu8 = std::pow(cu, 8);

        const double su2 = std::pow(su, 2);
        const double su3 = std::pow(su, 3);
        const double su4 = std::pow(su, 4);
        const double su5 = std::pow(su, 5);
        const double su6 = std::pow(su, 6);
        const double su7 = std::pow(su, 7);
        const double su8 = std::pow(su, 8);

        const double aprime = (64.0 * su8 - 128.0 * su6 + 60.0 * su4 + 0.4 * su * cv - (1.0 / 6.0) * cu * cv -
                               0.5 * std::cos(3 * u) * cv);

        const double a = (-aprime * cv + (2.0 / 3.0) * sv * sv * cu * std::cos(2.0 * u));

        const double bprime =
            ((26 + 2.0 / 3.0) * su7 * cv - 55.0 * su5 * cv - (37 + 1.0 / 3.0) * su3 * cu6 * cv + 28.0 * su3 * cv +
             (10 + 2.0 / 3.0) * su * cu8 * cv - (10 + 2.0 / 3.0) * su * cu6 * cv - 4.0 * std::sin(2.0 * u) +
             22.4 * cu7 * cv - 35.2 * cu5 * cv + 12.2 * cu3 * cv + 0.6 * cu * cv);

        const double cprime = ((5 + 1.0 / 3.0) * su5 * cu + 3.2 * su4 - (10 + 2.0 / 3.0) * su3 * cu - 6.4 * su2 +
                               2.5 * std::sin(2 * u) + 3.0);

        const double b = (((1.0 / 3.0) * std::sin(2.0 * u) + 0.4) * bprime * cu - cprime * aprime * su3);

        const double c = ((5.0 / 6.0) * std::sin(2.0 * u) + 1);

        const double d = (-((1.0 / 3.0) * std::sin(2.0 * u) + 0.4) * bprime * cv +
                          (2.0 / 3.0) * cprime * su3 * sv * sv * std::cos(2.0 * u));

        double s = std::sqrt(a * a * (0.16 * c * c) + b * b * sv * sv + d * d);

        if (s > gMax_)
        {
            OMPL_ERROR("Norm of gradient (%.10f) larger than maximum norm (%.10f).", s, gMax_);
            throw "Wrong norm error.";
        }
        s = s / gMax_;

        const double mu = rng_.uniformReal(0, 1);
        if (mu <= s)
        {
            auto *K = state->as<KleinBottleStateSpace::StateType>();
            K->setUV(u, v);
            acceptedSampleFound = true;
        }
    }
}

void KleinBottleStateSampler::sampleUniformNear(State *state, const State *near, double distance)
{
    auto *K = state->as<KleinBottleStateSpace::StateType>();
    const auto *Knear = near->as<KleinBottleStateSpace::StateType>();
    K->setU(rng_.uniformReal(Knear->getU() - distance, Knear->getU() + distance));
    K->setV(rng_.uniformReal(Knear->getV() - distance, Knear->getV() + distance));
    space_->enforceBounds(state);
}

void KleinBottleStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
{
    auto *K = state->as<KleinBottleStateSpace::StateType>();
    const auto *Kmean = mean->as<KleinBottleStateSpace::StateType>();
    K->setU(rng_.gaussian(Kmean->getU(), stdDev));
    K->setV(rng_.gaussian(Kmean->getV(), stdDev));

    space_->enforceBounds(state);
}

KleinBottleStateSpace::KleinBottleStateSpace()
{
    setName("KleinBottle" + getName());
    type_ = STATE_SPACE_KLEIN_BOTTLE;

    // We model the Klein bottle as a regular cylinder, but where both ends are
    // glued inversely together. For more information, check out the
    // wikipedia article: https://en.wikipedia.org/wiki/Klein_bottle.
    // Both interpolation and distance computation have to take
    // the gluing into account when crossing over the boundary.
    // ------<-------
    // |            |
    // |            |
    // v            v u-dimension (0 to pi)
    // |            |
    // |            |
    // ------>-------
    //  v-dimension (0 to 2*pi)
    //
    //  Gluing:
    // u=pi+0.001:  0 ----------- -pi pi ---------- 0
    // u=0       :  -pi ----------- 0 0 ---------- pi

    StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
    R1->as<RealVectorStateSpace>()->setBounds(0, pi);

    StateSpacePtr SO2(std::make_shared<SO2StateSpace>());

    addSubspace(R1, 1.0);
    addSubspace(SO2, 1.0);

    lock();
}

StateSamplerPtr KleinBottleStateSpace::allocDefaultStateSampler() const
{
    return std::make_shared<KleinBottleStateSampler>(this);
}

double KleinBottleStateSpace::distance(const State *state1, const State *state2) const
{
    const double u1 = state1->as<KleinBottleStateSpace::StateType>()->getU();
    const double u2 = state2->as<KleinBottleStateSpace::StateType>()->getU();

    const double diffU = u2 - u1;

    if (std::abs(diffU) <= 0.5 * pi)
    {
        return CompoundStateSpace::distance(state1, state2);
    }
    else
    {
        const double d_u = pi - std::abs(diffU);

        const double v1 = state1->as<KleinBottleStateSpace::StateType>()->getV();
        double v2 = state2->as<KleinBottleStateSpace::StateType>()->getV();

        // reverse v2 (valid for both directions)
        v2 = (v2 > 0.0 ? pi - v2 : -pi - v2);

        double d_v = std::abs(v2 - v1);
        d_v = (d_v > pi) ? 2.0 * pi - d_v : d_v;

        double dist = d_u + d_v;

        return dist;
    }
}

void KleinBottleStateSpace::interpolate(const State *from, const State *to, double t, State *state) const
{
    const double u1 = from->as<KleinBottleStateSpace::StateType>()->getU();
    const double u2 = to->as<KleinBottleStateSpace::StateType>()->getU();

    double diffU = u2 - u1;

    if (std::abs(diffU) <= 0.5 * pi)
    {
        // interpolate as if it would be a cylinder
        CompoundStateSpace::interpolate(from, to, t, state);
    }
    else
    {
        // Interpolate along u-dimension
        if (diffU > 0.0)
        {
            diffU = pi - diffU;
        }
        else
        {
            diffU = -pi - diffU;
        }

        double u = u1 - diffU * t;

        bool crossed = false;
        if (u > pi)
        {
            u -= pi;
            crossed = true;
        }
        else if (u < 0.0)
        {
            u += pi;
            crossed = true;
        }

        state->as<KleinBottleStateSpace::StateType>()->setU(u);

        double v1 = from->as<KleinBottleStateSpace::StateType>()->getV();
        double v2 = to->as<KleinBottleStateSpace::StateType>()->getV();

        // If we crossed the gluing, we need to invert the "from"-state, otherwise
        // we need to invert the "to"-state (similar to default SO2 interpolation)
        if (crossed)
        {
            v1 = (v1 > 0.0 ? pi - v1 : -pi - v1);
        }
        else
        {
            v2 = (v2 > 0.0 ? pi - v2 : -pi - v2);
        }

        double diffV = v2 - v1;
        double v = 0;

        if (std::abs(diffV) <= pi)
        {
            v = v1 + diffV * t;
        }
        else
        {
            if (diffV > 0.0)
                diffV = 2.0 * pi - diffV;
            else
                diffV = -2.0 * pi - diffV;
            v = v1 - diffV * t;

            if (v > pi)
                v -= 2.0 * pi;
            else if (v < -pi)
                v += 2.0 * pi;
        }

        state->as<KleinBottleStateSpace::StateType>()->setV(v);
    }
}

State *KleinBottleStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

Eigen::Vector3f KleinBottleStateSpace::toVector(const State *state) const
{
    // Formula from https://en.wikipedia.org/wiki/Klein_bottle#Bottle_shape
    const auto *s = state->as<KleinBottleStateSpace::StateType>();
    const float u = s->getU();
    const float v = s->getV() + pi;  // NOTE: SO2 state space has bounds [-pi, +pi]

    assert(u >= 0.0);
    assert(u <= pi);
    assert(v >= 0.0);
    assert(v <= 2 * pi);

    double cu = std::cos(u);
    double cv = std::cos(v);
    double su = std::sin(u);
    double sv = std::sin(v);
    double cu2 = std::pow(cu, 2);
    double cu3 = std::pow(cu, 3);
    double cu4 = std::pow(cu, 4);
    double cu5 = std::pow(cu, 5);
    double cu6 = std::pow(cu, 6);
    double cu7 = std::pow(cu, 7);

    double a = 3 * cv - 30 * su + 90 * cu4 * su - 60 * cu6 * su + 5 * cu * cv * su;

    Eigen::Vector3f q;
    q[0] = -2.0 / 15.0 * cu * a;

    double b = 3 * cv - 3 * cu2 * cv - 48 * cu4 * cv + 48 * cu6 * cv - 60 * su + 5 * cu * cv * su - 5 * cu3 * cv * su -
               80 * cu5 * cv * su + 80 * cu7 * cv * su;

    q[1] = -1.0 / 15.0 * su * b;

    q[2] = 2.0 / 15.0 * (3 + 5 * cu * su) * sv;

    return q;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_MLOGTWO2NMB_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_MLOGTWO2NMB_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Mlogtwo2nmb.

    @par Semantic:

    @code
    T r = Mlogtwo2nmb<T>();
    @endcode

    is similar to:

    @code
      r =  -log(exp2(T(Nbmantissabits<T>())));
    @endcode

**/
  template<typename T> T Mlogtwo2nmb();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Mlogtwo2nmb.

      Generate the  constant mlogtwo2nmb.

      @return The Mlogtwo2nmb constant for the proper type
    **/
    const boost::dispatch::functor<tag::mlogtwo2nmb_> mlogtwo2nmb = {};
  }
} }
#endif

#include <boost/simd/constant/definition/mlogtwo2nmb.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//
// 	Copyright (c) 2020, Amit Singh, amitsingh19975@gmail.com
// 	Copyright (c) 2021, Cem Bassoy, cem.bassoy@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor/tensor.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>
#include <boost/test/unit_test.hpp>

#include "utility.hpp"

#include <cstdlib>
#include <functional>
#include <tuple>

BOOST_AUTO_TEST_SUITE(test_tensor_static_rank_expression)

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture
{
  template<size_t N>
  using extents_t = boost::numeric::ublas::extents<N>;

  static constexpr auto extents =
    std::make_tuple(
//      extents_t<0>       {},
      extents_t<2>    {1,1},
      extents_t<2>    {1,2},
      extents_t<2>    {2,1},
      extents_t<2>    {2,3},
      extents_t<3>  {2,3,1},
      extents_t<3>  {4,1,3},
      extents_t<3>  {1,2,3},
      extents_t<3>  {4,2,3},
      extents_t<4>{4,2,3,5} );
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_static_rank_expression_retrieve_extents, value,  test_types, fixture)
{
  namespace ublas  = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto uplus1 = [](auto const& a){return a + value_t(1); };
  auto uplus2 = [](auto const& a){return value_t(2) + a; };
  auto bplus  = std::plus <value_t>{};
  auto bminus = std::minus<value_t>{};

  for_each_in_tuple(extents, [&](auto const& /*unused*/, auto const& e){

    static constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;

    auto t = tensor_t(e);
    auto v = value_t{};
    for(auto& tt: t){ tt = v; v+=value_t{1}; }

    BOOST_CHECK( ublas::detail::retrieve_extents( t ) == e );

    // uexpr1 = t+1
    // uexpr2 = 2+t
    auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_t>( t, uplus1 );
    auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_t>( t, uplus2 );

    BOOST_CHECK( ublas::detail::retrieve_extents( uexpr1 ) == e );
    BOOST_CHECK( ublas::detail::retrieve_extents( uexpr2 ) == e );

    // bexpr_uexpr = (t+1) + (2+t)
    auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_t>( uexpr1, uexpr2, bplus );

    BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_uexpr ) == e );

    // bexpr_bexpr_uexpr = ((t+1) + (2+t)) - t
    auto bexpr_bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_t>( bexpr_uexpr, t, bminus );

    BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_bexpr_uexpr ) == e );

  });

  for_each_in_tuple(extents, [&](auto I, auto const& e1){

    if ( I >= std::tuple_size_v<decltype(extents)> - 1 ){
      return;
    }

    constexpr auto size1 = std::tuple_size_v<std::decay_t<decltype(e1)>>;
    using tensor_type1 = ublas::tensor_static_rank<value_t, size1, layout_t>;

    for_each_in_tuple(extents, [&,I](auto J, auto const& e2){

      if( J != I + 1 ){
        return;
      }

      static constexpr auto size1 = std::tuple_size_v<std::decay_t<decltype(e1)>>;
      static constexpr auto size2 = std::tuple_size_v<std::decay_t<decltype(e2)>>;
      using tensor_type2 = ublas::tensor_static_rank<value_t, size2, layout_t>;

      auto v = value_t{};

      tensor_type1 t1(e1);
      for(auto& tt: t1){ tt = v; v+=value_t{1}; }

      tensor_type2 t2(e2);
      for(auto& tt: t2){ tt = v; v+=value_t{2}; }

      BOOST_CHECK( ublas::detail::retrieve_extents( t1 ) != ublas::detail::retrieve_extents( t2 ) );

      // uexpr1 = t1+1
      // uexpr2 = 2+t2
      auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type1>( t1, uplus1 );
      auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type2>( t2, uplus2 );

      BOOST_CHECK( ublas::detail::retrieve_extents( t1 )     == ublas::detail::retrieve_extents( uexpr1 ) );
      BOOST_CHECK( ublas::detail::retrieve_extents( t2 )     == ublas::detail::retrieve_extents( uexpr2 ) );
      BOOST_CHECK( ublas::detail::retrieve_extents( uexpr1 ) != ublas::detail::retrieve_extents( uexpr2 ) );

      if constexpr( size1 == size2 ){
        // bexpr_uexpr = (t1+1) + (2+t2)
        auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type1>( uexpr1, uexpr2, bplus );

        BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_uexpr ) == ublas::detail::retrieve_extents(t1) );

        // bexpr_bexpr_uexpr = ((t1+1) + (2+t2)) - t2
        auto bexpr_bexpr_uexpr1 = ublas::detail::make_binary_tensor_expression<tensor_type1>( bexpr_uexpr, t2, bminus );

        BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_bexpr_uexpr1 ) == ublas::detail::retrieve_extents(t2) );

        // bexpr_bexpr_uexpr = t2 - ((t1+1) + (2+t2))
        auto bexpr_bexpr_uexpr2 = ublas::detail::make_binary_tensor_expression<tensor_type1>( t2, bexpr_uexpr, bminus );

        BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_bexpr_uexpr2 ) == ublas::detail::retrieve_extents(t2) );
      }

    });
  });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_static_rank_expression_all_extents_equal, value,  test_types, fixture)
{
  namespace ublas  = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto uplus1 = [](auto const& a){return a + value_t(1); };
  auto uplus2 = [](auto const& a){return value_t(2) + a; };
  auto bplus  = std::plus <value_t>{};
  auto bminus = std::minus<value_t>{};

  for_each_in_tuple(extents, [&](auto const& /*unused*/, auto& e){
    static constexpr auto size = std::tuple_size_v<std::decay_t<decltype(e)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;

    auto t = tensor_t(e);
    auto v = value_t{};
    for(auto& tt: t){ tt = v; v+=value_t{1}; }

    BOOST_CHECK( ublas::detail::all_extents_equal( t , e ) );

    // uexpr1 = t+1
    // uexpr2 = 2+t
    auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_t>( t, uplus1 );
    auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_t>( t, uplus2 );

    BOOST_CHECK( ublas::detail::all_extents_equal( uexpr1, e ) );
    BOOST_CHECK( ublas::detail::all_extents_equal( uexpr2, e ) );

    // bexpr_uexpr = (t+1) + (2+t)
    auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_t>( uexpr1, uexpr2, bplus );

    BOOST_CHECK( ublas::detail::all_extents_equal( bexpr_uexpr, e ) );

    // bexpr_bexpr_uexpr = ((t+1) + (2+t)) - t
    auto bexpr_bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_t>( bexpr_uexpr, t, bminus );

    BOOST_CHECK( ublas::detail::all_extents_equal( bexpr_bexpr_uexpr , e ) );

  });

  for_each_in_tuple(extents, [&](auto I, auto& e1){

    if ( I >= std::tuple_size_v<decltype(extents)> - 1){
      return;
    }

    static constexpr auto size1 = std::tuple_size_v<std::decay_t<decltype(e1)>>;
    using tensor_type1 = ublas::tensor_static_rank<value_t, size1, layout_t>;

    for_each_in_tuple(extents, [&](auto J, auto& e2){

      if( J != I + 1 ){
        return;
      }

      static constexpr auto size2 = std::tuple_size_v<std::decay_t<decltype(e2)>>;
      using tensor_type2 = ublas::tensor_static_rank<value_t, size2, layout_t>;

      auto v = value_t{};

      tensor_type1 t1(e1);
      for(auto& tt: t1){ tt = v; v+=value_t{1}; }

      tensor_type2 t2(e2);
      for(auto& tt: t2){ tt = v; v+=value_t{2}; }

      BOOST_CHECK( ublas::detail::all_extents_equal( t1, ublas::detail::retrieve_extents(t1) ) );
      BOOST_CHECK( ublas::detail::all_extents_equal( t2, ublas::detail::retrieve_extents(t2) ) );

      // uexpr1 = t1+1
      // uexpr2 = 2+t2
      auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type1>( t1, uplus1 );
      auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type2>( t2, uplus2 );

      BOOST_CHECK( ublas::detail::all_extents_equal( uexpr1, ublas::detail::retrieve_extents(uexpr1) ) );
      BOOST_CHECK( ublas::detail::all_extents_equal( uexpr2, ublas::detail::retrieve_extents(uexpr2) ) );

      if constexpr( size1 == size2 ){
        // bexpr_uexpr = (t1+1) + (2+t2)
        auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type1>( uexpr1, uexpr2, bplus );

        BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_uexpr, ublas::detail::retrieve_extents( bexpr_uexpr  ) ) );

        // bexpr_bexpr_uexpr = ((t1+1) + (2+t2)) - t2
        auto bexpr_bexpr_uexpr1 = ublas::detail::make_binary_tensor_expression<tensor_type1>( bexpr_uexpr, t2, bminus );

        BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr1, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr1  ) ) );

        // bexpr_bexpr_uexpr = t2 - ((t1+1) + (2+t2))
        auto bexpr_bexpr_uexpr2 = ublas::detail::make_binary_tensor_expression<tensor_type1>( t2, bexpr_uexpr, bminus );

        BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr2, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr2  ) ) );

        // bexpr_uexpr2 = (t1+1) + t2
        auto bexpr_uexpr2 = ublas::detail::make_binary_tensor_expression<tensor_type1>( uexpr1, t2, bplus );
        BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_uexpr2, ublas::detail::retrieve_extents( bexpr_uexpr2  ) ) );

        // bexpr_uexpr2 = ((t1+1) + t2) + t1
        auto bexpr_bexpr_uexpr3 = ublas::detail::make_binary_tensor_expression<tensor_type1>( bexpr_uexpr2, t1, bplus );
        BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr3, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr3  ) ) );

        // bexpr_uexpr2 = t1 + (((t1+1) + t2) + t1)
        auto bexpr_bexpr_uexpr4 = ublas::detail::make_binary_tensor_expression<tensor_type1>( t1, bexpr_bexpr_uexpr3, bplus );
        BOOST_CHECK( ! ublas::detail::all_extents_equal( bexpr_bexpr_uexpr4, ublas::detail::retrieve_extents( bexpr_bexpr_uexpr4  ) ) );
      }

    });
  });

}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2004 Jeremy Siek <jsiek@cs.indiana.edu>
// 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)
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/transitive_closure.hpp>
#include <iostream>
using namespace std;

using namespace boost;
typedef adjacency_list<> graph_t;

int main(int argc, char *argv[]) {
  graph_t g(5),g_TC;

  add_edge(0,2,g);
  add_edge(1,0,g);
  add_edge(1,2,g);
  add_edge(1,4,g);
  add_edge(3,0,g);
  add_edge(3,2,g);
  add_edge(4,2,g);
  add_edge(4,3,g);

  transitive_closure(g,g_TC);

  cout << "original graph: 0->2, 1->0, 1->2, 1->4, 3->0, 3->2, 4->2, 4->3"
       << endl;
  cout << "transitive closure: ";
  graph_t::edge_iterator i,iend;
  for(boost::tie(i,iend) = edges(g_TC);i!=iend;++i) {
    cout << source(*i,g_TC) << "->" << target(*i,g_TC) << " ";
  }
  cout << endl;
}


#ifndef HBasisForSurrogateModelBaseClass
#define HBasisForSurrogateModelBaseClass

#include "BlackBoxData.hpp"
#include <Eigen/Dense>
#include <vector>
#include <iostream>

//! Base class for definiton of basis for surrogate models
/*!
 Defines the required structure of basis for surrogate models to work with NOWPAC
*/
class BasisForSurrogateModelBaseClass {
  protected:
    //! Number of arguments of surrogate model
    int dim;
    //! Number of basic basis functions
    /*!
     Number of basis function, for example quadratic monomials,
     for surrogate basis functions
    */
    int nb_basis_functions;
    //! Coefficients of surrogate basis functions in terms of basic basis function
    std::vector<Eigen::VectorXd> basis_coefficients;
  public:
    //! Constructor
    /*!
     Constructor to set number of arguments (dimension) of the basis
     \param n Number of arguments (dimension)
    */
    BasisForSurrogateModelBaseClass ( int n ) : dim ( n ) { };
    //! Destructor
    ~BasisForSurrogateModelBaseClass ( ) { };
    //! Function to compute the basis coefficients for the basis functions
    /*!
     Function to compute the basis coefficients for the basis functions
     \param x vector vectors of nodes x[0], ..., x[n] contain the interpolation nodes
    */
    virtual void compute_basis_coefficients ( std::vector< std::vector<double> > const& ) = 0;
    //! Function to evaluate the basis functions
    /*!
     Function to evaluate the basis functions. It returns a vector of basis values at
     the point x.
     \param x point x at which the basis functions are evaluated
     \returns the values of all basis functions evaluated at point x
    */
    virtual std::vector<double> &evaluate ( std::vector<double> const& ) = 0;
    //! Returns the value of a basis function at zero
    /*!
     Returns the value of basis function i at zero
     \param i number of basis function whose value is queried
     \returns the value of the basis function i at zero
    */
    virtual double &value ( int ) = 0;
    //! Returns the gradient of a basis function zero
    /*!
     Returns the gradient of basis function i at zero
     \param i number of basis function whose gradient at zero is queried
     \returns the gradient of the basis function i at zero
    */
    virtual std::vector<double> &gradient ( int ) = 0;
    //! Returns the Hessian matrix of a basis function zero
    /*!
     Returns the Hessian matrix of basis function i at zero
     \param i number of basis function whose Hessian matrix at zero is queried
     \returns the Hessian matrix of the basis function i at zero
    */
    virtual std::vector< std::vector<double> > &hessian ( int ) = 0;
    //! Function to evaluate a basis function
    /*!
     Function to evaluate the i-th basis function. It returns the value of the i-th
     basis function.
     \param x point x at which the i-th basis function are evaluated
     \param i number of basis function to be evaluated
     \returns the value of the i-th bais function at point x
    */
    virtual double evaluate ( std::vector<double> const&, int) = 0;
    //! Function to query the dimension of the domain the basis functions
    /*!
     Function to query the dimension of the domain the basis functions
     \returns the dimension of the domain
    */
    int dimension ( ) { return dim; }
};

#endif


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

/*
 Copyright (C) 2015 Peter Caspers
 Copyright (C) 2015 Roland Lichters

 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 betaeta.hpp
    \brief Hagan / Woodward beta-eta model
*/

#ifndef quantlib_model_betaeta_hpp
#define quantlib_model_betaeta_hpp

#include <ql/experimental/models/betaetacore.hpp>
#include <ql/models/model.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/indexes/swapindex.hpp>
#include <ql/instruments/vanillaswap.hpp>

#include <boost/unordered_map.hpp>

namespace QuantLib {

/*! cf. Hagan, Woodward: Markov interest rate models,
    Applied Mathematical Finance 6, 233–260 (1999)

    We assume a reflecting barrier at x = -1/beta (cf.
    the last paragraph on p. 241).

    We assume a piecewise constant reversion \kappa and
    set \lambda(t) := (1-exp(-\kappa*t))/\kappa
    note that these are effective (integrated) rather
    than forward-forward reversions though
*/

// TODO there is a big overlap with the Gaussian1d model interface
// refactor this, i.e. create a common base class and build the
// engines on top of this

class BetaEta : public TermStructureConsistentModel,
                public CalibratedModel,
                public LazyObject {
  public:
    // constant mean reversion
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Real> &volatilities, const Real reversion,
            const Real beta, const Real eta);
    // piecewise mean reversion (with same step dates as volatilities)
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Real> &volatilities,
            const std::vector<Real> &reversions, const Real beta,
            const Real eta);
    // constant mean reversion with floating model data
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Handle<Quote> > &volatilities,
            const Handle<Quote> reversion, const Handle<Quote> beta,
            const Handle<Quote> eta);
    // piecewise mean reversion with floating model data
    BetaEta(const Handle<YieldTermStructure> &termStructure,
            const std::vector<Date> &volstepdates,
            const std::vector<Handle<Quote> > &volatilities,
            const std::vector<Handle<Quote> > &reversions,
            const Handle<Quote> beta, const Handle<Quote> eta);

    const Real numeraire(const Time t, const Real x = 0.0,
                         const Handle<YieldTermStructure> &yts =
                             Handle<YieldTermStructure>()) const;

    const Real zerobond(const Time T, const Time t = 0.0, const Real x = 0.0,
                        const Handle<YieldTermStructure> &yts =
                            Handle<YieldTermStructure>()) const;

    const Real numeraire(const Date &referenceDate, const Real y = 0.0,
                         const Handle<YieldTermStructure> &yts =
                             Handle<YieldTermStructure>()) const;
    const Real zerobond(const Date &maturity,
                        const Date &referenceDate = Null<Date>(),
                        const Real x = 0.0,
                        const Handle<YieldTermStructure> &yts =
                            Handle<YieldTermStructure>()) const;

    const Real forwardRate(const Date &fixing,
                           const Date &referenceDate = Null<Date>(),
                           const Real x = 0.0,
                           boost::shared_ptr<IborIndex> iborIdx =
                               boost::shared_ptr<IborIndex>()) const;

    const Real swapRate(const Date &fixing, const Period &tenor,
                        const Date &referenceDate = Null<Date>(),
                        const Real x = 0.0,
                        boost::shared_ptr<SwapIndex> swapIdx =
                            boost::shared_ptr<SwapIndex>()) const;

    const Real swapAnnuity(const Date &fixing, const Period &tenor,
                           const Date &referenceDate = Null<Date>(),
                           const Real x = 0.0,
                           boost::shared_ptr<SwapIndex> swapIdx =
                               boost::shared_ptr<SwapIndex>()) const;

    const Array &reversion() const { return reversion_.params(); }
    const Array &volatility() const { return sigma_.params(); }
    const Real beta() const { return pBeta_.params()[0]; }
    const Real eta() const { return pEta_.params()[0]; }

    /*! Generates a grid of values for the state variable $x$
       at time $T$ conditional on $x(t)=x$, covering stdDevs
       standard deviations assuming an approximate variance
       $\int_t^T \alpha^2$ for $x$. The grid consists of
       2*gridPoints+1 points */
    const Disposable<Array> xGrid(const Real stdDevs, const int gridPoints,
                                  const Real T = 1.0, const Real t = 0,
                                  const Real x = 0) const;

    /*! integrates f against the conditional density $x(t)|x(t0)=x0$
        covering stdDevs approximate standard deviations in the same
        sense as in the xGrid implementation */
    const Real integrate(const Real stdDevs,
                         const boost::function<Real(Real)> &f, const Real t0,
                         const Real x0, const Real t) const;

    /*! for testing purposes we can switch off the usage of tabulated values */
    const void useTabulation(const bool useTabulation) {
        if (useTabulation_ != useTabulation) {
            useTabulation_ = useTabulation;
            update();
        }
    }

    // calibration constraints
    // TODO what is of practical use here ?

    Disposable<std::vector<bool> > MoveVolatility(Size i) {
        QL_REQUIRE(i < volatilities_.size(),
                   "volatility with index " << i << " does not exist (0..."
                                            << volatilities_.size() - 1 << ")");
        std::vector<bool> res(reversions_.size() + volatilities_.size() + 2,
                              true);
        res[reversions_.size() + i] = false;
        return res;
    }

    // With fixed reversion, beta and eta calibrate the volatilities
    // one by one to the given helpers. The same comments as in the
    // corresonding method in Gsr hold.
    void calibrateVolatilitiesIterative(
        const std::vector<boost::shared_ptr<CalibrationHelper> > &helpers,
        OptimizationMethod &method, const EndCriteria &endCriteria,
        const Constraint &constraint = Constraint(),
        const std::vector<Real> &weights = std::vector<Real>()) {

        for (Size i = 0; i < helpers.size(); i++) {
            std::vector<boost::shared_ptr<CalibrationHelper> > h(1, helpers[i]);
            calibrate(h, method, endCriteria, constraint, weights,
                      MoveVolatility(i));
        }
    }

  private:
    void generateArguments() { notifyObservers(); }

    // see Gaussian1dModel

    struct CachedSwapKey {
        const boost::shared_ptr<SwapIndex> index;
        const Date fixing;
        const Period tenor;
        const bool operator==(const CachedSwapKey &o) const {
            return index->name() == o.index->name() && fixing == o.fixing &&
                   tenor == o.tenor;
        }
    };

    struct CachedSwapKeyHasher
        : std::unary_function<CachedSwapKey, std::size_t> {
        std::size_t operator()(CachedSwapKey const &x) const {
            std::size_t seed = 0;
            boost::hash_combine(seed, x.index->name());
            boost::hash_combine(seed, x.fixing.serialNumber());
            boost::hash_combine(seed, x.tenor.length());
            boost::hash_combine(seed, x.tenor.units());
            return seed;
        }
    };

    typedef boost::unordered_map<CachedSwapKey, boost::shared_ptr<VanillaSwap>,
                                 CachedSwapKeyHasher> CacheType;

    mutable CacheType swapCache_;

    // retrieve underlying swap from cache if possible, otherwise
    // create it and store it in the cache
    boost::shared_ptr<VanillaSwap>
    underlyingSwap(const boost::shared_ptr<SwapIndex> &index,
                   const Date &expiry, const Period &tenor) const {

        CachedSwapKey k = {index, expiry, tenor};
        CacheType::iterator i = swapCache_.find(k);
        if (i == swapCache_.end()) {
            boost::shared_ptr<VanillaSwap> underlying =
                index->clone(tenor)->underlyingSwap(expiry);
            swapCache_.insert(std::make_pair(k, underlying));
            return underlying;
        }
        return i->second;
    }

    void update() { LazyObject::update(); }

    void performCalculations() const {
        evaluationDate_ = Settings::instance().evaluationDate();
        enforcesTodaysHistoricFixings_ =
            Settings::instance().enforcesTodaysHistoricFixings();
        updateTimes();
    }

    void updateTimes() const;
    void updateVolatility();
    void updateReversion();
    void updateBeta();
    void updateEta();

    void initialize();

    Parameter &reversion_, &sigma_, &pBeta_, &pEta_;
    std::vector<Handle<Quote> > volatilities_;
    std::vector<Handle<Quote> > reversions_;
    Handle<Quote> beta_;
    Handle<Quote> eta_;
    mutable Real betaLink_, etaLink_; // redundant, just used as a link to
                                      // the core computation class

    std::vector<Date> volstepdates_; // these are shared between volatilities
                                     // and reversions in case of piecewise
                                     // reversions
    mutable std::vector<Time> volsteptimes_;
    mutable Array volsteptimesArray_; // redundant, just used as a link to
                                      // the core computation class

    mutable Date evaluationDate_;
    mutable bool enforcesTodaysHistoricFixings_;

    boost::shared_ptr<BetaEtaCore> core_;
    boost::shared_ptr<Integrator> integrator_, integrator2_;

    class integrand {
      public:
        integrand(const Real t0, const Real x0, const Real t,
                  const boost::function<Real(Real)> &f, const BetaEtaCore &core)
            : t0_(t0), x0_(x0), t_(t), f_(f), core_(core) {}
        Real operator()(Real x) const {
            return f_(x) * core_.p(t0_, x0_, t_, x);
        }

      private:
        const Real t0_, x0_, t_;
        const boost::function<Real(Real)> &f_;
        const BetaEtaCore &core_;
    };
    friend class integrand;

    bool useTabulation_; // for testing, normally it should be true

    struct VolatilityObserver : public Observer {
        VolatilityObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateVolatility(); }
        BetaEta *p_;
    };
    struct ReversionObserver : public Observer {
        ReversionObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateReversion(); }
        BetaEta *p_;
    };
    struct BetaObserver : public Observer {
        BetaObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateBeta(); }
        BetaEta *p_;
    };
    struct EtaObserver : public Observer {
        EtaObserver(BetaEta *p) : p_(p) {}
        void update() { p_->updateEta(); }
        BetaEta *p_;
    };

    boost::shared_ptr<VolatilityObserver> volatilityObserver_;
    boost::shared_ptr<ReversionObserver> reversionObserver_;
    boost::shared_ptr<BetaObserver> betaObserver_;
    boost::shared_ptr<EtaObserver> etaObserver_;
};

// implementation

inline const Real
BetaEta::numeraire(const Date &referenceDate, const Real x,
                   const Handle<YieldTermStructure> &yts) const {

    return numeraire(termStructure()->timeFromReference(referenceDate), x, yts);
}

inline const Real
BetaEta::zerobond(const Date &maturity, const Date &referenceDate, const Real x,
                  const Handle<YieldTermStructure> &yts) const {

    return zerobond(termStructure()->timeFromReference(maturity),
                    referenceDate != Null<Date>()
                        ? termStructure()->timeFromReference(referenceDate)
                        : 0.0,
                    x, yts);
}

inline const Real BetaEta::integrate(const Real stdDevs,
                                     const boost::function<Real(Real)> &f,
                                     const Real t0, const Real x0,
                                     const Real t) const {
    Real s = std::sqrt(core_->tau(t0, t));
    integrand phi(t0, x0, t, f, *core_);
    Real result;
    // left integration bound should be greater or equal to barrier
    Real a = std::max(x0 - stdDevs * s, -1.0 / beta_->value());
    Real b = x0 + stdDevs * s;
    try {
        result = (*integrator_)(phi, a, b);
    } catch (QuantLib::Error) {
        result = (*integrator2_)(phi, a, b);
    }

    // singular term

    result +=
        core_->prob_y_0(t0, x0, t, useTabulation_) * f(-1.0 / beta_->value());
    return result;
}

} // namespace QuantLib

#endif


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to 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)

#include <cstdint>
#include <cinttypes>
#include <boost/math/ccmath/div.hpp>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/multiprecision/cpp_int.hpp>

template <typename Z>
constexpr void test()
{
    constexpr auto test_val1 = boost::math::ccmath::div(Z(1), Z(1));
    static_assert(test_val1.quot == Z(1));
    static_assert(test_val1.rem == Z(0));

    constexpr auto test_val2 = boost::math::ccmath::div(Z(1'000'000), Z(3));
    static_assert(test_val2.quot == Z(333'333));
    static_assert(test_val2.rem == Z(1));
}

#if !defined(BOOST_MATH_NO_CONSTEXPR_DETECTION) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
int main()
{
    test<int>();
    test<long>();
    test<long long>();
    test<std::intmax_t>();

    test<std::int32_t>();
    test<std::int64_t>();
    test<std::uint32_t>();

    test<boost::multiprecision::int128_t>();
    test<boost::multiprecision::int256_t>();
    test<boost::multiprecision::int512_t>();

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


//
// Copyright (c) 2015-2018 CNRS
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <boost/test/unit_test.hpp>

#include <iostream>

//#define __SSE3__
#include <fenv.h>
#ifdef __SSE3__
#include <pmmintrin.h>
#endif

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_against_algo )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);
  pinocchio::Data data(model); data.M.fill (0.);
  pinocchio::Data data_other(model); data_other.M.fill (0.);

  VectorXd q (VectorXd::Random(model.nq));
  VectorXd v (VectorXd::Random(model.nv));

  // -------
  q.setZero ();
  v.setZero ();

  computeAllTerms(model,data,q,v);

  nonLinearEffects(model,data_other,q,v);
  crba(model,data_other,q);
  getJacobianComFromCrba(model, data_other);
  computeJointJacobians(model,data_other,q);
  centerOfMass(model, data_other, q, v, true);
  kineticEnergy(model, data_other, q, v, true);
  potentialEnergy(model, data_other, q, true);

  BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
  BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
              .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
  BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
  BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));

  for (int k=0; k<model.njoints; ++k)
  {
    BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
    BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
    BOOST_CHECK_CLOSE(data.mass[(size_t)k], data_other.mass[(size_t)k], 1e-12);
  }

  BOOST_CHECK_CLOSE(data.kinetic_energy, data_other.kinetic_energy, 1e-12);
  BOOST_CHECK_CLOSE(data.potential_energy, data_other.potential_energy, 1e-12);

  // -------
  q.setZero ();
  v.setOnes ();

  computeAllTerms(model,data,q,v);

  nonLinearEffects(model,data_other,q,v);
  crba(model,data_other,q);
  getJacobianComFromCrba(model, data_other);
  computeJointJacobians(model,data_other,q);
  centerOfMass(model, data_other, q, v, true);
  kineticEnergy(model, data_other, q, v, true);
  potentialEnergy(model, data_other, q, true);

  BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
  BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
              .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
  BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
  BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));

  for (int k=0; k<model.njoints; ++k)
  {
    BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
    BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
    BOOST_CHECK_CLOSE(data.mass[(size_t)k], data_other.mass[(size_t)k], 1e-12);
  }

  BOOST_CHECK_CLOSE(data.kinetic_energy, data_other.kinetic_energy, 1e-12);
  BOOST_CHECK_CLOSE(data.potential_energy, data_other.potential_energy, 1e-12);

//   -------
  q.setOnes ();
  q.segment<4> (3).normalize();
  v.setOnes ();

  computeAllTerms(model,data,q,v);

  nonLinearEffects(model,data_other,q,v);
  crba(model,data_other,q);
  getJacobianComFromCrba(model, data_other);
  computeJointJacobians(model,data_other,q);
  centerOfMass(model, data_other, q, v, true);
  kineticEnergy(model, data_other, q, v, true);
  potentialEnergy(model, data_other, q, true);

  BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
  BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
              .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
  BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
  BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));

  for (int k=0; k<model.njoints; ++k)
  {
    BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
    BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
    BOOST_CHECK_CLOSE(data.mass[(size_t)k], data_other.mass[(size_t)k], 1e-12);
  }

  BOOST_CHECK_CLOSE(data.kinetic_energy, data_other.kinetic_energy, 1e-12);
  BOOST_CHECK_CLOSE(data.potential_energy, data_other.potential_energy, 1e-12);

  // -------
  q.setRandom ();
  q.segment<4> (3).normalize();
  v.setRandom ();

  computeAllTerms(model,data,q,v);

  nonLinearEffects(model,data_other,q,v);
  crba(model,data_other,q);
  getJacobianComFromCrba(model, data_other);
  computeJointJacobians(model,data_other,q);
  centerOfMass(model, data_other, q, v, true);
  kineticEnergy(model, data_other, q, v, true);
  potentialEnergy(model, data_other, q, true);

  BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
  BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
              .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
  BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
  BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));

  for (int k=0; k<model.njoints; ++k)
  {
    BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
    BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
    BOOST_CHECK_CLOSE(data.mass[(size_t)k], data_other.mass[(size_t)k], 1e-12);
  }

  BOOST_CHECK_CLOSE(data.kinetic_energy, data_other.kinetic_energy, 1e-12);
  BOOST_CHECK_CLOSE(data.potential_energy, data_other.potential_energy, 1e-12);
}

BOOST_AUTO_TEST_SUITE_END ()