// finite_difference.cpp example program to demonstrate finite difference calculations
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include "common.hpp"
#include <boost/multiprecision/cpp_int.hpp>
// silence any posit arithmetic exceptions: this basically enables a silent signalling NaR
#define POSIT_THROW_ARITHMETIC_EXCEPTION 0
#include <universal/posit/posit>

/* concepts
   The complete definition of a template function or class should contain the list of required
   concepts as is done for functions in the Standard template Library; see http://www.sgi.com/tech/stl/

   */
template <typename F, typename T>
class forward_difference {
public:
	forward_difference(const F& f, const T& h) : f(f), h(h) {}
	T operator()(const T& x) const { return (f(x+h) - f(x)) / h; }
private:
	const F& f;
	T        h;
};

template<typename F, typename T>
class backward_difference {
public:
	backward_difference(const F& f, const T& h) : f(f), h(h) {}
	T operator()(const T& x) const { return (f(x) - f(x - h)) / h; }
private:
	const F& f;
	T        h;
};

template<typename F, typename T>
class central_difference {
public:
	central_difference(const F& f, const T& h) : f(f), h(h) {}
	T operator()(const T& x) const { return (f(x + T(0.5)*h) - f(x - T(0.5)*h)) / h; }
private:
	const F& f;
	T        h;
};

// Use recursion to define arbitrary n-th order derivatives

template<unsigned N, typename F, typename T>
class nth_derivative {
	using n_minus_1_derivative = nth_derivative<N - 1, F, T>;
public:
	nth_derivative(const F& f, const T& h) : h(h), fprev(f, h) {}
	T operator()(const T& x) const { return (fprev(x + h) - fprev(x)) / h; }
private:
	n_minus_1_derivative fprev;
	T                    h;
};

// termination condition
template<typename F, typename T>
class nth_derivative<1, F, T> : public forward_difference<F, T> {
	using forward_difference<F, T>::forward_difference;
};

/*
   Define sine/cosine/tangent functors to use.
   Depending on the provided type, the sin(x)/cos(x)/tan(x) functions
   are ADL matched to functions in different namespaces.
   For regular float/double/long double these trigonometry functions
   will be provided by the std namespace.
   For the sw::unum posit/valid types, these functions will be provided
   by the sw::unum namespace.
 */

// trigonometry sine functor
template<typename T>
struct sine_f {
	T operator() (const T& x) const { return sin(x); }
};

// a functor with a scale parameter
template<typename T>
class scaled_sine_f {
public:
	scaled_sine_f(const T& scale) : scale(scale) {}
	T operator() (const T& x) const { return scale * sin(x); }
private:
	T scale;
};

// trigonometry cosine functor
template<typename T>
struct cosine_f {
	T operator() (const T& x) const { return cos(x); }
};

// trigonometry tangent functor
template<typename T>
struct tangent_f {
	T operator() (const T& x) const { return tan(x); }
};

template<typename Real>
void EnumerateFirstDerivativeError(const Real& x) {
	using namespace std;
	using namespace sw::unum; // for m_pi_4

	cout << "sin at " << double(x) << " : " << sine_f<Real>()(x) << endl;
	cout << "cos at " << double(x) << " : " << cosine_f<Real>()(x) << endl;
//	cout << "tan at " << double(x) << " : " << tan_f<Real>()(x) << endl;

	using forward_sin_f = forward_difference<sine_f<Real>, Real>;
	using backward_sin_f = backward_difference<sine_f<Real>, Real>;
	using central_sin_f = central_difference<sine_f<Real>, Real>;

	constexpr int WIDTH = 16;

	cout << "Finite Difference approximation of the first derivative of the function sine(x)\n";
	cout << "Using " << typeid(Real).name() << endl;
	Real h = Real(0.05);
	sine_f<Real> sin_o;
	cout << setw(WIDTH) << "step (h)" << setw(WIDTH) << "backward" << setw(WIDTH) << "error" << setw(WIDTH) << "forward" << setw(WIDTH) << "error" << setw(WIDTH) << "central" << setw(WIDTH) << "error\n";
	for (unsigned i = 0; i < 10; ++i) {
		// backward = left
		backward_sin_f ld_sin_o(sin_o, h);
		Real ld_sin = ld_sin_o(x);
		Real lerror = ld_sin - cos(x);

		// forward = right
		forward_sin_f rd_sin_o(sin_o, h);
		Real rd_sin = rd_sin_o(x);
		Real rerror = rd_sin - cos(x);

		// central = middle
		central_sin_f md_sin_o(sin_o, h);
		Real md_sin = md_sin_o(x);
		Real merror = md_sin - cos(x);

		cout << setw(WIDTH) << h << setw(WIDTH) << ld_sin << setw(WIDTH) << lerror << setw(WIDTH) << rd_sin << setw(WIDTH) << rerror << setw(WIDTH) << md_sin << setw(WIDTH) << merror << endl;

		h *= 0.5;
	}
}

template<typename Real>
void EnumerateSecondDerivativeError(const Real& x) {
	using namespace std;
	using namespace sw::unum; // for m_pi_4

	cout << "sin at " << double(x) << " : " << sine_f<Real>()(x) << endl;
	cout << "cos at " << double(x) << " : " << cosine_f<Real>()(x) << endl;
	//	cout << "tan at " << double(x) << " : " << tan_f<Real>()(x) << endl;

	using forward_2nd_sin_f = nth_derivative<2, sine_f<Real>, Real>;
	//using backward_2nd_sin_f = backward_difference<sine_f<Real>, Real>;
	//using central_2nd_sin_f = central_difference<sine_f<Real>, Real>;

	constexpr int WIDTH = 16;

	cout << "Finite Difference approximation of the second derivative of the function sine(x)\n";
	cout << "Using " << typeid(Real).name() << endl;
	Real h = Real(0.05);
	sine_f<Real> sin_o;
	cout << setw(WIDTH) << "step (h)" << setw(WIDTH) << "backward" << setw(WIDTH) << "error" << setw(WIDTH) << "forward" << setw(WIDTH) << "error" << setw(WIDTH) << "central" << setw(WIDTH) << "error\n";
	for (unsigned i = 0; i < 10; ++i) {
		// backward = left
//		backward_sin_f ld_sin_o(sin_o, h);
		Real ld2_sin = NAN; //  ld2_sin_o(x);
		Real lerror = ld2_sin + sin(x);

		// forward = right
		forward_2nd_sin_f rd2_sin_o(sin_o, h);
		Real rd2_sin = rd2_sin_o(x);
		Real rerror = rd2_sin + sin(x);

		// central = middle
//		central_sin_f md_sin_o(sin_o, h);
		Real md2_sin = NAN; //  md2_sin_o(x);
		Real merror = md2_sin + sin(x);

		cout << setw(WIDTH) << h << setw(WIDTH) << ld2_sin << setw(WIDTH) << lerror << setw(WIDTH) << rd2_sin << setw(WIDTH) << rerror << setw(WIDTH) << md2_sin << setw(WIDTH) << merror << endl;

		h *= 0.5;
	}
}

template<typename Real>
void EnumerateThirdDerivativeError(const Real& x) {
	using namespace std;
	using namespace sw::unum; // for m_pi_4

	cout << "sin at " << double(x) << " : " << sine_f<Real>()(x) << endl;
	cout << "cos at " << double(x) << " : " << cosine_f<Real>()(x) << endl;
	//	cout << "tan at " << double(x) << " : " << tan_f<Real>()(x) << endl;

	using forward_3rd_sin_f = nth_derivative<3, sine_f<Real>, Real>;
	//using backward_2nd_sin_f = backward_difference<sine_f<Real>, Real>;
	//using central_2nd_sin_f = central_difference<sine_f<Real>, Real>;

	constexpr int WIDTH = 16;

	cout << "Finite Difference approximation of the 3rd derivative of the function sine(x)\n";
	cout << "Using " << typeid(Real).name() << endl;
	Real h = Real(0.05);
	sine_f<Real> sin_o;
	cout << setw(WIDTH) << "step (h)" << setw(WIDTH) << "backward" << setw(WIDTH) << "error" << setw(WIDTH) << "forward" << setw(WIDTH) << "error" << setw(WIDTH) << "central" << setw(WIDTH) << "error\n";
	for (unsigned i = 0; i < 10; ++i) {
		// backward = left
//		backward_sin_f ld_sin_o(sin_o, h);
		Real ld3_sin = NAN; //  ld3_sin_o(x);
		Real lerror = ld3_sin + cos(x);

		// forward = right
		forward_3rd_sin_f rd3_sin_o(sin_o, h);
		Real rd3_sin = rd3_sin_o(x);
		Real rerror = rd3_sin + cos(x);

		// central = middle
//		central_sin_f md_sin_o(sin_o, h);
		Real md3_sin = NAN; //  md2_sin_o(x);
		Real merror = md3_sin + cos(x);

		cout << setw(WIDTH) << h << setw(WIDTH) << ld3_sin << setw(WIDTH) << lerror << setw(WIDTH) << rd3_sin << setw(WIDTH) << rerror << setw(WIDTH) << md3_sin << setw(WIDTH) << merror << endl;

		h *= 0.5;
	}
}

// compare accuracy of first derivative using different Real types
void CompareTypeAccuracyOnFirstDerivative() {
	using namespace sw::unum;

	constexpr unsigned NR_SAMPLES = 3;
	long double samples[3];
	samples[0] = m_pi_4;
	samples[1] = m_1_pi / 3.0l;
	samples[2] = m_pi_2;

	for (unsigned i = 0; i < NR_SAMPLES; ++i) {
		{
			using Real = float;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = double;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<32, 2>;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<64, 3>;
			EnumerateFirstDerivativeError<Real>(Real(samples[i]));
		}
	}
}

// compare accuracy of second derivative using different Real types
void CompareTypeAccuracyOnSecondDerivative() {
	using namespace sw::unum;

	constexpr unsigned NR_SAMPLES = 3;
	long double samples[3];
	samples[0] = m_pi_4;
	samples[1] = m_1_pi / 3.0l;
	samples[2] = m_pi_2;

	for (unsigned i = 0; i < NR_SAMPLES; ++i) {
		{
			using Real = float;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = double;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<32, 2>;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<64, 3>;
			EnumerateSecondDerivativeError<Real>(Real(samples[i]));
		}
	}
}

// compare accuracy of third derivative using different Real types
void CompareTypeAccuracyOnThirdDerivative() {
	using namespace sw::unum;

	constexpr unsigned NR_SAMPLES = 3;
	long double samples[3];
	samples[0] = m_pi_4;
	samples[1] = m_1_pi / 3.0l;
	samples[2] = m_pi_2;

	for (unsigned i = 0; i < NR_SAMPLES; ++i) {
		{
			using Real = float;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = double;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<32, 2>;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}

		{
			using Real = posit<64, 3>;
			EnumerateThirdDerivativeError<Real>(Real(samples[i]));
		}
	}
}

