/*
* Copyright (c) by CryptoLab inc.
* This program is licensed under a
* Creative Commons Attribution-NonCommercial 3.0 Unported License.
* You should have received a copy of the license along with this
* work.  If not, see <http://creativecommons.org/licenses/by-nc/3.0/>.
*/
#include "TestScheme.h"

#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ.h>

#include "Ciphertext.h"
#include "EvaluatorUtils.h"
#include "Ring.h"
#include "Scheme.h"
#include "SchemeAlgo.h"
#include "SecretKey.h"
#include "StringUtils.h"
#include "TimeUtils.h"
#include "SerializationUtils.h"

using namespace std;
using namespace NTL;

//----------------------------------------------------------------------------------
//   STANDARD TESTS
//----------------------------------------------------------------------------------

void TestScheme::testEncrypt(long logq, long logp, long logn) {
	cout << "!!! START TEST ENCRYPT !!!" << endl;
	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	long n = (1 << logn);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	Ciphertext cipher;

	timeutils.start("Encrypt");
	scheme.encrypt(cipher, mvec, n, logp, logq);
	timeutils.stop("Encrypt");

	timeutils.start("Decrypt");
	complex<double>* dvec = scheme.decrypt(secretKey, cipher);
	timeutils.stop("Decrypt");

	StringUtils::compare(mvec, dvec, n, "val");

	cout << "!!! END TEST ENCRYPT !!!" << endl;
}

void TestScheme::testEncryptSingle(long logq, long logp) {
	cout << "!!! START TEST ENCRYPT SINGLE !!!" << endl;
	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	complex<double> mval = EvaluatorUtils::randomComplex();
	Ciphertext cipher;

	timeutils.start("Encrypt Single");
	scheme.encryptSingle(cipher, mval, logp, logq);
	timeutils.stop("Encrypt Single");

	complex<double> dval = scheme.decryptSingle(secretKey, cipher);

	StringUtils::compare(mval, dval, "val");

	cout << "!!! END TEST ENCRYPT SINGLE !!!" << endl;
}

void TestScheme::testAdd(long logq, long logp, long logn) {
	cout << "!!! START TEST ADD !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	long n = (1 << logn);
	complex<double>* mvec1 = EvaluatorUtils::randomComplexArray(n);
	complex<double>* mvec2 = EvaluatorUtils::randomComplexArray(n);
	complex<double>* madd = new complex<double>[n];

	for(long i = 0; i < n; i++) {
		madd[i] = mvec1[i] + mvec2[i];
	}

	Ciphertext cipher1, cipher2;
	scheme.encrypt(cipher1, mvec1, n, logp, logq);
	scheme.encrypt(cipher2, mvec2, n, logp, logq);

	timeutils.start("Addition");
	scheme.addAndEqual(cipher1, cipher2);
	timeutils.stop("Addition");

	complex<double>* dadd = scheme.decrypt(secretKey, cipher1);

	StringUtils::compare(madd, dadd, n, "add");

	cout << "!!! END TEST ADD !!!" << endl;
}

void TestScheme::testMult(long logq, long logp, long logn) {
	cout << "!!! START TEST MULT !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	long n = (1 << logn);
	complex<double>* mvec1 = EvaluatorUtils::randomComplexArray(n);
	complex<double>* mvec2 = EvaluatorUtils::randomComplexArray(n);
	complex<double>* mmult = new complex<double>[n];
	for(long i = 0; i < n; i++) {
		mmult[i] = mvec1[i] * mvec2[i];
	}

	Ciphertext cipher1, cipher2;
	scheme.encrypt(cipher1, mvec1, n, logp, logq);
	scheme.encrypt(cipher2, mvec2, n, logp, logq);

	timeutils.start("Multiplication");
	scheme.multAndEqual(cipher1, cipher2);
	timeutils.stop("Multiplication");

	complex<double>* dmult = scheme.decrypt(secretKey, cipher1);

	StringUtils::compare(mmult, dmult, n, "mult");

	cout << "!!! END TEST MULT !!!" << endl;
}

void TestScheme::testiMult(long logq, long logp, long logn) {
	cout << "!!! START TEST i MULTIPLICATION !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	long n = (1 << logn);

	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	complex<double>* imvec = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		imvec[i].real(-mvec[i].imag());
		imvec[i].imag(mvec[i].real());
	}

	Ciphertext cipher;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start("Multiplication by i");
	scheme.imultAndEqual(cipher);
	timeutils.stop("Multiplication by i");

	complex<double>* idvec = scheme.decrypt(secretKey, cipher);

	StringUtils::compare(imvec, idvec, n, "imult");

	cout << "!!! END TEST i MULTIPLICATION !!!" << endl;
}

//----------------------------------------------------------------------------------
//   ROTATE & CONJUGATE
//----------------------------------------------------------------------------------

void TestScheme::testRotateFast(long logq, long logp, long logn, long logr) {
	cout << "!!! START TEST ROTATE FAST !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	long n = (1 << logn);
	long r = (1 << logr);
	scheme.addLeftRotKey(secretKey, r);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	Ciphertext cipher;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start("Left Rotate Fast");
	scheme.leftRotateFastAndEqual(cipher, r);
	timeutils.stop("Left Rotate Fast");

	complex<double>* dvec = scheme.decrypt(secretKey, cipher);

	EvaluatorUtils::leftRotateAndEqual(mvec, n, r);
	StringUtils::compare(mvec, dvec, n, "rot");

	cout << "!!! END TEST ROTATE BY POWER OF 2 BATCH !!!" << endl;
}

void TestScheme::testConjugate(long logq, long logp, long logn) {
	cout << "!!! START TEST CONJUGATE !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	scheme.addConjKey(secretKey);

	long n = (1 << logn);

	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	complex<double>* mvecconj = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		mvecconj[i] = conj(mvec[i]);
	}

	Ciphertext cipher;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start("Conjugate");
	scheme.conjugateAndEqual(cipher);
	timeutils.stop("Conjugate");

	complex<double>* dvecconj = scheme.decrypt(secretKey, cipher);
	StringUtils::compare(mvecconj, dvecconj, n, "conj");

	cout << "!!! END TEST CONJUGATE !!!" << endl;
}

//----------------------------------------------------------------------------------
//   POWER & PRODUCT TESTS
//----------------------------------------------------------------------------------

void TestScheme::testPowerOf2(long logq, long logp, long logn, long logdeg) {
	cout << "!!! START TEST POWER OF 2 !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;
	long degree = 1 << logdeg;
	complex<double>* mvec = new complex<double>[n];
	complex<double>* mpow = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		mvec[i] = EvaluatorUtils::randomCircle();
		mpow[i] = pow(mvec[i], degree);
	}

	Ciphertext cipher, cpow;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start("Power of 2");
	algo.powerOf2(cpow, cipher, logp, logdeg);
	timeutils.stop("Power of 2");

	complex<double>* dpow = scheme.decrypt(secretKey, cpow);
	StringUtils::compare(mpow, dpow, n, "pow2");

	cout << "!!! END TEST POWER OF 2 !!!" << endl;
}

//-----------------------------------------

void TestScheme::testPower(long logq, long logp, long logn, long degree) {
	cout << "!!! START TEST POWER !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;
	complex<double>* mvec = EvaluatorUtils::randomCircleArray(n);
	complex<double>* mpow = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		mpow[i] = pow(mvec[i], degree);
	}

	Ciphertext cipher, cpow;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start("Power");
	algo.power(cpow, cipher, logp, degree);
	timeutils.stop("Power");

	complex<double>* dpow = scheme.decrypt(secretKey, cpow);
	StringUtils::compare(mpow, dpow, n, "pow");

	cout << "!!! END TEST POWER !!!" << endl;
}

//----------------------------------------------------------------------------------
//   FUNCTION TESTS
//----------------------------------------------------------------------------------

void TestScheme::testInverse(long logq, long logp, long logn, long steps) {
	cout << "!!! START TEST INVERSE !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;
	complex<double>* mvec = EvaluatorUtils::randomCircleArray(n, 0.1);
	complex<double>* minv = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		minv[i] = 1. / mvec[i];
	}

	Ciphertext cipher, cinv;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start("Inverse");
	algo.inverse(cinv, cipher, logp, steps);
	timeutils.stop("Inverse");

	complex<double>* dinv = scheme.decrypt(secretKey, cinv);
	StringUtils::compare(minv, dinv, n, "inv");

	cout << "!!! END TEST INVERSE !!!" << endl;
}

void TestScheme::testLogarithm(long logq, long logp, long logn, long degree) {
	cout << "!!! START TEST LOGARITHM !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n, 0.1);
	complex<double>* mlog = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		mlog[i] = log(mvec[i] + 1.);
	}

	Ciphertext cipher, clog;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start(LOGARITHM);
	algo.function(clog, cipher, LOGARITHM, logp, degree);
	timeutils.stop(LOGARITHM);

	complex<double>* dlog = scheme.decrypt(secretKey, clog);
	StringUtils::compare(mlog, dlog, n, LOGARITHM);

	cout << "!!! END TEST LOGARITHM !!!" << endl;
}

void TestScheme::testExponent(long logq, long logp, long logn, long degree) {
	cout << "!!! START TEST EXPONENT !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	complex<double>* mexp = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		mexp[i] = exp(mvec[i]);
	}

	Ciphertext cipher, cexp;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start(EXPONENT);
	algo.function(cexp, cipher, EXPONENT, logp, degree);
	timeutils.stop(EXPONENT);

	complex<double>* dexp = scheme.decrypt(secretKey, cexp);
	StringUtils::compare(mexp, dexp, n, EXPONENT);

	cout << "!!! END TEST EXPONENT !!!" << endl;
}

void TestScheme::testExponentLazy(long logq, long logp, long logn, long degree) {
	cout << "!!! START TEST EXPONENT LAZY !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	complex<double>* mexp = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		mexp[i] = exp(mvec[i]);
	}
	Ciphertext cipher, cexp;
	scheme.encrypt(cipher, mvec, n, logp, logQ);

	timeutils.start(EXPONENT + " lazy");
	algo.functionLazy(cexp, cipher, EXPONENT, logp, degree);
	timeutils.stop(EXPONENT + " lazy");

	complex<double>* dexp = scheme.decrypt(secretKey, cexp);
	StringUtils::compare(mexp, dexp, n, EXPONENT);

	cout << "!!! END TEST EXPONENT LAZY !!!" << endl;
}

//-----------------------------------------

void TestScheme::testSigmoid(long logq, long logp, long logn, long degree) {
	cout << "!!! START TEST SIGMOID !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;

	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	complex<double>* msig = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		msig[i] = exp(mvec[i]) / (1. + exp(mvec[i]));
	}

	Ciphertext cipher, csig;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start(SIGMOID);
	algo.function(csig, cipher, SIGMOID, logp, degree);
	timeutils.stop(SIGMOID);

	complex<double>* dsig = scheme.decrypt(secretKey, csig);
	StringUtils::compare(msig, dsig, n, SIGMOID);

	cout << "!!! END TEST SIGMOID !!!" << endl;
}

