#ifndef __PROXIMALS__
#define __PROXIMALS__

#include <Eigen/Dense>
#include <limits>

namespace proxopp
{
class proxOperator
{
public:
	proxOperator() {}
	virtual ~proxOperator() {}

	virtual float f(Eigen::VectorXf& x)
	{
		// f(x)
		return 0.0f;
	}

	virtual Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda)
	{
		// prox_{\lambda f}(x)
		return x;
	}

	virtual Eigen::VectorXf operator()(Eigen::VectorXf& x)
	{
		(*this)(x, 1.0);
	}

	virtual Eigen::VectorXf prox(Eigen::VectorXf& x, float lambda)
	{
		return (*this)(x, lambda);
	}
};

//	The proximal operator of the L1 norm
//	soft_thresholding(x,lambda) = sign(x)*max(|x| - lambda, 0) (element-wise)

class softThresholdingOperator : public proxOperator
{
public:
	float f(Eigen::VectorXf& x) override
	{
		return x.lpNorm<1>();
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda) override
	{
		return x.array().sign()*((x.array().abs()-lambda).max(Eigen::ArrayXf::Zero(x.rows())));
	}
};

//	The proximal operator of the (convex) indicator
//	function of the set {x | Ax = b} is
//	x - At(AtA)^(-1)(Ax - b)

class proxLinearEquality : public proxOperator
{
public:
	proxLinearEquality(Eigen::MatrixXf& A, Eigen::VectorXf& b) : _A(A), _b(b)
	{
		_Q = A.transpose()*((A*A.transpose()).inverse());
		_Qb = _Q*_b;
		_QA = _Q*_A;
	}

	float f(Eigen::VectorXf& x) override
	{
		if((_A*x - _b).norm() < tol)
		{
			return 0;
		} else {
			return std::numeric_limits<float>::max();
		}
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda) override
	{
		return (x - _QA*x + _Qb);
	}
private:
	Eigen::MatrixXf _A;
	Eigen::MatrixXf _b;
	Eigen::MatrixXf _Q;
	Eigen::MatrixXf _QA;
	Eigen::VectorXf _Qb;

	const double tol = 1e-6;
};

//	Proximal operator of the squared L2 norm

class proximalL2Square : public proxOperator
{
public:
	float f(Eigen::VectorXf& x) override
	{
		return x.squaredNorm();
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda) override
	{
		return x/(1.0f+lambda);
	}
};

//	Proximal operator of euclidean norm
//	Block thresholding

class proximalL2 : public proxOperator
{
public:
	float f(Eigen::VectorXf& x)
	{
		return x.lpNorm<2>();
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda)
	{
		const float norm2 = x.lpNorm<2>();

		if(lambda > norm2) return Eigen::VectorXf::Zero(x.rows());

		return x*(1 - lambda / norm2);
	}
};

//	PROJECTION ON BALLS

//	Projector onto L2 ball

class proximalL2Ball : public proxOperator
{
public:
	proximalL2Ball(float lambda) : radius(lambda) {}

	float f(Eigen::VectorXf& x)
	{
		return x.lpNorm<2>() <= radius ? 0 : std::numeric_limits<float>::max();
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda = 0.0)
	{
		float norm2 = x.lpNorm<2>();

		if(norm2 == 0.0f)
			return Eigen::VectorXf::Zero(x.rows());

		return x*radius/x.lpNorm<2>();
	}
private:
	float radius;
};

//	Fast Projection onto the L1 Ball using sorting
//	Using algorithm from
//	Held, M., Wolfe, P., Crowder, H.: Validation of subgradient optimization.
//	Mathematical Programming6, 62–88 (1974)

//	Alternatively, see Condat, L.
// 	Fast Projection onto the Simplex and the L1-Ball
//	https://www.gipsa-lab.grenoble-inp.fr/~laurent.condat/publis/Condat_simplexproj.pdf
//	It corresponds to Algorithm 1

class proximalL1Ball : public proxOperator
{
public:
	proximalL1Ball(float lambda) : radius(lambda) {}

	float f(Eigen::VectorXf& x)
	{
		return (x.lpNorm<1>() < radius) ? 0 : std::numeric_limits<float>::max();
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda = 0.0f)
	{
		if(x.lpNorm<1>() < radius) return x;

		Eigen::ArrayXf u = Eigen::ArrayXf(x.array().abs());
		std::sort(u.data(), u.data()+u.size(), std::greater<float>());

		int k=1, K=1;
		float tau = 0.0f;

		for(k=1;k<=x.rows();k++)
		{
			float mean = 0.0f;

			for(int i = 0; i < k; i++)
			{
				mean += u[i];
			}
			mean = (mean-radius)/k;

			if(mean < u[k-1])
			{
				K 	= k-1;
				tau = mean;
			}
		}
		std::cout << tau << std::endl;

		Eigen::VectorXf xtau = x.array().abs()-tau*Eigen::ArrayXf::Ones(x.rows());
		return x.array().sign()*(xtau).array().max(Eigen::VectorXf::Zero(x.rows()).array());
	}
private:
	float radius;
};

//	Proximal operator of the indicator function of
//	the set {x | max abs(x_i) <= lambda} (Linf ball)

class proximalLinfBall : public proxOperator
{
public:
	proximalLinfBall(float lambda) : radius(lambda) {}

	float f(Eigen::VectorXf& x) override
	{
		return (x.lpNorm<Eigen::Infinity>() < radius) ? 0 : std::numeric_limits<float>::max();
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x, float lambda = 0.0f) override
	{
		for(int i = 0; i < x.rows(); i++)
		{
			if(x[i] > radius) 	x[i] = radius;
			if(x[i] < -radius) 	x[i] = -radius;
		}
	}
private:
	float radius;
};

//	Projection on the intersection of L1 and Linf balls
//	with binary search

class proximalL1LinfBall : public proxOperator
{
public:
	proximalL1LinfBall(float radius_l1, float radius_linf) : r_l1(radius_l1),
	r_linf(radius_linf)
	{}

	float f(Eigen::VectorXf& x)
	{
		if(x.lpNorm<1>()<r_l1 && x.lpNorm<Eigen::Infinity>() < r_linf) return 0.0f;
		return std::numeric_limits<float>::max();
	}

	Eigen::VectorXf operator()(Eigen::VectorXf& x)
	{
		int n = x.rows();
		proximalLinfBall projLinf(r_linf);
		Eigen::VectorXf y = projLinf(x);

		if(y.lpNorm<1>() < r_l1) return y;

		const double eps = 1e-6;
		const int itermax = 50;
		int k = 0;
		float nu_l = 0.0f;
		float nu_r = x.array().abs().maxCoeff();
		Eigen::ArrayXf z1,z2;

		while((k < itermax) && (nu_l - nu_r > eps))
		{
			float nu_m = 0.5*(nu_l+nu_r);

			z1 = y.array().abs()-nu_m*Eigen::ArrayXf::Ones(n);
			z1 = z1.max(Eigen::ArrayXf::Zero(n));
			z2 = z1.min(r_linf*Eigen::ArrayXf::Ones(n));

			if(z2.lpNorm<1>() < r_l1)
			{
				nu_r = nu_m;
			} else {
				nu_l = nu_m;
			}

			k++;
		}

		return z2*x.array().sign();
	}
private:
	float r_l1; // radius of L1 ball
	float r_linf;  // radius of Linf ball (max coeff of abs values)
};

} // namespace proxopp

#endif


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

using namespace Eigen;
using namespace std;

void main() {

	{
		Eigen::Matrix2d mat;
		mat << 1, 2,
			3, 4;
		cout << "mat: \n" << mat << endl;
		cout << "Here is mat.sum():       " << mat.sum() << endl;
		cout << "Here is mat.prod():      " << mat.prod() << endl;
		cout << "Here is mat.mean():      " << mat.mean() << endl;
		cout << "Here is mat.minCoeff():  " << mat.minCoeff() << endl;
		cout << "Here is mat.maxCoeff():  " << mat.maxCoeff() << endl;
		cout << "Here is mat.trace():     " << mat.trace() << endl;
	}
	{
		VectorXf v(2);
		MatrixXf m(2, 2), n(2, 2);

		v << -1,
			2;

		m << 1, -2,
			-3, 4;
		cout << "v.squaredNorm() = " << v.squaredNorm() << endl;
		cout << "v.norm() = " << v.norm() << endl;
		cout << "v.lpNorm<1>() = " << v.lpNorm<1>() << endl;
		cout << "v.lpNorm<2>() = " << v.lpNorm<2>() << endl;
		cout << "v.lpNorm<Infinity>() = " << v.lpNorm<Infinity>() << endl;
		cout << endl;
		cout << "m.squaredNorm() = " << m.squaredNorm() << endl;
		cout << "m.norm() = " << m.norm() << endl;
		cout << "m.lpNorm<1>() = " << m.lpNorm<1>() << endl;
		cout << "m.lpNorm<2>() = " << m.lpNorm<2>() << endl;
		cout << "m.lpNorm<Infinity>() = " << m.lpNorm<Infinity>() << endl;
	}
	{
		MatrixXf m(2, 2);
		m << 1, -2, -3, 4;
		cout << "1-norm(m) = " << m.cwiseAbs().colwise().sum().maxCoeff() << " == " <<
			m.colwise().lpNorm<1>().maxCoeff() << endl;
		cout << "infty-norm(m) = " << m.cwiseAbs().rowwise().sum().maxCoeff()
			<< " == " << m.rowwise().lpNorm<1>().maxCoeff() << endl;
	}

	{
		ArrayXXf a(2, 2);

		a << 1, 2,
			3, 4;
		cout << "(a > 0).all()   = " << (a > 0).all() << endl;
		cout << "(a > 0).any()   = " << (a > 0).any() << endl;
		cout << "(a > 0).count() = " << (a > 0).count() << endl;
		cout << endl;
		cout << "(a > 2).all()   = " << (a > 2).all() << endl;
		cout << "(a > 2).any()   = " << (a > 2).any() << endl;
		cout << "(a > 2).count() = " << (a > 2).count() << endl;
	}

	{
		// Visitors
		Eigen::MatrixXf m(2, 2);

		m << 1, 2,
			4, 4;
		//get location of maximum
		MatrixXf::Index maxRow, maxCol;
		float max = m.maxCoeff(&maxRow, &maxCol);
		//get location of minimum
		MatrixXf::Index minRow, minCol;
		float min = m.minCoeff(&minRow, &minCol);
		cout << "Max: " << max << ", at: " <<
			maxRow << "," << maxCol << endl;
		cout << "Min: " << min << ", at: " <<
			minRow << "," << minCol << endl;
	}
	{
		Eigen::MatrixXf mat(2, 4);
		mat << 1, 2, 6, 9,
			   3, 1, 7, 2;
		cout << "column's maximum : " << endl;
		cout << mat.colwise().maxCoeff() << endl;
	}
	{
		Eigen::MatrixXf mat(2, 4);
		Eigen::VectorXf v(2);

		mat << 1, 2, 6, 9,
			   3, 1, 7, 2;
		v << 0,
			 1;
		// add v to each column of m
		mat.colwise() += v;
		cout << "Broadcasting colwise: \n" << mat << endl;

		Eigen::VectorXf w(4);
		w << 0, 1, 2, 3;

		mat.rowwise() += w.transpose();

		cout << "Broadcasting rowwise: \n" << mat << endl;
	}
	{
		Eigen::MatrixXf m(2, 4);
		Eigen::VectorXf v(2);
		m << 1, 23, 6, 9,
			 3, 11, 7, 2;
		v << 2,
			 3;
		MatrixXf::Index index;
		// find nearest neighbour
		(m.colwise() - v).colwise().squaredNorm().minCoeff(&index);
		cout << "Nearest neighbour is column " << index << ":" << endl;
		cout << m.col(index) << endl;

	}
	system("pause");
}


#include <armadillo>
#include <gnuplot-iostream.h>
#include <memory>

using namespace arma;
using namespace std;

class Conjunto {
public:
	virtual double membresia(double x) const = 0;
	virtual void graficar(Gnuplot& gp, double escala = 1) const = 0;
	virtual double centroide_x() const = 0;
	virtual double area(double altura) const = 0;
};

class ConjuntoTrapezoidal : public Conjunto {
public:
	ConjuntoTrapezoidal(vec puntos);

	double membresia(double x) const override;
	void graficar(Gnuplot& gp, double escala = 1) const override;
	double centroide_x() const override;
	double area(double altura) const override;

	const double a, b, c, d;
};