int main()
try {
	using namespace std;
//	using namespace boost::multiprecision;
	using namespace sw::unum;

	CompareTypeAccuracyOnFirstDerivative();
	CompareTypeAccuracyOnSecondDerivative();
	CompareTypeAccuracyOnThirdDerivative();

	{
		using Real = float;
		EnumerateSecondDerivativeError<Real>(Real(m_pi_4));
	}

	{
		using Real = float;
		EnumerateThirdDerivativeError<Real>(Real(m_pi_4));
	}

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


#include "problemes.h"
#include "arithmetique.h"

#include <boost/math/constants/constants.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef std::pair<nombre, nombre> paire;

namespace {
    typedef std::tuple<long double, long double, long double> triplet;

    long double aire(long double k) {
        long double rayon = std::abs(1.0L / k);
        return rayon * rayon * M_PIl;
    }

    long double iteration(std::map<triplet, nombre> &apollonios) {
        long double A = 0.0L;
        std::map<triplet, nombre> resultat;
        for (auto p: apollonios) {
            // k4 = k1 + k2 + k3 +/- 2*sqrt(k1.k2 + k2.k3 + k3.k1)
            auto[k1, k2, k3] = p.first;
            long double k4 = k1 + k2 + k3 + 2.0L * std::sqrt(k1 * k2 + k2 * k3 + k1 * k3);
            A += p.second * aire(k4);
            resultat[std::make_tuple(k4, k1, k2)] += p.second;
            resultat[std::make_tuple(k4, k1, k3)] += p.second;
            resultat[std::make_tuple(k4, k2, k3)] += p.second;
        }

        std::swap(apollonios, resultat);
        return A;
    }
}

ENREGISTRER_PROBLEME(199, "Iterative Circle Packing") {
    // Three circles of equal radius are placed inside a larger circle such that each pair of circles is tangent to one
    // another and the inner circles do not overlap. There are four uncovered "gaps" which are to be filled iteratively
    // with more tangent circles.
    //
    // At each iteration, a maximally sized circle is placed in each gap, which creates more gaps for the next iteration.
    // After 3 iterations (pictured), there are 108 gaps and the fraction of the area which is not covered by circles is
    // 0.06790342, rounded to eight decimal places.
    //
    // What fraction of the area is not covered by circles after 10 iterations?
    // Give your answer rounded to eight decimal places using the format x.xxxxxxxx.
    std::map<triplet, nombre> apollonios;

    // Théorème de Descartes : (k1 + k2 + k3 + k4)² = sqrt(k1² + k2² + k3² + k4²)
    // k4 = k1 + k2 + k3 +/- 2*sqrt(k1.k2 + k2.k3 + k3.k1)
    long double k0 = -1.0L;
    long double k1 = k0 / (3.0L - 2 * std::sqrt(3.0L)); // k1 = k2 = k3

    apollonios[std::make_tuple(k1, k1, k1)] = 1;
    apollonios[std::make_tuple(k1, k1, k0)] = 3;

    long double a0 = aire(k0);
    long double A = 3.0L * aire(k1);

    std::cout << std::fixed << std::setprecision(8);
    std::cout << "Iteration 0: " << 1.0L - A / a0 << std::endl;

    for (nombre n = 1; n <= 10; ++n) {
        A += iteration(apollonios);
        std::cout << "Iteration " << n << ": " << 1.0L - A / a0 << std::endl;
    }

    long double resultat = 1.0L - A / a0;
    return std::to_fixed(resultat, 8);
}


/******************************************************************************
Prácticas de cálculo matricial de estructuras
https://github.com/ingmec-ual/practicas-calculo-matricial-estructuras

Copyright 2017 - Jose Luis Blanco Claraco <jlblanco@ual.es>
University of Almeria
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
Complete BSD-3-clause License: https://opensource.org/licenses/BSD-3-Clause
******************************************************************************/

#include "libcalculomatricial.h"
#include <Eigen/Dense>
#include <iostream>

int main()
{
	try
	{
		const double E = 200*1e9; // Modulo de Young (Pa)
		const double A = 2*1e-4;  // Area (m^2)

		// Matrices de rigidez de las barras individuales en
		// coordenadas LOCALES, y convertir a globales usando:
		// T*K*T' =
		//  [Ri*Kii*Rit, Ri*Kij*Rjt]
		//  [Rj*Kji*Rit, Rj*Kjj*Rjt]
		// -----------------------------------------------
		// barra a:
		Eigen::Matrix2d Ka_11_loc, Ka_12_loc, Ka_22_loc, Ka_11, Ka_12, Ka_22;
		calcmat::matriz_barra_art_art(0.5 /*L*/, E, A, Ka_11_loc /*Kii*/, Ka_22_loc /*Kjj*/, Ka_12_loc /*Kij*/);
		const double ang_a = 0.0 * M_PI / 180;

		{
			const auto Ra_i = calcmat::matriz_rotacion2(ang_a), Ra_j = Ra_i;  // En este caso Ri=Rj al ser ambas de 2x2
			Ka_11 = Ra_i*Ka_11_loc*Ra_i.transpose();
			Ka_22 = Ra_j*Ka_22_loc*Ra_j.transpose();
			Ka_12 = Ra_i*Ka_12_loc*Ra_j.transpose();
		}

		// barra b:
		Eigen::Matrix2d Kb_11_loc, Kb_13_loc, Kb_33_loc, Kb_11, Kb_13, Kb_33;
		calcmat::matriz_barra_art_art(1.0 /*L*/, E, A, Kb_11_loc /*Kii*/, Kb_33_loc /*Kjj*/, Kb_13_loc /*Kij*/);
		const double ang_b = 90.0 * M_PI / 180;

		{
			const auto Rb_i = calcmat::matriz_rotacion2(ang_b), Rb_j = Rb_i;  // En este caso Ri=Rj al ser ambas de 2x2
			Kb_11 = Rb_i*Kb_11_loc*Rb_i.transpose();
			Kb_33 = Rb_j*Kb_33_loc*Rb_j.transpose();
			Kb_13 = Rb_i*Kb_13_loc*Rb_j.transpose();
		}

		// barra c:
		Eigen::Matrix2d Kc_22_loc, Kc_23_loc, Kc_33_loc, Kc_22, Kc_23, Kc_33;
		calcmat::matriz_barra_art_art( hypot(0.5,1.0) /*L*/, E, A, Kc_22_loc /*Kii*/, Kc_33_loc /*Kjj*/, Kc_23_loc /*Kij*/);
		const double ang_c = atan2(1.0 /*y*/, -0.5 /*x*/);

		{
			const auto R23i = calcmat::matriz_rotacion2(ang_c), R23j = R23i;  // En este caso Ri=Rj al ser ambas de 2x2
			Kc_22 = R23i*Kc_22_loc*R23i.transpose();
			Kc_33 = R23j*Kc_33_loc*R23j.transpose();
			Kc_23 = R23i*Kc_23_loc*R23j.transpose();
		}

		// Ensamblar la matriz de rigidez de la estructura:
		// -----------------------------------------------
		Eigen::Matrix<double, 6, 6> K;
		K.setZero(); // Initializa a todo ceros

		K.block<2, 2>(0, 0) = Ka_11 + Kb_11;
		K.block<2, 2>(2, 2) = Ka_22 + Kc_22;
		K.block<2, 2>(4, 4) = Kb_33 + Kc_33;

		K.block<2, 2>(0, 2) = Ka_12;
		K.block<2, 2>(2, 0) = Ka_12.transpose();

		K.block<2, 2>(0, 4) = Kb_13;
		K.block<2, 2>(4, 0) = Kb_13.transpose();

		K.block<2, 2>(2, 4) = Kc_23;
		K.block<2, 2>(4, 2) = Kc_23.transpose();

		std::cout << "K:\n" << K << std::endl << std::endl;

		// Resolver:
		//  UL = KLL^{-1} * ( FL - KLR * UR )
		//  FR = KRR * UR + KRL * UL
		// -----------------------------------------------
		const std::array<int, 3> idxs_R = {0,1,3}; // gdl "restringidos"
		const std::array<int, 3> idxs_L = {2,4,5}; // gdl "libres"

		const auto KLL = indexing(K, idxs_L, idxs_L);
		const auto KRR = indexing(K, idxs_R, idxs_R);
		const auto KLR = indexing(K, idxs_L, idxs_R);
		const auto KRL = KLR.transpose();

		Eigen::Matrix<double, 3, 1> FL; // Cargas en gdl "libres"
		FL << 0, 1000, 0;

		Eigen::Matrix<double, 3, 1> UR; // Desplazamientos en gdl "restringidos" (apoyos)
		UR << 0,0,0;

		// UL = KLL^{-1} * ( FL - KLR * UR )
		const Eigen::Matrix<double, 3, 1> UL = KLL.colPivHouseholderQr().solve( FL - KLR*UR );

		// FR = KRR * UR + KRL * UL
		const Eigen::Matrix<double, 3, 1>  FR = KRR * UR + KRL * UL;

		// Ensamblar vector U completo:
		Eigen::Matrix<double, 6, 1>  U;
		indexing(U, idxs_L) = UL;
		indexing(U, idxs_R) = UR;

		std::cout << "UL: " << UL.transpose() << std::endl;
		std::cout << "FR: " << FR.transpose() << std::endl;
		//std::cout << "U: " << U.transpose() << std::endl;

		// Esfuerzos en cada barra
		// f^a_loc = K_^a_loc * T^a^t * u^a   (formula 3.6.4 del libro)
		// ----------------------------------------------------------------

		// Barra a:
		Eigen::Matrix<double, 4, 4> Ka_loc, Ta;
		Ka_loc <<
			Ka_11_loc, Ka_12_loc,
			Ka_12_loc.transpose(), Ka_22_loc;
		Ta <<
			calcmat::matriz_rotacion2(ang_a), Eigen::Matrix2d::Zero(),
			Eigen::Matrix2d::Zero(), calcmat::matriz_rotacion2(ang_a);
		const Eigen::Vector4d ua = indexing(U, std::array<int, 4>{0, 1, 2, 3});
		const Eigen::Vector4d fa = Ka_loc * Ta.transpose() * ua;

		// Barra b:
		Eigen::Matrix<double, 4, 4> Kb_loc, Tb;
		Kb_loc <<
			Kb_11_loc, Kb_13_loc,
			Kb_13_loc.transpose(), Kb_33_loc;
		Tb <<
			calcmat::matriz_rotacion2(ang_b), Eigen::Matrix2d::Zero(),
			Eigen::Matrix2d::Zero(), calcmat::matriz_rotacion2(ang_b);
		const Eigen::Vector4d ub = indexing(U, std::array<int, 4>{0, 1, 4, 5});
		const Eigen::Vector4d fb = Kb_loc * Tb.transpose() * ub;

		// Barra c:
		Eigen::Matrix<double, 4, 4> Kc_loc, Tc;
		Kc_loc <<
			Kc_22_loc, Kc_23_loc,
			Kc_23_loc.transpose(), Kc_33_loc;
		Tc <<
			calcmat::matriz_rotacion2(ang_c), Eigen::Matrix2d::Zero(),
			Eigen::Matrix2d::Zero(), calcmat::matriz_rotacion2(ang_c);
		const Eigen::Vector4d uc = indexing(U, std::array<int, 4>{2, 3, 4, 5});
		const Eigen::Vector4d fc = Kc_loc * Tc.transpose() * uc;

		std::cout << std::endl << "Esfuerzos en barras (coordenadas locales):" << std::endl;
		std::cout << "fa: " << fa.transpose() << std::endl;
		std::cout << "fb: " << fb.transpose() << std::endl;
		std::cout << "fc: " << fc.transpose() << std::endl;

		return 0; // el programa finaliza sin errores
	}
	catch (std::exception &e)
	{
		std::cerr << e.what() << std::endl;
		return -1;
	}
}


// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

#include <boost/algorithm/string.hpp>

#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>

#include <nuklei/LinearAlgebra.h>

namespace nuklei
{

  namespace la
  {
    void eigenDecomposition(Matrix3 &eVectors, Vector3& eValues, const Matrix3& sym)
    {
       coord_t data[9];
       for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j)
        data[i*3+j] = sym(i,j);

       gsl_matrix_view m
         = gsl_matrix_view_array (data, 3, 3);

       gsl_vector *eval = gsl_vector_alloc (3);
       gsl_matrix *evec = gsl_matrix_alloc (3, 3);

       gsl_eigen_symmv_workspace * w =
         gsl_eigen_symmv_alloc (3);

       gsl_eigen_symmv (&m.matrix, eval, evec, w);

       gsl_eigen_symmv_free (w);

       gsl_eigen_symmv_sort (eval, evec,
                             GSL_EIGEN_SORT_ABS_DESC);

       for (int i = 0; i < 3; i++)
       {
         eValues[i]
            = gsl_vector_get (eval, i);
         //NUKLEI_ASSERT(eValues[i] >= 0);
         gsl_vector_view evec_i
            = gsl_matrix_column (evec, i);
         for (int j = 0; j < 3; ++j)
           eVectors(j, i) = gsl_vector_get(&evec_i.vector, j);
       }

       // There should be a more efficient way to do this...
       if (Vector3(eVectors.GetColumn(0)).Cross(eVectors.GetColumn(1)).Dot(eVectors.GetColumn(2)) < 0)
         eVectors.SetColumn(2, -Vector3(eVectors.GetColumn(2)));

       gsl_vector_free (eval);
       gsl_matrix_free (evec);
    }

    // The following function
    //     Vector3 project(const Plane3& plane, const Vector3& point)
    // is currently defined in KernelCollectionPCA.cpp

    double determinant(const GMatrix& m)
    {
      NUKLEI_FAST_ASSERT(m.GetRows() == m.GetColumns());
      const int dim = m.GetRows();

      gsl_matrix *lu;
      gsl_permutation *perm;
      int signum = 0;

      lu = gsl_matrix_alloc(dim, dim);
      perm = gsl_permutation_alloc(dim);

      for (int i = 0; i < dim; ++i)
        for (int j = 0; j < dim; ++j)
        {
          gsl_matrix_set(lu, i, j, m(i,j));
        }

      gsl_linalg_LU_decomp(lu, perm, &signum);
      double det = gsl_linalg_LU_det(lu, signum);

      gsl_matrix_free(lu);
      gsl_permutation_free(perm);

      return det;
    }

    GMatrix inverse(const GMatrix& m)
    {
      NUKLEI_FAST_ASSERT(m.GetRows() == m.GetColumns());
      const int dim = m.GetRows();

      gsl_matrix *lu, *inverse;
      gsl_permutation *perm;
      int signum = 0;

      lu = gsl_matrix_alloc(dim, dim);
      inverse = gsl_matrix_alloc(dim, dim);
      perm = gsl_permutation_alloc(dim);

      for (int i = 0; i < dim; ++i)
        for (int j = 0; j < dim; ++j)
        {
          gsl_matrix_set(lu, i, j, m(i,j));
        }

      gsl_linalg_LU_decomp(lu, perm, &signum);
      gsl_linalg_LU_invert(lu, perm, inverse);

      GMatrix inv(dim, dim);
      for (int i = 0; i < dim; ++i)
        for (int j = 0; j < dim; ++j)
        {
          inv(i,j) = gsl_matrix_get(inverse, i, j);
        }

      gsl_matrix_free(lu);
      gsl_matrix_free(inverse);
      gsl_permutation_free(perm);

      return inv;
    }

  }

  std::ostream& operator<<(std::ostream &out, const GMatrix &m)
  {
    for (int i = 0; i < m.GetRows(); ++i)
    {
      for (int j = 0; j < m.GetColumns(); ++j)
        NUKLEI_ASSERT(out << m(i,j) << " ");
      NUKLEI_ASSERT(out << "\n");
    }
    out << std::flush;
    return out;
  }

  std::istream& operator>>(std::istream &in, GMatrix &m)
  {
    std::vector< std::vector<double> > rows;
    std::string line;
    int ntok = -1;
    while (std::getline(in, line))
    {
      cleanLine(line);
      std::vector<std::string> tokens;
      boost::split(tokens, line, boost::is_any_of(" "), boost::token_compress_on);
      if (tokens.front() == "")
        tokens.erase(tokens.begin());
      if (tokens.back() == "")
        tokens.pop_back();

      if (ntok == -1)
        ntok = tokens.size();
      else
        NUKLEI_ASSERT(ntok == tokens.size());

      if (tokens.size() == 0)
        break;

      rows.push_back(std::vector<double>());
      for (std::vector<std::string>::const_iterator i = tokens.begin();
           i != tokens.end(); ++i)
        rows.back().push_back(numify<double>(*i));
    }

    m.SetSize(rows.size(), ntok);
    for (int i = 0; i < m.GetRows(); ++i)
      for (int j = 0; j < m.GetColumns(); ++j)
        m(i,j) = rows.at(i).at(j);

    return in;
  }

}