void TestScheme::testSigmoidLazy(long logq, long logp, long logn, long degree) {
	cout << "!!! START TEST SIGMOID LAZY !!!" << endl;

	srand(time(NULL));
//	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);
	SchemeAlgo algo(scheme);

	long n = 1 << logn;
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(n);
	complex<double>* msig = new complex<double>[n];
	for (long i = 0; i < n; ++i) {
		msig[i] = exp(mvec[i]) / (1. + exp(mvec[i]));
	}

	Ciphertext cipher, csig;
	scheme.encrypt(cipher, mvec, n, logp, logq);

	timeutils.start(SIGMOID + " lazy");
	algo.functionLazy(csig, cipher, SIGMOID, logp, degree);
	timeutils.stop(SIGMOID + " lazy");

	complex<double>* dsig = scheme.decrypt(secretKey, csig);
	StringUtils::compare(msig, dsig, n, SIGMOID);

	cout << "!!! END TEST SIGMOID LAZY !!!" << endl;
}

void TestScheme::testWriteAndRead(long logq, long logp, long logSlots) {
	cout << "!!! START TEST WRITE AND READ !!!" << endl;

	cout << "!!! END TEST WRITE AND READ !!!" << endl;
}

void TestScheme::testBootstrap(long logq, long logp, long logSlots, long logT) {
	cout << "!!! START TEST BOOTSTRAP !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	timeutils.start("Key generating");
	scheme.addBootKey(secretKey, logSlots, logq + 4);
	timeutils.stop("Key generated");

	long slots = (1 << logSlots);
	complex<double>* mvec = EvaluatorUtils::randomComplexArray(slots);

	Ciphertext cipher;
	scheme.encrypt(cipher, mvec, slots, logp, logq);

	cout << "cipher logq before: " << cipher.logq << endl;

	scheme.modDownToAndEqual(cipher, logq);
	scheme.normalizeAndEqual(cipher);
	cipher.logq = logQ;
	cipher.logp = logq + 4;

	Ciphertext rot;
	timeutils.start("SubSum");
	for (long i = logSlots; i < logNh; ++i) {
		scheme.leftRotateFast(rot, cipher, (1 << i));
		scheme.addAndEqual(cipher, rot);
	}
	scheme.divByPo2AndEqual(cipher, logNh);
	timeutils.stop("SubSum");

	timeutils.start("CoeffToSlot");
	scheme.coeffToSlotAndEqual(cipher);
	timeutils.stop("CoeffToSlot");

	timeutils.start("EvalExp");
	scheme.evalExpAndEqual(cipher, logT);
	timeutils.stop("EvalExp");

	timeutils.start("SlotToCoeff");
	scheme.slotToCoeffAndEqual(cipher);
	timeutils.stop("SlotToCoeff");

	cipher.logp = logp;
	cout << "cipher logq after: " << cipher.logq << endl;

	complex<double>* dvec = scheme.decrypt(secretKey, cipher);

	StringUtils::compare(mvec, dvec, slots, "boot");

	cout << "!!! END TEST BOOTSRTAP !!!" << endl;
}

void TestScheme::testBootstrapSingleReal(long logq, long logp, long logT) {
	cout << "!!! START TEST BOOTSTRAP SINGLE REAL !!!" << endl;

	srand(time(NULL));
	SetNumThreads(8);
	TimeUtils timeutils;
	Ring ring;
	SecretKey secretKey(ring);
	Scheme scheme(secretKey, ring);

	timeutils.start("Key generating");
	scheme.addBootKey(secretKey, 0, logq + 4);
	timeutils.stop("Key generated");

	double mval = EvaluatorUtils::randomReal();

	Ciphertext cipher;
	scheme.encryptSingle(cipher, mval, logp, logq);

	cout << "cipher logq before: " << cipher.logq << endl;
	scheme.modDownToAndEqual(cipher, logq);
	scheme.normalizeAndEqual(cipher);
	cipher.logq = logQ;

	Ciphertext rot, cconj;
	timeutils.start("SubSum");
	for (long i = 0; i < logNh; ++i) {
		scheme.leftRotateFast(rot, cipher, 1 << i);
		scheme.addAndEqual(cipher, rot);
	}
	scheme.conjugate(cconj, cipher);
	scheme.addAndEqual(cipher, cconj);
	scheme.divByPo2AndEqual(cipher, logN);
	timeutils.stop("SubSum");

	timeutils.start("EvalExp");
	scheme.evalExpAndEqual(cipher, logT);
	timeutils.stop("EvalExp");

	cout << "cipher logq after: " << cipher.logq << endl;

	cipher.logp = logp;
	complex<double> dval = scheme.decryptSingle(secretKey, cipher);

	StringUtils::compare(mval, dval.real(), "boot");

	cout << "!!! END TEST BOOTSRTAP SINGLE REAL !!!" << endl;
}


#ifndef HAMILTONIANS_KITAEVHEX_HPP
#define HAMILTONIANS_KITAEVHEX_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

class KitaevHex
{
private:
	const int n_;
	const int m_;
	const double Jx_;
	const double Jy_;
	const double Jz_;

	const double h_;

public:

	KitaevHex(int n, int m, double J, double h = 0.0)
		: n_(n), m_(m), Jx_(J), Jy_(J), Jz_(J), h_(h)
	{
		assert(n % 2 == 0);
		assert(m % 2 == 0);
	}

	KitaevHex(int n, int m, double Jx, double Jy, double Jz)
		: n_(n), m_(m), Jx_(Jx), Jy_(Jy), Jz_(Jz), h_{0.0}
	{
		assert(n % 2 == 0);
		assert(m % 2 == 0);
	}

	int blackIdx(int row, int col) const
	{
		const int k = n_/2;
		row = ((row%m_) + m_) % m_;
		col = ((col%k) + k) % k;
		return row*n_ + col;
	}
	int whiteIdx(int row, int col) const
	{
		const int k = n_/2;
		row = ((row%m_) + m_) % m_;
		col = ((col%k) + k) % k;
		return row*n_ + col + (n_/2);
	}

	std::vector<std::pair<int,int> > xLinks() const
	{
		std::vector<std::pair<int,int> > res;
		const int k = n_/2;
		for(int row = 0; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row, i), whiteIdx(row, i-1));
			}
		}
		for(int row = 1; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row,i), whiteIdx(row,i));
			}
		}
		return res;
	}

	std::vector<std::pair<int,int> > yLinks() const
	{
		std::vector<std::pair<int,int> > res;
		const int k = n_/2;
		for(int row = 0; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row, i), whiteIdx(row,i));
			}
		}
		for(int row = 1; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row, i), whiteIdx(row,i+1));
			}
		}

		return res;
	}

	std::vector<std::pair<int,int> > zLinks() const
	{
		std::vector<std::pair<int,int> > res;
		const int k = n_/2;
		for(int row = 0; row < m_; row++)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(whiteIdx(row, i), blackIdx(row+1,i));
			}
		}
		return res;
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "KITAEVHEX"},
			{"Jx", Jx_},
			{"Jy", Jy_},
			{"Jz", Jz_},
			{"h", h_},
			{"n", n_},
			{"m", m_},
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		typename State::Scalar s = 0.0;
		constexpr std::complex<double> I(0.,1.);

		for(int i = 0; i < n_*m_; ++i)
		{
			//s += h_*smp.sigmaAt(i); //hz
			s += h_*smp.ratio(i); //hx
		}

		for(auto &xx: xLinks())
		{
			s += Jx_*smp.ratio(xx.first, xx.second); //xx
		}
		for(auto &yy: yLinks())
		{
			int zzval = smp.sigmaAt(yy.first)*smp.sigmaAt(yy.second);
			s += -Jy_*zzval*smp.ratio(yy.first, yy.second); //yy
		}
		for(auto &zz: zLinks())
		{
			int zzval = smp.sigmaAt(zz.first)*smp.sigmaAt(zz.second);
			s += Jz_*zzval; //yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> m;

		for(int i = 0; i < n_*m_; ++i)
		{
			double k = (1-2*int((col >> i) & 1));
			//m[col] += h_*k; //hz
			m[col ^ (1<<i)] += h_; //hx
		}

		for(auto &xx: xLinks())
		{
			int t = (1 << xx.first) | (1 << xx.second);
			m[col ^ t] += Jx_; //xx
		}
		for(auto &yy: yLinks())
		{
			int zzval = (1-2*int((col >> yy.first) & 1))*(1-2*int((col >> yy.second) & 1));
			int t = (1 << yy.first) | (1 << yy.second);
			m[col ^ t] += -Jy_*zzval; //yy
		}
		for(auto &zz: zLinks())
		{
			int zzval = (1-2*int((col >> zz.first) & 1))*(1-2*int((col >> zz.second) & 1));
			m[col] += Jz_*zzval; //zz
		}

		return m;
	}
};
#endif//HAMILTONIANS_KITAEVHEX_HPP


#ifndef TURBOTRACK_HPP
#define TURBOTRACK_HPP

#if defined TURBOTRACK_USE_EIGEN
	#include <Eigen/Dense>

	namespace turbotrack {
		using vec2 = Eigen::Vector2f;
		using vec3 = Eigen::Vector3f;
		using quat = Eigen::Quaternionf;
	}
#elif defined TURBOTRACK_USE_GLM
	#define GLM_ENABLE_EXPERIMENTAL
	#include <glm/ext.hpp>
	#include <glm/glm.hpp>
	#include <glm/gtc/quaternion.hpp>

	namespace turbotrack {
		using vec2 = glm::vec2;
		using vec3 = glm::vec3;
		using quat = glm::quat;
	}
#endif

namespace turbotrack {

enum class TrackballType {
	// chen_et_al, not implemented yet
	shoemake,
	holroyd
};

// See equation 33 in Henriksen et al.
vec3 shoemake_projection(const vec2 &mouse, float radius);

// See equation 46 in Henriksen et al.
vec3 holroyd_projection(const vec2 &mouse, float radius);

quat mouse_move(const vec2 &old_pos,
                const vec2 &new_pos, float radius = 1.0,
                TrackballType type = TrackballType::holroyd);

} // namespace turbotrack

#endif // TURBOTRACK_HPP


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 <fcppt/strong_typedef.hpp>
#include <fcppt/text.hpp>
#include <fcppt/io/cout.hpp>
#include <fcppt/random/variate.hpp>
#include <fcppt/random/distribution/basic.hpp>
#include <fcppt/random/distribution/parameters/uniform_int.hpp>
#include <fcppt/random/generator/minstd_rand.hpp>
#include <fcppt/random/generator/seed_from_chrono.hpp>
#include <fcppt/type_iso/boost_units.hpp>
#include <fcppt/type_iso/strong_typedef.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <fcppt/config/external_end.hpp>