class ConjuntoGaussiano : public Conjunto {
public:
	ConjuntoGaussiano(double t_media, double t_sigma);

	double membresia(double x) const override;
	void graficar(Gnuplot& gp, double escala = 1) const override;
	double centroide_x() const override;
	double area(double altura) const override;
	const double media, sigma;
};

enum class tipoConjunto {
    trapezoidal,
    gaussiano
};

class SistemaBorroso {
public:
	SistemaBorroso(const mat& matrizEntrada,
	               tipoConjunto tipoEntrada);
	SistemaBorroso(const mat& matrizEntrada,
	               tipoConjunto tipoEntrada,
	               const mat& matrizSalida,
	               tipoConjunto tipoSalida,
	               const uvec& t_reglasMapeo = {});

	vec activacionesEntrada(double x) const;
	vec mapeoEntradaSalida(const vec& activacionesEntrada) const;
	double defuzzyficarSalida(vec activaciones) const;
	double salidaSistema(double entrada) const;

    const vector<unique_ptr<Conjunto>>& conjuntosEntrada() const { return m_conjuntosEntrada; };
    const vector<unique_ptr<Conjunto>>& conjuntosSalida() const { return m_conjuntosSalida; };

private:
    vector<unique_ptr<Conjunto>> m_conjuntosEntrada;
    vector<unique_ptr<Conjunto>> m_conjuntosSalida;
    const uvec m_reglasMapeo;
};

ConjuntoTrapezoidal::ConjuntoTrapezoidal(vec puntos)
    : a{puntos(0)}
    , b{puntos(1)}
    , c{puntos(2)}
    , d{puntos(3)}
{
	// chequear orden de puntos
	if (!puntos.is_sorted())
		throw runtime_error("los puntos no estan ordenados");

	if (puntos.n_elem != 4)
		throw runtime_error("no se tienen cuatro puntos");
}

double ConjuntoTrapezoidal::membresia(double x) const
{
	if (x < a or x > d)
		return 0;
	else if (a <= x and x < b)
		return (x - a) / (b - a);
	else if (b <= x and x <= c)
		return 1;
	else if (c < x and x <= d)
		return 1 - (x - c) / (d - c);
	else
        throw runtime_error("Nunca se debería llegar acá (en ConjuntoTrapezoidal::membresia)");
}

void ConjuntoTrapezoidal::graficar(Gnuplot& gp, double escala) const
{
	const mat puntos = {{a, 0},
	                    {b, 1 * escala},
	                    {c, 1 * escala},
	                    {d, 0}};

	gp << gp.file1d(puntos) << "with lines";
}

double ConjuntoTrapezoidal::centroide_x() const
{
	// Ver http://www.efunda.com/math/areas/Trapezoid.cfm
	const double A = c - b;
	const double B = d - a;
	const double C = b - a;

	const double numerador = 2 * A * C
	                         + A * A
	                         + C * B
	                         + A * B
	                         + B * B;
	const double denominador = 3 * (A + B);
	const double result = numerador / denominador;

	// El resultado está como si el trapezoide tuviera el punto
	// a en (0, 0).
	// Para corregir esto, hace falta sumarle a.

	return result + a;
}

double ConjuntoTrapezoidal::area(double altura) const
{
	return ((d - a) + (c - b)) * altura / 2;
}

ConjuntoGaussiano::ConjuntoGaussiano(double t_media, double t_sigma)
    : media{t_media}
    , sigma{t_sigma}
{
	if (t_sigma <= 0)
		throw runtime_error("el sigma debe ser mayor o igual a cero");
}

double ConjuntoGaussiano::membresia(double x) const
{
	return exp(-0.5 * (pow((x - media) / sigma, 2)));
}

void ConjuntoGaussiano::graficar(Gnuplot& gp, double escala) const
{
	const string exponencial = to_string(escala)
	                           + " * exp(-0.5 * ((x - "
	                           + to_string(media)
	                           + ") / "
	                           + to_string(sigma)
	                           + ") ** 2)";

	gp << exponencial << "with lines";
}

double ConjuntoGaussiano::centroide_x() const
{
	return media;
}

double ConjuntoGaussiano::area(double altura) const
{
	// PREGUNTAR:
	// ¿Se puede escalar el área original de la gaussiana
	// por el grado de activación y obtener el área correcta?
	return sigma * sqrt(2 * datum::pi) * altura;
}

vector<unique_ptr<Conjunto>> parsearTrapecios(const mat& matrizConjuntos)
{
    vector<unique_ptr<Conjunto>> result;

	for (unsigned int i = 0; i < matrizConjuntos.n_rows; ++i) {
		result.push_back(unique_ptr<Conjunto>{new ConjuntoTrapezoidal{matrizConjuntos.row(i).t()}});
	}

	return result;
}

vector<unique_ptr<Conjunto>> parsearGaussianas(const mat& matrizConjuntos)
{
	if (matrizConjuntos.n_cols != 2)
		throw runtime_error("La matriz debe tener dos columnas");

    vector<unique_ptr<Conjunto>> result;

	for (unsigned int i = 0; i < matrizConjuntos.n_rows; ++i) {
		result.push_back(unique_ptr<Conjunto>{
		    new ConjuntoGaussiano{matrizConjuntos(i, 0),
		                          matrizConjuntos(i, 1)}});
	}

	return result;
}

// NOTA
// Después de usar esta función hace falta cerrar el ploteo
// ya sea ploteando algo más, o ploteando "NaN notitle".
void graficarConjuntos(const vector<unique_ptr<Conjunto>>& conjuntos,
                       Gnuplot& gp,
                       vec escalas = {})
{
	if (escalas.empty())
		escalas = ones(conjuntos.size());

	gp << "plot ";

	for (unsigned int i = 0; i < conjuntos.size(); ++i) {
		conjuntos[i]->graficar(gp, escalas(i));
		gp << " title 'Conjunto " << (i + 1) << "', ";
	}
}

SistemaBorroso::SistemaBorroso(const mat& matrizEntrada, tipoConjunto tipoEntrada)
{
	switch (tipoEntrada) {
	case tipoConjunto::trapezoidal:
        m_conjuntosEntrada = parsearTrapecios(matrizEntrada);
		break;

	case tipoConjunto::gaussiano:
        m_conjuntosEntrada = parsearGaussianas(matrizEntrada);
		break;
	}
}

SistemaBorroso::SistemaBorroso(const mat& matrizEntrada,
                               tipoConjunto tipoEntrada,
                               const mat& matrizSalida,
                               tipoConjunto tipoSalida,
                               const uvec& t_reglasMapeo)
    : m_reglasMapeo{t_reglasMapeo}
{
	switch (tipoEntrada) {
	case tipoConjunto::trapezoidal:
        m_conjuntosEntrada = parsearTrapecios(matrizEntrada);
		break;

	case tipoConjunto::gaussiano:
        m_conjuntosEntrada = parsearGaussianas(matrizEntrada);
		break;
	}

	switch (tipoSalida) {
	case tipoConjunto::trapezoidal:
        m_conjuntosSalida = parsearTrapecios(matrizSalida);
		break;

	case tipoConjunto::gaussiano:
        m_conjuntosSalida = parsearGaussianas(matrizSalida);
		break;
	}

	if (!t_reglasMapeo.empty()) {
        if (t_reglasMapeo.n_elem != m_conjuntosEntrada.size())
			throw runtime_error(
                "La cantidad de reglas tiene que ser la misma "
                "que la cantidad de conjuntos de entrada");

		if (unique(t_reglasMapeo).eval().n_elem != t_reglasMapeo.n_elem)
			throw runtime_error(
                "No puede haber reglas que mapeen al mismo "
                "conjunto de salida");

        if (max(t_reglasMapeo) != m_conjuntosSalida.size()
		    || min(t_reglasMapeo != 1))
			throw runtime_error(
                "Los valores de las reglas están fuera de rango");
	}
}

vec SistemaBorroso::activacionesEntrada(double x) const
{
    vec result(m_conjuntosEntrada.size());

    for (unsigned int i = 0; i < m_conjuntosEntrada.size(); ++i) {
        result(i) = m_conjuntosEntrada[i]->membresia(x);
    }

	return result;
}

vec SistemaBorroso::mapeoEntradaSalida(const vec& activacionesEntrada) const
{
    if (m_reglasMapeo.empty())
		throw runtime_error("No se tienen reglas de mapeo");

    vec activacionesSalida(m_conjuntosSalida.size());

    for (unsigned int i = 0; i < m_reglasMapeo.n_elem; ++i) {
		// El "- 1" es para pasar el vector de reglas de mapeo de base 1 a base 0
        activacionesSalida(m_reglasMapeo(i) - 1) = activacionesEntrada(i);
    }

	return activacionesSalida;
}

double SistemaBorroso::defuzzyficarSalida(vec activaciones) const
{
    if (activaciones.size() != m_conjuntosSalida.size())
		throw runtime_error(
            "La cantidad de activaciones y la cantidad "
            "de conjuntos de salida no coinciden");

	double numerador = 0, denominador = 0;

    for (unsigned int i = 0; i < m_conjuntosSalida.size(); ++i) {
        numerador += m_conjuntosSalida[i]->centroide_x()
                     * m_conjuntosSalida[i]->area(activaciones(i));
        denominador += m_conjuntosSalida[i]->area(activaciones(i));
	}

	return numerador / denominador;
}

double SistemaBorroso::salidaSistema(double entrada) const
{
	const vec actEntrada = activacionesEntrada(entrada);
	const vec actSalida = mapeoEntradaSalida(actEntrada);
	const double salida = defuzzyficarSalida(actSalida);

	return salida;
}


#include <Eigen/Core>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <mathtoolbox/gradient-descent.hpp>
#include <optimization-test-functions.hpp>

int main()
{
    constexpr otf::FunctionType type     = otf::FunctionType::Rosenbrock;
    constexpr int               num_dims = 10;

    std::srand(static_cast<unsigned>(std::time(nullptr)));

    const Eigen::VectorXd x_init = Eigen::VectorXd::Random(num_dims);
    const auto            f      = [](const Eigen::VectorXd& x) { return otf::GetValue(x, type); };
    const auto            g      = [](const Eigen::VectorXd& x) { return otf::GetGrad(x, type); };

    const Eigen::VectorXd lower_bound; // Empty vector indicates no lower bound constraint
    const Eigen::VectorXd upper_bound; // Empty vector indicates no upper bound constraint

    constexpr double epsilon       = 1e-12;
    constexpr double default_alpha = 1e-01;
    constexpr int    max_num_iters = 100000;

    Eigen::VectorXd x_star;
    unsigned        num_iters;
    mathtoolbox::optimization::RunGradientDescent(
        x_init, f, g, lower_bound, upper_bound, epsilon, default_alpha, max_num_iters, x_star, num_iters);

    const Eigen::VectorXd expected_solution = otf::GetSolution(num_dims, type);
    const double          expected_value    = otf::GetValue(expected_solution, type);
    const double          initial_value     = otf::GetValue(x_init, type);
    const double          found_value       = otf::GetValue(x_star, type);

    std::cout << "#iterations: " << num_iters << std::endl;
    std::cout << "Initial solution: " << x_init.transpose() << " (" << initial_value << ")" << std::endl;
    std::cout << "Found solution: " << x_star.transpose() << " (" << found_value << ")" << std::endl;
    std::cout << "Expected solution: " << expected_solution.transpose() << " (" << expected_value << ")" << std::endl;

    return 0;
}


// -*- coding: utf-16 -*-
#pragma once

/*! @file include/fractions.hpp
 *  This is a C++ Library header.
 */

#include <boost/operators.hpp>
// #include <cmath>
#include "common_concepts.h"
#include <numeric>
#include <type_traits>
#include <utility>