#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <cmath>
using Eigen::MatrixXd;
using Eigen::VectorXd;
using namespace std;

VectorXd calculateError(VectorXd state,double gt_x,double gt_y,double gt_vx,double gt_vy){
  VectorXd gt(4);
  gt << gt_x,gt_y,gt_vx,gt_vy;
  VectorXd error = state-gt;
  return error;
}

MatrixXd calculateJacobian(VectorXd state){
   double px = state(0);
   double py = state(1);
   double vx = state(2);
   double vy = state(3);
   MatrixXd jacobian(3,4);
   double sqrt_power = sqrt((pow(px,2)+pow(py,2)));
   double constant = (px*px + py*py)*sqrt_power;
   jacobian << (px/sqrt_power),(py/sqrt_power),0,0,
               (-1*py/(pow(px,2)+pow(py,2))),(px/(pow(px,2)+pow(py,2))),0,0,
               py*(vx*py - vy*px)/constant, px*(px*vy - py*vx)/constant, px/sqrt_power, py/sqrt_power;
   return jacobian;
}

VectorXd radar2State(VectorXd m_radar){
   double r_m_rho = m_radar(0);
   double r_m_pi = m_radar(1);
   double r_m_rdot = m_radar(2);
   double r_m_px = r_m_rho*cos(r_m_pi);
        if ( r_m_px < 0.0001 ) {
        r_m_px = 0.0001;
      }
        double r_m_py = r_m_rho*sin(r_m_pi);
        if(r_m_py< 0.0001){
          r_m_py = 0.0001;
        }
        double r_m_vx = r_m_rdot*cos(r_m_pi);
        double r_m_vy = r_m_rdot*sin(r_m_pi);
  VectorXd r_x(4) ;
  r_x << r_m_px,r_m_py,r_m_vx,r_m_vy;
  return r_x;
}

int main()
{
  ifstream reader("C:\\DEV\\website\\data\\obj_pose-laser-radar-synthetic-input.txt");
  int tick = -10;
  //Matrix for laser
   MatrixXd R_laser_ = MatrixXd(2, 2);//measurement noise
    //measurement covariance matrix - laser
   R_laser_ << 0.0225, 0,
        0, 0.0225;

    MatrixXd l_H(2,4);// used to convert predicted state to measurement state, shape wise
     l_H << 1,0,0,0,
          0,1,0,0;

    VectorXd l_m_x(2);//measured state

  //Matrix for Radar
   MatrixXd R_radar_ = MatrixXd(3, 3);// measurement noise
    //measurement covariance matrix - radar
   R_radar_ << 0.09, 0, 0,
        0, 0.0009, 0,
        0, 0, 0.09;

   VectorXd R_m_x(3);
   MatrixXd Hj(3,4);

  string line;
  string part;

  //Global Variables , same for laser and radar
  MatrixXd P(4,4);//process covariance matrix
  MatrixXd p_P(4,4);//predicted covariance matrix
  MatrixXd F(4,4);//prediction matrix
  F << 1,0,0,0,
         0,1,0,0,
         0,0,1,0,
         0,0,0,1;
  MatrixXd Q(4,4);//process covariance matrix
  VectorXd y;// difference between predicted and measured
  MatrixXd I = MatrixXd::Identity(4,4);
  VectorXd x(4);//state
  VectorXd p_x(4);//predicted state
  VectorXd noise(4);//noise due to model simplicity
  MatrixXd S;
  MatrixXd K;
  VectorXd p_R_x(3);//state measurement in radar space

  char type;
  int count = 0;//for initializing
  double gt_x;//ground truth
  double gt_y;//ground truth
  double gt_vy;//ground truth
  double gt_vx;//ground truth

  double noise_ax = 9.0;//noise due to model simplicity
  double noise_ay = 9.0;//noise due to model simplicity

  long long c_timestamp;
  long long p_timestamp;
  double dt;

  float m_px,m_py,m_vx,m_vy;
  float m_rho,m_pi,m_rdot;

  while(getline(reader,line)){
    istringstream my_stream(line);
    my_stream>>type;
    if(type=='L'){
      my_stream>>m_px;
      my_stream>>m_py;
      l_m_x<< m_px,m_py;
      my_stream>>c_timestamp;
      //for intializing state and covariance matrix
      if(count==0){
        x <<m_px,m_py,0,0;
        P << 1, 0, 0, 0,
              0, 1, 0, 0,
              0, 0, 1000, 0,
              0, 0, 0, 1000;
        p_timestamp = c_timestamp;
        count++;
        continue;
      }
      dt = (c_timestamp - p_timestamp)/1000000.0;
       p_timestamp = c_timestamp;
      F(0,2) = dt;
      F(1,3) = dt;
      noise<< (noise_ax*dt*dt/2),(noise_ay*dt*dt/2),(noise_ax*dt),(noise_ay*dt);
      p_x = F*x + noise;

      Q << (dt*dt*dt*dt*noise_ax*noise_ax/4),0,(dt*dt*dt*noise_ax*noise_ax/2),0,
            0,(dt*dt*dt*dt*noise_ay*noise_ay/4),0,(dt*dt*dt*noise_ay*noise_ay/2),
            (dt*dt*dt*noise_ax*noise_ax/2),0,(dt*dt*noise_ax*noise_ax),0,
            0,(dt*dt*dt*noise_ay*noise_ay/2),0,(dt*dt*noise_ay*noise_ay);
      p_P = F*P*F.transpose() + Q;
      y = l_m_x-(l_H*p_x);
      S = l_H*p_P*l_H.transpose() + R_laser_;
      K = p_P*l_H.transpose()*S.inverse();
      x = p_x + K*y;
      P = (I-K*l_H)*p_P;
      my_stream>>gt_x;
      my_stream>>gt_y;
      my_stream>>gt_vx;
      my_stream>>gt_vy;
      cout<<"Error"<<calculateError(x,gt_x,gt_y,gt_vx,gt_vy)<<endl;
       VectorXd gt(4);
       gt << gt_x,gt_y,gt_vx,gt_vy;

      cout<<"predicted state = "<<x<<endl;
      cout<<"ground truth"<<gt<<endl;
    }
    if(type=='R'){
     my_stream>>m_rho;
      my_stream>>m_pi;
      my_stream>>m_rdot;
      R_m_x << m_rho,m_pi,m_rdot;
      my_stream>>c_timestamp;
       if(count==0){
        x  = radar2State(R_m_x);
        P << 1, 0, 0, 0,
              0, 1, 0, 0,
              0, 0, 1000, 0,
              0, 0, 0, 1000;
        p_timestamp = c_timestamp;
        count++;
        continue;
      }
      dt = (c_timestamp - p_timestamp)/1000000.0;
      p_timestamp = c_timestamp;
      F(0,2) = dt;
      F(1,3) = dt;
      noise<< (noise_ax*dt*dt/2),(noise_ay*dt*dt/2),(noise_ax*dt),(noise_ay*dt);
      p_x = F*x + noise;
      Q << (dt*dt*dt*dt*noise_ax*noise_ax/4),0,(dt*dt*dt*noise_ax*noise_ax/2),0,
            0,(dt*dt*dt*dt*noise_ay*noise_ay/4),0,(dt*dt*dt*noise_ay*noise_ay/2),
            (dt*dt*dt*noise_ax*noise_ax/2),0,(dt*dt*noise_ax*noise_ax),0,
            0,(dt*dt*dt*noise_ay*noise_ay/2),0,(dt*dt*noise_ay*noise_ay);
      p_P = F*P*F.transpose() + Q;
      double px = p_x(0);
      double py = p_x(1);
      double vx = p_x(2);
      double vy = p_x(3);
      double rho = sqrt(px*px + py*py);
      double theta = atan2(py, px);
      double rho_dot = (px*vx + py*vy) / rho;
      p_R_x << rho,theta,rho_dot;
      y = R_m_x - p_R_x;
      while ( y(1) > M_PI || y(1) < -M_PI ) {
    if ( y(1) > M_PI ) {
      y(1) -= M_PI;
    } else {
      y(1) += M_PI;
    }
  }
      Hj = calculateJacobian(p_x);
      S = Hj*p_P*Hj.transpose() + R_radar_;
      K = p_P*Hj.transpose()*S.inverse();
      x = p_x + K*y;
      P = (I-K*Hj)*p_P;
      my_stream>>gt_x;
      my_stream>>gt_y;
      my_stream>>gt_vx;
      my_stream>>gt_vy;
       VectorXd gt(4);
       gt << gt_x,gt_y,gt_vx,gt_vy;
      cout<<"Error"<<calculateError(x,gt_x,gt_y,gt_vx,gt_vy)<<endl;
      cout<<"predicted state = "<<x<<endl;
      cout<<"ground truth"<<gt<<endl;
    }
  }
  return 0;
}


#include <iostream>
#include <math.h>
#include <vector>
#include <Eigen/Dense>

using namespace std;

//Evaluate the Chebyshev polynomials up to order n in x.
vector<double> chebpolmult(const int &n,const double &x)
{
    vector<double> V={1,x};
    for (int k=1; k<n; k++)
        V.push_back(2*x*V[k]-V[k-1]);
    return V;
}

//Check orthogonality of Chebyshev polynomials.
int main(){
    int n=10;
    vector<double> V;
    Eigen::MatrixXd scal(n+1,n+1);
    for (int j=0; j<n+1; j++) {
        V=chebpolmult(n,cos(M_PI*(2*j+1)/2/(n+1)));
        for (int k=0; k<n+1; k++) scal(j,k)=V[k];
    }
    cout<<"Scalar products: "<<endl;
    for (int k=0; k<n+1; k++)
        for (int l=k+1; l<n+1; l++)
            cout<<scal.col(k).dot(scal.col(l))<<endl;
}

Nummerical Methods for CSE/PS9/solutions_ps9/ChebOrthog.cpp


// find_location.cpp