int
main()
{
//![random_complex_distribution]
	typedef
	boost::units::quantity<
		boost::units::si::length,
		int
	> meter;

	FCPPT_MAKE_STRONG_TYPEDEF(
		meter,
		radius
	);

	typedef
	fcppt::random::distribution::basic<
		fcppt::random::distribution::parameters::uniform_int<
			radius
		>
	>
	distribution;
//![random_complex_distribution]

	typedef
	fcppt::random::generator::minstd_rand
	generator_type;

	generator_type generator(
		fcppt::random::generator::seed_from_chrono<
			generator_type::seed
		>()
	);

	typedef fcppt::random::variate<
		generator_type,
		distribution
	> variate;

//![random_complex_variate]
	variate rng(
		generator,
		distribution(
			distribution::param_type::min(
				radius(
					0 * boost::units::si::meter
				)
			),
			distribution::param_type::max(
				radius(
					10 * boost::units::si::meter
				)
			)
		)
	);
//![random_complex_variate]

//![random_complex_output]
	for(
		unsigned i = 0;
		i < 10;
		++i
	)
		fcppt::io::cout()
			<< rng().get().value()
			<< FCPPT_TEXT(' ');
//![random_complex_output]

	fcppt::io::cout()
		<< FCPPT_TEXT('\n');

	typedef
	fcppt::random::distribution::basic<
		fcppt::random::distribution::parameters::uniform_int<
			meter
		>
	>
	meter_distribution;

	typedef fcppt::random::variate<
		generator_type,
		meter_distribution
	> meter_variate;

	meter_variate meter_rng(
		generator,
		meter_distribution(
			meter_distribution::param_type::min(
				0 * boost::units::si::meter
			),
			meter_distribution::param_type::max(
				10 * boost::units::si::meter
			)
		)
	);

	for(
		unsigned i = 0;
		i < 10;
		++i
	)
		fcppt::io::cout()
			<< meter_rng().value()
			<< FCPPT_TEXT(' ');

	fcppt::io::cout()
		<< FCPPT_TEXT('\n');
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
typedef Matrix<float,1,Dynamic> MatrixType;
typedef Map<MatrixType> MapType;
typedef Map<const MatrixType> MapTypeConst;   // a read-only map
const int n_dims = 5;

MatrixType m1(n_dims), m2(n_dims);
m1.setRandom();
m2.setRandom();
float *p = &m2(0);  // get the address storing the data for m2
MapType m2map(p,m2.size());   // m2map shares data with m2
MapTypeConst m2mapconst(p,m2.size());  // a read-only accessor for m2

cout << "m1: " << m1 << endl;
cout << "m2: " << m2 << endl;
cout << "Squared euclidean distance: " << (m1-m2).squaredNorm() << endl;
cout << "Squared euclidean distance, using map: " <<
  (m1-m2map).squaredNorm() << endl;
m2map(3) = 7;   // this will change m2, since they share the same array
cout << "Updated m2: " << m2 << endl;
cout << "m2 coefficient 2, constant accessor: " << m2mapconst(2) << endl;
/* m2mapconst(2) = 5; */   // this yields a compile-time error

}
  return 0;
}


// include statements
#include <iostream>
#include <cstddef>
#include <cstdlib>
#include <ctime>
#include <limits>
#include <cmath>
#include <random>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include "matplotlibcpp.h"

// Node definiton
struct Node{
    float x;
    float y;
    Node *parent;
};

//array of all nodes
Node node_list[1000];

// counter to keep track of no of elements in node_list
int counter=0;

// for finding the final path
Node location[500];

// for back propagation
int backprop = 0;

//Obstcale Points
//change point to change the location of obstacle
int obstacle_array[][2]={{4,4}, {5, 4}, {5, 6}, {6, 6}, {6,7}, {5, 7}, {5,8}, {4,8}, {4,7}, {3,7}, {3,6}, {4,6}};

//no of elements in obstacle array
int obstacle_size = sizeof(obstacle_array)/sizeof(obstacle_array[0]);

//  sampling probablity to set rate of sampling
float sampling_probablity;
// variable for sampling radius
float sampling_radius;