namespace fun
{

/**
 * @brief absolute (for unsigned)
 *
 * @tparam T
 * @param a
 * @return T
 */
template <typename T>
requires std::is_unsigned_v<T>
inline constexpr auto abs(const T& a) -> T
{
    return a;
}

/**
 * @brief absolute
 *
 * @tparam T
 */
template <typename T>
requires (!std::is_unsigned_v<T> && ordered_ring<T>)
inline constexpr auto abs(const T& a) -> T
{
    return (a < T(0)) ? -a : a;
}

/*!
 * @brief Greatest common divider
 *
 * @tparam _Mn
 * @param[in] __m
 * @param[in] __n
 * @return _Mn
 */
template <Integral _Mn>
inline constexpr auto gcd_recur(_Mn __m, _Mn __n) -> _Mn
{
    if (__n == 0)
    {
        return abs(__m);
    }
    return gcd_recur(__n, __m % __n);
}

/*!
 * @brief Greatest common divider
 *
 * @tparam _Mn
 * @param[in] __m
 * @param[in] __n
 * @return _Mn
 */
template <Integral _Mn>
inline constexpr auto gcd(_Mn __m, _Mn __n) -> _Mn
{
    if (__m == 0)
    {
        return abs(__n);
    }
    return gcd_recur(__m, __n);
}

/*!
 * @brief Least common multiple
 *
 * @tparam _Mn
 * @param[in] __m
 * @param[in] __n
 * @return _Mn
 */
template <Integral _Mn>
inline constexpr auto lcm(_Mn __m, _Mn __n) -> _Mn
{
    if (__m == 0 || __n == 0)
    {
        return 0;
    }
    return (abs(__m) / gcd(__m, __n)) * abs(__n);
}

template <Integral Z>
struct Fraction : boost::totally_ordered<Fraction<Z>,
                      boost::totally_ordered2<Fraction<Z>, Z,
                          boost::multipliable2<Fraction<Z>, Z,
                              boost::dividable2<Fraction<Z>, Z>>>>
{
    Z _num;
    Z _den;

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] num
     * @param[in] den
     */
    constexpr Fraction(Z num, Z den)
        : _num {std::move(num)}
        , _den {std::move(den)}
    {
        this->normalize();
    }

    constexpr void normalize()
    {
        Z common = gcd(this->_num, this->_den);
        if (common == Z(1) || common == Z(0))
        {
            return;
        }
        if (this->_den < Z(0))
        {
            common = -common;
        }
        this->_num /= common;
        this->_den /= common;
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] num
     */
    constexpr explicit Fraction(Z&& num)
        : _num {std::move(num)}
        , _den(Z(1))
    {
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] num
     */
    constexpr explicit Fraction(const Z& num)
        : _num {num}
        , _den(1)
    {
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] num
     */
    constexpr Fraction()
        : _num(0)
        , _den(1)
    {
    }

    /*!
     * @brief
     *
     * @return const Z&
     */
    [[nodiscard]] constexpr auto num() const noexcept -> const Z&
    {
        return _num;
    }

    /*!
     * @brief
     *
     * @return const Z&
     */
    [[nodiscard]] constexpr auto den() const noexcept -> const Z&
    {
        return _den;
    }

    /*!
     * @brief
     *
     * @return Fraction
     */
    [[nodiscard]] constexpr auto abs() const -> Fraction
    {
        return Fraction(abs(this->_num), abs(this->_den));
    }

    /*!
     * @brief
     *
     */
    constexpr void reciprocal() noexcept(std::is_nothrow_swappable_v<Z>)
    {
        std::swap(this->_num, this->_den);
    }