// Copyright Paul A. Bristow 2007.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Example of finding location (mean)
// for normal (Gaussian) & Cauchy distribution.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[find_location1
/*`
First we need some includes to access the normal distribution,
the algorithms to find location (and some std output of course).
*/

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.
#include <boost/math/distributions/cauchy.hpp> // for cauchy_distribution
  using boost::math::cauchy; // typedef provides default type is double.
#include <boost/math/distributions/find_location.hpp>
  using boost::math::find_location;
  using boost::math::complement; // Needed if you want to use the complement version.
  using boost::math::policies::policy;

#include <iostream>
  using std::cout; using std::endl;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
//] [/find_location1]

int main()
{
  cout << "Example: Find location (mean)." << endl;
  try
  {
//[find_location2
/*`
For this example, we will use the standard normal distribution,
with mean (location) zero and standard deviation (scale) unity.
This is also the default for this implementation.
*/
  normal N01;  // Default 'standard' normal distribution with zero mean and
  double sd = 1.; // normal default standard deviation is 1.
/*`Suppose we want to find a different normal distribution whose mean is shifted
so that only fraction p (here 0.001 or 0.1%) are below a certain chosen limit
(here -2, two standard deviations).
*/
  double z = -2.; // z to give prob p
  double p = 0.001; // only 0.1% below z

  cout << "Normal distribution with mean = " << N01.location()
    << ", standard deviation " << N01.scale()
    << ", has " << "fraction <= " << z
    << ", p = "  << cdf(N01, z) << endl;
  cout << "Normal distribution with mean = " << N01.location()
    << ", standard deviation " << N01.scale()
    << ", has " << "fraction > " << z
    << ", p = "  << cdf(complement(N01, z)) << endl; // Note: uses complement.
/*`
[pre
Normal distribution with mean = 0, standard deviation 1, has fraction <= -2, p = 0.0227501
Normal distribution with mean = 0, standard deviation 1, has fraction > -2, p = 0.97725
]
We can now use ''find_location'' to give a new offset mean.
*/
   double l = find_location<normal>(z, p, sd);
   cout << "offset location (mean) = " << l << endl;
/*`
that outputs:
[pre
offset location (mean) = 1.09023
]
showing that we need to shift the mean just over one standard deviation from its previous value of zero.

Then we can check that we have achieved our objective
by constructing a new distribution
with the offset mean (but same standard deviation):
*/
  normal np001pc(l, sd); // Same standard_deviation (scale) but with mean (location) shifted.
/*`
And re-calculating the fraction below our chosen limit.
*/
cout << "Normal distribution with mean = " << l
    << " has " << "fraction <= " << z
    << ", p = "  << cdf(np001pc, z) << endl;
  cout << "Normal distribution with mean = " << l
    << " has " << "fraction > " << z
    << ", p = "  << cdf(complement(np001pc, z)) << endl;
/*`
[pre
Normal distribution with mean = 1.09023 has fraction <= -2, p = 0.001
Normal distribution with mean = 1.09023 has fraction > -2, p = 0.999
]

[h4 Controlling Error Handling from find_location]
We can also control the policy for handling various errors.
For example, we can define a new (possibly unwise)
policy to ignore domain errors ('bad' arguments).

Unless we are using the boost::math namespace, we will need:
*/
  using boost::math::policies::policy;
  using boost::math::policies::domain_error;
  using boost::math::policies::ignore_error;

/*`
Using a typedef is often convenient, especially if it is re-used,
although it is not required, as the various examples below show.
*/
  typedef policy<domain_error<ignore_error> > ignore_domain_policy;
  // find_location with new policy, using typedef.
  l = find_location<normal>(z, p, sd, ignore_domain_policy());
  // Default policy policy<>, needs "using boost::math::policies::policy;"
  l = find_location<normal>(z, p, sd, policy<>());
  // Default policy, fully specified.
  l = find_location<normal>(z, p, sd, boost::math::policies::policy<>());
  // A new policy, ignoring domain errors, without using a typedef.
  l = find_location<normal>(z, p, sd, policy<domain_error<ignore_error> >());
/*`
If we want to use a probability that is the
[link complements complement of our probability],
we should not even think of writing `find_location<normal>(z, 1 - p, sd)`,
but, [link why_complements to avoid loss of accuracy], use the complement version.
*/
  z = 2.;
  double q = 0.95; // = 1 - p; // complement.
  l = find_location<normal>(complement(z, q, sd));

  normal np95pc(l, sd); // Same standard_deviation (scale) but with mean(location) shifted
  cout << "Normal distribution with mean = " << l << " has "
    << "fraction <= " << z << " = "  << cdf(np95pc, z) << endl;
  cout << "Normal distribution with mean = " << l << " has "
    << "fraction > " << z << " = "  << cdf(complement(np95pc, z)) << endl;
  //] [/find_location2]
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

//[find_location_example_output
/*`
[pre
Example: Find location (mean).
Normal distribution with mean = 0, standard deviation 1, has fraction <= -2, p = 0.0227501
Normal distribution with mean = 0, standard deviation 1, has fraction > -2, p = 0.97725
offset location (mean) = 1.09023
Normal distribution with mean = 1.09023 has fraction <= -2, p = 0.001
Normal distribution with mean = 1.09023 has fraction > -2, p = 0.999
Normal distribution with mean = 0.355146 has fraction <= 2 = 0.95
Normal distribution with mean = 0.355146 has fraction > 2 = 0.05
]
*/
//] [/find_location_example_output]


//
// Created by haohanwang on 1/24/16.
//

#ifndef ALGORITHMS_LINEARREGRESSION_HPP
#define ALGORITHMS_LINEARREGRESSION_HPP

#include "Model.hpp"

#include <Eigen/Dense>
#include <unordered_map>

#ifdef BAZEL
#include "Models/ModelOptions.hpp"
#else
#include "../Models/ModelOptions.hpp"
#endif

using namespace Eigen;

class LinearRegression : public virtual Model {
private:
    float L1_reg;
    float L2_reg;
    MatrixXf betaAll;

    static constexpr float default_L1_reg = 0;
    static constexpr float default_L2_reg = 0;
public:
    LinearRegression();
    LinearRegression(const unordered_map<string, string>& options);

    void setL1_reg(float);
    float getL1_reg();
    void setL2_reg(float);
    float getL2_reg();

    // general use methods
    MatrixXf derivative();
    float cost();

    // algorithm use methods
    // proximal gradient descent
    MatrixXf proximal_derivative();
    MatrixXf proximal_operator(VectorXf, float);

    void assertReadyToRun();

    void updateBetaAll(MatrixXf);
    MatrixXf getBetaAll();
};

#endif //ALGORITHMS_LINEARREGRESSION_HPP


#include "fmatrix.h"
#include "sfm_utils.h"
#include "defines.h"

#include <iostream>
#include <Eigen/SVD>
#include <opencv2/calib3d.hpp>

namespace {

    void infoF(const cv::Matx33d &Fcv)
    {
        Eigen::MatrixXd F;
        copy(Fcv, F);

        Eigen::JacobiSVD<Eigen::MatrixXd> svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::MatrixXd U = svdf.matrixU();
        Eigen::VectorXd s = svdf.singularValues();
        Eigen::MatrixXd V = svdf.matrixV();

        std::cout << "F info:\nF:\n" << F << "\nU:\n" << U << "\ns:\n" << s << "\nV:\n" << V << std::endl;
    }

    cv::Matx33d estimateFMatrixDLT(const cv::Vec2d *m0, const cv::Vec2d *m1, int count)
    {
        int a_rows = count;
        int a_cols = 9;

        Eigen::MatrixXd A(a_rows, a_cols);

        for (int i_pair = 0; i_pair < count; ++i_pair) {

            double x0 = m0[i_pair][0];
            double y0 = m0[i_pair][1];

            double x1 = m1[i_pair][0];
            double y1 = m1[i_pair][1];

//            std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl;

            A.row(i_pair) << x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1.0;
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::VectorXd null_space = svda.matrixV().col(8);

        Eigen::MatrixXd F(3, 3);
        F.row(0) << null_space[0], null_space[1], null_space[2];
        F.row(1) << null_space[3], null_space[4], null_space[5];
        F.row(2) << null_space[6], null_space[7], null_space[8];

        Eigen::JacobiSVD<Eigen::MatrixXd> svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::MatrixXd U = svdf.matrixU();
        Eigen::VectorXd s = svdf.singularValues();
        Eigen::MatrixXd V = svdf.matrixV();

        Eigen::MatrixXd S = Eigen::MatrixXd(3, 3);
        S.setZero();
        S(0, 0) = s[0];
        S(1, 1) = s[1];
        S(2, 2) = 0.0;

        F = U * S * V.transpose();

        cv::Matx33d Fcv;
        copy(F, Fcv);

        return Fcv;
    }

    // get transform matrix so that after transform centroid of points will be zero and Root Mean Square distance from centroid will be sqrt(2)
    cv::Matx33d getNormalizeTransform(const std::vector<cv::Vec2d> &m, bool verbose=true)
    {
        cv::Vec2d centroid(0.0, 0.0);

        if (m.empty()) {
            throw std::runtime_error("Can't normalize transform");
        }

        for (int i = 0; i < (int) m.size(); ++i) {
            centroid += m[i];
        }

        centroid /= (double) m.size();

        double rms = 0;
        for (int i = 0; i < (int) m.size(); ++i) {
            cv::Vec2d d = m[i] - centroid;
            rms += d[0] * d[0] + d[1] * d[1];
        }
        rms = std::sqrt(rms / (2 * m.size()));

        if (rms == 0) {
            throw std::runtime_error("Can't normalize transform");
        }

        double s = std::sqrt(2) / rms;

        if (verbose) {
            std::cout << "NORMALIZE TRANSFORM: centroid = " << centroid << ", scale = " << s << std::endl;
        }

        cv::Matx33d T(s, 0.0, -s*centroid[0],
                      0.0, s, -s*centroid[1],
                      0.0, 0.0, 1.0);

        return T;
    }

    cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T)
    {
        cv::Vec3d tmp = T * cv::Vec3d(pt[0], pt[1], 1.0);

        if (tmp[2] == 0) {
            throw std::runtime_error("infinite point");
        }

        return cv::Vec2d(tmp[0] / tmp[2], tmp[1] / tmp[2]);
    }

    cv::Matx33d estimateFMatrixRANSAC(const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, double threshold_px, bool verbose=true)
    {
        if (m0.size() != m1.size()) {
            throw std::runtime_error("estimateFMatrixRANSAC: m0.size() != m1.size()");
        }

        const int n_matches = m0.size();

        cv::Matx33d TN0 = getNormalizeTransform(m0, verbose);
        cv::Matx33d TN1 = getNormalizeTransform(m1, verbose);

        std::vector<cv::Vec2d> m0_t(n_matches);
        std::vector<cv::Vec2d> m1_t(n_matches);
        for (int i = 0; i < n_matches; ++i) {
            m0_t[i] = transformPoint(m0[i], TN0);
            m1_t[i] = transformPoint(m1[i], TN1);
        }

        {
//             check log: centroid should become close to zero, scale close to 1
            getNormalizeTransform(m0_t, verbose);
            getNormalizeTransform(m1_t, verbose);
        }

        // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
        // будет отличаться от случая с гомографией
        const int n_trials = 10000;

        const int n_samples = 8;
        uint64_t seed = 1;

        int best_support = 0;
        cv::Matx33d best_F;

        std::vector<int> sample;
        for (int i_trial = 0; i_trial < n_trials; ++i_trial) {
            phg::randomSample(sample, n_matches, n_samples, &seed);

            cv::Vec2d ms0[n_samples];
            cv::Vec2d ms1[n_samples];
            for (int i = 0; i < n_samples; ++i) {
                ms0[i] = m0_t[sample[i]];
                ms1[i] = m1_t[sample[i]];
            }

            cv::Matx33d F = estimateFMatrixDLT(ms0, ms1, n_samples);

            // denormalize
            F = TN1.t() * F * TN0;

            int support = 0;
            for (int i = 0; i < n_matches; ++i) {
                //                    todo todo todo                                                         todo todo todo
                if (phg::epipolarTest(m0[i], m1[i], F, threshold_px) && phg::epipolarTest(m1[i], m0[i], F.t(), threshold_px))
                {
                    ++support;
                }
            }

            if (support > best_support) {
                best_support = support;
                best_F = F;

                if (verbose) {
                    std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl;
                    infoF(F);
                }

                if (best_support == n_matches) {
                    break;
                }
            }
        }

        if (verbose) {
            std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl;
        }

        if (best_support == 0) {
            throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix");
        }

        return best_F;
    }

}

cv::Matx33d phg::findFMatrix(const std::vector <cv::Vec2d> &m0, const std::vector <cv::Vec2d> &m1, double threshold_px, bool verbose) {
    return estimateFMatrixRANSAC(m0, m1, threshold_px, verbose);
}

cv::Matx33d phg::findFMatrixCV(const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, double threshold_px) {
    return cv::findFundamentalMat(m0, m1, cv::FM_RANSAC, threshold_px);
}

cv::Matx33d phg::composeFMatrix(const cv::Matx34d &P0, const cv::Matx34d &P1)
{
    // compute fundamental matrix from general cameras
    // Hartley & Zisserman (17.3 - p412)

    cv::Matx33d F;

#define det4(a, b, c, d) \
      ((a)(0) * (b)(1) - (a)(1) * (b)(0)) * ((c)(2) * (d)(3) - (c)(3) * (d)(2)) - \
      ((a)(0) * (b)(2) - (a)(2) * (b)(0)) * ((c)(1) * (d)(3) - (c)(3) * (d)(1)) + \
      ((a)(0) * (b)(3) - (a)(3) * (b)(0)) * ((c)(1) * (d)(2) - (c)(2) * (d)(1)) + \
      ((a)(1) * (b)(2) - (a)(2) * (b)(1)) * ((c)(0) * (d)(3) - (c)(3) * (d)(0)) - \
      ((a)(1) * (b)(3) - (a)(3) * (b)(1)) * ((c)(0) * (d)(2) - (c)(2) * (d)(0)) + \
      ((a)(2) * (b)(3) - (a)(3) * (b)(2)) * ((c)(0) * (d)(1) - (c)(1) * (d)(0))

    int i, j;
    for (j = 0; j < 3; j++)
        for (i = 0; i < 3; i++) {
            // here the sign is encoded in the order of lines ~ai
            const auto a1 = P0.row((i + 1) % 3);
            const auto a2 = P0.row((i + 2) % 3);
            const auto b1 = P1.row((j + 1) % 3);
            const auto b2 = P1.row((j + 2) % 3);

            F(j, i) = det4(a1, a2, b1, b2);
        }

#undef det4

    return F;
}


#include <iostream>

using namespace std;

#include <ctime>

#include <Eigen/Dense>

using namespace Eigen;

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

    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;

    // SVD
    clock_t time_svd = clock();
    cout << "The least-squares solution is:\n"
            << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
    cout << "time:"
       << 1000 * (clock() - time_svd) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // QR
    clock_t time_qr = clock();
    cout << "\nThe solution using the QR decomposition is:\n"
        << A.colPivHouseholderQr().solve(b) << endl;
    cout << "time:"
       << 1000 * (clock() - time_qr) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // Normal Equations
    clock_t time_normal = clock();
    cout << "\nThe solution using normal equations is:\n"
        << (A.transpose() * A).ldlt().solve(A.transpose() * b) << endl;
    cout << "time:"
       << 1000 * (clock() - time_normal) / (double) CLOCKS_PER_SEC << "ms" << endl;

    return 0;
}


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Dense>
#include <vector>
#include <type_traits>

using namespace std;
using namespace Eigen;

//Codigo par abrir el archivo

MatrixXd openData(string fileToOpen)
{

    vector<double> matrixEntries;
    ifstream matrixDataFile(fileToOpen);
    string matrixRowString;
    string matrixEntry;

    int matrixRowNumber = 0;

    while (getline(matrixDataFile, matrixRowString))
    {
        stringstream matrixRowStringStream(matrixRowString);

        while (getline(matrixRowStringStream, matrixEntry, ','))
        {
            matrixEntries.push_back(stod(matrixEntry));
        }
        matrixRowNumber++;
    }

    return Map<Matrix<double, Dynamic, Dynamic, RowMajor>>(matrixEntries.data(), matrixRowNumber, matrixEntries.size() / matrixRowNumber);

}

//Codigo para guardar el archivo en cualquier formato

void saveData(string fileName, MatrixXd  matrix)
{

    const static IOFormat CSVFormat(FullPrecision, DontAlignCols, " ", "\n");

    ofstream file(fileName);
    if (file.is_open())
    {
        file << matrix.format(CSVFormat);
        file.close();
    }
}

//Inversa por Gauss Jordan

void Print(MatrixXd a, int n, int m)
{
    FILE *output_file;
    output_file=fopen("Inversa_Gauss_Jordan.txt", "w");

    for (int i = 0; i < n; i++) {
        for (int j = n; j < m; j++) {
           fprintf(output_file,"%.3f  ", a(i,j));
        }
        fprintf(output_file,"\n");
    }

     return;
}

void Gauss(MatrixXd m){

   MatrixXd I_ = MatrixXd::Identity(m.rows(),m.cols());

   MatrixXd C(m.rows(), m.cols()+I_.cols());

   C << m, I_;

   C.row(0).swap(C.row(m.rows() - 1));

   for (int i = 0; i < m.rows(); i++) {

        for (int j = 0; j < m.rows(); j++) {

            if (j != i) {

                float temp = C(j,i) / C(i,i);
                for (int k = 0; k < 2*m.rows(); k++) {

                    C(j,k) -= C(i,k)*temp;
                }
            }
        }
    }

    for (int i = 0; i < m.rows(); i++) {

        float temp_ = C(i,i);

        for (int j = 0; j < 2 * m.rows(); j++) {

            C(i,j) = C(i,j)/temp_;
        }

    }

    Print(C, m.cols(), 2*m.cols());
}

//Inversa por cofactores

void getCofactor(int** A , int** temp , int p, int q, int n)
{
    int i = 0, j = 0;

    for (int row = 0; row < n; row++)
    {
        for (int col = 0; col < n; col++)
        {

            if (row != p && col != q)
            {
                temp[i][j++] = A[row][col];

                if (j == n - 1)
                {
                    j = 0;
                    i++;
                }
            }
        }
    }
}

int determinant(int**A, int n)
{
    int D = 0;
    if (n == 1)
        return A[0][0];

    int** temp = new int* [n];
    for (int i = 0; i < n; i++) {
        temp[i] = new int[n];
    }

    int sign = 1;
    for (int f = 0; f < n; f++)
    {

        getCofactor(A, temp, 0, f, n);
        D += sign * A[0][f] * determinant(temp, n - 1);

        sign = -sign;
    }

    return D;
}

void adjoint(int** A, int** adj, int N)
{

    if (N == 1)
    {
        adj[0][0] = 1;
        return;
    }

    int sign = 1;
    int** temp = new int* [N];
    for (int i = 0; i < N; i++) {
        temp[i] = new int[N];
    }

    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < N; j++)
        {

            getCofactor(A, temp, i, j, N);

            sign = ((i + j) % 2 == 0) ? 1 : -1;

            adj[j][i] = (sign) * (determinant(temp, N - 1));
        }
    }
}