// function to sample random points, the rate of sampling is biased by using a sampling probablity
Node sampler(int goal_x, int goal_y, float prob)
{
    std::random_device rd; //Will be used to obtain a seed for the random number engine
    std::mt19937 generator(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<float> distribution_biased(0.0, 1.0);
    std::uniform_real_distribution<float> distribution_x(0.0, 15.0);
	std::uniform_real_distribution<float> distribution_y(0.0, 15.0);
    float random_prob = distribution_biased(generator);
    Node sample_node={};
    if(random_prob>prob)
    {
        sample_node.x= distribution_x(generator);
        sample_node.y= distribution_y(generator);
        sample_node.parent= NULL;
    }
    else
    {
        sample_node.x= goal_x;
        sample_node.y= goal_y;
        sample_node.parent= NULL;
    }

    return sample_node;
}

//finds distance between two Node's
float distance(Node node1,Node node2)
{
    float dist=sqrt(((node1.x-node2.x)*(node1.x-node2.x))+((node1.y-node2.y)*(node1.y-node2.y)));
    return dist;
}

//function to find the nearest node
int find_nearest(Node current)
{
    int nearest_node;
    float minimum_dist= 20;
    for(int i=0;i<counter;i++)
    {
        float dist=distance(current,node_list[i]);
        if(dist<minimum_dist)
        {
            minimum_dist=dist;
            nearest_node=i;
        }
    }

    return nearest_node;

}

//checks intersection between obstacles and path
bool check_collision(Node end1, Node end2)
{

    typedef boost::geometry::model::d2::point_xy<double> point_xy;
    typedef boost::geometry::model::polygon<point_xy> polygon_t;
    typedef boost::geometry::model::linestring<point_xy> linestring_t;

    polygon_t obstacle;
    linestring_t ls1;

    std::vector< point_xy > pointls;

    point_xy point1(end1.x , end1.y);
    pointls.push_back( point1 );

    point_xy point2(end2.x , end2.y);
    pointls.push_back( point2 );

    boost::geometry::assign_points( ls1, pointls );

    std::vector< point_xy > ObstaclePoints;

    for(int i=0;i<obstacle_size;i++)
    {
        point_xy point(obstacle_array[i][0] ,obstacle_array[i][1]);
        ObstaclePoints.push_back( point );
    }
    boost::geometry::assign_points( obstacle, ObstaclePoints);
    bool collision= boost::geometry::intersects(ls1, obstacle);

    return collision;
}
//main planner
bool planner(float start_x, float start_y, float goal_x, float goal_y, float sampling_radius)
{
    //variable to see if goal has been reached
    bool flag = 1;

    // defining the start as a Node
    Node start={start_x,start_y};
    Node goal={goal_x,goal_y};
    Node sample_node={};
    int nearest_node;

    //add start to the node_list
    node_list[counter]=start;
    counter++;

    for (int j=0; j<500;)
    {
        //samples a random point
        sample_node = sampler(goal_x,goal_y,sampling_probablity);

        // finds the nearest node to the sampled point
        nearest_node = find_nearest(sample_node);
        float dist = distance(sample_node,node_list[nearest_node]);

        // checking if tghe sampled node lies inside the sampling radius
        if (dist > sampling_radius)
        {
         //computing the angle b/w near and rand wrt x-axis
         float angle = atan2(sample_node.y - node_list[nearest_node].y,sample_node.x - node_list[nearest_node].x);

        // modifies sample_node in the direction of nearest node 0.5 distance from it
        sample_node.x=  node_list[nearest_node].x + sampling_radius*cos(angle);
        sample_node.y=  node_list[nearest_node].y + sampling_radius*sin(angle);
        }

        //obstacle checking
        bool collision = check_collision(sample_node, node_list[nearest_node]);
        if(collision==1)
        {
            continue;
        }
        else
        {
            sample_node.parent= &node_list[nearest_node];
            node_list[counter]= sample_node;
            counter++;
            j++;

        }

        float goal_dist=distance(sample_node,goal);
        // to check if we have reached the goal, tolerance of 0.1 is considered
        if(goal_dist<=0.1)
        {
            goal.parent= &sample_node;
            node_list[counter]=goal;
            counter++;
            flag = 0;
            //Goal Reached
            break;
        }
    }
    if(flag==1)
    {
        //Goal not found
        return 0;
    }

    //back propogation to find the final path from goal to start
    Node *current= &goal;
    while(current->parent != NULL)
    {
        location[backprop]=*current;
        backprop++;
        current=current->parent;
    }

    // to add the starting node
    location[backprop]=start;
    backprop++;

    return 1;
}

//plotting function
void plotting(bool sucess)
{
    namespace plt = matplotlibcpp;
    //plot obstacle
    std::vector<double> obstacle_x = {};
    std::vector<double> obstacle_y = {};

    for(int i=0;i<obstacle_size;i++)
    {
        obstacle_x.push_back(obstacle_array[i][0]);
        obstacle_y.push_back(obstacle_array[i][1]);
    }
    obstacle_x.push_back(obstacle_array[0][0]);
    obstacle_y.push_back(obstacle_array[0][1]);

    plt::plot(obstacle_x, obstacle_y,"k");

    //plotiing nodes
    std::vector<double> scatter_x = {};
    std::vector<double> scatter_y = {};

    for(int i=0;i<counter;i++)
    {

            scatter_x.push_back(node_list[i].x);
            scatter_y.push_back(node_list[i].y);

    }

    plt::plot(scatter_x, scatter_y,".m");

    //plotting tree
    for(int i=0;i<counter;i++)
    {
        if(node_list[i].parent != NULL)
        {
            std::vector<double> tree_x = {};
            std::vector<double> tree_y = {};
            tree_x.push_back(node_list[i].x);
            tree_y.push_back(node_list[i].y);
            tree_x.push_back(node_list[i].parent->x);
            tree_y.push_back(node_list[i].parent->y);
            plt::plot(tree_x, tree_y,"m");
        }
    }

    if(sucess==1)
    {
        //plot final path from start to goal
        int i = 0;
        while(i<backprop)
        {
            if(location[i].parent != NULL)
            {
                std::vector<double> final_x = {};
                std::vector<double> final_y = {};
                final_x.push_back(location[i].x);
                final_y.push_back(location[i].y);
                final_x.push_back(location[i].parent->x);
                final_y.push_back(location[i].parent->y);
                plt::plot(final_x, final_y,".b-");
            }
            i++;
        }
    }
    // plotting start and goal
    std::vector<double> starting_x = {};
    std::vector<double> starting_y = {};
    starting_x.push_back(location[0].x);
    starting_y.push_back(location[0].y);
    plt::plot(starting_x,starting_y,".k");
    std::vector<double> ending_x = {};
    std::vector<double> ending_y = {};
    ending_x.push_back(location[backprop].x);
    ending_y.push_back(location[backprop].y);
    plt::plot(ending_x,ending_y,".k");
    plt::grid(true);
    plt::show();

}
using namespace std;
//starting of the main function
int main()
{   float start_x, start_y, goal_x, goal_y;

    // taking inputs from the user
    cout<<"Enter sampling probablity : ";
    cin>> sampling_probablity;

    cout<<"Enter sampling radius (prefered to be less than 1): ";
    cin>> sampling_radius;

    cout<<"Enter x postion of start : ";
    cin>> start_x;

    cout<<"Enter y postion of start : ";
    cin>> start_y;

    cout<<"Enter x postion of goal: ";
    cin>> goal_x;

    cout<<"Enter y postion of goal : ";
    cin>> goal_y;

    //calling planner
    bool sucesss = planner(start_x,start_y,goal_x,goal_y,sampling_radius);
    //calling plotting
    plotting(sucesss);

    if(sucesss)
    {
        std::cout<<"goal reached"<<"\n";
    }
    else
    {
        std::cout<<"goal not reached";
    }

    std::cout<<"Total no of nodes = "<<counter<<"\n"<<"No of nodes in final path = "<<backprop<<"\n";

    return 0;
}


/** \file ITL_Preconditioner.h */

#pragma once

// MTL4 headers
#include <boost/numeric/itl/itl.hpp>
#include <boost/numeric/itl/pc/ilu_0.hpp>
#include <boost/numeric/itl/pc/ic_0.hpp>
#include <boost/numeric/mtl/vector/assigner.hpp>

// AMDiS headers
#include "MTL4Types.hpp"
#include "DOFMatrix.hpp"
#include "CreatorInterface.hpp"
#include "solver/LinearSolverInterface.hpp"
#include "solver/itl/masslumping.hpp"

namespace AMDiS
{
  /**
   * \ingroup Solver
   *
   * \brief Common base class for wrappers to use ITL preconditioners in AMDiS.
   */
  template <class MatrixType, class VectorType>
  struct ITL_PreconditionerBase : public PreconditionerInterface
  {
    virtual void init(SolverMatrix<Matrix<DOFMatrix*>> const& A,
                      MatrixType const& fullMatrix) = 0;

    virtual void exit() {}

    virtual void solve(VectorType const& x, VectorType& y) const = 0;

    virtual void adjoint_solve(VectorType const& x, VectorType& y) const = 0;
  };

  template <class MatrixType, class VectorType>
  itl::pc::solver<ITL_PreconditionerBase<MatrixType, VectorType>, VectorType, false>
  solve(ITL_PreconditionerBase<MatrixType, VectorType> const& P, VectorType const& vin)
  {
    return {P, vin};
  }

  template <class MatrixType, class VectorType>
  itl::pc::solver<ITL_PreconditionerBase<MatrixType, VectorType>, VectorType, true>
  adjoint_solve(ITL_PreconditionerBase<MatrixType, VectorType> const& P, VectorType const& vin)
  {
    return {P, vin};
  }

  /**
   * \ingroup Solver
   *
   * \brief Wrapper for using ITL preconditioners in AMDiS.
   */
  template <class Preconditioner, class MatrixType, class VectorType>
  class ITL_Preconditioner : public ITL_PreconditionerBase<MatrixType, VectorType>
  {
  public:
    using Self        = ITL_Preconditioner;
    using precon_base = ITL_PreconditionerBase<MatrixType, VectorType>;

    /// Creator class
    struct Creator : public CreatorInterfaceName<precon_base>
    {
      virtual precon_base* create() override
      {
        return new Self();
      }
    };

    /// Constructor.
//     ITL_Preconditioner() = default;

    /// Destructor.
    ~ITL_Preconditioner()
    {
      delete precon;
      precon = NULL;
    }

    /// Implementation of \ref ITL_PreconditionerBase::init()
    virtual void init(SolverMatrix<Matrix<DOFMatrix*>> const& /*A*/,
                      MatrixType const& fullMatrix) override
    {
      delete precon;
      precon = new Preconditioner(fullMatrix);
    }

    /// Implementation of \ref PreconditionerInterface::exit()
    virtual void exit() override
    {
      delete precon;
      precon = NULL;
    }

    /// Implementation of \ref ITL_PreconditionerBase::solve()
    virtual void solve(VectorType const& vin, VectorType& vout) const override
    {
      TEST_EXIT_DBG(precon)("No preconditioner initialized!\n");
      precon->solve(vin, vout);
    }

    /// Implementation of \ref ITL_PreconditionerBase::adjoint_solve()
    virtual void adjoint_solve(VectorType const& vin, VectorType& vout) const override
    {
      TEST_EXIT_DBG(precon)("No preconditioner initialized!\n");
      precon->adjoint_solve(vin, vout);
    }

  private:
    Preconditioner* precon = NULL;
  };

  /**
   * \ingroup Solver
   * \class AMDiS::DiagonalPreconditioner
   * \brief ITL_Preconditioner implementation of diagonal (jacobi) preconditioner,
   * \implements ITL_Preconditioner
   *
   * Diagonal preconditioner \f$ M^{-1} \f$ for the system \f$ Ax=b \f$ is defined as: \f$ M=diag(A) \f$.
   */
  using DiagonalPreconditioner =
    ITL_Preconditioner<itl::pc::diagonal<MTLTypes::MTLMatrix>,
		       MTLTypes::MTLMatrix, MTLTypes::MTLVector>;

  /**
   * \ingroup Solver
   * \class AMDiS::DiagonalPreconditioner
   * \brief ITL_Preconditioner implementation of diagonal (jacobi) preconditioner,
   * \implements ITL_Preconditioner
   *
   * Diagonal preconditioner \f$ M^{-1} \f$ for the system \f$ Ax=b \f$ is defined as: \f$ M_ii=sum_j(A_ij) \f$.
   */
  using MassLumpingPreconditioner =
    ITL_Preconditioner<itl::pc::masslumping<MTLTypes::MTLMatrix>,
		       MTLTypes::MTLMatrix, MTLTypes::MTLVector>;

  /**
   * \ingroup Solver
   * \class AMDiS::IdentityPreconditioner
   * \brief ITL_Preconditioner implementation of identity preconditioner,
   * \implements ITL_Preconditioner
   *
   * Identity preconditioner. Behaves like no preconditioning.
   */
  using IdentityPreconditioner =
    ITL_Preconditioner<itl::pc::identity<MTLTypes::MTLMatrix>,
		       MTLTypes::MTLMatrix, MTLTypes::MTLVector>;

  /**
   * \ingroup Solver
   * \class AMDiS::ILUPreconditioner
   * \brief ITL_Preconditioner implementation of ILU (Incomplete LU factorization)
   * preconditioner. \implements ITL_Preconditioner
   *
   * The preconditioner is used from ITL. It corresponds for instance to
   * "Iterative Methods for Sparce Linear Systems", second edition, Yousef Saad.
   *  The preconditioner is described in chapter 10.3 (algorithm 10.4).
   */
  using ILUPreconditioner =
    ITL_Preconditioner<itl::pc::ilu_0<MTLTypes::MTLMatrix>,
		       MTLTypes::MTLMatrix, MTLTypes::MTLVector>;

  /**
   * \ingroup Solver
   * \class AMDiS::ICPreconditioner
   * \brief ITL_Preconditioner implementation of IC (Incomplete Cholesky factorization)
   * preconditioner. \implements ITL_Preconditioner
   *
   * IC (Incomplete Cholesky factorization) preconditioner.
   */
  using ICPreconditioner =
    ITL_Preconditioner<itl::pc::ic_0<MTLTypes::MTLMatrix>,
		       MTLTypes::MTLMatrix, MTLTypes::MTLVector>;

} // namespace AMDiS


/*! \file svg_test_boxplot.cpp
   \brief Tests for svg boxplot.
   \details

   \author Jacob Voytko and Paul A. Bristow
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2007

// 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 <boost/svg_plot/svg_1d_plot.hpp>
#include <boost/svg_plot/svg_boxplot.hpp>

#include <vector>
using std::vector;
#include <map>
using std::multimap;
#include <cmath>
using ::sin;
#include <iostream>
using std::cout;
using std::endl;

double h(double x)
{
    return 50 / (x);
}

double f(double x)
{
    return 40 + 25 * sin(x * 50);
}

int main()
{
    using namespace boost::svg;
    std::vector<double> data1, data2;

    for(double i=.1; i < 10; i+=.1)
    { // Fill the vectors with some data.
        data1.push_back(h(i));
        data2.push_back(f(i));
    }

    svg_1d_plot my_1d_plot;
    my_1d_plot.background_border_color(black)
           .title("1D plots of Common Functions");
    my_1d_plot.plot(data1, "[50 / x]");
    my_1d_plot.plot(data2, "[40 + 25 * sin(50x)]");

    my_1d_plot.write("./svg_test_1d.svg");

    // Now plot the same data using boxplot.
    svg_boxplot my_box_plot;
    my_box_plot.background_border_color(black)
           .title("Boxplots of Common Functions");

    my_box_plot.plot(data1, "[50 / x]");
    my_box_plot.plot(data2, "[40 + 25 * sin(50x)]");
    my_box_plot.y_autoscale(data1);  // Compute autoscale values for the plot.

    my_box_plot.write("./svg_test_boxplot.svg");
   return 0;
} // int main()


#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/index/rtree.hpp>

// to store queries results
#include <vector>

// just for output
#include <iostream>
#include <boost/foreach.hpp>
#include <omp.h>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

int main()
{
    typedef bg::model::point<float, 2, bg::cs::cartesian> point;
    typedef bg::model::box<point> box;
    typedef std::pair<box, unsigned> value;

    // create the rtree using default constructor
    bgi::rtree< value, bgi::quadratic<16> > rtree;

    // create some values
    for ( unsigned i = 0 ; i < 10 ; ++i )
    {
        // create a box
        box b(point(i + 0.0f, i + 0.0f), point(i + 0.5f, i + 0.5f));
        // insert new value
        rtree.insert(std::make_pair(b, i));
    }

    // find values intersecting some area defined by a box

    std::vector<box> boxes;
    boxes.push_back(box(point(0, 0), point(5, 5)));
    boxes.push_back(box(point(1, 1), point(4, 4)));
    boxes.push_back(box(point(2, 2), point(3, 3)));

    #pragma omp parallel for
    for(int i=0; i < boxes.size(); i++)
    {
        int tid = omp_get_thread_num();
        // Borrowed from here
        // https://stackoverflow.com/questions/4106992/parallelize-output-using-openmp
        std::stringstream buf;
        buf << "---- spatial query from thread: " << tid << "----\n";
        std::vector<value> result_s;
        rtree.query(bgi::intersects(boxes[i]),std::back_inserter(result_s));
        // display results
        buf << "spatial query box:" << "\n";
        buf << bg::wkt<box>(boxes[i]) << "\n";
        buf << "spatial query result:" << "\n";
        BOOST_FOREACH(value const& v, result_s)
            buf << bg::wkt<box>(v.first) << " - " << v.second << "\n";
        // Write the buffer to output
        // #pragma omp critical
        std::cout << buf.rdbuf();
    }
    return 0;
}


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.
//
// This is an adapted version of the "Calc1" example in Proto's documentation
// to work with compile-time integers too.

#include <boost/hana.hpp>

#include <boost/proto/context.hpp>
#include <boost/proto/core.hpp>

#include <cassert>
namespace proto = boost::proto;
namespace hana = boost::hana;
using namespace hana::literals;

template <int n>
struct placeholder { };

// Define some placeholders
proto::terminal<placeholder<1>>::type const _1 = {{}};
proto::terminal<placeholder<2>>::type const _2 = {{}};

// Define a calculator context, for evaluating arithmetic expressions
template <typename M, typename N>
struct calculator_context
  : proto::callable_context<calculator_context<M, N> const>
{
  // The values bound to the placeholders
  M m;
  N n;

  constexpr calculator_context(M m, N n) : m{m}, n{n} { }

  // The result of evaluating arithmetic expressions
  template <typename Sig>
  struct result;

  template <typename This, typename Terminal>
  struct result<This(Terminal, placeholder<1> const&)> {
    using type = M;
  };

  template <typename This, typename Terminal>
  struct result<This(Terminal, placeholder<2> const&)> {
    using type = N;
  };

  // Handle the evaluation of the placeholder terminals
  constexpr auto operator()(proto::tag::terminal, placeholder<1>) const
  { return m; }

  constexpr auto operator()(proto::tag::terminal, placeholder<2>) const
  { return n; }
};

template <typename Expr, typename M, typename N>
constexpr auto evaluate(Expr expr, M m, N n) {
  // Create a calculator context with d1 and d2 substituted for _1 and _2
  calculator_context<M, N> const ctx{m, n};

  // Evaluate the calculator expression with the calculator_context
  return proto::eval(expr, ctx);
}

int main() {
// sample(proto)
auto expr = (_1 - _2) / _2;

// compile-time computations
static_assert(decltype(evaluate(expr, 6_c, 2_c))::value == 2, "");

// runtime computations
int i = 6, j = 2;
assert(evaluate(expr, i, j) == 2);
// end-sample
}


/*
 * AttitudeMagCalib.cpp
 *
 *  Copyright (c) 2013 Gareth Cross. All rights reserved.
 *
 *  This file is part of kr_attitude_eskf.
 *
 *	Created on: 23/06/2014
 *		  Author: gareth
 */

#include <kr_attitude_eskf/AttitudeMagCalib.hpp>
#include <Eigen/Cholesky>
#include <cmath>

using namespace Eigen;

namespace kr {

AttitudeMagCalib::AttitudeMagCalib() { reset(); }

void AttitudeMagCalib::reset() {
  binH_.clear();
  binV_.clear();
  calibrated_ = false;
  bias_.setZero();
  scale_.setOnes();
}

void AttitudeMagCalib::appendSample(const quat &att,
                                    const vec3 &field) {
  SampleBin bin;
  bin.field = field;
  bin.q = att;

  const vec3 localG = att.conjugate().matrix() * vec3(0,0,1);

  //  determine local angle
  if (std::abs(localG[2]) < 0.1) {
    //  world vertical is approx. in the local X/Y plane
    const vec3 worldZ = att.matrix() * vec3(0,0,1);
    const scalar_t ang = std::atan2(worldZ[1], worldZ[0]);
    const int key = (ang + M_PI) / (2*M_PI) * kBinMaxCount;
    binV_[key] = bin;
  } else if (std::abs(localG[2]) > 0.9) {
    //  world vertical is approx. vertical
    const vec3 worldX = att.matrix() * vec3(1,0,0);
    const scalar_t ang = std::atan2(worldX[1], worldX[0]);
    const int key = (ang + M_PI) / (2*M_PI) * kBinMaxCount;
    binH_[key] = bin;
  }
}

bool AttitudeMagCalib::isReady() const {
  if (binV_.size() < kBinMaxCount*8/10) {
    return false;
  }
  if (binH_.size() < kBinMaxCount*8/10) {
    return false;
  }
  return true;
}

bool AttitudeMagCalib::isCalibrated() const { return calibrated_; }

void AttitudeMagCalib::calibrate(AttitudeMagCalib::CalibrationType type) {

  if (!isReady()) {
    throw insufficient_data();
  }

  if (type == AttitudeMagCalib::FullCalibration) {
    //  perform full estimation of bias and scale
    vec3 bias(0,0,0), scl(1,1,1);

    std::vector<vec3> meas;
    for (const std::pair<int, SampleBin>& s : binH_) {
      meas.push_back(s.second.field);
    }
    for (const std::pair<int, SampleBin>&s : binV_) {
      meas.push_back(s.second.field);
    }
    const size_t N = meas.size();

    //  fit to sphere
    Matrix<scalar_t,Eigen::Dynamic,Eigen::Dynamic> A(N, 4);
    Matrix<scalar_t,Eigen::Dynamic,1> b(N, 1);
    for (size_t i=0; i < meas.size(); i++) {
      A(i,0) = 2*meas[i][0];
      A(i,1) = 2*meas[i][1];
      A(i,2) = 2*meas[i][2];
      A(i,3) = 1;
      b(i,0) = meas[i][0]*meas[i][0] + meas[i][1]*meas[i][1] +
          meas[i][2]*meas[i][2];
    }
    //  solve system for center
    const Matrix<scalar_t,4,1> x = A.colPivHouseholderQr().solve(b);
    bias = x.block<3,1>(0,0);

    scalar_t mean_rad = 0.0, mean_rad_sqr = 0.0;
    //  calculate estimate of mean radius
    for (const vec3 &v : meas) {
      const scalar_t r = (v - bias).norm();
      mean_rad += r;
      mean_rad_sqr += r * r;
    }
    mean_rad /= N;
    mean_rad_sqr /= N;

    //  refine with GN-NLS
    Matrix<scalar_t, Eigen::Dynamic, 6> J(N, 6);
    Matrix<scalar_t, Eigen::Dynamic, 1> r(N, 1);
    Matrix<scalar_t, Eigen::Dynamic, Eigen::Dynamic> W(N,N);
    W.setZero();

    for (int iter = 0; iter < 20; iter++) {
      for (size_t i = 0; i < meas.size(); i++) {
        const scalar_t x = (meas[i][0] - bias[0]) / scl[0];
        const scalar_t y = (meas[i][1] - bias[1]) / scl[1];
        const scalar_t z = (meas[i][2] - bias[2]) / scl[2];
        const scalar_t r2 = x*x + y*y + z*z;

        //  residual
        r(i,0) = mean_rad*mean_rad - r2;
        //  jacobian
        J(i,0) = -2 * x * x / scl[0];
        J(i,1) = -2 * y * y / scl[1];
        J(i,2) = -2 * z * z / scl[2];
        J(i,3) = -2 * x / scl[0];
        J(i,4) = -2 * y / scl[1];
        J(i,5) = -2 * z / scl[2];
      }

      //  calculate mean squared error
      scalar_t sigmaSquared=0;
      for (int j=0; j < r.rows(); j++) {
        sigmaSquared += r(j,0)*r(j,0);
      }
      sigmaSquared /= r.rows();
      //  calculate cauchy weights
      for (int j=0; j < r.rows(); j++) {
        const scalar_t errSqr = r(j,0)*r(j,0);
        W(j,j) = 1.0 / (1 + errSqr/sigmaSquared);
      }

      Matrix<scalar_t, 6, 6> H = J.transpose() * W * J;
      for (int i = 0; i < 3; i++) {  //  prior on scale
        H(i,i) *= 1.01;
      }
      Eigen::LDLT<Matrix<scalar_t,6,6>> LDLT(H);
      const Matrix<scalar_t, 6, 1> update = LDLT.solve(J.transpose() * W * r);

      scl += update.block<3, 1>(0, 0);
      bias += update.block<3, 1>(3, 0);
    }

    bias_ = bias;
    scale_ = scl;
  } else {
    bias_.setZero();
    scale_.setOnes();
  }

  calibrated_ = true;
}

} //  namespace kr


#pragma once

#include <polyfem/ProblemWithSolution.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class LinearProblem : public ProblemWithSolution
	{
	public:
		LinearProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return true; }
	};

	class QuadraticProblem : public ProblemWithSolution
	{
	public:
		QuadraticProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return true; }
	};

	class CubicProblem : public ProblemWithSolution
	{
	public:
		CubicProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return true; }
	};

	class SineProblem : public ProblemWithSolution
	{
	public:
		SineProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return true; }
	};

	class ZeroBCProblem : public ProblemWithSolution
	{
	public:
		ZeroBCProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return true; }
	};

	class MinSurfProblem : public Problem
	{
	public:
		MinSurfProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return false; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool is_scalar() const override { return true; }
		bool has_exact_sol() const override { return false; }
	};

	class TimeDependentProblem : public Problem
	{
	public:
		TimeDependentProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return false; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return true; }
		bool is_time_dependent() const override { return true; }
	};

	class GenericScalarProblemExact : public ProblemWithSolution
	{
	public:
		GenericScalarProblemExact(const std::string &name);

		bool is_scalar() const override { return true; }
		bool is_time_dependent() const override { return func_ <= 1; }
		bool is_constant_in_time() const override { return false; }

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

		VectorNd eval_fun(const VectorNd &pt, double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, double t) const override;

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	private:
		int func_;
	};
} // namespace polyfem