    /*!
     * @brief
     *
     * @return Fraction
     */
    constexpr auto operator-() const -> Fraction
    {
        auto res = Fraction(*this);
        res._num = -res._num;
        return res;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator+(const Fraction& frac) const -> Fraction
    {
        if (this->_den == frac._den)
        {
            return Fraction(this->_num + frac._num, this->_den);
        }
        auto d = this->_den * frac._den;
        auto n = frac._den * this->_num + this->_den * frac._num;
        return Fraction(n, d);
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator-(const Fraction& frac) const -> Fraction
    {
        return *this + (-frac);
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator*(const Fraction& frac) const -> Fraction
    {
        auto n = this->_num * frac._num;
        auto d = this->_den * frac._den;
        return Fraction(std::move(n), std::move(d));
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator/(Fraction frac) const -> Fraction
    {
        frac.reciprocal();
        return *this * frac;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator+(const Z& i) const -> Fraction
    {
        auto n = this->_num + this->_den * i;
        return Fraction(std::move(n), this->_den);
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator-(const Z& i) const -> Fraction
    {
        return *this + (-i);
    }

    // /*!
    //  * @brief
    //  *
    //  * @param[in] i
    //  * @return Fraction
    //  */
    // constexpr Fraction operator*(const Z& i) const noexcept
    // {
    //     auto n = _num * i;
    //     return Fraction(n, _den);
    // }

    // /*!
    //  * @brief
    //  *
    //  * @param[in] i
    //  * @return Fraction
    //  */
    // constexpr Fraction operator/(const Z& i) const noexcept
    // {
    //     auto d = _den * i;
    //     return Fraction(_num, d);
    // }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator+=(const Fraction& frac) -> Fraction&
    {
        return *this = *this + frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator-=(const Fraction& frac) -> Fraction&
    {
        return *this = *this - frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator*=(const Fraction& frac) -> Fraction&
    {
        return *this = *this * frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator/=(const Fraction& frac) -> Fraction&
    {
        return *this = *this / frac;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator+=(const Z& i) -> Fraction&
    {
        return *this = *this + i;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator-=(const Z& i) -> Fraction&
    {
        return *this = *this - i;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator*=(const Z& i) -> Fraction&
    {
        const auto common = gcd(i, this->_den);
        if (common == Z(1))
        {
            this->_num *= i;
        }
        // else if (common == Z(0)) [[unlikely]] // both i and den are zero
        // {
        //     this->_num = Z(0);
        // }
        else
        {
            this->_num *= (i / common);
            this->_den /= common;
        }
        return *this;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator/=(const Z& i) -> Fraction&
    {
        const auto common = gcd(this->_num, i);
        if (common == Z(1))
        {
            this->_den *= i;
        }
        // else if (common == Z(0)) [[unlikely]] // both i and num are zero
        // {
        //     this->_den = Z(0);
        // }
        else
        {
            this->_den *= (i / common);
            this->_num /= common;
        }
        return *this;
    }

    /*!
     * @brief Three way comparison
     *
     * @param[in] frac
     * @return auto
     */
    template <typename U>
    constexpr auto cmp(const Fraction<U>& frac) const -> Fraction&
    {
        if (this->_den == frac._den)
        {
            return (this->_num - frac._num) * this->_den;
        }
        return this->_num * frac._den - this->_den * frac._num;
    }

    template <typename U>
    constexpr auto operator==(const Fraction<U>& rhs) const -> bool
    {
        if (this->_den == rhs._den)
        {
            return this->_num == rhs._num;
        }

        return this->_num * rhs._den == this->_den * rhs._num;
    }

    template <typename U>
    constexpr auto operator<(const Fraction<U>& rhs) const -> bool
    {
        if (this->_den == rhs._den)
        {
            return this->_num < rhs._num;
        }

        return this->_num * rhs._den < this->_den * rhs._num;
    }

    /**
     * @brief
     *
     */
    constexpr auto operator==(const Z& rhs) const -> bool
    {
        return this->_den == Z(1) && this->_num == rhs;
    }

    /**
     * @brief
     *
     */
    constexpr auto operator<(const Z& rhs) const -> bool
    {
        return this->_num < this->_den * rhs;
    }

    /**
     * @brief
     *
     */
    constexpr auto operator>(const Z& rhs) const -> bool
    {
        return this->_num > this->_den * rhs;
    }

    // /*!
    //  * @brief
    //  *
    //  * @return double
    //  */
    // constexpr explicit operator double())
    // {
    //     return double(_num) / _den;
    // }

    // /**
    //  * @brief
    //  *
    //  */
    // friend constexpr bool operator<(const Z& lhs, const Fraction<Z>& rhs))
    // {
    //     return lhs * rhs.den() < rhs.num();
    // }
};

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator+(const Z& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator-(const Z& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return c + (-frac);
}

// /*!
//  * @brief
//  *
//  * @param[in] c
//  * @param[in] frac
//  * @return Fraction<Z>
//  */
// template <typename Z>
// constexpr Fraction<Z> operator*(const Z& c, const Fraction<Z>& frac)
// {
//     return frac * c;
// }

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator+(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator-(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return (-frac) + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator*(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac * c;
}

/*!
 * @brief
 *
 * @tparam _Stream
 * @tparam Z
 * @param[in] os
 * @param[in] frac
 * @return _Stream&
 */
template <typename _Stream, typename Z>
auto operator<<(_Stream& os, const Fraction<Z>& frac) -> _Stream&
{
    os << frac.num() << "/" << frac.den();
    return os;
}

// For template deduction
// Integral{Z} Fraction(const Z &, const Z &) noexcept -> Fraction<Z>;

} // namespace fun


#include <iostream>

#include <boost/multiprecision/gmp.hpp>

#include "headers/euler.hpp"

const int MIN_A_B = 1;
const int MAX_A_B = 100;

typedef boost::multiprecision::mpz_int big_int;

int main(int argc, char** argv) {
  int largestA;
  int largestB;
  big_int largestDigitSum = 0;

  for (int a = MIN_A_B; a <= MAX_A_B; a++) {
    for (int b = MIN_A_B; b <= MAX_A_B; b++) {
      big_int n = euler::power<big_int>(a, b);

      big_int digitSum = 0;

      while (n) {
        digitSum += n % 10;
        n /= 10;
      }

      if (digitSum > largestDigitSum) {
        largestDigitSum = digitSum;
        largestA = a;
        largestB = b;
      }
    }
  }

  std::cout << "Largest digit sum of A^B is " << largestDigitSum << " "
            << "for A=" << largestA << ", B=" << largestB
            << std::endl;

  return 0;
}


/*!
 * @file
 * @brief
 * @copyright alphya 2021
 * 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 NYARUGA_UTIL_GROUP_HPP
#define NYARUGA_UTIL_GROUP_HPP

#include <nyaruga_util/algebra.hpp>
#include <Eigen/Dense>
#include <complex>
#include <cmath>
#include <concepts>
#include <numbers>

namespace nyaruga {

namespace util {

namespace detail {

template <std::floating_point T = double>
struct SU1
{
    using value_type = std::complex<T> ;
private:
    std::complex<T> val_;
    constexpr SU1(const std::complex<T>& v) noexcept : val_(v) {}
public:
    constexpr void set_val(const T& theta) noexcept { val_ = exp(std::complex<T>{0,-1}*theta); }
    constexpr std::complex<T> val(const T& theta) const noexcept { val_  = exp(std::complex<T>{0,-1}*theta); return val_; }
    constexpr std::complex<T> val() const noexcept{ return val_; }
    constexpr operator std::complex<T>() const noexcept{ return val_; }
    constexpr SU1() noexcept = default;
    constexpr SU1(const SU1& v) noexcept : val_(v.val_) {}
    constexpr SU1(const T& theta) noexcept : val_(exp(std::complex<T>{0,-1}*theta)) {}
    friend SU1 operator*(const SU1& lhs, const SU1 & rhs) noexcept { return SU1(lhs.val()*rhs.val()); }
    constexpr SU1 conj() { return std::conj(val_); }
};

template <std::floating_point T = double>
struct SO2
{
    using value_type = Eigen::Matrix<T,2,2> ;
private:
    Eigen::Matrix<T,2,2> val_;
    constexpr SO2(const Eigen::Matrix<T,2,2>& v) noexcept : val_(v) {}
public:
    constexpr void set_val(const T& theta) noexcept { val_ << cos(theta), -sin(theta), sin(theta), cos(theta); }
    constexpr Eigen::Matrix<T,2,2> val(const T& theta) const noexcept { val_ << cos(theta), -sin(theta), sin(theta), cos(theta); return val_; };
    constexpr Eigen::Matrix<T,2,2> val() const noexcept{ return val_; };
    constexpr operator Eigen::Matrix<T,2,2>() const noexcept { return val_; }
    constexpr SO2() noexcept = default;
    constexpr SO2(const SO2& v) noexcept : val_(v.val_) {}
    constexpr SO2(const T& theta) noexcept { val_ << cos(theta), -sin(theta), sin(theta), cos(theta);}
    friend SO2 operator*(const SO2& lhs, const SO2 & rhs) noexcept { return SO2(lhs.val()*rhs.val()); }
    constexpr SO2 transpose() { return SO2(val_.transpose()); }
};

template <std::floating_point T = double>
struct SO3 // http://www.tuhep.phys.tohoku.ac.jp/~watamura/kougi/GP2012_4.pdf
{
    using value_type = Eigen::Matrix<T,3,3> ;
private:
    Eigen::Matrix<T,3,3> val_;
    constexpr SO3(const Eigen::Matrix<T,3,3>& v) noexcept : val_(v) {}

    // alpha, beta, gamma はオイラー角
    constexpr value_type calc_val(const T& alpha, const T& beta, const T& gamma) noexcept
    {
        value_type RX, RY, RZ;
        RX << 1, 0, 0,  0, cos(alpha), -sin(alpha),  0, sin(alpha), cos(alpha);
        RY << cos(beta), 0, sin(beta),  0, 1, 0,  -sin(beta), 0, cos(beta);
        RZ << cos(gamma), -sin(gamma), 0,  sin(gamma), cos(gamma), 0,  0, 0, 1;
        return RX*(RY*RZ);
    }

public:
    // alpha, beta, gamma はオイラー角
    constexpr void set_val(const T& alpha, const T& beta, const T& gamma) noexcept { val_ = calc_val(alpha, beta, gamma); }
    constexpr value_type val(const T& alpha, const T& beta, const T& gamma) const noexcept { val_ = calc_val(alpha, beta, gamma); return val_; };
    constexpr value_type val() const noexcept{ return val_; };
    constexpr operator value_type() const noexcept { return val_; }
    constexpr SO3() noexcept = default;
    constexpr SO3(const SO3& v) noexcept : val_(v.val_) {}
    constexpr SO3(const T& alpha, const T& beta, const T& gamma) noexcept : val_(calc_val(alpha, beta, gamma)) {}
    friend SO3 operator*(const SO3& lhs, const SO3 & rhs) noexcept { return SO3(lhs.val()*rhs.val()); }
    constexpr SO3 transpose() { return SO3(val_.transpose()); }
};

} // detail

using detail::SU1;
using detail::SO2;
using detail::SO3;

} // util

} // nyaruga

/* how to use

# include "group.hpp"

using namespace nyaruga::util;
using namespace std::numbers;

int main(){
    SU1 a(pi/2*23);
    SU1 b(pi/32);
    std::cout << norm((a*b).val()) << "\n"; // 1
    std::cout << (a.conj()*(a)).val() << "\n\n"; // E

    SO2 c(pi/2+2);
    SO2 d(pi/2*24+4);
    std::cout << (c*d).val() << "\n";
    std::cout << (c*c.transpose()).val() << "\n"; // E
    std::cout << (c*d).val().determinant() << "\n\n"; //1

    SO3 e(pi/2+2, 23., 4.);
    SO3 f(pi/2*24+4, 3., 42.);
    std::cout << (e*f).val() << "\n";
    std::cout << (e*e.transpose()).val() << "\n"; // E
    std::cout << (e*f).val().determinant() << "\n"; //1
}

*/

#endif // #ifndef NYARUGA_UTIL_GROUP_HPP


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

// The vector function with parameters for which the Jacobian is needed
VectorXdual f(const VectorXdual& x, dual p)
{
    return x * exp(p);
}

int main()
{
    VectorXdual x(5);    // the input vector x with 5 variables
    x << 1, 2, 3, 4, 5;  // x = [1, 2, 3, 4, 5]

    dual p = 3;    // the input parameter vector p with 3 variables

    VectorXdual F;  // the output vector F = f(x, p) evaluated together with Jacobian below

    MatrixXd Jpx = jacobian(f, wrtpack(p, x), at(x, p), F);  // evaluate the function and the Jacobian matrix [dF/dp, dF/dx]

    cout << "F = \n" << F << endl;    // print the evaluated output vector F
    cout << "Jpx = \n" << Jpx << endl;  // print the evaluated Jacobian matrix [dF/dp, dF/dx]
}


#include "register.h"

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/QR>

#include <igl/random_points_on_mesh.h>
#include <igl/point_mesh_squared_distance.h>
#include <igl/per_face_normals.h>

void closest_rotation(
	const Eigen::Matrix3d & M,
	Eigen::Matrix3d & R)
{

	// My code
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::Matrix3d Omega = Eigen::Matrix3d::Identity();
	Omega(2, 2) = (U*V.transpose()).determinant();
	R = (U * Omega * V.transpose()).transpose();
}

void point_triangle_distance(
	const Eigen::RowVector3d & x,
	const Eigen::RowVector3d & aa,
	const Eigen::RowVector3d & bb,
	const Eigen::RowVector3d & cc,
	double & dd,
	Eigen::RowVector3d & p)
{

	Eigen::RowVector3d B = aa;
	Eigen::RowVector3d E0 = bb - aa;
	Eigen::RowVector3d E1 = cc - aa;
	Eigen::RowVector3d P = x;

	double a = E0.dot(E0);
	double b = E0.dot(E1);
	double c = E1.dot(E1);
	double d = E0.dot(B - P);
	double e = E1.dot(B - P);
	double f = (B - P).dot(B - P);

	double s = b * e - c * d;
	double t = b * d - a * e;
	double det = a * c - b * b;

	if (s + t <= det) {
		if (s < 0) {
			if (t < 0) {
				// ===============
				// region 4
				if (d < 0) {
					t = 0;
					if (-d >= a)
						s = 1;
					else
						s = -d / a;
				}
				else {
					s = 0;
					if (e >= 0)
						t = 0;
					else if (-e >= c)
						t = 1;
					else
						t = -e / c;
				}
				// ===============
			}
			else {
				// ===============
				// region 3
				s = 0;
				if (e >= 0)
					t = 0;
				else if (-e >= c)
					t = 1;
				else
					t = -e / c;
				// ===============
			}
		}
		else if (t < 0) {
			// ===============
			// region 5
			t = 0;
			if (d >= 0)
				s = 0;
			else if (-d >= a)
				s = 1;
			else
				s = -d / a;
			// ===============
		}
		else {
			// ===============
			// region 0
			s /= det;
			t /= det;
			// ===============
		}
	}
	else {
		if (s < 0) {
			// ===============
			// region 2
			double temp0 = b + d;
			double temp1 = c + e;
			if (temp1 > temp0) {
				double num = temp1 - temp0;
				double denom = a - 2 * b + c;
				if (num >= denom)
					s = 1;
				else
					s = num / denom;
				t = 1 - s;
			}
			else {
				s = 0;
				if (temp1 <= 0)
					t = 1;
				else if (e >= 0)
					t = 0;
				else
					t = -e / c;
			}
			// ===============
		}
		else if (t < 0) {
			// ===============
			// region 6
			double temp0 = b + e;
			double temp1 = a + d;
			if (temp1 > temp0) {
				double num = temp1 - temp0;
				double denom = a - 2 * b + c;
				if (num >= denom)
					t = 1;
				else
					t = num / denom;
				s = 1 - t;
			}
			else {
				t = 0;
				if (temp1 <= 0)
					s = 1;
				else if (d >= 0)
					s = 0;
				else
					s = -d / a;
			}
			// ===============
		}
		else {
			// ===============
			// region 1
			double num = (c + e) - (b + d);
			if (num <= 0)
				s = 0;
			else {
				double denom = a - 2 * b + c;
				if (num >= denom)
					s = 1;
				else
					s = num / denom;
			}
			t = 1 - s;
			// ===============
		}
	}
	p = B + s * E0 + t * E1;
	dd = (p - x).norm();
}

void point_to_plane_rigid_matching(
	const Eigen::MatrixXd & X,
	const Eigen::MatrixXd & P,
	const Eigen::MatrixXd & N,
	Eigen::Matrix3d & R,
	Eigen::RowVector3d & t)
{
	R = Eigen::Matrix3d::Identity();
	t = Eigen::RowVector3d::Zero();

	int k = X.rows();
	// construct b
	Eigen::VectorXd RHS, u;
	Eigen::MatrixXd LHS;
	RHS.resize(k);
	LHS.resize(k, 6);
	u.resize(6);
	for (int ii = 0; ii < k; ii++) {
		// assemble RHS
		RHS(ii) = N(ii, 0)*(P(ii, 0) - X(ii, 0)) +
			N(ii, 1)*(P(ii, 1) - X(ii, 1)) +
			N(ii, 2)*(P(ii, 2) - X(ii, 2));
		// assemble A
		LHS.row(ii) << N(ii, 2)*X(ii, 1) - N(ii, 1)*X(ii, 2),
			N(ii, 0)*X(ii, 2) - N(ii, 2)*X(ii, 0),
			N(ii, 1)*X(ii, 0) - N(ii, 0)*X(ii, 1),
			N(ii, 0),
			N(ii, 1),
			N(ii, 2);
	}
	u = LHS.colPivHouseholderQr().solve(RHS);
	Eigen::Matrix3d M;
	M << 1, -u(2), u(1),
		u(2), 1, -u(0),
		-u(1), u(0), 1;
	closest_rotation(M, R);
	t(0) = u(3);
	t(1) = u(4);
	t(2) = u(5);
}

void point_mesh_distance(
	const Eigen::MatrixXd & X,
	const Eigen::MatrixXd & VY,
	const Eigen::MatrixXi & FY,
	Eigen::VectorXd & D,
	Eigen::MatrixXd & P,
	Eigen::MatrixXd & N)
{
	// compute face normals
	Eigen::MatrixXd FN;
	igl::per_face_normals(VY, FY, Eigen::Vector3d(1, 1, 1).normalized(), FN);

	D.resize(X.rows());
	P.resize(X.rows(), 3);
	N.resize(X.rows(), 3);

	Eigen::RowVector3d Xpt, a, b, c, p;
	double dist;
	double minDist;
	int minIdx;
	double pmin_x, pmin_y, pmin_z;
	for (int ii = 0; ii < X.rows(); ii++) { // loop over points X
		minDist = std::numeric_limits<double>::max();

		for (int jj = 0; jj < FY.rows(); jj++) { // loop over triangles
			Xpt = X.row(ii);
			a = VY.row(FY(jj, 0));
			b = VY.row(FY(jj, 1));
			c = VY.row(FY(jj, 2));
			point_triangle_distance(Xpt, a, b, c, dist, p);

			if (dist < minDist) {
				minDist = dist;
				minIdx = jj;
				pmin_x = p(0);
				pmin_y = p(1);
				pmin_z = p(2);
			}
		}
		D(ii) = minDist;
		P.row(ii) << pmin_x, pmin_y, pmin_z;
		N.row(ii) = FN.row(minIdx);
	}
}

double icp_single_iteration(
	const Eigen::MatrixXd & VX,
	const Eigen::MatrixXi & FX,
	const Eigen::MatrixXd & VY,
	const Eigen::MatrixXi & FY,
	const int num_samples,
	Eigen::Matrix3d & R,
	Eigen::RowVector3d & t)
{
	R = Eigen::Matrix3d::Identity();
	t = Eigen::RowVector3d::Zero();

	Eigen::MatrixXd X, P, N;
	Eigen::VectorXd D;

	random_points_on_mesh(num_samples, VX, FX, X);

	point_mesh_distance(X, VY, FY, D, P, N);

	// point-to-plane
	point_to_plane_rigid_matching(X, P, N, R, t);

	return D.sum()/D.rows();
}


#include <armadillo>

int main() {

    // To generate the (symmetric positive-definite) matrix:

    // const size_t dim = 5;
    // arma::mat S(dim, dim, arma::fill::randu);
    // S = 0.5 * (S + S.t());
    // S += dim * arma::eye(arma::size(S));
    // S.save("S.dat", arma::arma_ascii);

    // To load the pre-generated matrix:

    arma::mat S;
    S.load("S.dat");

    S.print("S");

    // arma::vec Lam_S_vec(dim);
    // arma::mat Lam_S_mat(dim, dim);
    // arma::mat L_S(dim, dim);
    arma::vec Lam_S_vec;
    arma::mat Lam_S_mat;
    arma::mat L_S;

    arma::eig_sym(Lam_S_vec, L_S, S);
    // What's wrong with this?
    // Lam_S_mat = Lam_S_vec * arma::eye<arma::mat>(Lam_S_vec.n_elem, Lam_S_vec.n_elem);
    Lam_S_mat = arma::diagmat(Lam_S_vec);
    arma::mat Lam_sqrt_inv = arma::sqrt(arma::inv(Lam_S_mat));
    arma::mat symm_orthog = L_S * Lam_sqrt_inv * L_S.t();

    L_S.print("L_S");
    Lam_S_vec.print("Lam_S_vec");
    Lam_S_mat.print("Lam_S_mat");
    Lam_sqrt_inv.print("Lam_sqrt_inv");
    symm_orthog.print("symm_orthog");

    // Demonstration that matrix operations require diagonalization,
    // operation on the eigenvalues, then back-transformation to the
    // original basis.
    arma::mat S_inv = arma::inv(S);
    arma::mat Lam_inv = arma::inv(Lam_S_mat);
    arma::mat S_inv_2 = L_S * Lam_inv * L_S.t();
    arma::mat S_pinv = arma::pinv(S);
    arma::mat S_inv_sympd = arma::inv_sympd(S);

    S_inv.print("S_inv");
    S_inv_2.print("S_inv_2");
    S_pinv.print("S_pinv");
    S_inv_sympd.print("S_inv_sympd");

    return 0;

}


#include <iostream>
#include <sys/time.h>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#define N 100000

using namespace boost::random;
using namespace boost::multiprecision;

cpp_bin_float_50 random_gen(){
	boost::random::random_device seeder;
	boost::random::mt19937 gen(seeder());

	cpp_bin_float_50 rand_num = generate_canonical<cpp_bin_float_50, std::numeric_limits<cpp_bin_float_50>::digits>(gen);
	return rand_num;
}

void seq_monte_carlo(){
	int count;
	int i;
	//int n = vm["n"].as<int>();
	int n = N;

	cpp_bin_float_50 pi;
	cpp_bin_float_50 x,y;
	cpp_bin_float_50 PI = boost::math::constants::pi<cpp_bin_float_50>();

	count = 0;
	for(i = 0; i < n; i++){
		x = random_gen();
		y = random_gen();
		for(int j =0;j<1000;j++){}
		if(x*x + y*y <= 1.0){
			count++;
		}
	}
	pi = 4.0 * (double) count / (double) n;

	cpp_bin_float_50 error = (pi - PI) / PI;

	std::cout << std::setprecision(20);
	std::cout << pi << std::endl;
	std::cout << PI << std::endl;
	std::cout << error << std::endl;

}

void par_monte_carlo(){
	int count_private[N] = {0};
	int count = 0;
	int i;
	//int n = vm["n"].as<int>();
	int n = N;
	int chunk_size = 1000;

	cpp_bin_float_50 pi;
	cpp_bin_float_50 x,y;
	cpp_bin_float_50 PI = boost::math::constants::pi<cpp_bin_float_50>();

	#pragma omp parallel \
	  shared(count_private, chunk_size) private(i,x,y)
	{
                #pragma omp for schedule(dynamic,chunk_size) nowait
		for(i = 0; i < n; i++){
			x = random_gen();
			y = random_gen();
			if(x*x + y*y <= 1.0){
				count_private[i]++;
			}
		}
	}
	#pragma omp parallel for reduction(+:count)
	for(i = 0; i < n; i++){
		count += count_private[i];
	}
	pi = 4.0 * (double) count / (double) n;

	cpp_bin_float_50 error = (pi - PI) / PI;

	std::cout << std::setprecision(20);
	std::cout << pi << std::endl;
	std::cout << PI << std::endl;
	std::cout << error << std::endl;
}

long time_cal(void (*func)()){
	//clock_t begin = clock();
//	std::time_t start, end;
//	long delta = 0;
//	start = std::time(NULL);

	struct timespec start,end;

	clock_gettime(CLOCK_REALTIME, &start);
	(*func)();
	clock_gettime(CLOCK_REALTIME, &end);
	long delta = end.tv_sec - start.tv_sec;

	std::cout << start.tv_sec << " " << start.tv_nsec << std::endl;
	std::cout << end.tv_sec << " " << end.tv_nsec << std::endl;
	return delta;
}

int main(int argc, char *argv[]){

//	options_description desc{"Options"};
//	desc.add_options()
//		("help,h", "Help screen")
//    		("n", value<int>()->default_value(10000), "Number of random counts")
//	;
//	variables_map vm;
//	store(parse_command_line(argc, argv, desc), vm);
//	notify(vm);
//
//	std::random_device seeder;
//	boost::random::mt19937 gen(seeder());

	auto seq_time = time_cal(seq_monte_carlo);
	auto par_time = time_cal(par_monte_carlo);
	std::cout << seq_time << std::endl;
	std::cout << par_time << std::endl;
}


// negative_binomial_example1.cpp

// Copyright Paul A. Bristow 2007, 2010.

// 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)

// Example 1 of using negative_binomial distribution.

//[negative_binomial_eg1_1

/*`
Based on [@http://en.wikipedia.org/wiki/Negative_binomial_distribution
a problem by Dr. Diane Evans,
Professor of Mathematics at Rose-Hulman Institute of Technology].

Pat is required to sell candy bars to raise money for the 6th grade field trip.
There are thirty houses in the neighborhood,
and Pat is not supposed to return home until five candy bars have been sold.
So the child goes door to door, selling candy bars.
At each house, there is a 0.4 probability (40%) of selling one candy bar
and a 0.6 probability (60%) of selling nothing.

What is the probability mass (density) function (pdf) for selling the last (fifth)
candy bar at the nth house?

The Negative Binomial(r, p) distribution describes the probability of k failures
and r successes in k+r Bernoulli(p) trials with success on the last trial.
(A [@http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli trial]
is one with only two possible outcomes, success of failure,
and p is the probability of success).
See also [@ http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli distribution]
and [@http://www.math.uah.edu/stat/bernoulli/Introduction.xhtml Bernoulli applications].

In this example, we will deliberately produce a variety of calculations
and outputs to demonstrate the ways that the negative binomial distribution
can be implemented with this library: it is also deliberately over-commented.

First we need to #define macros to control the error and discrete handling policies.
For this simple example, we want to avoid throwing
an exception (the default policy) and just return infinity.
We want to treat the distribution as if it was continuous,
so we choose a discrete_quantile policy of real,
rather than the default policy integer_round_outwards.
*/
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real
/*`
After that we need some includes to provide easy access to the negative binomial distribution,
[caution It is vital to #include distributions etc *after* the above #defines]
and we need some std library iostream, of course.
*/
#include <boost/math/distributions/negative_binomial.hpp>
  // for negative_binomial_distribution
  using boost::math::negative_binomial; // typedef provides default type is double.
  using  ::boost::math::pdf; // Probability mass function.
  using  ::boost::math::cdf; // Cumulative density function.
  using  ::boost::math::quantile;

#include <iostream>
  using std::cout; using std::endl;
  using std::noshowpoint; using std::fixed; using std::right; using std::left;
#include <iomanip>
  using std::setprecision; using std::setw;

#include <limits>
  using std::numeric_limits;
//] [negative_binomial_eg1_1]

int main()
{
  cout <<"Selling candy bars - using the negative binomial distribution."
    << "\nby Dr. Diane Evans,"
    "\nProfessor of Mathematics at Rose-Hulman Institute of Technology,"
    << "\nsee http://en.wikipedia.org/wiki/Negative_binomial_distribution\n"
    << endl;
  cout << endl;
  cout.precision(5);
  // None of the values calculated have a useful accuracy as great this, but
  // INF shows wrongly with < 5 !
  // https://connect.microsoft.com/VisualStudio/feedback/ViewFeedback.aspx?FeedbackID=240227
//[negative_binomial_eg1_2
/*`
It is always sensible to use try and catch blocks because defaults policies are to
throw an exception if anything goes wrong.

A simple catch block (see below) will ensure that you get a
helpful error message instead of an abrupt program abort.
*/
  try
  {
/*`
Selling five candy bars means getting five successes, so successes r = 5.
The total number of trials (n, in this case, houses visited) this takes is therefore
  = sucesses + failures or k + r = k + 5.
*/
    double sales_quota = 5; // Pat's sales quota - successes (r).
/*`
At each house, there is a 0.4 probability (40%) of selling one candy bar
and a 0.6 probability (60%) of selling nothing.
*/
    double success_fraction = 0.4; // success_fraction (p) - so failure_fraction is 0.6.
/*`
The Negative Binomial(r, p) distribution describes the probability of k failures
and r successes in k+r Bernoulli(p) trials with success on the last trial.
(A [@http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli trial]
is one with only two possible outcomes, success of failure,
and p is the probability of success).

We therefore start by constructing a negative binomial distribution
with parameters sales_quota (required successes) and probability of success.
*/
    negative_binomial nb(sales_quota, success_fraction); // type double by default.
/*`
To confirm, display the success_fraction & successes parameters of the distribution.
*/
    cout << "Pat has a sales per house success rate of " << success_fraction
      << ".\nTherefore he would, on average, sell " << nb.success_fraction() * 100
      << " bars after trying 100 houses." << endl;

    int all_houses = 30; // The number of houses on the estate.

    cout << "With a success rate of " << nb.success_fraction()
      << ", he might expect, on average,\n"
        "to need to visit about " << success_fraction * all_houses
        << " houses in order to sell all " << nb.successes() << " bars. " << endl;
/*`
[pre
Pat has a sales per house success rate of 0.4.
Therefore he would, on average, sell 40 bars after trying 100 houses.
With a success rate of 0.4, he might expect, on average,
to need to visit about 12 houses in order to sell all 5 bars.
]

The random variable of interest is the number of houses
that must be visited to sell five candy bars,
so we substitute k = n - 5 into a negative_binomial(5, 0.4)
and obtain the __pdf of the distribution of houses visited.
Obviously, the best possible case is that Pat makes sales on all the first five houses.

We calculate this using the pdf function:
*/
    cout << "Probability that Pat finishes on the " << sales_quota << "th house is "
      << pdf(nb, 5 - sales_quota) << endl; // == pdf(nb, 0)
/*`
Of course, he could not finish on fewer than 5 houses because he must sell 5 candy bars.
So the 5th house is the first that he could possibly finish on.

To finish on or before the 8th house, Pat must finish at the 5th, 6th, 7th or 8th house.
The probability that he will finish on *exactly* ( == ) on any house
is the Probability Density Function (pdf).
*/
    cout << "Probability that Pat finishes on the 6th house is "
      << pdf(nb, 6 - sales_quota) << endl;
    cout << "Probability that Pat finishes on the 7th house is "
      << pdf(nb, 7 - sales_quota) << endl;
    cout << "Probability that Pat finishes on the 8th house is "
      << pdf(nb, 8 - sales_quota) << endl;
/*`
[pre
Probability that Pat finishes on the 6th house is 0.03072
Probability that Pat finishes on the 7th house is 0.055296
Probability that Pat finishes on the 8th house is 0.077414
]

The sum of the probabilities for these houses is the Cumulative Distribution Function (cdf).
We can calculate it by adding the individual probabilities.
*/
    cout << "Probability that Pat finishes on or before the 8th house is sum "
      "\n" << "pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = "
      // Sum each of the mass/density probabilities for houses sales_quota = 5, 6, 7, & 8.
      << pdf(nb, 5 - sales_quota) // 0 failures.
        + pdf(nb, 6 - sales_quota) // 1 failure.
        + pdf(nb, 7 - sales_quota) // 2 failures.
        + pdf(nb, 8 - sales_quota) // 3 failures.
      << endl;
/*`[pre
pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = 0.17367
]

Or, usually better, by using the negative binomial *cumulative* distribution function.
*/
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 8 << "th house is "
      << cdf(nb, 8 - sales_quota) << endl;
/*`[pre
Probability of selling his quota of 5 bars on or before the 8th house is 0.17367
]*/
    cout << "\nProbability that Pat finishes exactly on the 10th house is "
      << pdf(nb, 10 - sales_quota) << endl;
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 10 << "th house is "
      << cdf(nb, 10 - sales_quota) << endl;
/*`
[pre
Probability that Pat finishes exactly on the 10th house is 0.10033
Probability of selling his quota of 5 bars on or before the 10th house is 0.3669
]*/
    cout << "Probability that Pat finishes exactly on the 11th house is "
      << pdf(nb, 11 - sales_quota) << endl;
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 11 << "th house is "
      << cdf(nb, 11 - sales_quota) << endl;
/*`[pre
Probability that Pat finishes on the 11th house is 0.10033
Probability of selling his quota of 5 candy bars
on or before the 11th house is 0.46723
]*/
    cout << "Probability that Pat finishes exactly on the 12th house is "
      << pdf(nb, 12 - sales_quota) << endl;

    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 12 << "th house is "
      << cdf(nb, 12 - sales_quota) << endl;
/*`[pre
Probability that Pat finishes on the 12th house is 0.094596
Probability of selling his quota of 5 candy bars
on or before the 12th house is 0.56182
]
Finally consider the risk of Pat not selling his quota of 5 bars
even after visiting all the houses.
Calculate the probability that he /will/ sell on
or before the last house:
Calculate the probability that he would sell all his quota on the very last house.
*/
    cout << "Probability that Pat finishes on the " << all_houses
      << " house is " << pdf(nb, all_houses - sales_quota) << endl;
/*`
Probability of selling his quota of 5 bars on the 30th house is
[pre
Probability that Pat finishes on the 30 house is 0.00069145
]
when he'd be very unlucky indeed!

What is the probability that Pat exhausts all 30 houses in the neighborhood,
and *still* doesn't sell the required 5 candy bars?
*/
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << all_houses << "th house is "
      << cdf(nb, all_houses - sales_quota) << endl;
/*`
[pre
Probability of selling his quota of 5 bars
on or before the 30th house is 0.99849
]

So the risk of failing even after visiting all the houses is 1 - this probability,
  ``1 - cdf(nb, all_houses - sales_quota``
But using this expression may cause serious inaccuracy,
so it would be much better to use the complement of the cdf:
So the risk of failing even at, or after, the 31th (non-existent) houses is 1 - this probability,
  ``1 - cdf(nb, all_houses - sales_quota)``
But using this expression may cause serious inaccuracy.
So it would be much better to use the __complement of the cdf (see __why_complements).
*/
    cout << "\nProbability of failing to sell his quota of " << sales_quota
      << " bars\neven after visiting all " << all_houses << " houses is "
      << cdf(complement(nb, all_houses - sales_quota)) << endl;
/*`
[pre
Probability of failing to sell his quota of 5 bars
even after visiting all 30 houses is 0.0015101
]
We can also use the quantile (percentile), the inverse of the cdf, to
predict which house Pat will finish on.  So for the 8th house:
*/
 double p = cdf(nb, (8 - sales_quota));
 cout << "Probability of meeting sales quota on or before 8th house is "<< p << endl;
/*`
[pre
Probability of meeting sales quota on or before 8th house is 0.174
]
*/
    cout << "If the confidence of meeting sales quota is " << p
        << ", then the finishing house is " << quantile(nb, p) + sales_quota << endl;

    cout<< " quantile(nb, p) = " << quantile(nb, p) << endl;
/*`
[pre
If the confidence of meeting sales quota is 0.17367, then the finishing house is 8
]
Demanding absolute certainty that all 5 will be sold,
implies an infinite number of trials.
(Of course, there are only 30 houses on the estate,
so he can't ever be *certain* of selling his quota).
*/
    cout << "If the confidence of meeting sales quota is " << 1.
        << ", then the finishing house is " << quantile(nb, 1) + sales_quota << endl;
    //  1.#INF == infinity.
/*`[pre
If the confidence of meeting sales quota is 1, then the finishing house is 1.#INF
]
And similarly for a few other probabilities:
*/
    cout << "If the confidence of meeting sales quota is " << 0.
        << ", then the finishing house is " << quantile(nb, 0.) + sales_quota << endl;

    cout << "If the confidence of meeting sales quota is " << 0.5
        << ", then the finishing house is " << quantile(nb, 0.5) + sales_quota << endl;

    cout << "If the confidence of meeting sales quota is " << 1 - 0.00151 // 30 th
        << ", then the finishing house is " << quantile(nb, 1 - 0.00151) + sales_quota << endl;
/*`
[pre
If the confidence of meeting sales quota is 0, then the finishing house is 5
If the confidence of meeting sales quota is 0.5, then the finishing house is 11.337
If the confidence of meeting sales quota is 0.99849, then the finishing house is 30
]

Notice that because we chose a discrete quantile policy of real,
the result can be an 'unreal' fractional house.

If the opposite is true, we don't want to assume any confidence, then this is tantamount
to assuming that all the first sales_quota trials will be successful sales.
*/
    cout << "If confidence of meeting quota is zero\n(we assume all houses are successful sales)"
      ", then finishing house is " << sales_quota << endl;
/*`
[pre
If confidence of meeting quota is zero (we assume all houses are successful sales), then finishing house is 5
If confidence of meeting quota is 0, then finishing house is 5
]
We can list quantiles for a few probabilities:
*/

    double ps[] = {0., 0.001, 0.01, 0.05, 0.1, 0.5, 0.9, 0.95, 0.99, 0.999, 1.};
    // Confidence as fraction = 1-alpha, as percent =  100 * (1-alpha[i]) %
    cout.precision(3);
    for (unsigned i = 0; i < sizeof(ps)/sizeof(ps[0]); i++)
    {
      cout << "If confidence of meeting quota is " << ps[i]
        << ", then finishing house is " << quantile(nb, ps[i]) + sales_quota
        << endl;
   }

/*`
[pre
If confidence of meeting quota is 0, then finishing house is 5
If confidence of meeting quota is 0.001, then finishing house is 5
If confidence of meeting quota is 0.01, then finishing house is 5
If confidence of meeting quota is 0.05, then finishing house is 6.2
If confidence of meeting quota is 0.1, then finishing house is 7.06
If confidence of meeting quota is 0.5, then finishing house is 11.3
If confidence of meeting quota is 0.9, then finishing house is 17.8
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.99, then finishing house is 24.8
If confidence of meeting quota is 0.999, then finishing house is 31.1
If confidence of meeting quota is 1, then finishing house is 1.#INF
]

We could have applied a ceil function to obtain a 'worst case' integer value for house.
``ceil(quantile(nb, ps[i]))``

Or, if we had used the default discrete quantile policy, integer_outside, by omitting
``#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real``
we would have achieved the same effect.

The real result gives some suggestion which house is most likely.
For example, compare the real and integer_outside for 95% confidence.

[pre
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.95, then finishing house is 21
]
The real value 20.1 is much closer to 20 than 21, so integer_outside is pessimistic.
We could also use integer_round_nearest policy to suggest that 20 is more likely.

Finally, we can tabulate the probability for the last sale being exactly on each house.
*/
   cout << "\nHouse for " << sales_quota << "th (last) sale.  Probability (%)" << endl;
   cout.precision(5);
   for (int i = (int)sales_quota; i < all_houses+1; i++)
   {
     cout << left << setw(3) << i << "                             " << setw(8) << cdf(nb, i - sales_quota)  << endl;
   }
   cout << endl;
/*`
[pre
House for 5 th (last) sale.  Probability (%)
5                               0.01024
6                               0.04096
7                               0.096256
8                               0.17367
9                               0.26657
10                              0.3669
11                              0.46723
12                              0.56182
13                              0.64696
14                              0.72074
15                              0.78272
16                              0.83343
17                              0.874
18                              0.90583
19                              0.93039
20                              0.94905
21                              0.96304
22                              0.97342
23                              0.98103
24                              0.98655
25                              0.99053
26                              0.99337
27                              0.99539
28                              0.99681
29                              0.9978
30                              0.99849
]

As noted above, using a catch block is always a good idea, even if you do not expect to use it.
*/
  }
  catch(const std::exception& e)
  { // Since we have set an overflow policy of ignore_error,
    // an overflow exception should never be thrown.
     std::cout << "\nMessage from thrown exception was:\n " << e.what() << std::endl;
/*`
For example, without a ignore domain error policy, if we asked for ``pdf(nb, -1)`` for example, we would get:
[pre
Message from thrown exception was:
 Error in function boost::math::pdf(const negative_binomial_distribution<double>&, double):
 Number of failures argument is -1, but must be >= 0 !
]
*/
//] [/ negative_binomial_eg1_2]
  }
   return 0;
}  // int main()

/*

Output is:

Selling candy bars - using the negative binomial distribution.
by Dr. Diane Evans,
Professor of Mathematics at Rose-Hulman Institute of Technology,
see http://en.wikipedia.org/wiki/Negative_binomial_distribution
Pat has a sales per house success rate of 0.4.
Therefore he would, on average, sell 40 bars after trying 100 houses.
With a success rate of 0.4, he might expect, on average,
to need to visit about 12 houses in order to sell all 5 bars.
Probability that Pat finishes on the 5th house is 0.01024
Probability that Pat finishes on the 6th house is 0.03072
Probability that Pat finishes on the 7th house is 0.055296
Probability that Pat finishes on the 8th house is 0.077414
Probability that Pat finishes on or before the 8th house is sum
pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = 0.17367
Probability of selling his quota of 5 bars
on or before the 8th house is 0.17367
Probability that Pat finishes exactly on the 10th house is 0.10033
Probability of selling his quota of 5 bars
on or before the 10th house is 0.3669
Probability that Pat finishes exactly on the 11th house is 0.10033
Probability of selling his quota of 5 bars
on or before the 11th house is 0.46723
Probability that Pat finishes exactly on the 12th house is 0.094596
Probability of selling his quota of 5 bars
on or before the 12th house is 0.56182
Probability that Pat finishes on the 30 house is 0.00069145
Probability of selling his quota of 5 bars
on or before the 30th house is 0.99849
Probability of failing to sell his quota of 5 bars
even after visiting all 30 houses is 0.0015101
Probability of meeting sales quota on or before 8th house is 0.17367
If the confidence of meeting sales quota is 0.17367, then the finishing house is 8
 quantile(nb, p) = 3
If the confidence of meeting sales quota is 1, then the finishing house is 1.#INF
If the confidence of meeting sales quota is 0, then the finishing house is 5
If the confidence of meeting sales quota is 0.5, then the finishing house is 11.337
If the confidence of meeting sales quota is 0.99849, then the finishing house is 30
If confidence of meeting quota is zero
(we assume all houses are successful sales), then finishing house is 5
If confidence of meeting quota is 0, then finishing house is 5
If confidence of meeting quota is 0.001, then finishing house is 5
If confidence of meeting quota is 0.01, then finishing house is 5
If confidence of meeting quota is 0.05, then finishing house is 6.2
If confidence of meeting quota is 0.1, then finishing house is 7.06
If confidence of meeting quota is 0.5, then finishing house is 11.3
If confidence of meeting quota is 0.9, then finishing house is 17.8
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.99, then finishing house is 24.8
If confidence of meeting quota is 0.999, then finishing house is 31.1
If confidence of meeting quota is 1, then finishing house is 1.#J
House for 5th (last) sale.  Probability (%)
5                               0.01024
6                               0.04096
7                               0.096256
8                               0.17367
9                               0.26657
10                              0.3669
11                              0.46723
12                              0.56182
13                              0.64696
14                              0.72074
15                              0.78272
16                              0.83343
17                              0.874
18                              0.90583
19                              0.93039
20                              0.94905
21                              0.96304
22                              0.97342
23                              0.98103
24                              0.98655
25                              0.99053
26                              0.99337
27                              0.99539
28                              0.99681
29                              0.9978
30                              0.99849

*/


#include <iostream>
#include <cmath>
#include <vector>
#include <algorithm>
#include <ios>
#include <limits>
#include <utility>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>

void clearInputBuffer();
int mainMenu();
double getNumber();
void getNumbers(std::vector <double>& nums);
char getOperation();
int getStatisticalAverage();
int getEquationType();
std::pair <double, double> calculateRootsOfQuadratic(double a, double b, double discriminant);

int main() {
	std::cout << "Welcome to my calculator!\n";
	while(true) {
		int function{ mainMenu() };
		if(function == 1) {
			double firstNumber{ getNumber() };
			char operation{ getOperation() };
			double secondNumber{ 0 };

			if(operation != '!') {
				secondNumber = getNumber();
			}
			switch(operation) {
			case '+':
				std::cout << firstNumber << " + " << secondNumber << " = " << firstNumber + secondNumber << "\n\n";
				break;
			case '-':
				std::cout << firstNumber << " - " << secondNumber << " = " << firstNumber - secondNumber << "\n\n";
				break;
			case '*':
				std::cout << firstNumber << " * " << secondNumber << " = " << firstNumber * secondNumber << "\n\n";
				break;
			case '/':
				if(secondNumber) {
					std::cout << firstNumber << " / " << secondNumber << " = " << firstNumber / secondNumber << "\n\n";
				} else {
					std::cout << "Illegal operation! Cannont divide by 0.\n\n";
				}
				break;
			case '^':
				std::cout << firstNumber << " ^ " << secondNumber << " = " << pow(firstNumber, secondNumber) << "\n\n";
				break;
			case '!':
				if(std::floor(firstNumber) == firstNumber && firstNumber >= 0) {
					std::cout << firstNumber << "! = " << std::fixed << static_cast<boost::multiprecision::cpp_int>(boost::math::factorial<boost::multiprecision::cpp_dec_float_50>(static_cast<unsigned int>(firstNumber))) << "\n\n";
				} else {
					std::cout << "The number must be an integer!\n\n";
				}
				break;
			case '%':
				std::cout << firstNumber << "% of " << secondNumber << " = " << firstNumber / 100 * secondNumber << "\n\n";
				break;
			}
		} else if(function == 2) {
			int statisticalAverage{ getStatisticalAverage() };
			if(statisticalAverage == 1) {
				std::vector <double> nums;
				getNumbers(nums);
				double sum{ 0 };

				for(double x : nums) {
					sum += x;
				}
				std::cout << "The mean is " << sum / nums.size() << '\n';
			} else if(statisticalAverage == 2) {
				std::vector <double> nums;
				getNumbers(nums);
				sort(nums.begin(), nums.end());
				if(nums.size() % 2) {
					std::cout << "The median is: " << nums[nums.size() / 2] << '\n';
				} else {
					std::cout << "The median is: " << (nums[nums.size() / 2 - 1] + nums[nums.size() / 2]) / 2 << '\n';
				}
			} else if(statisticalAverage == 3) {
				std::vector <double> nums;
				getNumbers(nums);
				sort(nums.begin(), nums.end());
				double mode{ 0 };
				double element{ nums[0] };
				int noOfOccurrencesOfMode{ 0 };
				int noOfElement{ 0 };

				for(int i{ 0 }; i < nums.size(); i++) {
					if(nums[i] == element) {
						noOfElement++;
					} else {
						if(noOfElement > noOfOccurrencesOfMode) {
							mode = element;
							noOfOccurrencesOfMode = noOfElement;
							noOfElement = 1;
						}
						element = nums[i];
					}
				}
				if(noOfElement > noOfOccurrencesOfMode) {
					mode = element;
				}
				std::cout << "The mode is: " << mode << '\n';
			} else {
				return 0;
			}
			clearInputBuffer();
		} else {
			int equationType{ getEquationType() };
			if(equationType == 1) {
				std::cout << "Make sure the equation is in the form ax^2 + bx + c = 0\n";
				std::cout << "Enter a, b, and c:\n";
				double a, b, c;

				while(!(std::cin >> a >> b >> c)) {
					std::cout << "Please enter numbers for a, b, and c.\n";
					clearInputBuffer();
				}
				double discriminant{ b * b - 4 * a * c };
				std::pair <double, double> x{ calculateRootsOfQuadratic(a, b, discriminant).first, calculateRootsOfQuadratic(a, b, discriminant).second };
				if(discriminant > 0) {
					std::cout << "x1 = " << x.first << " x2 = " << x.second << '\n';
				} else if(!discriminant) {
					std::cout << "x = " << x.first << '\n';
				} else {
					std::cout << "x1 = " << x.first << " + " << x.second << "i\n";
					std::cout << "x2 = " << x.first << " - " << x.second << "i\n";
				}
			}
		}
	}
}

void clearInputBuffer() {
	std::cin.clear();
	std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}

int mainMenu() {
	int input;
	std::cout << "\n1. Arithmetic mode\n2. Statistical average mode\n3. Equation solver mode\n4. Quit\n\n";
	std::cout << "Enter your choice: ";

	while(!(std::cin >> input) || (input != 1 && input != 2 && input != 3 && input != 4)) {
		std::cout << "Invalid Input! Please try again: ";
		clearInputBuffer();
	}
	return input;
}

double getNumber() {
	double input;
	std::cout << "Enter a number: ";

	while(!(std::cin >> input)) {
		std::cout << "Invalid Input! Please try again: ";
		clearInputBuffer();
	}
	return input;
}

void getNumbers(std::vector <double>& nums) {
	char input;
	double num;
	std::cout << "Enter the numbers (q to stop entering numbers):\n";

	while(true) {
		std::cin >> input;
		if(input == 'q') {
			break;
		} else {
			std::cin.putback(input);
			if(!(std::cin >> num)) {
				std::cout << "Invalid Input! Please try again:\n";
				clearInputBuffer();
			} else {
				nums.push_back(num);
			}
		}
	}
}

char getOperation() {
	char input;
	std::cout << "Enter an operator (+, -, *, /, ^, !, or %): ";

	while(std::cin >> input && (input != '+' && input != '-' && input != '*' && input != '/' && input != '^' && input != '!' && input != '%')) {
		std::cout << "Invalid Input! Please try again: ";
		clearInputBuffer();
	}
	return input;
}

int getStatisticalAverage() {
	int input;
	std::cout << "\n1. Mean\n2. Median\n3. Mode\n\n";
	std::cout << "Enter your choice: ";

	while(!(std::cin >> input) || (input != 1 && input != 2 && input != 3)) {
		std::cout << "Invalid Input! Please try again: ";
		clearInputBuffer();
	}
	return input;
}

int getEquationType() {
	int input;
	std::cout << "\n1. Quadratic\n\n";
	std::cout << "Enter your choice: ";

	while(!(std::cin >> input) || (input != 1)) {
		std::cout << "Invalid Input! Please try again: ";
		clearInputBuffer();
	}
	return input;
}

std::pair <double, double> calculateRootsOfQuadratic(double a, double b, double discriminant) {
	if(discriminant > 0) {
		return std::make_pair((-b + sqrt(discriminant)) / (2 * a), (-b - sqrt(discriminant)) / (2 * a));
	} else if(!discriminant) {
		return std::make_pair(-b / (2 * a), 0);
	} else {
		return std::make_pair(-b / (2 * a), sqrt(-discriminant) / (2 * a));
	}
}


#include <boost/gil/detail/math.hpp>
#include <cmath>

namespace boost{ namespace gil{

/// \defgroup ImageProcessingMath
/// \brief Math operations for IP algorithms
///
/// This is mostly handful of mathemtical
/// operations that are required by other
/// image processing algorithms

/// \brief Normalized cardinal sine
/// \ingroup ImageProcessingMath
///
/// normalized_sinc(x) = sin(pi * x) / (pi * x)
double normalized_sinc(double x)
{
    return std::sin(x * boost::gil::pi) / (x * boost::gil::pi);
}

/// \brief Lanczos response at point x
/// \ingroup ImageProcessingMath
///
/// Lanczos response is defined as:
/// x == 0: 1
/// -a < x && x < a: 0
/// otherwise: normalized_sinc(x) / normalized_sinc(x / a)
double lanczos(double x, std::ptrdiff_t a)
{
    if (x == 0)
    {
        return 1;
    }
    if (-a < x && x < a)
    {
        return normalized_sinc(x)
               / normalized_sinc(x / static_cast<double>(a));
    }

    return 0;
}
}}


// Find best-fit plane to an array of points
// adapted from https://gist.github.com/ialhashim/0a2554076a6cf32831ca

#include <Eigen/Core>
#include <Eigen/Dense>

template<class Vector3>
std::pair<Vector3, Vector3> best_plane_from_points(const std::vector<Vector3> & c)
{
	// copy coordinates to  matrix in Eigen format
	size_t num_atoms = c.size();
	Eigen::Matrix< typename Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > coord(3, num_atoms);
	for (size_t i = 0; i < num_atoms; ++i) coord.col(i) = c[i];

	// calculate centroid
	Vector3 centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());

	// subtract centroid
	coord.row(0).array() -= centroid(0);
    coord.row(1).array() -= centroid(1);
    coord.row(2).array() -= centroid(2);

	// we only need the left-singular matrix here
	//  http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
	auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
	auto plane_normal = svd.matrixU().rightCols(1);
	return std::make_pair(centroid, plane_normal);
}

template<class Vector3>
std::pair < Vector3, Vector3 > best_line_from_points(const std::vector<Vector3> & c)
{
	// copy coordinates to  matrix in Eigen format
	size_t num_atoms = c.size();
	Eigen::Matrix< typename Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3);
	for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = c[i];

	Vector3 origin = centers.colwise().mean();
	Eigen::MatrixXd centered = centers.rowwise() - origin.transpose();
	Eigen::MatrixXd cov = centered.adjoint() * centered;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);
	Vector3 axis = eig.eigenvectors().col(2).normalized();

	return std::make_pair(origin, axis);
}


#include <iostream>
#include <vector>
#include <string>
#include <math.h>
#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
namespace mp = boost::multiprecision;

int modInverse(mp::cpp_int a, mp::cpp_int m)
{
    mp::cpp_int m0 = m;
    mp::cpp_int y = 0, x = 1;

    if (m == 1)
        return 0;

    while (a > 1)
    {
        // q is quotient
        mp::cpp_int q = a / m;
        mp::cpp_int t = m;

        // m is remainder now, process same as
        // Euclid's algo
        m = a % m, a = t;
        t = y;

        // Update y and x
        y = x - q * y;
        x = t;
    }

    // Make x positive
    if (x < 0)
        x += m0;

    return int(x);
}

int main()
{
    int plainMessage = 35; //plain text
    int publicBase = 5;    //public base known to all
    int N = 89;            // modulo
    int prvKey_BOB = 8;    // number Bob chooses
    int prvKey_ALICE = 13; // number Alice chooses

    //Bob to send Alice the encrypted message

    //ALice can create a public key to share with Bob
    //Alice gives Bob her pubKey_ALICE, public base and N
    mp::cpp_int pubKey_ALICE = mp::pow(mp::cpp_int(publicBase), prvKey_ALICE) % N;

    //Bob can create a public key with what Alice provided
    //it's also referred as the ephemeral key
    mp::cpp_int pubKey_BOB = mp::pow(mp::cpp_int(publicBase), prvKey_BOB) % N;

    //Bob creates encryption key from what Alice sent him, the pubKey_ALICE
    //it's also referred as the masking key/session key
    mp::cpp_int encryptionKey = mp::pow(mp::cpp_int(pubKey_ALICE), prvKey_BOB) % N;

    //Bob encrypts the msg and sends it to Alice along with the ephemeral key (pubKey_BOB)
    //This is where El Gamal differs from Diffi-Helman:
    // Encrpted message and the ephemeral key are shared with ALice at the same time.
    mp::cpp_int encryptedMsg = (encryptionKey * plainMessage) % N;

    //Alice now has the encrypted message and Bob's public key
    //ALice first needs to figure out the encryption key Bob used
    mp::cpp_int aliceFindsOutKey = mp::pow(mp::cpp_int(pubKey_BOB), prvKey_ALICE) % N;

    //Alice then needs to calculate the inverse the key
    mp::cpp_int aliceInversesKey = modInverse(aliceFindsOutKey, mp::cpp_int(N));

    //Alice can decrypt the message
    mp::cpp_int aliceDecryptsMsg = (encryptedMsg * aliceInversesKey) % N;

    cout << "pubKey_BOB" << endl
         << pubKey_BOB << endl
         << "pubKey_ALICE" << endl
         << pubKey_ALICE << endl
         << "encryptionKey" << endl
         << encryptionKey << endl
         << "encryptedMsg" << endl
         << encryptedMsg << endl
         << "aliceFindsOutKey" << endl
         << aliceFindsOutKey << endl
         << "aliceInversesKey" << endl
         << aliceInversesKey << endl
         << "aliceDecryptsMsg" << endl
         << aliceDecryptsMsg << endl
         << "encryption is decrypted: " << endl
         << (plainMessage ==
             aliceDecryptsMsg)
         << endl;

    //alternative decryption without inversing the masking key from Bob
    mp::cpp_int alternativeDecryptionOutput = (mp::pow(mp::cpp_int(pubKey_BOB), int(N - 1 - mp::cpp_int(prvKey_ALICE)))) * encryptedMsg % N;
    cout << "alternative decryption output: " << endl
         << alternativeDecryptionOutput << endl
         << "encryption is decrypted: " << endl
         << (plainMessage ==
             alternativeDecryptionOutput)
         << endl;

    return 0;
}

/*
El Gamal Encryption Notes:
1. Compations are equivalent between DF and El Gamal
2. Alice sticks to her public key in El Gamal.
3. N and P are chosen by Alice.
4. The ephemeral key must be different for every plain text
5. Bob is required to used a different private key every time he encryptes
so that the ephemeral key and masking key will be different. Even though the same value
is encrypted, the cipher text would be different.
6. El Gamal is a probabilistic encryption scheme as Bob choosing is private key plays
the role of randomiser. This key should be between 2 and N-2

Computational Aspects:
Square and multiplication algorithm can be used to calculate:
    pubKey_ALICE
    pubKey_BOB
    encryptionKey
    aliceFindsOutKey
Extended Euclidean algo to compute the inverse
    aliceInversesKey

Fermat's little theorem = x^p-1 = 1 mod p, where p is prime
    plainText = aliceInversesKey * encryptedMsg = pubKey_BOB^(N-1-prvKey_ALICE) *encryptedMsg mod P

*/

/*
ALternative flow:
1. Alice computes public key (A) = g^a mod p, where a is the private key and g is the public generator
3. Alice sends the public key, g and p to Bob
2. Bob  creates an ephemeral key, k
3. Bob computes C1 = g^k mod p,
4. Bob computes C2 = m * A^k mod p
5. Alice computes x and x^-1
    x = c1 mod p and then x^-1
6. ALice decrypts the message
    m = c2 * x^-1 mod p
*/


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

using namespace Eigen;

double mean_squared_error(VectorXd, VectorXd);

int main(){
    using std::cout;
    using std::endl;

    VectorXd t = VectorXd::Zero(10);
    VectorXd y = VectorXd::Zero(10);
    double mse;

    t(2) = 1;
    y << 0.1, 0.05, 0.6, 0.0, 0.05, 0.1, 0.0, 0.1, 0.0, 0.0;

    mse = mean_squared_error(t, y);
    cout << "mean squared error: " << mse << endl;

    return 0;
}

double mean_squared_error(VectorXd t, VectorXd y){
    return (t - y).array().square().sum() / 2;
}


#include <Eigen/Core>
#include "LBFGS.h"
#include <iostream>

using Eigen::VectorXd;
using namespace LBFGSpp;
using namespace std;

double quadratic(const VectorXd& x, VectorXd& grad)
{
    //compute f(x) and grad(x)
    const int n = x.size();
    VectorXd d(n);
    for (int i=0; i<n; ++i)
    {
        d[i] = i;
    }

    double f = (x-d).squaredNorm();
    grad = 2.0 * (x-d);

    return f;
}

int main()
{
    const int n = 10; //dimension
    LBFGSParam<double> param;
    LBFGSSolver<double> solver(param);

    VectorXd x = VectorXd::Zero(n);
    double fx; // f(x)
    int niter = solver.minimize(quadratic, x, fx);

    cout << niter << " iterations" << endl;
    cout << "x = \n" << x.transpose() << endl;
    cout << "f(x) = " << fx << endl;

    return 0;
}


#include <iostream>
#include <cmath>
#include "Log.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"

int main( int argc, char** argv )
{
    //Set the log first
    za::my_logger::logger_type log = za::my_logger::get();
    za::Log logManager;
    logManager.set_log_file("./log/logSophus.log");
    // Rotation matrix rotated 90 degrees along the Z axis
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    Sophus::SO3 SO3_R(R);               // Sophus::SO(3) can be constructed directly from the rotation matrix
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // Can also be constructed from rotation vectors
    Eigen::Quaterniond q(R);            // Or quaternion
    Sophus::SO3 SO3_q( q );
    // The above expressions are all equivalent
    // When outputting SO(3), output as so(3)
    BOOST_LOG_SEV(log, za::report)<<"SO(3) from matrix: "<<SO3_R<<"\n";
    BOOST_LOG_SEV(log, za::report)<<"SO(3) from vector: "<<SO3_v<<"\n";
    BOOST_LOG_SEV(log, za::report)<<"SO(3) from quaternion :"<<SO3_q<<"\n";

    // Use logarithmic mapping to get its Lie algebra
    Eigen::Vector3d so3 = SO3_R.log();
    BOOST_LOG_SEV(log, za::report)<<"so3 = "<<so3.transpose()<<"\n";
    // hat is a vector to an antisymmetric matrix
    BOOST_LOG_SEV(log, za::report)<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<"\n";
    // In contrast, vee is antisymmetric to vector
    BOOST_LOG_SEV(log, za::report)<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<"\n"; //Transpose is purely for the output to be more beautiful

    // Update of incremental disturbance model
    Eigen::Vector3d update_so3(1e-4, 0, 0); //Assuming that the amount of updates is so much
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    BOOST_LOG_SEV(log, za::report)<<"SO3 updated = "<<SO3_updated<<"\n";

    /********************Next Demo*****************************/
    BOOST_LOG_SEV(log, za::report)<<"************Next Demo*************"<<"\n";
    // Similar to SE(3) operations
    Eigen::Vector3d t(1,0,0);           // Translate along the X axis 1
    Sophus::SE3 SE3_Rt(R, t);           //From R,t build SE(3)
    Sophus::SE3 SE3_qt(q,t);            // From q,t build SE(3)
    BOOST_LOG_SEV(log, za::report)<<"SE3 from R,t= "<<"\n"<<SE3_Rt<<"\n";
    BOOST_LOG_SEV(log, za::report)<<"SE3 from q,t= "<<"\n"<<SE3_qt<<"\n";
    // Lie algebra se(3) is a six-dimensional vector, typedef for convenience
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    BOOST_LOG_SEV(log, za::report)<<"se3 = "<<se3.transpose()<<"\n";
    // Observing the output, you will find that in Sophus, the translation of se(3) is in the front and the rotation is in the back.
    // Similarly, there are two operators hat and vee
    BOOST_LOG_SEV(log, za::report)<<"se3 hat = "<<"\n"<<Sophus::SE3::hat(se3)<<"\n";
    BOOST_LOG_SEV(log, za::report)<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<"\n";

    // Finally, demonstrate the update
    Vector6d update_se3; //Update amount
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    BOOST_LOG_SEV(log, za::report)<<"SE3 updated = "<<"\n"<<SE3_updated.matrix()<<"\n";

    return 0;
}


#include <armadillo>
#include <iostream>
#include "EigenLapack.hpp"

int main(int argc, char **argv)
{
   /* arma::mat A = arma::randu<arma::mat>(50,50);
    arma::mat B = A.t()*A;  // generate a symmetric matrix

    arma::vec eigval;
    arma::mat eigvec;

    arma::eig_sym(eigval, eigvec, B);

    // for matrices with complex elements

    arma::cx_mat C = arma::randu<arma::cx_mat>(50,50);
    arma::cx_mat D = C.t()*C;

    arma::vec eigval2;
    arma::cx_mat eigvec2;

    eig_sym(eigval2, eigvec2, D);

    std::cout << "Eigen values of random 50 x 50\n";
    eigval2.raw_print();*/

    arma::mat A1(3,3);
    A1(0,0) = 3;
    A1(0,1) = -2;
    A1(0,2) = 4;
    A1(1,0) = -2;
    A1(1,1) = 8;
    A1(1,2) = 2;
    A1(2,0) = 4;
    A1(2,1) = 2;
    A1(2,2) = 3;

    arma::vec val1;
    arma::mat vec1;

    eig_sym(val1, vec1, A1);

    std::cout << "Testing known 3 x 3 \n";
    A1.raw_print();
    std::cout << "Eigen values: \n";
    val1.raw_print();

    std::cout << "Eigen vectors: \n";
    vec1.raw_print();

    arma::cx_mat Acx(3,3);
    Acx(0,0) = 3;
    Acx(0,1) = -2;
    Acx(0,2) = 4;
    Acx(1,0) = -2;
    Acx(1,1) = 8;
    Acx(1,2) = 2;
    Acx(2,0) = 4;
    Acx(2,1) = 2;
    Acx(2,2) = 3;

    std::cout << "Input array: \n";
    Acx.raw_print();

    {
        arma::cx_mat Ac1 = Acx;
        EVWorker ev(3);
        std::cout << "Order = " << ev.size << "\n";
        ev.largestEigen( Ac1.memptr(), true);

        std::cout << "Largest eigen value: " << ev.eigval[0] << "\n";
        {
            arma::cx_vec vec(ev.eigvec, 3, false);
            vec.raw_print();
        }
    }

    {
        arma::cx_mat Ac1 = Acx;

        EVWorker ev(3);
        std::cout << "Order  " << ev.size << "\n";
        ev.smallestEigen( Ac1.memptr(), true);

        std::cout << "Smallest eigen value: " << ev.eigval[0] << "\n";
        {
            arma::cx_vec vec(ev.eigvec, 3, false);
            vec.raw_print();
        }
    }

    {
        arma::cx_mat Apos(3,3, arma::fill::eye);
        Apos *= 2;
        Apos += Acx;

        arma::cx_mat Ainv = Apos.i();

        std::cout << "Matrix inverse: \n";
        Ainv.raw_print();

        EVWorker ev(3);
        arma::cx_mat Ac1 = Apos + 0.0;

        int stat = ev.positiveDefiniteInverse(Ac1.memptr());

        std::cout << "Lapack inverse: " << stat << "\n";
        Ac1.raw_print();

        arma::cx_mat test = Apos * Ac1;
        test.raw_print();

    }

}


/*
 * Author: Pedro Peixoto <pedrosp@ime.usp.br>
 */

#include <iostream>
#include <iomanip>
#include <libmath/EigenMatrix3.hpp>
#include <sweet/SWEETError.hpp>

#if SWEET_EIGEN
#include <Eigen/Eigenvalues>
#endif

int main(
		int i_argc,
		char *const i_argv[]
)
{

#if !SWEET_EIGEN
	SWEETError("Cannot test this without Eigen library. Please compile with --eigen=enable");
#endif

	Eigen::MatrixXcf A(3,3) ;
	std::cout<<std::endl;
	std::cout<<"Eigenvalue solver tests"<<std::endl;
	std::cout<<std::endl;
	std::cout<<"----------------------------------------------------------"<<std::endl;
	std::cout<<std::endl;
	std::cout<<"Testing real matrix with complex evals 1,-i,i"<<std::endl;

	//Define matrix with ev 1,-i,i
	A(0,0)=1;
	A(0,1)=0;
	A(0,2)=0;
	A(1,0)=0;
	A(1,1)=0;
	A(1,2)=-1;
	A(2,0)=0;
	A(2,1)=1;
	A(2,2)=0;
	std::cout << A << std::endl << std::endl;

	//Eigen solver
	Eigen::ComplexEigenSolver<Eigen::MatrixXcf> ces;
	ces.compute(A);
	//std::cout << "The eigenvalues of A are:" << std::endl << ces.eigenvalues() << std::endl;

	std::complex<double> eval[3];
	std::complex<double> evec[3][3];
	for(int i=0; i<3; i++)
	{
		eval[i]=ces.eigenvalues()[i];
		std::cout << "Eigenvalue "<< i << " : " << eval[i] << std::endl;

	}
	std::cout << "The matrix of eigenvectors, V, is:" << std::endl << ces.eigenvectors() << std::endl << std::endl;

	//std::complex<float> lambda = ces.eigenvalues()[0];
	//std::cout << "Consider the first eigenvalue, lambda = " << lambda << std::endl;
	//Eigen vectors
	//Eigen::VectorXcf v = ces.eigenvectors().col(0);
	//std::cout << "If v is the corresponding eigenvector, then lambda * v = " << std::endl << lambda * v << std::endl;
	//std::cout << "... and A * v = " << std::endl << A * v << std::endl << std::endl;
	//std::cout << "Finally, V * D * V^(-1) = " << std::endl
	//     << ces.eigenvectors() * ces.eigenvalues().asDiagonal() * ces.eigenvectors().inverse() << std::endl;

	std::cout<<std::endl;
	std::cout<<std::endl;
	std::cout<<"----------------------------------------------------------"<<std::endl;
	std::cout<<std::endl;
	std::cout<<"Testing complex matrix with complex evals 1,-i,i"<<std::endl;

	std::complex<double> I;;
	I=-1.0;
	I=std::sqrt(I);
	std::cout<<I<<std::endl;
	//Define matrix with ev 1,-i,i
	A(0,0)=1;
	A(0,1)=0;
	A(0,2)=0;
	A(1,0)=0;
	A(1,1)=0;
	A(1,2)=I;
	A(2,0)=0;
	A(2,1)=I;
	A(2,2)=0;
	std::cout << A << std::endl << std::endl;

	//Eigen solver
	ces.compute(A);
	//std::cout << "The eigenvalues of A are:" << std::endl << ces.eigenvalues() << std::endl;

	for(int i=0; i<3; i++)
	{
		eval[i]=ces.eigenvalues()[i];
		std::cout << "Eigenvalue "<< i << " : " << eval[i] << std::endl;

	}
	std::cout << "The matrix of eigenvectors, V, is:" << std::endl << ces.eigenvectors() << std::endl << std::endl;

	//const Eigen::IOFormat fmt(3, Eigen::DontAlignCols, "\t", " ", "", "", "", "");
	//const Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", "\n", "", "", "", "");
	//const Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");

	//std::complex<float> lambda = ces.eigenvalues()[0];
	//std::cout << "Consider the first eigenvalue, lambda = " << lambda << std::endl;
	//Eigen vectors
	//Eigen::VectorXcf v = ces.eigenvectors().col(0);
	//std::cout << "If v is the corresponding eigenvector, then lambda * v = " << std::endl << lambda * v << std::endl;
	//std::cout << "... and A * v = " << std::endl << A * v << std::endl << std::endl;
	//std::cout << "Finally, V * D * V^(-1) = " << std::endl
	//     << ces.eigenvectors() * ces.eigenvalues().asDiagonal() * ces.eigenvectors().inverse() << std::endl;

	std::cout<< "Done!"<<std::endl;
}