MatrixXd inverse(MatrixXd m, int N)
{

    int rows = m.rows();
    int cols = m.cols();
    int** matrix = new int* [rows];

    for (int i = 0; i < rows; i++)
    {
        matrix[i] = new int[cols];
    }

    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            *(*(matrix + i) + j) = m(i, j);

    float** aux = new float* [rows];

    for (int i = 0; i < rows; i++) {
        aux[i] = new float[cols];
    }

    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            *(*(aux + i) + j) = m(i, j);

    MatrixXd mx(N, N);

    int det = determinant(matrix, N);

    int** adj = new int* [N];
    for (int i = 0; i < N; i++)
        adj[i] = new int[N];
    adjoint(matrix, adj,N);
    for (int i = 0; i < N; i++)
        for (int j = 0; j < N; j++)
            aux[i][j] = adj[i][j] / float(det);

    for (int j = 0; j < N; j++) {
        for (int i = 0; i < N; i++) {
            mx(j, i) = aux[j][i];

        }
    }
    return mx;

}

//Otra forma de encontrar la inversa
//En vez de encontrar un vecotor dinamico lo que hacemos es trabajar directamente con MatrixXd

/*

int determ(MatrixXd m,int n)
  {

  int det=0, p, h, k, i, j;
  MatrixXd temp =  MatrixXd::Zero(m.rows(),m.cols());

  if(n==1) {
    return m(0,0);
  }

  else if(n==2){
    det=(m(0,0)*m(1,1)-m(0,1)*m(1,0));
    return det;
  }

  else {
    for(p=0;p<n;p++) {
      h = 0;
      k = 0;
      for(i=1;i<n;i++) {
        for( j=0;j<n;j++) {
          if(j==p) {
            continue;
          }

          temp(h,k) = m(i,j);
          k++;

          if(k==n-1) {
            h++;
            k = 0;
          }

        }
      }

      det=det+m(0,p)*pow(-1,p)*determ(temp,n-1);
    }

    return det;
  }

}

void cofact(MatrixXd m, int n)
{
    int i = 0, j = 0;

    MatrixXd temp = MatrixXd::Zero(m.rows(),m.cols());
    MatrixXd adj = MatrixXd::Zero(m.rows(),m.cols());

    for (int  p = 0; p<n; p++){

        for (int q = 0; q<n; q++){

            for (int row = 0; row < n; row++){

                for (int col = 0; col < n; col++){

                        if (row != q && col != p){

                            temp(i,j) = m(row,col);

                                if (j == n - 1)
                                {
                                    j = 0;
                                    i++;
                                }

                     }

                }

         }

        adj(q,p) = pow(-1,q+p)*determ(temp,n - 1);

      }

    }

}

Cabe destacar que el principal problema recido en que la adjunta da una matriz de ceros, por lo que no es posible encontrar la inversa

void Inverse(MatrixXd m, MatrixXd adj, int n){

    int i;
    int j;

    MatrixXd b = MatrixXd::Zero(m.rows(),m.cols());
    MatrixXd I_ = MatrixXd::Zero(m.rows(),m.cols());

    for (i=0;j<n;i++){

        for (j = 0; j<n; j++){

                b(i,j) = adj(j,i);

        }

    }

    //int d = determ(m,n);

    for (i=0;j<n;i++){

        for (j = 0; j<n; j++){

                I_(i,j) = b(i,j)/d;

        }

    }

   cout << I_ << endl;

}

*/

int main(){

    MatrixXd data_;
    data_ = openData("nums.txt");

    cout << "Matriz" << endl;
    cout << data_ << endl;

    Gauss(data_);

    MatrixXd cofact = inverse(data_, data_.rows());
    saveData("Inversa_por_cofactores.txt",cofact);

    MatrixXd Eigen_ = data_.inverse();
    saveData("Inversa_Eigen.txt",Eigen_);

    return 0;
}


#include <Eigen/Dense>
#include <iostream>
#include <complex>
#include <string>
#include <sstream>
typedef std::complex<double> Complex;
int main(int argc, char **argv) {
  using namespace std;
  string usage = string("Usage: ")
    + argv[0]
    + " [-?] [-v potential] [-k wvnumber]";

  if (argc>1 && argv[1]==string("-?")) {
    cout << usage << endl;
    exit (0);
  }

  // default values
   double v=0.5;
   double k=0.2;

  try {
    // overwritten by command line:
    for (int i=1; i<argc;i+=2) {
      istringstream stream(argv[i+1]);
      if (string(argv[i])=="-v") stream >> v;
      if (string(argv[i])=="-k") stream >> k;
    }
  }
  catch (exception &) {
    cout << usage << endl;
    exit (0);
  }

  Complex I(0,1.0);
  Complex nk=k*sqrt(Complex(1-v));

  Eigen::VectorXcd Y(4);
  Y(0)= -exp(-I*k);
  Y(1)= 0;
  Y(2)= -I*k*exp(-I*k);
  Y(3)= 0;

  Eigen::MatrixXcd A(4,4);
  // First row:
  A(0,0)= exp(I*k)      ;
  A(0,1)=-exp(-I*nk)    ;
  A(0,2)= -exp(I*nk)    ;
  A(0,3)=0              ;

  // Second row:
  A(1,0)= 0             ;
  A(1,1)= exp(I*nk)     ;
  A(1,2)= exp(-I*nk)    ;
  A(1,3)=-exp(I*k)      ;

  // Third row:
  A(2,0)= -I*k*exp(I*k)  ;
  A(2,1)=-I*nk*exp(-I*nk);
  A(2,2)= I*nk*exp(I*nk) ;
  A(2,3)=0                 ;

  // Fourth row:
  A(3,0)= 0                ;
  A(3,1)=I*nk*exp(I*nk)  ;
  A(3,2)=-I*nk*exp(-I*nk);
  A(3,3)=-I*k*exp(I*k)   ;

  Eigen::MatrixXcd AInv= A.inverse();
  Eigen::VectorXcd BCDF=AInv*Y;
  Complex B=BCDF(0), F=BCDF(3);
  cout << "Complex coefficients" << endl;
  cout << BCDF << endl;
  cout << endl;
  cout << "Reflection coefficient  : " << norm(B)         << endl;
  cout << "Transmission coefficient: " << norm(F)         << endl;
  cout << "Sum:                      " << norm(B)+norm(F) << endl;

}