#include <igl/decimate.h>
#include <igl/qslim.h>
#include <Eigen/Core>
#include <iostream>
#include <set>

#ifdef MEX
#  include <mex.h>
#  include <igl/C_STR.h>
#  include <igl/matlab/mexErrMsgTxt.h>
#  undef assert
#  define assert( isOK ) ( (isOK) ? (void)0 : (void) ::mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#endif

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>

void mexFunction(
         int          nlhs,
         mxArray      *plhs[],
         int          nrhs,
         const mxArray *prhs[]
         )
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  MatrixXd V,W;
  MatrixXi F,G;
  VectorXi J,I;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  mexErrMsgTxt(nrhs>=3,"nrhs should be >= 3");
  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,F);
  mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
  mexErrMsgTxt(F.cols()==3,"F must be #F by 3");
  mexErrMsgTxt(
    mxIsDouble(prhs[2]) && mxGetM(prhs[2])==1 && mxGetN(prhs[2])==1,
    "fraction to decimate should be scalar");
  double ratio = * mxGetPr(prhs[2]);
  mexErrMsgTxt((ratio>0 && ratio<1) || (ratio>0 && ratio<F.rows()) ,
    "Ratio should be in (0,1) or [1,#F)");
  const size_t max_m = ratio<1 ? ratio*F.rows() : ratio;

  enum DecimateMethod
  {
    DECIMATE_METHOD_NAIVE = 0,
    DECIMATE_METHOD_QSLIM = 1,
    NUM_DECIMATE_METHODS = 2
  } method = DECIMATE_METHOD_NAIVE;
  {
    int i = 3;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Method",name) == 0)
      {
        validate_arg_char(i,nrhs,prhs,name);
        const char * type_name = mxArrayToString(prhs[++i]);
        if(strcmp("naive",type_name)==0)
        {
          method = DECIMATE_METHOD_NAIVE;
        }else if(strcmp("qslim",type_name)==0)
        {
          method = DECIMATE_METHOD_QSLIM;
        }else
        {
          mexErrMsgTxt(false,C_STR("Unknown method: "<<method));
        }
      }else
      {
        mexErrMsgTxt(false,C_STR("Unknown parameter: "<<name));
      }
      i++;
    }
  }

  switch(method)
  {
    case DECIMATE_METHOD_NAIVE:
      decimate(V,F,max_m,W,G,J,I);
      break;
    case DECIMATE_METHOD_QSLIM:
      qslim(V,F,max_m,W,G,J,I);
      break;
    default:
      mexErrMsgTxt(false,"Unkown method.");
      break;
  }

  switch(nlhs)
  {
    case 4:
      prepare_lhs_index(I,plhs+3);
    case 3:
      prepare_lhs_index(J,plhs+2);
    case 2:
      prepare_lhs_index(G,plhs+1);
    case 1:
      prepare_lhs_double(W,plhs+0);
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_HPP

#include <boost/assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/policies/compare.hpp>
#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
#include <boost/geometry/io/wkt/wkt.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Let the buffer create rounded corners
\ingroup strategies
\details This strategy can be used as JoinStrategy for the buffer algorithm.
    It creates a rounded corners around each convex vertex. It can be applied
    for (multi)linestrings and (multi)polygons.
    This strategy is only applicable for Cartesian coordinate systems.

\qbk{
[heading Example]
[buffer_join_round]
[heading Output]
[$img/strategies/buffer_join_round.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_join_miter join_miter]
}
 */
class join_round
{
public :

    //! \brief Constructs the strategy
    //! \param points_per_circle points which would be used for a full circle
    explicit inline join_round(std::size_t points_per_circle = 90)
        : m_points_per_circle(points_per_circle)
    {}

private :
    template
    <
        typename PromotedType,
        typename Point,
        typename DistanceType,
        typename RangeOut
    >
    inline void generate_points(Point const& vertex,
                Point const& perp1, Point const& perp2,
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        PromotedType dx1 = get<0>(perp1) - get<0>(vertex);
        PromotedType dy1 = get<1>(perp1) - get<1>(vertex);
        PromotedType dx2 = get<0>(perp2) - get<0>(vertex);
        PromotedType dy2 = get<1>(perp2) - get<1>(vertex);

        BOOST_ASSERT(buffer_distance != 0);

        dx1 /= buffer_distance;
        dy1 /= buffer_distance;
        dx2 /= buffer_distance;
        dy2 /= buffer_distance;

        PromotedType angle_diff = acos(dx1 * dx2 + dy1 * dy2);

        PromotedType two = 2.0;
        PromotedType steps = m_points_per_circle;
        int n = boost::numeric_cast<int>(steps * angle_diff
                    / (two * geometry::math::pi<PromotedType>()));

        if (n <= 1)
        {
            return;
        }

        PromotedType const angle1 = atan2(dy1, dx1);
        PromotedType diff = angle_diff / PromotedType(n);
        PromotedType a = angle1 - diff;

        for (int i = 0; i < n - 1; i++, a -= diff)
        {
            Point p;
            set<0>(p, get<0>(vertex) + buffer_distance * cos(a));
            set<1>(p, get<1>(vertex) + buffer_distance * sin(a));
            range_out.push_back(p);
        }
    }

public :

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Fills output_range with a rounded shape around a vertex
    template <typename Point, typename DistanceType, typename RangeOut>
    inline bool apply(Point const& ip, Point const& vertex,
                Point const& perp1, Point const& perp2,
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        typedef typename coordinate_type<Point>::type coordinate_type;
        typedef typename boost::range_value<RangeOut>::type output_point_type;

        typedef typename geometry::select_most_precise
            <
                typename geometry::select_most_precise
                    <
                        coordinate_type,
                        typename geometry::coordinate_type<output_point_type>::type
                    >::type,
                double
            >::type promoted_type;

        geometry::equal_to<Point> equals;
        if (equals(perp1, perp2))
        {
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
            std::cout << "Corner for equal points " << geometry::wkt(ip) << " " << geometry::wkt(perp1) << std::endl;
#endif
            return false;
        }

        // Generate 'vectors'
        coordinate_type vix = (get<0>(ip) - get<0>(vertex));
        coordinate_type viy = (get<1>(ip) - get<1>(vertex));

        promoted_type length_i = geometry::math::sqrt(vix * vix + viy * viy);
        DistanceType const bd = geometry::math::abs(buffer_distance);
        promoted_type prop = bd / length_i;

        Point bp;
        set<0>(bp, get<0>(vertex) + vix * prop);
        set<1>(bp, get<1>(vertex) + viy * prop);

        range_out.push_back(perp1);
        generate_points<promoted_type>(vertex, perp1, perp2, bd, range_out);
        range_out.push_back(perp2);
        return true;
    }

    template <typename NumericType>
    static inline NumericType max_distance(NumericType const& distance)
    {
        return distance;
    }

#endif // DOXYGEN_SHOULD_SKIP_THIS

private :
    std::size_t m_points_per_circle;
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_HPP


#include <exception>
#include <fstream>
#include <sstream>
#include <iostream>
#include <iterator>
#include <string>
#include <range/v3/all.hpp>
#include <cstdint>
#include <boost/circular_buffer.hpp>
#include <initializer_list>
using Ring = boost::circular_buffer<int>;
namespace views = ranges::views;
struct CrabCup
{
private:
  Ring ring_;

public:
  CrabCup(std::initializer_list<int> l, int n) : ring_(n)
  {
    ranges::copy(l, std::back_inserter(ring_));

    auto indexs = views::ints | views::drop(l.size() + 1) | views::take(n - l.size()) | views::common;
    ranges::copy(indexs, std::back_inserter(ring_));
  }
  CrabCup(std::initializer_list<int> l) : ring_(l.begin(), l.end())
  {
  }

  friend std::ostream &operator<<(std::ostream &os, const CrabCup &crabcup)
  {
    std::ostream_iterator<int> oit(std::cout, "");
    ranges::copy(crabcup.ring_, oit);
    return os;
  }
  void round()
  {
    std::ostream_iterator<int> oit(std::cout, ",");
    auto n = ring_.front();
    std::vector<int> v(ring_.begin() + 4, ring_.end());
    auto indexs = views::ints | views::take(ring_.size()) | views::drop(4) | views::common;
    std::vector<int> vi(indexs.begin(), indexs.end());
    ranges::zip_view flatmap(v, vi);
    ranges::sort(flatmap);
    auto dstpos = ranges::lower_bound(v, n);
    if (dstpos == v.begin()) {
      dstpos = v.end();
    }
    --dstpos;
    int len = dstpos - v.begin();
    auto id = vi[len];
    std::rotate(ring_.begin() + 1, ring_.begin() + 4, ring_.begin() + id + 1);

    ring_.push_back(n);
  }
  void findone()
  {
    while (ring_.front() != 1) {
      ring_.push_back(ring_.front());
    }
    ring_.pop_front();
  }
};

int main()
{
  CrabCup c({ 4, 6, 7, 5, 2, 8, 1, 9, 3 }, 20);
  for (auto i : views::ints | views::take(30) | views::common) {
    c.round();
  }
  c.findone();
  std::cout << c << std::endl;
  return 0;
}


/*
 * Copyright 2020-2021 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include "coma/Core"
#include "macros.hpp"
#include "doctest/doctest.h"
#include <Eigen/QR>
#include <tuple>

struct Space4 {
    static constexpr int space = 4;
};

struct Space6 {
    static constexpr int space = 6;
};

struct OrderF {
    static constexpr int order = 5;
};

struct OrderD {
    static constexpr int order = coma::Dynamic;
};

template <typename T1, typename T2, typename T3>
struct TypeTriple
{
    using first_type = T1;
    using second_type = T2;
    using third_type = T3;
};

#define test_triples \
    TypeTriple<float, Space4, OrderF>, \
    TypeTriple<double, Space4, OrderF>, \
    TypeTriple<float, Space6, OrderF>, \
    TypeTriple<double, Space6, OrderF>, \
    TypeTriple<float, Space4, OrderD>, \
    TypeTriple<double, Space4, OrderD>, \
    TypeTriple<float, Space6, OrderD>, \
    TypeTriple<double, Space6, OrderD>

class CMTM24 {
    using PMat = coma::Transform<double>;
    using MVd = coma::MotionVector<double>;

public:
    CMTM24(const PMat& pt, const MVd& n, const MVd& dn, const MVd& ddn)
        : A(pt)
        , nu(n)
        , dnu(dn)
        , ddnu(ddn)
    {
    }

    friend CMTM24 operator*(const CMTM24& lhs, const CMTM24& rhs)
    {
        MVd Anu = rhs.A.invMul(lhs.nu);
        MVd Adnu = rhs.A.invMul(lhs.dnu);

        PMat A = lhs.A * rhs.A;
        MVd nu = Anu + rhs.nu;
        MVd dnu = Adnu + rhs.dnu + Anu.cross(rhs.nu);
        MVd ddnu = rhs.A.invMul(lhs.ddnu) + rhs.ddnu + (Anu.cross(rhs.nu) + 2. * Adnu).cross(rhs.nu) + Anu.cross(rhs.dnu);
        return CMTM24{ A, nu, dnu, ddnu };
    }

public:
    PMat A;
    MVd nu;
    MVd dnu;
    MVd ddnu;
};

TEST_CASE_TEMPLATE("CMTM", T, test_triples)
{
    using namespace coma;
    using Scalar = typename T::first_type;
    constexpr int space = T::second_type::space;
    constexpr int order = T::third_type::order;
    constexpr int dynOrder = OrderF::order;
    using qt_t = Eigen::Quaternion<Scalar>;
    using v3_t = Eigen::Matrix<Scalar, 3, 1>;
    using v6_t = Eigen::Matrix<Scalar, 6, 1>;
    using m3_t = Eigen::Matrix<Scalar, 3, 3>;
    using mv_t = MotionVector<Scalar>;
    using cmtm_t = CMTM<Scalar, space, order>;
    using transform_t = typename cmtm_t::transform_t;
    using mvx_t = typename cmtm_t::mvx_t;

    DISABLE_CONVERSION_WARNING_BEGIN
    qt_t q = qt_t::UnitRandom();
    DISABLE_CONVERSION_WARNING_END

    m3_t R = q.toRotationMatrix();
    v3_t p = v3_t::Random();

    transform_t A{ R, p };
    mv_t m{ v6_t::Random() };
    mvx_t mx{ m, m, m, m, m };

    {
        // default ctor
        cmtm_t cmtm;
        if constexpr (order == Dynamic) {
            cmtm_t cmtm2{ dynOrder };
            REQUIRE(cmtm2.order() == dynOrder);
            REQUIRE(cmtm2.nMat() == dynOrder + 1);
            REQUIRE(cmtm2.rows() == space * (dynOrder + 1));
            REQUIRE(cmtm2.rows() == cmtm2.cols());
            REQUIRE(cmtm2.size() == static_cast<int>(std::pow((dynOrder + 1) * space, 2)));
            REQUIRE(cmtm2.matrix().size() == cmtm2.size());
            REQUIRE(cmtm2.matrix(dynOrder / 2) == cmtm2.matrix().template topLeftCorner<space*(dynOrder / 2), space*(dynOrder / 2)>());
            if constexpr (space == 6) {
                REQUIRE(cmtm2.dualMatrix().size() == cmtm2.size());
                REQUIRE(cmtm2.dualMatrix(dynOrder / 2) == cmtm2.dualMatrix().template topLeftCorner<6 * (dynOrder / 2), 6 * (dynOrder / 2)>());
            }
        } else {
            REQUIRE(cmtm.order() == order);
            REQUIRE(cmtm.nMat() == order + 1);
            REQUIRE(cmtm.rows() == space * (order + 1));
            REQUIRE(cmtm.rows() == cmtm.cols());
            REQUIRE(cmtm.size() == static_cast<int>(std::pow((order + 1) * space, 2)));
            REQUIRE(cmtm.matrix().size() == cmtm.size());
            REQUIRE(cmtm.template matrix<order / 2>() == cmtm.matrix().template topLeftCorner<space*(order / 2), space*(order / 2)>());
            if constexpr (space == 6) {
                REQUIRE(cmtm.dualMatrix().size() == cmtm.size());
                REQUIRE(cmtm.template dualMatrix<order / 2>() == cmtm.dualMatrix().template topLeftCorner<6 * (order / 2), 6 * (order / 2)>());
            }
        }
    }
    {
        // Identity
        cmtm_t cmtm;
        cmtm = cmtm_t::Identity(5);
        REQUIRE(cmtm.transform() == transform_t::Identity());
        REQUIRE(cmtm.motion() == mvx_t::Zero(5));
        if constexpr (order != Dynamic) {
            cmtm = cmtm_t::Identity();
            REQUIRE(cmtm.transform() == transform_t::Identity());
            REQUIRE(cmtm.motion() == mvx_t::Zero());
        }
    }
    {
        // setIdentity
        cmtm_t cmtm;
        cmtm.setIdentity(dynOrder);
        REQUIRE(cmtm == cmtm_t::Identity(dynOrder));
        if constexpr (order != Dynamic) {
            cmtm.transform() = A;
            cmtm.motion()[0] = m;
            cmtm.setIdentity();
            REQUIRE(cmtm == cmtm_t::Identity());
        }
    }
    {
        // lvalues ctor with tangent vector
        cmtm_t cmtm{ A, mx };
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == mx);
    }
    {
        // lvalues ctor without tangent vector
        cmtm_t cmtm{ A, m, m, m, m, m };
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == mx);
    }
    {
        // rvalues ctor with tangent vector
        cmtm_t cmtm{ transform_t{ q, p }, mvx_t{ m, m, m, m, m } };
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == mx);
    }
    {
        // rvalues ctor without tangent vector
        mvx_t smi{ v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones() };
        cmtm_t cmtm{ transform_t{ q, p }, v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones() };
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == smi);
    }
    {
        // copy ctor
        cmtm_t cmtm1{ A, m, m, m, m, m };
        cmtm_t cmtm2{ cmtm1 };
        REQUIRE(cmtm2.transform() == A);
        REQUIRE(cmtm2.motion() == mx);
    }
    {
        // move ctor
        cmtm_t cmtm1{ A, m, m, m, m, m };
        cmtm_t cmtm2{ std::move(cmtm1) };
        REQUIRE(cmtm2.transform() == A);
        REQUIRE(cmtm2.motion() == mx);
    }
    {
        // assign op
        cmtm_t cmtm1{ A, m, m, m, m, m };
        cmtm_t cmtm2;
        cmtm2 = cmtm1;
        REQUIRE(cmtm2.transform() == A);
        REQUIRE(cmtm2.motion() == mx);
    }
    {
        // move-assign op
        cmtm_t cmtm1{ A, m, m, m, m, m };
        cmtm_t cmtm2;
        cmtm2 = std::move(cmtm1);
        REQUIRE(cmtm2.transform() == A);
        REQUIRE(cmtm2.motion() == mx);
    }
    {
        // lvalues set with tangent vector
        cmtm_t cmtm;
        cmtm.set(A, mx);
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == mx);
    }
    {
        // lvalues set without tangent vector
        cmtm_t cmtm;
        cmtm.set(A, m, m, m, m, m);
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == mx);
    }
    {
        // rvalues ctor with tangent vector
        cmtm_t cmtm;
        cmtm.set(transform_t{ q, p }, mvx_t{ m, m, m, m, m });
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == mx);
    }
    {
        // rvalues ctor without tangent vector
        mvx_t smi{ v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones() };
        cmtm_t cmtm;
        cmtm.set(transform_t{ q, p }, v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones(), v6_t::Ones());
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion() == smi);
    }
    {
        // deconstruction
        cmtm_t cmtm{ A, mx };
        cmtm.deconstruct();
        REQUIRE(cmtm.transform() == A);
        REQUIRE(cmtm.motion().isApprox(mx));
    }
    {
        // operator==(CMTM6n)
        cmtm_t cmtm1{ A, m, m, m, m, m };
        cmtm_t cmtm2{ A, m, m, m, m, m };
        REQUIRE(cmtm1 == cmtm2);
    }
    {
        // operator!=(CMTM6n)
        cmtm_t cmtm1{ A, m, m, m, m, m };
        cmtm_t cmtm2{ transform_t::Identity(), m, m, m, m, m };
        REQUIRE(cmtm1 != cmtm2);
        for (int i = 0; i < order; ++i) {
            mvx_t smt{ m, m, m, m, m };
            smt[i] = mv_t::Zero();
            cmtm_t cmtm3{ A, smt };
            REQUIRE(cmtm1 != cmtm3);
        }
    }
    {
        // isApprox(CMTM6n)
        m3_t m_eps = m3_t::Ones() * 0.1 * dummy_precision<Scalar>();
        v6_t v_eps = v6_t::Ones() * 0.1 * dummy_precision<Scalar>();
        cmtm_t cmtm1{ A, m, m, m, m, m };
        cmtm_t cmtm2{ transform_t{ R + m_eps, p }, m, m, m, m, m };
        REQUIRE(cmtm1 != cmtm2);
        REQUIRE(cmtm1.isApprox(cmtm2));
        for (int i = 0; i < order; ++i) {
            mvx_t smt{ m, m, m, m, m };
            smt[i] = mv_t{ m.vector() + v_eps };
            cmtm_t cmtm3{ A, smt };
            REQUIRE(cmtm1 != cmtm3);
            REQUIRE(cmtm1.isApprox(cmtm2));
        }
    }
}

TEST_CASE("CMTM 0-order")
{
    using namespace coma;
    using cmtm0_t = CMTM<double, 4, 0>;
    using cmtmd_t = CMTM<double, 4, Dynamic>;

    {
        // Init
        cmtm0_t cmtm0{};
        cmtmd_t cmtmd{ 0 };
        REQUIRE(cmtm0.order() == 0);
        REQUIRE(cmtmd.order() == 0);
        REQUIRE(cmtm0.nMat() == 1);
        REQUIRE(cmtmd.nMat() == 1);
        REQUIRE(cmtm0.rows() == 4);
        REQUIRE(cmtmd.rows() == 4);
        REQUIRE(cmtm0.cols() == 4);
        REQUIRE(cmtmd.cols() == 4);
        REQUIRE(cmtm0.size() == 16);
        REQUIRE(cmtmd.size() == 16);
        REQUIRE(cmtm0.matrix().size() == cmtm0.size());
        REQUIRE(cmtmd.matrix().size() == cmtmd.size());
    }
    {
        // Resize
        cmtm0_t cmtm0{};
        cmtmd_t cmtmd;
        cmtm0.resize(0);
        cmtmd.resize(1);
        REQUIRE(cmtmd.order() == 1);
        REQUIRE(cmtmd.nMat() == 2);
        REQUIRE(cmtmd.rows() == 8);
        REQUIRE(cmtmd.cols() == 8);
        REQUIRE(cmtmd.size() == 64);
        REQUIRE(cmtmd.matrix().size() == cmtmd.size());
    }
}

TEST_CASE_TEMPLATE("CMTM operations", T, Space4, Space6)
{
    using namespace coma;
    constexpr int space = T::space;
    constexpr int order = 3;
    Eigen::Matrix3d R = Eigen::Quaterniond::UnitRandom().toRotationMatrix();
    Eigen::Vector3d p = Eigen::Vector3d::Random();
    Transform<double> A{ R, p };
    MotionVector<double> nu{ Eigen::Vector6d::Random() };
    MotionVector<double> dnu{ Eigen::Vector6d::Random() };
    MotionVector<double> ddnu{ Eigen::Vector6d::Random() };

    CMTM24 c1{ Transform<double>{ R, p }, nu, dnu, ddnu };
    CMTM24 c2{ Transform<double>{ R, p }, nu, dnu, ddnu };
    MotionVectorX<double, order> mvx1{ nu, dnu, ddnu };
    MotionVectorX<double, order> mvx2{ nu, dnu, ddnu };
    CMTM<double, space, order> c3{ A, mvx1 };
    CMTM<double, space, order> c4{ A, mvx2 };

    auto c12 = c1 * c2;
    auto c34 = c3 * c4;
    REQUIRE(c34.order() == order);

    auto checkEq = [](auto c1, auto c2) {
        REQUIRE(c1.angular().isApprox(c2.angular()));
        REQUIRE(c1.linear().isApprox(c2.linear()));
    };

    auto motions34 = c34.motion();

    // Multiplication test
    REQUIRE(c12.A.rotation() == c34.transform().rotation());
    REQUIRE(c12.A.translation() == c34.transform().translation());
    checkEq(c12.nu, motions34[0]);
    checkEq(c12.dnu, motions34[1]);
    checkEq(c12.ddnu, motions34[2]);

    // Matrix test
    REQUIRE(c34.matrix().isApprox(c3.matrix() * c4.matrix()));

    // Dual matrix test
    if constexpr (space == 6) {
        REQUIRE(c34.dualMatrix().isApprox(c3.dualMatrix() * c4.dualMatrix()));
        Eigen::Vector6d randVec = Eigen::Vector6d::Random();
        ForceVector<double> fv{ randVec };
        MotionVector<double> mv{ randVec };

        const auto& factors = factorial_factors<double, order + 1>;
        MotionVectorX<double, order + 1> v{ mv, mv, mv, mv };
        ForceVectorX<double, order + 1> f{ fv, fv, fv, fv };
        Eigen::Matrix<double, 6 * (order + 1), 1> v2, f2;
        v2 << randVec, randVec, randVec, randVec;
        f2 << randVec, randVec, randVec, randVec;
        for (int i = 0; i < order + 1; ++i) {
            size_t ui = static_cast<size_t>(i);
            v2.segment<6>(6 * i) /= factors[ui];
            f2.segment<6>(6 * i) /= factors[ui];
        }
        Eigen::VectorXd vr = c3.matrix() * v2;
        Eigen::VectorXd fr = c3.dualMatrix() * f2;
        for (int i = 0; i < order + 1; ++i) {
            size_t ui = static_cast<size_t>(i);
            vr.segment<6>(6 * i) *= factors[ui];
            fr.segment<6>(6 * i) *= factors[ui];
        }

        auto c3v = c3 * v;
        auto c3f = c3.dualMul(f);
        REQUIRE(c3v.vector().isApprox(vr));
        REQUIRE(c3f.vector().isApprox(fr));
    }

    // Inverse test
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr{ c3.rows(), c3.cols() };
    Eigen::MatrixXd c3Inv = c3.inverse().matrix();
    Eigen::MatrixXd m3Inv = qr.compute(c3.matrix()).inverse();
    Eigen::MatrixXd c34Inv = c34.inverse().matrix();
    Eigen::MatrixXd m34Inv = qr.compute(c34.matrix()).inverse();

    REQUIRE(c3.inverse().matrix().isApprox(qr.compute(c3.matrix()).inverse(), dummy_precision<double>()));
    REQUIRE(c34.inverse().matrix().isApprox(qr.compute(c34.matrix()).inverse(), dummy_precision<double>()));

    // test for Dynamic
    MotionVectorX<double, Dynamic> mvxd{ nu, dnu, ddnu };
    CMTM<double, space, Dynamic> c5{ A, mvxd };
    CMTM<double, space, Dynamic> c6;
    c6.set(A, nu, dnu, ddnu);

    auto c56 = c5 * c6;

    auto motions56 = c56.motion();

    // Test
    REQUIRE(c12.A.rotation() == c56.transform().rotation());
    REQUIRE(c12.A.translation() == c56.transform().translation());
    checkEq(c12.nu, motions56[0]);
    checkEq(c12.dnu, motions56[1]);
    checkEq(c12.ddnu, motions56[2]);

    c6.set(A, nu, dnu, ddnu, MotionVector<double>{ Eigen::Vector6d::Random() });
    REQUIRE_THROWS_AS(c5 * c6, std::runtime_error);
}


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include <gcs/constraints/all_different.hh>
#include <gcs/constraints/arithmetic.hh>
#include <gcs/constraints/comparison.hh>
#include <gcs/constraints/abs.hh>
#include <gcs/problem.hh>
#include <gcs/solve.hh>

#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>

#include <boost/program_options.hpp>

using namespace gcs;

using std::cerr;
using std::cout;
using std::endl;
using std::string;
using std::to_string;
using std::pair;
using std::vector;

using namespace std::literals::string_literals;

namespace po = boost::program_options;

auto main(int argc, char * argv[]) -> int
{
    po::options_description display_options{ "Program options" };
    display_options.add_options()
        ("help", "Display help information")
        ("prove", "Create a proof");

    po::options_description all_options{ "All options" };
    all_options.add_options()
        ("abs", "Use abs constraint")
        ;

    all_options.add(display_options);

    po::variables_map options_vars;

    try {
        po::store(po::command_line_parser(argc, argv)
                .options(all_options)
                .run(), options_vars);
        po::notify(options_vars);
    }
    catch (const po::error & e) {
        cerr << "Error: " << e.what() << endl;
        cerr << "Try " << argv[0] << " --help" << endl;
        return EXIT_FAILURE;
    }

    if (options_vars.count("help")) {
        cout << "Usage: " << argv[0] << " [options] [size]" << endl;
        cout << endl;
        cout << display_options << endl;
        return EXIT_SUCCESS;
    }

    Problem p = options_vars.count("prove") ? Problem{ Proof{ "crystal_maze.opb", "crystal_maze.veripb" } } : Problem{ };

    vector<IntegerVariableID> xs;
    for (int i = 0 ; i < 8 ; ++i)
        xs.push_back(p.create_integer_variable(1_i, 8_i, "box" + to_string(i)));

    p.post(AllDifferent{ xs });
    p.branch_on(xs);

    vector<pair<int, int> > edges{ { 0, 1 }, { 0, 2 }, { 0, 3 }, { 0, 4 },
        { 1, 3 }, { 1, 4 }, { 1, 5 }, { 2, 3 }, { 2, 6 }, { 3, 4 }, { 3, 6 },
        { 3, 7 }, { 4, 5 }, { 4, 6 }, { 4, 7 }, { 5, 7 }, { 6, 7 } };

    vector<IntegerVariableID> diffs, abs_diffs;
    for (auto & [ x1, x2 ] : edges) {
        diffs.push_back(p.create_integer_variable(-7_i, 7_i, "diff" + to_string(x1) + "_" + to_string(x2)));
        if (options_vars.count("abs")) {
            abs_diffs.push_back(p.create_integer_variable(2_i, 7_i, "absdiff" + to_string(x1) + "_" + to_string(x2)));
            p.post(Abs{ diffs.back(), abs_diffs.back() });
        }
        else {
            p.post(NotEquals{ diffs.back(), constant_variable(0_i) });
            p.post(NotEquals{ diffs.back(), constant_variable(1_i) });
            p.post(NotEquals{ diffs.back(), constant_variable(-1_i) });
        }

        p.post(Minus{ xs[x1], xs[x2], diffs.back() });
    }

    auto stats = solve(p, [&] (const State & s) -> bool {
            cout << "  " << s(xs[0]) << " " << s(xs[1]) << endl;
            cout << s(xs[2]) << " " << s(xs[3]) << " " << s(xs[4]) << " " << s(xs[5]) << endl;
            cout << "  " << s(xs[6]) << " " << s(xs[7]) << endl;
            cout << endl;
            return true;
            });

    cout << stats;

    return EXIT_SUCCESS;
}


/*
 * simulated_telemetry_server.cpp
 *
 *  Created on: Dec 5, 2018
 *      Author: ttw2xk
 */

#include "f1_datalogger/car_data/f1_2018/car_data.h"
#include "f1_datalogger/car_data/f1_2020/car_data.h"
#include <boost/program_options.hpp>
#include <iostream>
#include <boost/asio.hpp>
#include <memory>
#include <thread>
#include <math.h>
#include <boost/math/constants/constants.hpp>
#include <boost/math_fwd.hpp>
namespace po = boost::program_options;
void exit_with_help(po::options_description& desc)
{
        std::stringstream ss;
        ss << "F1 Simulated Telemetry Server. Command line arguments are as follows:" << std::endl;
        desc.print(ss);
        std::printf("%s", ss.str().c_str());
        exit(0); // @suppress("Invalid arguments")
}
int main(int argc, char** argv) {
        using boost::asio::ip::udp;
        using namespace deepf1;
        using namespace deepf1::twenty_eighteen;
	unsigned int packet_size = sizeof(deepf1::twenty_eighteen::PacketMotionData);// BUFLEN;
        unsigned int sleep_time;

        std::string address, port;
        po::options_description desc("Allowed Options");

        try{
                desc.add_options()
                ("help,h", "Displays options and exits")
                ("address,a", po::value<std::string>(&address)->default_value("127.0.0.1"), "IPv4 Address to send data to")
                ("port_number,p", po::value<std::string>(&port)->default_value("20777"), "Port number to send data to")
                ("sleep_time,s", po::value<unsigned int>(&sleep_time)->default_value(17), "Number of milliseconds to sleep between simulated packets")
                ;
	        po::variables_map vm;
                po::store(po::parse_command_line(argc, argv, desc), vm);
                po::notify(vm);
                if (vm.find("help") != vm.end()) {
                        exit_with_help(desc);
                }
        }catch(boost::exception& e){
                exit_with_help(desc);
        }
        boost::asio::io_service io_service;
        udp::resolver resolver(io_service);
        udp::resolver::query query(udp::v4(), address, port);
        udp::endpoint receiver_endpoint = *resolver.resolve(query);
        udp::socket socket(io_service);
        socket.open(udp::v4());

        std::shared_ptr<PacketCarTelemetryData> data(new PacketCarTelemetryData);
        data->m_header.m_packetFormat=2018;
        data->m_header.m_packetId=PacketID::CARTELEMETRY;
        data->m_header.m_packetVersion=18;
        data->m_header.m_playerCarIndex=0;
        float fake_time = 0;
        float dt = 1E-3*( (float) sleep_time );
        float period = 1.0;
        float freq=1/period;
	float pi = boost::math::constants::pi<float>();
	float twopi = boost::math::constants::two_pi<float>();
        float factor = 2.0;
        int id=0;
        while (true) {
                data->m_header.m_frameIdentifier=id;
                data->m_header.m_sessionTime=fake_time;
                data->m_header.m_sessionUID=id;
                data->m_carTelemetryData[data->m_header.m_playerCarIndex].m_steer = (int8_t)(100.0*std::sin(twopi*freq*fake_time));
                data->m_carTelemetryData[data->m_header.m_playerCarIndex].m_throttle = (uint8_t)50 + (uint8_t)(50.0*std::cos(twopi*freq*fake_time));
                data->m_carTelemetryData[data->m_header.m_playerCarIndex].m_brake = (uint8_t)50;
                std::printf("Sending fake UDP data\n");
                std::printf("Fake Time: %f.\n", fake_time);
                std::printf("Steering: %d.\n", data->m_carTelemetryData[data->m_header.m_playerCarIndex].m_steer);
                std::printf("Throttle: %u.\n", data->m_carTelemetryData[data->m_header.m_playerCarIndex].m_throttle);
                std::printf("Brake: %u.\n", data->m_carTelemetryData[data->m_header.m_playerCarIndex].m_brake);
                socket.send_to(boost::asio::buffer(boost::asio::buffer(data.get(), packet_size)), receiver_endpoint);
                fake_time += dt;
                id++;
                std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
        }
        return 0;
		/* */
}


#ifndef SYMMETRICCORRESPONDENCEFILTER_HPP
#define SYMMETRICCORRESPONDENCEFILTER_HPP

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <stdio.h>
#include "../global.hpp"
#include "BaseCorrespondenceFilter.hpp"
#include "CorrespondenceFilter.hpp"
#include "helper_functions.hpp"

typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< float, 1, registration::NUM_FEATURES> FeatureVec; //matrix Mx6 of type float
typedef Eigen::Vector3f Vec3Float;
typedef Eigen::SparseMatrix<float, 0, int> SparseMat;
typedef Eigen::Triplet<float> Triplet;
typedef Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic> MatDynInt; //matrix MxN of type unsigned int
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> MatDynFloat;

namespace registration {

class SymmetricCorrespondenceFilter: public BaseCorrespondenceFilter
{
    /*
    # GOAL
    For each element in inFloatingFeatures, we're going to find corresponding
    features in the elements of inTargetFeatures.

    A push-pull approach is used to find correspondences. Not only
    will we look for correspondences for inFloatingFeatures in the
    inTargetFeatures set, we'll also do the opposite: find correspondences for
    the inTargetFeatures set in the inFloatingFeatures set. These findings are
    combined which creates an effect where the inTargetFeatures sort of attract
    the inFloatingFeatures towards itself.

    # INPUTS
    -inFloatingFeatures
    -inTargetFeatures
    -inTargetFlags

    # PARAMETERS
    -numNeighbours(=3):
    number of nearest neighbours
    -flagThreshold(=0.9):
    threshold that the weighted corresponding flag needs to make in order to be flagged as 1.0.
    Otherwise, it receives flag 0.0f.

    # OUTPUT
    -outCorrespondingFeatures
    -outCorrespondingFlags
    */

    public:
        //CorrespondenceFilter(); //default constructor
        //~CorrespondenceFilter(); //destructor

        void set_floating_input(const FeatureMat * const inFloatingFeatures,
                                const VecDynFloat * const inFloatingFlags);
        void set_target_input(const FeatureMat * const inTargetFeatures,
                            const VecDynFloat * const inTargetFlags);
        void set_parameters(const size_t numNeighbours,
                            const float flagThreshold,
                            const bool _equalizePushPull);
        void update();

    protected:

    private:

        //# Internal Data structures
        CorrespondenceFilter _pushFilter;
        CorrespondenceFilter _pullFilter;

        //# Parameters
        bool _equalizePushPull = false;

        //# Internal functions
        //## Function to update the internal push and pull correspondence filters
        void _update_push_and_pull();
        //## Function to convert the sparse affinity weights into corresponding
        //## features and flags
        void _affinity_to_correspondences();
};

}//namespace registration

#endif // SYMMETRICCORRESPONDENCEFILTER_HPP