//
// Created by Harold on 2021/5/1.
//

#include <Eigen/Dense>

#include <iostream>

// 2D Zero-Order Gaussian Regression Filter
// TODO: verify
Eigen::MatrixXd RGR0_2D(Eigen::MatrixXd const& data,
	                    double lambdax = 0.25,
	                    double lambday = 0.25,
	                    double dx = 0.01,
	                    double dy = 0.01,
	                    double tol = 1e-10,
	                    unsigned max_iteration_no = 9) {
	auto ny = data.rows();
	auto nx = data.cols();
	double constant = sqrt(log(2) / 2 / EIGEN_PI / EIGEN_PI);

	unsigned iterationNo = 0;
	Eigen::MatrixXd delta = Eigen::MatrixXd::Ones(ny, nx);
	double CBx = 1;
	double CB_old = 1;

	Eigen::MatrixXd w4(ny, nx);
	Eigen::MatrixXd r4(ny, nx);

	while (CBx > tol) {
		for (auto kx = 0; kx < nx; ++kx)
			for (auto ky = 0; ky < ny; ++ky) {
				auto px = Eigen::VectorXd::LinSpaced(nx, 0, nx);
				auto py = Eigen::VectorXd::LinSpaced(ny, 0, ny);
				Eigen::VectorXd v1 = exp(-0.5 * ((py.array() - ky) * dy / constant / lambday).array().pow(2));
				Eigen::RowVectorXd v2 = exp(-0.5 * ((px.array() - kx) * dx / constant / lambdax).array().pow(2)).transpose();
				Eigen::MatrixXd S = (1 / sqrt(2 * EIGEN_PI) / pow(constant, 2) / lambdax / lambday) * v1 * v2;
				Eigen::MatrixXd SMOD = S / S.sum();
				w4(ky, kx) = (SMOD * data * delta).sum() / (SMOD * delta).sum();
			}
		r4 = data - w4;
		double CB_next = 4.4 * r4.array().abs().sum() / r4.size();
		CBx = abs((CB_old - CB_next) / CB_old);
		for (auto i = 0; i < nx; i++)
			for (auto j = 0; j < ny; j++) {
				if (abs(r4(j, i) / CB_next) < 1)
					delta(j, i) = pow(1 - pow(r4(j, i) / CB_next, 2), 2);
				else
					delta(j, i) = 0;
			}
		CB_old = CB_next;
		++iterationNo;
		std::cout << "CBx: " << CBx << ", iteration no: " << iterationNo << std::endl;
		if (iterationNo > max_iteration_no)
			break;
	}
	return w4;
}

// 1D Second-Order Gaussian Regression Filter
// w(k,p)= Ax2 + Bx + C where x = (k-p)dx
Eigen::VectorXd RGR2_1D(Eigen::VectorXd const& data,
	                    double lambdac = 0.8,
	                    double dx = 0.01,
	                    double tol = 1e-10,
	                    unsigned max_iteration_no = 29) {
	auto n = data.size();
	double constant = sqrt(log(2) / 2 / EIGEN_PI / EIGEN_PI);
	Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(n, 0, n) * dx;

	unsigned iterationNo = 0;
	Eigen::VectorXd delta = Eigen::VectorXd::Ones(n);
	double CBx = 1;
	double CB_old = 1;

	Eigen::Matrix3d M;
	Eigen::Vector3d Q;
	Eigen::Vector3d P;
	Eigen::VectorXd w1(n);
	Eigen::VectorXd r1(n);

	while (CBx > tol) {
		for (auto k = 0; k < n; k++) {
			auto p = Eigen::VectorXd::LinSpaced(n, 0, n);
			Eigen::VectorXd S = (1/sqrt(2 * EIGEN_PI * EIGEN_PI)/constant/lambdac) * exp(-0.5 * ((k - p.array()) * dx / constant / lambdac).array().pow(2));
			S /= S.sum();
			x = (k - p.array()) * dx;
			M(0, 0) = (delta.array() * S.array() * (x.array().pow(0))).sum();  // A0
			M(0, 1) = (delta.array() * S.array() * (x.array().pow(1))).sum();  // A1
			M(0, 2) = (delta.array() * S.array() * (x.array().pow(2))).sum();  // A2
			M(1, 0) = M(0, 1);  // A1
			M(1, 1) = M(0, 2);  // A2
			M(1, 2) = (delta.array() * S.array() * (x.array().pow(3))).sum();  // A3
			M(2, 0) = M(0, 2);  // A2
			M(2, 1) = M(1, 2);  // A3
			M(2, 2) = (delta.array() * S.array() * (x.array().pow(4))).sum();  // A4

			Q(0) = (delta.array() * data.array() * S.array()).sum();  // F0
			Q(1) = (delta.array() * data.array() * S.array() * x.array()).sum();  // F1
			Q(2) = (delta.array() * data.array() * S.array() * (x.array().pow(2))).sum();  // F2

			P = M.inverse() * Q;
			w1(k) = P(0);
		}

		r1 = data - w1;
		double CB_next = 4.4 * r1.array().abs().sum() / r1.size();
		CBx = abs((CB_old - CB_next) / CB_old);
		for (auto j = 0; j < n; j++)
			if (abs(r1(j) / CB_next) < 1)
				delta(j) = pow(1 - pow(r1(j) / CB_next, 2), 2);
			else
				delta(j) = 0;
		CB_old = CB_next;
		++iterationNo;
		std::cout << "CBx: " << CBx << ", iteration no: " << iterationNo << std::endl;
		if (iterationNo > max_iteration_no)
			break;
	}
	return w1;
}

int main() {

	/*
	Eigen::MatrixXd data(15, 15);
	data.setRandom();
	std::cout << data << '\n' << std::endl;
	auto res = RGR0_2D(data);
	std::cout << res << std::endl;
	*/

	/*
	Eigen::VectorXd data1(100);
	data1.setRandom();
	std::cout << data1.transpose() <<'\n' << std::endl;
	auto res1 = RGR2_1D(data1);
	std::cout << res1.transpose() << std::endl;
	*/

	///*
	unsigned N = 20;
	double A = 2;
	Eigen::VectorXd data1(N);
	for (auto i = 0; i < N; ++i)
		data1(i) = A * sin((EIGEN_PI / N * i)) + 0.1 * rand() / (RAND_MAX / 2) - 1;

	std::cout << data1.transpose() << '\n' << std::endl;
	auto res1 = RGR2_1D(data1);
	std::cout << res1.transpose() << std::endl;
	//*/
	return 0;
}


#include <iostream>
#include <chrono>
#include <random>
#include <map>

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include "matplotlibcpp.h"

using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

template <class T>
T g(T x, T a, T b, T c)
{
  return ceres::exp(a * x * x + b * x + c);
}

struct CurveFittingCost {

  CurveFittingCost(double x, double y) : _x(x), _y(y) {}

  template <class T>
  bool operator() (const T* const abc, T *residuals) const
  {
    residuals[0] = T(_y) - g(T(_x), abc[0], abc[1], abc[2]);
    return true;
  }

  private:
  const double _x, _y;
};

int main(int argc, char **argv) {
  std::cout << "Gauss-Newton \n";

  double aa = 1.0, bb = 2.0, cc = 1.0;
  double a = 2.0, b = -1.0, c = 5.5;

  double obs_sigma = 1.0;
  std::default_random_engine generator;
  std::normal_distribution<double> obs_noise_distrib(0.0, obs_sigma);

  int N = 100;
  std::vector<double> x_data(N), y_data(N);
  for (int i = 0; i < N; ++i)
  {
    x_data[i] = static_cast<double>(i) / 100.0;
    y_data[i] = g(x_data[i], aa, bb, cc) + obs_noise_distrib(generator);

  }

  // Add the residual blocks
  double abc[3] = {a, b, c}; // parameters
  ceres::Problem problem;
  for (int i = 0; i < N; ++i)
  {
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<CurveFittingCost, 1, 3>(
        new CurveFittingCost(x_data[i], y_data[i])
        ),
      nullptr,
      abc
    );
  }
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  std::cout << summary.BriefReport() << std::endl;
  std::cout << "Estimated: " << abc[0] << " "  << abc[1] << " "  << abc[2] << "\n";

  vector<double> final_y_data(N);
  for (int i = 0; i < N; ++i)
  {
    final_y_data[i] = g(x_data[i], abc[0], abc[1], abc[2]);
  }

  plt::scatter(x_data, y_data);
  std::map<std::string, std::string> parameters;
  parameters["c"] = "red";
  plt::scatter(x_data, final_y_data, 1.0, parameters);
  plt::show();

  return 0;
}


/*
 MIT License

 Copyright (c) 2019 Yuki Koyama

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included in all
 copies or substantial portions of the Software.

 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */

#ifndef OPTIMIZATION_TEST_FUNCTIONS_HPP_
#define OPTIMIZATION_TEST_FUNCTIONS_HPP_

#include <cassert>
#include <iostream>
#if defined(OTF_WITH_EIGEN)
#include <Eigen/Core>
#endif

namespace otf
{
    enum class FunctionType
    {
        Beale,
        Rosenbrock,
        Sphere,
    };

    inline double GetValue(const double x[], const int n, const FunctionType type)
    {
        switch (type)
        {
            case FunctionType::Beale:
            {
                assert(n == 2);
                const double a = 1.500 - x[0] + x[0] * x[1];
                const double b = 2.250 - x[0] + x[0] * x[1] * x[1];
                const double c = 2.625 - x[0] + x[0] * x[1] * x[1] * x[1];
                return a * a + b * b + c * c;
            }
            case FunctionType::Rosenbrock:
            {
                assert(n >= 2);
                double value = 0.0;
                for (int i = 0; i < n - 1; ++ i)
                {
                    value += 100.0 * (x[i + 1] - x[i] * x[i]) * (x[i + 1] - x[i] * x[i]) + (1.0 - x[i]) * (1.0 - x[i]);
                }
                return value;
            }
            case FunctionType::Sphere:
            {
                double value = 0.0;
                for (int i = 0; i < n; ++ i)
                {
                    value += x[i] * x[i];
                }
                return value;
            }
        }
    }

    inline void GetGrad(const double x[],
                        const int n,
                        const FunctionType type,
                        double grad[])
    {
        switch (type)
        {
            case FunctionType::Beale:
            {
                assert(n == 2);
                const double a = 1.500 - x[0] + x[0] * x[1];
                const double b = 2.250 - x[0] + x[0] * x[1] * x[1];
                const double c = 2.625 - x[0] + x[0] * x[1] * x[1] * x[1];
                grad[0] = 2.0 * a * (- 1.0 + x[1]) + 2.0 * b * (- 1.0 + x[1] * x[1]) + 2.0 * c * (- 1.0 + x[1] * x[1] * x[1]);
                grad[1] = 2.0 * a * x[0] + 2.0 * b * 2.0 * x[0] * x[1] + 2.0 * c * 3.0 * x[0] * x[1] * x[1];
                return;
            }
            case FunctionType::Rosenbrock:
            {
                assert(n >= 2);
                grad[0] = - 400.0 * x[0] * (x[1] - x[0] * x[0]) + 2.0 * (x[0] - 1.0);
                for (int i = 1; i < n - 1; ++ i)
                {
                    grad[i] = 200.0 * (x[i] - x[i - 1] * x[i - 1]) - 400.0 * x[i] * (x[i + 1] - x[i] * x[i]) + 2.0 * (x[i] - 1.0);
                }
                grad[n - 1] = 200.0 * (x[n - 1] - x[n - 2] * x[n - 2]);
                return;
            }
            case FunctionType::Sphere:
            {
                for (int i = 0; i < n; ++ i)
                {
                    grad[i] = 2.0 * x[i];
                }
                return;
            }
        }
    }

    inline void GetSolution(const int n,
                            const FunctionType type,
                            double x[])
    {
        switch (type)
        {
            case FunctionType::Beale:
            {
                assert(n == 2);
                x[0] = 3.0;
                x[1] = 0.5;
                return;
            }
            case FunctionType::Rosenbrock:
            {
                assert(n >= 2);
                for (int i = 0; i < n; ++ i)
                {
                    x[i] = 1.0;
                }
                return;
            }
            case FunctionType::Sphere:
            {
                for (int i = 0; i < n; ++ i)
                {
                    x[i] = 0.0;
                }
                return;
            }
        }
    }

#if defined(OTF_WITH_EIGEN)
    inline double GetValue(const Eigen::VectorXd& x, const FunctionType type)
    {
        return GetValue(x.data(), x.size(), type);
    }

    inline Eigen::VectorXd GetGrad(const Eigen::VectorXd& x, const FunctionType type)
    {
        Eigen::VectorXd grad(x.size());
        GetGrad(x.data(), x.size(), type, grad.data());
        return grad;
    }

    inline Eigen::VectorXd GetSolution(const int n, const FunctionType type)
    {
        Eigen::VectorXd x(n);
        GetSolution(n, type, x.data());
        return x;
    }
#endif
}

#endif


//
// Created by Ferdinando Fioretto on 11/2/15.
//

#ifndef MISC_UTILS_LMATRIX_HPP
#define MISC_UTILS_LMATRIX_HPP

#include <cmath>
#include <vector>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace boost::numeric::ublas;

namespace misc_utils
{
    // Linear Algebra
    namespace lmatrix
    {

        /* Matrix inversion routine.
        Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
        template<class T>
        bool InvertMatrix(const matrix<T>& input, matrix<T>& inverse)
        {
            typedef permutation_matrix<std::size_t> pmatrix;

            // create a working copy of the input
            matrix<T> A(input);

            // create a permutation matrix for the LU-factorization
            pmatrix pm(A.size1());

            // perform LU-factorization
            int res = lu_factorize(A, pm);
            if (res != 0)
                return false;

            // create identity matrix of "inverse"
            inverse.assign(identity_matrix<T> (A.size1()));

            // backsubstitute to get the inverse
            lu_substitute(A, pm, inverse);

            return true;
        }

        template<class T>
        matrix<T> to_matrix(const std::vector<std::vector<T>>& other) {
            matrix<T> ret(other.size(), other[0].size());
            for (int i=0; i<other.size(); i++) {
                for (int j=0; j<other[i].size(); j++) {
                    ret.insert_element(i, j, other[i][j]);
                }
            }
            return ret;
        }

        template<class T>
        std::vector<std::vector<T>> to_matrix(matrix<T>& other) {
            std::vector<std::vector<T>> ret(other.size1());
            for (int i = 0; i < other.size1(); i++)
                ret[i].resize(other.size2());

            for (int i = 0; i < other.size1(); i++) {
                for (int j = 0; j < other.size2(); j++) {
                    ret[i][j] = other.at_element(i, j);
                }
            }
            return ret;
        }

        template <class T>
        std::vector<std::vector<T>> prod(const std::vector<std::vector<T>>& A, const std::vector<std::vector<T>>& B) {
            matrix<T> _A = to_matrix(A);
            matrix<T> _B = to_matrix(B);
            matrix<T> C = boost::numeric::ublas::prod(_A, _B);
            return to_matrix(C);
        }

        template <class T>
        std::vector<std::vector<T>> invert(const std::vector<std::vector<T>>& A) {
            matrix<T> _A = to_matrix(A);
            int order = A.size();
            matrix<T> _Z(order, order);
            InvertMatrix(_A, _Z);
           return to_matrix(_Z);
        }

        template <class T>
        std::vector<std::vector<T>> transpose(const std::vector<std::vector<T>>& A) {
            matrix<T> _A = to_matrix(A);
            matrix<T> _At = boost::numeric::ublas::trans(_A);
            return to_matrix(_At);
        }

        //template <class T>
        inline std::vector<std::vector<double>> alloc(int orderI, int orderJ) {
            std::vector<std::vector<double>> Z(orderI);
            for (int i = 0; i < orderI; i++)
                Z[i].resize(orderJ, 0.0);
            return Z;
        }

        template <class T>
        std::string to_string(std::vector<std::vector<T>> matrix) {
            std::string ret;
            for (int i=0; i<matrix.size(); i++) {
                for (int j=0; j<matrix[i].size(); j++) {
                    ret += std::to_string(matrix[i][j]) + " ";
                }
                ret += "\n";
            }
            return ret;
        }
    }
}

#endif


/**
 *  @file    FiniteElement.hpp
 *  @brief   Defines basis functions.
 *  @author  Francois Roy
 *  @date    01/06/2020
 */
#ifndef FINITEELEMENT_H
#define FINITEELEMENT_H

#include <vector>
#include <cmath>
#include <math.h>
#include <Eigen/Core>
#include "spdlog/spdlog.h"

namespace numerical {
namespace fem {
/**
*  A linear mapping from \f$X\f$ to \f$x\f$ can be written as:
*
*  \f[
*    x = \frac{1}{2}\left(x_L+x_R\right)+\frac{1}{2}\left(x_R-x_L\right)X
*  \f]
*
*  @param X The local coordinate in the reference cell.
*  @param omega_e The left and right global coordinates of the element.
*  @return The global coordinate.
*/
template <typename T>
T affine_mapping(T X, const T (&omega_e)[2]){
    T x_L = omega_e[0];
    T x_R = omega_e[1];
    return 0.5*(x_L + x_R) + 0.5*(x_R - x_L)*X;
}
/**
*  Returns the \f$i\f$-th Lagrange polynomial.
*
*  \f[
*      \phi_i(x) = \prod_{j=0, j\neq i}^N \frac{x-x_j}{x_i-x_j}
*  \f]
*
*  @param x The local coordinate in the reference cell.
*  @param i The Lagrange polynomial index.
*  @param nodes The interpolation nodes in the reference cell.
*  @return The \f$i\f$-th Lagrange polynomial.
*/
template <typename T>
T lagrange_polynomial(T x, int i,
    const Eigen::Matrix<T, Eigen::Dynamic, 1>& nodes){
    T p = 1.0;
    for(int j=0; j<nodes.size(); j++){
        if(j != i){
            p *= (x - nodes[j])/(nodes[i] - nodes[j]);
          }
    }
    return p;
};
/**
*  Returns the first derivative of the \f$i\f$-th Lagrange polynomial.
*
*  \f[
*    \begin{align}
*      \frac{\partial}{\partial x}\phi_i(x) &=
*      \frac{\partial}{\partial X}\phi_i(X)\frac{\partial X}{\partial x}\\
*      &= \frac{\partial}{\partial X}\phi_i(X)\frac{2}{h}
*    \end{align}
*  \f]
*
*  For details see Langtangen2019, p.188 Cellwise computation.
*
*  The first derivative of the Lagrange polynomial with respect to local
*  coordinates \f$X\f$ can be obtained using the logarithm of the Lagrange
*  polynamial, i.e.
*
*  \f[
*    \textrm{ln}\left(\prod_{j=0, j\neq i}^N \frac{x-x_j}{x_i-x_j}\right) =
*      \sum_{j=0, j\neq i}^N \textrm{ln}\left(\frac{x-x_j}{x_i-x_j}\right)
*  \f]
*
*  The derivative of the logarithm is
*
*  \f[
*    \frac{\partial}{\partial X}\left(\textrm{ln}(f(x))\right) = \frac{1}{f(X)}
*      \frac{\partial}{\partial X}f(X)
*  \f]
*
*  with \f$f(x)=\frac{x-x_j}{x_i-x_j}\f$, we get
*
*  \f[
*    \frac{\partial}{\partial X}\phi_i(X) = \phi_i(X)\sum_{j=0, j\neq i}^N
*      \frac{1}{x-x_j}
*  \f]
*
*  Note that this result is only valid for \f$(x-x_j)\neq 0\f$.
*
*  @param x The local coordinate.
*  @param i The polynomial index.
*  @param nodes The interpolation nodes in the reference cell.
*  @param h The element size.
*  @param d The polynomial order, default=1.
*  @return The \f$i\f$-th Lagrange polynomial.
*/
template <typename T>
T lagrange_polynomial_first_derivative(T x, int i,
    const Eigen::Matrix<T, Eigen::Dynamic, 1>& nodes, T h, int d=1){
    T p = 0.0;
    T denom = 1.0;
    T lp = lagrange_polynomial(x, i, nodes);
    for(int j=0; j<nodes.size(); j++){
        if(j != i){ // d > 1
            denom *= (nodes[i] - nodes[j]);
            if(d == 1){
                p += 1.0;
            } else if(d ==2){
                p += (x - nodes[j]);
            }
          }
    }
    p *= 2.0 / h / denom;
    return p;
};
/**
*  Returns all local basis function \f$\phi\f$ and their derivatives,
*  in physical coordinates, as functions of the local point \f$X\f$ in a
*  reference cell with \f$d+1\f$ nodes.
*
*  Example:
*  ~~~~~~~~~~~~~~~{.cpp}
*  #include "numerical/fem/FiniteElement.hpp"
*
*  using namespace numerical;
*
*  int main()
*  {
*    double f0 = fem::phi(0, 0, 0.0);  // basis func 0 at X=0
*    double fp0 = fem::phi(0, 0, 0.0);  // 1st x-derivative at X=0
*    spdlog::info("f0: {}, fp0: {}", f0, fp0);
*    return 0;
*  }
*  ~~~~~~~~~~~~~~~
*  Output:
*  ~~~~~~~~~~~~~~~{.sh}
*  $ f0: 0.5, fp0: -2.0
*  ~~~~~~~~~~~~~~~
*
*  @param i 0 if function, 1 if first derivative.
*  @param j The index of the basis function.
*  @param x The local coordinate.
*  @param h The element size.
*  @param d The polynomial order.
*  @return The value of the basis function/derivative at the local
*    coordinate.
*/
template <typename T>
T phi(int i, int j, T x, T h=2.0, int d=1){
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
    Vec nodes = Vec::LinSpaced(d+1, -1.0, 1.0);  // local nodes
    if(j>d){
        spdlog::error("Invalid value, index j must be in the range [0,d].");
        return 0.0;
    }
    if(std::abs(x)>1){
        spdlog::error("Invalid value, x must be in the range [-1,1].");
        return 0.0;
    }
    if(i==0){  // function
        return lagrange_polynomial(x, j, nodes);
    } else if(i==1){
        return lagrange_polynomial_first_derivative(x, j, nodes, h, d);
    } else{
        spdlog::error("Invalid value, 'i' is either 0 or 1.");
        return 0.0;
    }
}
/**
*  Return points and weights for Gauss-Legendre rules on [-1,1]. The number of
*  points implemented are 1-4.
*
*  Numerical integration rules can be expressed in a common form:
*
*  \f[
*    \int_{-1}^{1} g(X)dX\approx \sum_{j=0}^M w_jg(\bar{X}_j)
*  \f]
*
*  where \f$\bar{X}_j\f$ are integration points, and \f$w_j\f$ are integration
*  weights, for \f$j=0,\ldots ,M\f$.
*
*  More accurate rules, for a given \f$M\f$, arise if the location of the
*  integration points are optimized for polynomial integrands. The
*  Gauss-Legendre quadrature constitute one such class of integration methods.
*  Two widely applied Gauss-Legendre rules in this family have the choice:
*
*  for \f$M=1\f$:
*
*  \f[
*    \bar{X}_0 = -\frac{1}{\sqrt{3}},\quad \bar{X}_1 = \frac{1}{\sqrt{3}},
*      \quad w_0=w_1=1
*  \f]
*
*  For \f$M=2\f$:
*
*  \f[
*    \bar{X}_0 = -\sqrt{\frac{3}{5}},\quad \bar{X}_1 = 0,
*      \quad \bar{X}_2 = \sqrt{\frac{3}{5}}, \quad w_0=w_2=\frac{5}{9},
*      \qaud w_1 = \frac{8}{9}
*  \f]
*
*  These rules integrate 3rd and 5th degree polynomials exactly. In general,
*  an \f$M\f$-point Gauss-Legendre rule integrates a polynomial of degree
*  \f$2M + 1\f$ exactly.
*
*  @param n The number of integration points.
*  @return The points and weights.
*/
template <typename T>
std::vector<std::vector<T>> GaussLegendre(int n){
    std::vector<T> points;
    std::vector<T> weights;
    if(n == 1){
        points = {0.0};
        weights = {2.0};
    } else if(n == 2){
        points = {-0.5773502691896257645091488,
                   0.5773502691896257645091488};
        weights = {1.0, 1.0};
    } else if(n == 3){
        points = {-0.7745966692414833770358531,
                  0.0,
                  0.7745966692414833770358531};
        weights = {0.5555555555555555555555556,
                   0.8888888888888888888888889,
                   0.5555555555555555555555556};
    } else if(n == 4){
        points = {-0.8611363115940525752239465,
                  -0.3399810435848562648026658,
                  0.3399810435848562648026658,
                  0.8611363115940525752239465};
        weights = {0.3478548451374538573730639,
                   0.6521451548625461426269361,
                   0.6521451548625461426269361,
                   0.3478548451374538573730639};
    } else {
        spdlog::error("Gauss-Legendre: The number of points is not supported");
        return {{}};
    }
    return {points, weights};
}

}  // namespace fem
} // namespace numerical

#endif  // FINITEELEMENT_H


/*  example for MatlabPlotter: Natural cubic splines (core problem)

	An in-class exercise using equations for the cubic splines as
	shown in the lecture/manuscript for Numerical Methods for CSE
	by Prof. R. Hiptmair, ETH Zürich

	Include the Eigen3 library as shown in documentation for Eigen3.

	use piping to store the .m file. Example call:
	natcsi >example.m

	given nodes (t_i,y_i) for i = 0,...,n

	and using eq 3.5.5, the calculation of y = s(x) boils down to
	- finding the correct interval [t_j-1,t_j] for x
	- apply s|[t_j-1,t_j](x) = s_j(x) =
		y_j-1 * (1 - 3 * tau^2 + 2 * tau^3) +
		y_j   * (3 * tau^2 - 2 * tau^3) +
		m_j-1 * (tau - 2 * tau^2 + tau^3) +
		m_j   * (-tau^2 + tau^3)

		with
		m1_j = m_j-1 = h_j * c_j-1,
		m2_j = m_j   = h_j * c_j

		c_j   = s'(t_j)

		h_j   = t_j - t_j-1

		tau   = (t - t_j-1) / h_j

	current runtime:
	- initialization: O(n), Thomas algorithm for solving the tridiagonal system matrix
	- evaluation:
		* worst case:        O(log n)
		* best/typical case: O(1)

	space requirements: O(n) all the time

	output: is given again as a Matlab script that
	allows plotting all data in Matlab.

	v1.1.1 2015-11-05 / 2015-11-29 Pirmin Schmid
*/

#include <cassert>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include "matlab_plotter.h"

using namespace std;
using namespace Eigen;

// cubic spline interpolant with natural boundaries
class NatCSI {
public:

	// build cubic spline interpolant with natural boundaries
	// input:  t  nodes of the grid for pairs (t_i, y_i), sorted!
	//         y  values y_i at t_i for pairs (t_i, y_i), sorted!
	// input in std::vector<double> type
	NatCSI(const vector<double>& t, const vector<double>& y) {
		// map std::vector<double> to a VectorXd
		VectorXd tmp_t = VectorXd::Map(t.data(), t.size());
		VectorXd tmp_y = VectorXd::Map(y.data(), y.size());

		// store a copy of the data
		this->t = tmp_t;
		this->y = tmp_y;

		compute_coefficients();
	}

	// build cubic spline interpolant with natural boundaries
	// input:  t  nodes of the grid for pairs (t_i, y_i), sorted!
	//         y  values y_i at t_i for pairs (t_i, y_i), sorted!
	// input in Eigen::VectorXd type
	NatCSI(const VectorXd& t, const VectorXd& y) {
		// store a copy of the input vectors
		this->t = t;
		this->y = y;

		compute_coefficients();
	}

	// interpolant evaluation at x
	// input:   x value where to evaluate the spline
	// return:  y = s(x) if x in defined range of s(x)
	//          or 0.0 otherwise
	double operator() (double x) {
		// out of defined interval
		if(x < min || max < x) {
			return 0.0;
		}

		// get proper j, using a heuristic
		// 1) check whether x is in last range, O(1)
		// 2) check whether x is in next range, O(1)
		// 3) use binary search tree, O(log n)
		if(!in_range(x, last_j)) {
			if(last_j < n && in_range(x, last_j+1)) {
				// note: the second test is only applied if last_j actually allows it
				last_j++;
			}
			else {
				find_index(x);
			}
		}

		// apply function in interval j
		int j = last_j; // just for convenience

		double tau  = (x - t[j-1]) / h[j];
		double tau2 = tau * tau;
		double tau3 = tau2 * tau;

		double result = y[j-1] * (1.0 - 3.0 * tau2 + 2.0 * tau3);
		result += y[j] * (3.0 * tau2 - 2.0 * tau3);
		result += m1[j] * (tau - 2.0 * tau2 + tau3);
		result += m2[j] * (-tau2 + tau3);
		return result;
	}

private:
	// we need to store the vectors t and y
	// and a vector for m and h as described above
	// m is calculated after having solved a LSE
	// A * c = d as described below (and in the manuscript)

	VectorXd t, y, h, m1, m2;

	// number of segments of the spline function
	// note: n is a valid index for the vectors. Their size is (n+1).
	int n = 0;

	// valid range for x
	double min = 0.0;
	double max = 0.0;

	// since most requests for x calculations are in vicinity
	// of the prior one (or even sorted), remembering
	// the last requested section will help to bring
	// evaluation time to O(1) compared to O(log n) if only
	// a binary search tree is used.
	// the binary search tree is currently used as a fallback
	// in case the current partition/segment or the next one
	// has not been successful.
	int last_j = 1;

	// returns true if x is in interval j [t_j-1, t_j]
	bool in_range(const double x, const int j) const {
		assert(1 <= j && j <= n);
		return t[j-1] <= x && x <= t[j];
	}

	// sets last_j to  index of matching interval j [t_j-1, t_j] if x in range
	// note: x had to be tested before whether it is actually within the range of the spline function
	// note: optimizations (first test whether last interval was already ok or test the next
	// interval have already been done when one calls this function)
	// finds next j in O(log n) time
	void find_index(const double x) {
		assert(min <= x && x <= max);

		int j = last_j;
		int a = 1;
		int b = n;
		bool found = false;
		while(!found) {
			if(x < t[j-1]) {
				b = j;
				j = (a+b)/2;
			}
			else if(t[j] < x) {
				if(a == b-1) {
					// special case needed due to integer division rounding down
					j = b;
					found = true;
				}
				else {
					a = j;
					j = (a+b)/2;
				}
			}
			else {
				// match
				found = true;
			}
		}

		last_j = j;
	}

	void compute_coefficients() {
		if(t.size() != y.size()) {
			// only minimalistic error management
			cout << "Error: size of t and y must be identical." << endl;
			exit(1);
		}

		// n
		n = (int)t.size() - 1;

		// min/max
		min = t[0];
		max = t[n];

		// h_j = t_j - t_j-i
		h.resize(n + 1);
		h.segment(1, n) = t.tail(n) - t.head(n);

		// build a virtual (n+1)x(n+1) system matrix A; however, only the diagonal vectors will actually be built
		// A = diag(a) + diag(b,1) + diag(b,-1), with size(a) = n+1 and size(b) = n

		// b_i = 1/h_i+1 for i=0 to n-1
		VectorXd b = h.tail(n).cwiseInverse();

		// a_i = 2/h_i + 2/h_i+1 for i=1 to n-1
		// a_0 = 2/h_1 (see natural cubic spline)
		// a_n = 2/h_n (see natural cubic spline)
		VectorXd a(n + 1);
		a.segment(1, n-1) = 2.0 * (h.segment(1, n-1).cwiseInverse() + h.tail(n-1).cwiseInverse());
		a[0] = 2.0 / h[1];
		a[n] = 2.0 / h[n];

		// right hand side d for A*c = d
		// for i=1 to n-1
		// d_i = 3.0 * ( (y_i - y_i-1) / h2_i  +  (y_i+1 - y_i) / h2_i+1 )
		// d_0 = 3.0 * (y_1 - y_0) / h2_1
		// d_n = 3.0 * (y_n - y_n-1) / h2_n

		VectorXd h2 = h.cwiseProduct(h); // h^2

		// note: y_i - y_i-1 is stored in delta_y[i-1]
		VectorXd delta_y = y.tail(n) - y.head(n);

		// quotient is either left or right side inside the brackets
		VectorXd quotient = delta_y.cwiseQuotient(h2.tail(n));

		// build d
		VectorXd d(n + 1);
		d.segment(1, n-1) = quotient.head(n-1) + quotient.tail(n-1);
		d[0] = delta_y[0] / h2[1];
		d[n] = delta_y[n-1] / h2[n];
		d = 3.0 * d;

		// Solution of this LSE with n+1 equations and n+1 unknowns c0 to cn
		// using the Thomas algorithm for tridiagonal matrices, needs O(n)
		// typical description with vectors b1..bn, a2..an, c1..cn-1
		// 'b' is a here
		// 'a' and 'c' are b here
		// 'x' is our c
		// note: different indices, too; also we have size n+1 instead of n

		// forward preparation
		VectorXd bprime(n);
		VectorXd dprime(n+1);

		// cprime -> bprime
		bprime[0] = b[0] / a[0];
		for(int i = 1; i < (n-1); ++i) {
			bprime[i] = b[i] / (a[i] - bprime[i-1] * b[i]);
		}

		// dprime
		dprime[0] = d[0] / a[0];
		for(int i = 1; i < n; ++i) {
			dprime[i] = (d[i] - dprime[i-1] * b[i]) / (a[i] - bprime[i-1] * b[i]);
		}

		// back substitution to get x, or c in our case
		VectorXd c(n + 1);
		c[n] = dprime[n];
		for(int i = n-1; i >= 0; --i) {
			c[i] = dprime[i] - bprime[i] * c[i+1];
		}

		// we do not actually need c but
		// m1_j = m_j-1 = h_j * c_j-1,
		// m2_j = m_j   = h_j * c_j
		m1.resize(n+1);
		m2.resize(n+1);
		m1.segment(1, n) = h.tail(n).cwiseProduct( c.head(n) );
		m2.segment(1, n) = h.tail(n).cwiseProduct( c.tail(n) );
	}

};

// just a quick implementation of the function
double f(const double x) {
	return exp(sin(2.0 * M_PI * x));
}

#define MIN -3
#define MAX 3
#define N_T 61
#define N_X 600

int main() {
	// initialization
	VectorXd t = VectorXd::LinSpaced(N_T, MIN, MAX);
	VectorXd y(N_T);
	for(int i = 0; i < N_T; ++i) {
		y[i] = f(t[i]);
	}

	NatCSI ncsi(t, y);

	// test 1
	VectorXd xx = VectorXd::LinSpaced(N_X, MIN, MAX);
	VectorXd yy(N_X);
	for(int i = 0; i < N_X; ++i) {
		yy[i] = ncsi(xx[i]);
	}

	// test 2. test the binary search in find_index()
	VectorXd y2(N_T);
	for(int i = N_T-1; i >= 0; --i) {
		y2[i] = ncsi(t[i]);
	}

	// test 3. test border case (last node)
	// make sure that the 1st AND the last node are highlighted
	// in the plot (last node: see special case handling due to
	// integer division in find_index() )
	VectorXd x3(2);
	VectorXd y3(2);
	x3 << t[0], t[N_T-1];
	for(int i = 0; i < x3.size(); ++i) {
		y3[i] = ncsi(x3[i]);
	}

	// test 4. test std::vector<double> (no legend item)
	vector<double> x4 = {MIN, MAX};
	vector<double> y4 = {1.0, 1.0};

	// plot results
	MatlabPlotter p;
	p.comment("Example: natural cubic splines");
	p.comment("Code generated by natcsi.cpp");
	p.figure("Natural CSI");
	p.plot_fx("exp(sin(2.*pi.*x))", MIN, MAX, N_X, "b-");
	p.hold();
	p.plot(t, y, "b*");
	p.plot(xx, yy, "r--");
	p.plot(t, y2, "ro");
	p.plot(x3, y3, "g*");
	p.plot(x4, y4, "k:");
	p.xylabels("x", "y");
	p.legend("exp(sin(2*pi*x))", "t0 to tn", "Natural CSI", "check findindex()", "check corner cases");
	p.hold(false);

	// for testing / illustration purpose: semilog scale for y axis
	p.figure("Natural CSI. semilog scale", MatlabPlotter::SEMILOGY);
	p.plot_fx("exp(sin(2.*pi.*x))", MIN, MAX, N_X, "b-");
	p.hold();
	p.plot(t, y, "b*");
	p.plot(xx, yy, "r--");
	p.plot(t, y2, "ro");
	p.plot(x3, y3, "g*");
	p.plot(x4, y4, "k:");
	p.xylabels("x", "y (log scale)");
	p.legend("exp(sin(2*pi*x))", "t0 to tn", "Natural CSI", "check findindex()", "check corner cases");
	p.hold(false);
}