#include "libphysica/Integration.hpp"

#include <algorithm>
#include <iostream>

#include <boost/math/quadrature/gauss.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>

#include "libphysica/Special_Functions.hpp"
#include "libphysica/Statistics.hpp"

namespace libphysica
{
using namespace boost::math::quadrature;

// 1. One-dimensional MC integration
// 1.1 One-dimensional integration via adaptive Simpson method

//Function to return a reasonable precision.
double Find_Epsilon(std::function<double(double)> func, double a, double b, double precision)
{
	double c	   = (a + b) / 2;
	double h	   = b - a;
	double fa	   = func(a);
	double fb	   = func(b);
	double fc	   = func(c);
	double S	   = (h / 6) * (fa + 4 * fc + fb);
	double epsilon = precision * S;
	return epsilon;
}

double Adaptive_Simpson_Integration(std::function<double(double)> func, double a, double b, double epsilon, double S, double fa, double fb, double fc, int bottom, bool& warning)
{
	double c	  = (a + b) / 2;
	double h	  = b - a;
	double d	  = (a + c) / 2;
	double e	  = (b + c) / 2;
	double fd	  = func(d);
	double fe	  = func(e);
	double Sleft  = (h / 12) * (fa + 4 * fd + fc);
	double Sright = (h / 12) * (fc + 4 * fe + fb);
	double S2	  = Sleft + Sright;
	if(bottom <= 0 || fabs(S2 - S) <= 15 * epsilon)	  //15 due to error analysis
	{
		if(bottom <= 0 && fabs(S2 - S) > 15 * epsilon)
			warning = true;
		return S2 + (S2 - S) / 15;
	}
	else
	{
		return Adaptive_Simpson_Integration(func, a, c, epsilon / 2, Sleft, fa, fc, fd, bottom - 1, warning) + Adaptive_Simpson_Integration(func, c, b, epsilon / 2, Sright, fc, fb, fe, bottom - 1, warning);
	}
}

//Recursive functions for one-dimensional integration
void Check_Integration_Limits(double& a, double& b, double& sign)
{
	if(a > b)
	{
		std::cerr << "Warning in libphysica::Integrate(): From the integral from a to b, a>b (a = " << a << ", b = " << b << "). Sign will get swapped." << std::endl;
		std::swap(a, b);
		sign = -1.0;
	}
}

double Integrate(std::function<double(double)> func, double a, double b, double epsilon, int maxRecursionDepth)
{
	double sign = +1.0;
	if(a == b)
		return 0.0;
	else
		Check_Integration_Limits(a, b, sign);
	double c	  = (a + b) / 2;
	double h	  = b - a;
	double fa	  = func(a);
	double fb	  = func(b);
	double fc	  = func(c);
	double S	  = (h / 6) * (fa + 4 * fc + fb);
	bool warning  = false;
	double result = Adaptive_Simpson_Integration(func, a, b, fabs(epsilon), S, fa, fb, fc, maxRecursionDepth, warning);
	if(warning)
	{
		std::cout << "Warning in libphysica::Integrate(): Numerical integration on the interval (" << a << "," << b << ") did not converge to the desired precision." << std::endl;
		std::cout << "\tDesired precision: " << Round(fabs(epsilon)) << " Result: " << Round(result) << std::endl;
	}
	if(std::isnan(result))
		std::cout << "Warning in libphysica::Integrate(): Result is nan." << std::endl;
	else if(std::isinf(result))
		std::cout << "Warning in libphysica::Integrate(): Result is inf." << std::endl;
	return sign * result;
}

// 1.2 1D integration with boost functions
double Integrate(std::function<double(double)> func, double a, double b, const std::string& method)
{
	double sign = 1.0;
	if(a == b)
		return 0.0;
	else
		Check_Integration_Limits(a, b, sign);
	if(method == "Trapezoidal")
		return sign * trapezoidal(func, a, b);
	else if(method == "Gauss-Legendre")
		return sign * gauss<double, 30>::integrate(func, a, b);
	else if(method == "Gauss-Kronrod")
		return sign * gauss_kronrod<double, 31>::integrate(func, a, b, 5, 1e-9);
	else
	{
		std::cerr << "Error in libphysica::Integrate(): Method " << method << " not recognized." << std::endl;
		std::exit(EXIT_FAILURE);
	}
}

// 2. Multidimensional integration
// 2.1 Multidimensional integration via nesting 1D integration
double Integrate_2D(std::function<double(double, double)> func, double x1, double x2, double y1, double y2, const std::string& method)
{
	auto integrand_x = [&func, y1, y2, method](double x) {
		auto integrand_y = [&func, x](double y) {
			return func(x, y);
		};
		return Integrate(integrand_y, y1, y2, method);
	};
	return Integrate(integrand_x, x1, x2, method);
}

double Integrate_3D(std::function<double(double, double, double)> func, double x1, double x2, double y1, double y2, double z1, double z2, const std::string& method)
{
	auto integrand_x = [&func, y1, y2, z1, z2, method](double x) {
		auto integrand_y = [&func, z1, z2, x, method](double y) {
			auto integrand_z = [&func, x, y, method](double z) {
				return func(x, y, z);
			};
			return Integrate(integrand_z, z1, z2, method);
		};
		return Integrate(integrand_y, y1, y2, method);
	};
	return Integrate(integrand_x, x1, x2, method);
}

double Integrate_3D(std::function<double(Vector)> func, double r1, double r2, double costheta_1, double costheta_2, double phi_1, double phi_2, const std::string& method)
{
	auto integrand = [&func](double r, double cos_theta, double phi) {
		Vector rVec = Spherical_Coordinates(r, acos(cos_theta), phi);
		return r * r * func(rVec);
	};
	return Integrate_3D(integrand, r1, r2, costheta_1, costheta_2, phi_1, phi_2, method);
}

// 2.2 Monte Carlo Integration
// Reference: Some of these functions are taken from http://numerical.recipes/webnotes/nr3web9.pdf

// Utility routine used by vegas, to rebin a vector of densities contained in row j of xi into new bins defined by a vector r.
void Rebin(const double rc, const int nd, std::vector<double>& r, std::vector<double>& xin, libphysica::Matrix& xi, const int j)
{
	int i, k = 0;
	double dr = 0.0, xn = 0.0, xo = 0.0;

	for(i = 0; i < nd - 1; i++)
	{
		while(rc > dr)
			dr += r[(++k) - 1];
		if(k > 1)
			xo = xi[j][k - 2];
		xn = xi[j][k - 1];
		dr -= rc;
		xin[i] = xn - (xn - xo) * dr / r[k - 1];
	}
	for(i = 0; i < nd - 1; i++)
		xi[j][i] = xin[i];
	xi[j][nd - 1] = 1.0;
}

double Integrate_MC_Vegas(std::function<double(std::vector<double>&, const double)> func, std::vector<double>& region, const int init, const int ncall, const int itmx, const int nprn)
{
	double integral, chi2a, standard_deviation;
	// Best make everything static, allowing restarts.
	static const int NDMX = 50, MXDIM = 10;
	static const double ALPH = 1.5, TINY = 1.0e-30;
	static int i, it, j, k, mds, nd, ndo, ng, npg;
	static double calls, dv2g, dxg, f, f2, f2b, fb, rc, ti;
	static double tsi, wgt, xjac, xn, xnd, xo, schi, si, swgt;
	static std::vector<int> ia(MXDIM), kg(MXDIM);
	static std::vector<double> dt(MXDIM), dx(MXDIM), r(NDMX), x(MXDIM), xin(NDMX);
	static libphysica::Matrix d(NDMX, MXDIM), di(NDMX, MXDIM), xi(MXDIM, NDMX);

	// Initialize  captive, static random number generator
	std::random_device rd;
	std::mt19937 PRNG(rd());

	int ndim = region.size() / 2;
	if(init <= 0)
	{
		mds = ndo = 1;
		for(j = 0; j < ndim; j++)
			xi[j][0] = 1.0;
	}
	if(init <= 1)
		si = swgt = schi = 0.0;
	if(init <= 2)
	{
		nd = NDMX;
		ng = 1;
		if(mds != 0)
		{
			ng	= int(pow(ncall / 2.0 + 0.25, 1.0 / ndim));
			mds = 1;
			if((2 * ng - NDMX) >= 0)
			{
				mds = -1;
				npg = ng / NDMX + 1;
				nd	= ng / npg;
				ng	= npg * nd;
			}
		}
		for(k = 1, i = 0; i < ndim; i++)
			k *= ng;
		npg	  = std::max(int(ncall / k), 2);
		calls = double(npg) * double(k);
		dxg	  = 1.0 / ng;
		for(dv2g = 1, i = 0; i < ndim; i++)
			dv2g *= dxg;
		dv2g = calls * dv2g * calls * dv2g / npg / npg / (npg - 1.0);
		xnd	 = nd;
		dxg *= xnd;
		xjac = 1.0 / calls;
		for(j = 0; j < ndim; j++)
		{
			dx[j] = region[j + ndim] - region[j];
			xjac *= dx[j];
		}
		if(nd != ndo)
		{
			for(i = 0; i < std::max(nd, ndo); i++)
				r[i] = 1.0;
			for(j = 0; j < ndim; j++)
				Rebin(ndo / xnd, nd, r, xin, xi, j);
			ndo = nd;
		}
		if(nprn >= 0)
		{
			std::cout << " Input parameters for vegas";
			std::cout << "  ndim= " << std::setw(4) << ndim;
			std::cout << "  ncall= " << std::setw(8) << calls << std::endl;
			std::cout << std::setw(34) << "  it=" << std::setw(5) << it;
			std::cout << "  itmx=" << std::setw(5) << itmx << std::endl;
			std::cout << std::setw(34) << "  nprn=" << std::setw(5) << nprn;
			std::cout << "  ALPH=" << std::setw(9) << ALPH << std::endl;
			std::cout << std::setw(34) << "  mds=" << std::setw(5) << mds;
			std::cout << "  nd=" << std::setw(5) << nd << std::endl;
			for(j = 0; j < ndim; j++)
			{
				std::cout << std::setw(30) << " x1[" << std::setw(2) << j;
				std::cout << "]= " << std::setw(11) << region[j] << " xu[";
				std::cout << std::setw(2) << j << "]= ";
				std::cout << std::setw(11) << region[j + ndim] << std::endl;
			}
		}
	}
	for(it = 0; it < itmx; it++)
	{
		ti = tsi = 0.0;
		for(j = 0; j < ndim; j++)
		{
			kg[j] = 1;
			for(i = 0; i < nd; i++)
				d[i][j] = di[i][j] = 0.0;
		}
		for(;;)
		{
			fb = f2b = 0.0;
			for(k = 0; k < npg; k++)
			{
				wgt = xjac;
				for(j = 0; j < ndim; j++)
				{
					xn	  = (kg[j] - libphysica::Sample_Uniform(PRNG)) * dxg + 1.0;
					ia[j] = std::max(std::min(int(xn), NDMX), 1);
					if(ia[j] > 1)
					{
						xo = xi[j][ia[j] - 1] - xi[j][ia[j] - 2];
						rc = xi[j][ia[j] - 2] + (xn - ia[j]) * xo;
					}
					else
					{
						xo = xi[j][ia[j] - 1];
						rc = (xn - ia[j]) * xo;
					}
					x[j] = region[j] + rc * dx[j];
					wgt *= xo * xnd;
				}
				f  = wgt * func(x, wgt);
				f2 = f * f;
				fb += f;
				f2b += f2;
				for(j = 0; j < ndim; j++)
				{
					di[ia[j] - 1][j] += f;
					if(mds >= 0)
						d[ia[j] - 1][j] += f2;
				}
			}
			f2b = sqrt(f2b * npg);
			f2b = (f2b - fb) * (f2b + fb);
			if(f2b <= 0.0)
				f2b = TINY;
			ti += fb;
			tsi += f2b;
			if(mds < 0)
			{
				for(j = 0; j < ndim; j++)
					d[ia[j] - 1][j] += f2b;
			}
			for(k = ndim - 1; k >= 0; k--)
			{
				kg[k] %= ng;
				if(++kg[k] != 1)
					break;
			}
			if(k < 0)
				break;
		}
		tsi *= dv2g;
		wgt = 1.0 / tsi;
		si += wgt * ti;
		schi += wgt * ti * ti;
		swgt += wgt;
		integral = si / swgt;
		chi2a	 = (schi - si * integral) / (it + 0.0001);
		if(chi2a < 0.0)
			chi2a = 0.0;
		standard_deviation = sqrt(1.0 / swgt);
		tsi				   = sqrt(tsi);
		if(nprn >= 0)
		{
			std::cout << " iteration no. " << std::setw(3) << (it + 1);
			std::cout << " : integral = " << std::setw(14) << ti;
			std::cout << " +/- " << std::setw(9) << tsi << std::endl;
			std::cout << " all iterations:  "
					  << " integral =";
			std::cout << std::setw(14) << integral << "+-" << std::setw(9) << standard_deviation;
			std::cout << " chi**2/IT n =" << std::setw(9) << chi2a << std::endl;
			if(nprn != 0)
			{
				for(j = 0; j < ndim; j++)
				{
					std::cout << " DATA FOR axis  " << std::setw(2) << j << std::endl;
					std::cout << "     X      delta i          X      delta i";
					std::cout << "          X       deltai" << std::endl;
					for(i = nprn / 2; i < nd - 2; i += nprn + 2)
					{
						std::cout << std::setw(8) << xi[j][i] << std::setw(12) << di[i][j];
						std::cout << std::setw(12) << xi[j][i + 1] << std::setw(12) << di[i + 1][j];
						std::cout << std::setw(12) << xi[j][i + 2] << std::setw(12) << di[i + 2][j];
						std::cout << std::endl;
					}
				}
			}
		}
		for(j = 0; j < ndim; j++)
		{
			xo		= d[0][j];
			xn		= d[1][j];
			d[0][j] = (xo + xn) / 2.0;
			dt[j]	= d[0][j];
			for(i = 2; i < nd; i++)
			{
				rc			= xo + xn;
				xo			= xn;
				xn			= d[i][j];
				d[i - 1][j] = (rc + xn) / 3.0;
				dt[j] += d[i - 1][j];
			}
			d[nd - 1][j] = (xo + xn) / 2.0;
			dt[j] += d[nd - 1][j];
		}
		for(j = 0; j < ndim; j++)
		{
			rc = 0.0;
			for(i = 0; i < nd; i++)
			{
				if(d[i][j] < TINY)
					d[i][j] = TINY;
				r[i] = pow((1.0 - d[i][j] / dt[j]) /
							   (log(dt[j]) - log(d[i][j])),
						   ALPH);
				rc += r[i];
			}
			Rebin(rc / xnd, nd, r, xin, xi, j);
		}
	}
	return integral;
}

double Integrate_MC_Brute_Force(std::function<double(std::vector<double>&, const double)> func, std::vector<double>& region, const int ncall)
{
	int dim = region.size() / 2.0;
	std::random_device rd;
	std::mt19937 PRNG(rd());

	double volume = 1.0;
	for(int i = 0; i < dim; i++)
		volume *= (region[i + dim] - region[i]);

	double sum = 0.0;
	// double sum_2 = 0.0;
	for(int i = 0; i < ncall; i++)
	{
		std::vector<double> args(dim);
		for(int j = 0; j < dim; j++)
			args[j] = region[j] + libphysica::Sample_Uniform(PRNG) * (region[j + dim] - region[j]);
		double fct = func(args, 0.0);
		sum += volume * fct;
		// sum_2 += volume * volume * fct * fct;
	}
	double integral = sum / ncall;
	// double standard_deviation = sqrt((sum_2 / ncall - integral * integral) / (ncall - 1.0));
	// double chi2a			  = 0.0;
	return integral;
}

double Integrate_MC(std::function<double(std::vector<double>&, const double)> func, std::vector<double>& region, const int ncalls, const std::string& method)
{
	if(method == "Brute force")
		return Integrate_MC_Brute_Force(func, region, ncalls);
	else if(method == "Vegas")
	{
		int init	   = 0;
		const int itmx = 5;
		const int nprn = -1;
		return Integrate_MC_Vegas(func, region, init, ncalls, itmx, nprn);
	}
	else
	{
		std::cerr << "Error in libphysica::Integrate_MC(): Method " << method << " not recognized." << std::endl;
		std::exit(EXIT_FAILURE);
	}
}

}	// namespace libphysica


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

#include <boost/range/adaptor/reversed.hpp>

typedef unsigned long long nombre;

ENREGISTRER_PROBLEME(14, "Longest Collatz sequence") {
    // The following iterative sequence is defined for the set of positive integers:
    //
    // n -> n/2 (n is even)
    // n -> 3n + 1 (n is odd)
    //
    // Using the rule above and starting with 13, we generate the following sequence:
    //
    // 13 -> 40 -> 20 -> 10 -> 5 -> 16 -> 8 -> 4 -> 2 -> 1
    // It can be seen that this sequence (starting at 13 and finishing at 1) contains 10 terms.
    // Although it has not been proved yet (Collatz Problem), it is thought that all starting numbers finish at 1.
    //
    // Which starting number, under one million, produces the longest chain?
    //
    // NOTE: Once the chain starts the terms are allowed to go above one million.
    std::map<nombre, nombre> cache;
    cache[1] = 1;

    nombre max_longueur = 1;
    nombre max_nombre = 1;
    for (nombre n = 2; n < 1000000; ++n) {
        std::vector<nombre> chaine;
        chaine.push_back(n);
        nombre p = n;
        while (cache.find(p) == cache.end()) {
            if (p % 2 == 0)
                p /= 2;
            else
                p = 3 * p + 1;
            chaine.push_back(p);
        }

        nombre longueur = cache[p];
        for (const auto &c : boost::adaptors::reverse(chaine)) {
            cache[c] = ++longueur;
        }

        if (cache[n] > max_longueur) {
            max_longueur = cache[n];
            max_nombre = n;
        }
    }

    return std::to_string(max_nombre);
}


#pragma once

#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

//! \file rkintegrator.hpp Solution for Problem 1a, implementing RkIntegrator class

//! \brief Implements a Runge-Kutta explicit solver for a given Butcher tableau for autonomous ODEs
//! \tparam State a type representing the space in which the solution lies, e.g. R^d, represented by e.g. Eigen::VectorXd.
template <class State>
class RKIntegrator {
public:
    //! \brief Constructor for the RK method.
    //! Performs size checks and copies A and b into internal storage
    //! \param[in] A matrix containing coefficents of Butcher tableau, must be (strictly) lower triangular (no check)
    //! \param[in] b vector containing coefficients of lower part of Butcher tableau
    RKIntegrator(const Eigen::MatrixXd & A, const Eigen::VectorXd & b): A_(A),b_(b){
		n=A_.rows();
		assert(n==b_.size() && "size missmatch custom error");
		assert(A_.rows()==A_.cols() && "matrix not square custom error");

        // TODO: implement size checks and initialize internal data (DONE?)
    }

    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using a RK scheme given in the Butcher tableau provided in the
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function. Must have State operator()(State x)
    //! \param[in] f function handle for rhs in y' = f(y), e.g. implemented using lambda funciton
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function>
    std::vector<State> solve(const Function &f, double T, const State & y0, unsigned int N) const {
		double h = T/N;
		std::vector<State> y_f;
		y_f.reserve(N);
		y_f.push_back(y0);

		State ytemp1 = y0;
        State ytemp2 = y0;
        // Pointers to swap previous value
        State * yold = &ytemp1;
        State * ynew = &ytemp2;

		for (int i =1; i<N;++i){
			step(f,h,*yold,*ynew);
			y_f.push_back(*ynew);
			std::swap(yold,ynew);
		}
		return y_f;
        // TODO: implement solver from 0 to T, calling function step appropriately
    }

private:

    //! \brief Perform a single step of the RK method for the solution of the autonomous ODE
    //! Compute a single explicit RK step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs. Must have State operator()(State x)
    //! \param[in] f function handle for ths f, s.t. y' = f(y)
    //! \param[in] h step size
    //! \param[in] y0 initial state
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function>
    void step(const Function &f, double h, const State & y0, State & y1) const {
		y1 = y0;
		std::vector<State> k;
        k.reserve(n);
		for (int i=0;i<n;++i){
			State incr=y0;
			for (int j=0;j<i;++j){
				incr+=h*A_(i,j)*k.at(j);
			}
			k.push_back(f(incr));
			y1+=h*b_(i)*k.back();
		}
        // TODO: implement a single step of the RK method using provided Butcher scheme
    }

    //! TODO: put here suitable internal data storage
    const Eigen::VectorXd b_;
    const Eigen::MatrixXd A_;
    unsigned n;
};

Nummerical Methods for CSE/PS12/rkintegrator_cus.hpp


#pragma once

#include <vector>
#include <cassert>
#include <iostream>
#include <iomanip>
#include <Eigen/Dense>

#include "dampnewton.hpp"

//! \file implicit_rkintegrator.hpp Solution for Problem 1, implementing implicit_RkIntegrator class

//! \brief Compute the Kronecker product $C = A \otimes B$.
//! \param[in] A Matrix $m \times n$
//! \param[in] B Matrix $l \times k$
//! \param[out] C Kronecker product of A and B of dim $ml \times nk$
Eigen::MatrixXd kron(const Eigen::MatrixXd & A, const Eigen::MatrixXd & B)
{
    Eigen::MatrixXd C(A.rows()*B.rows(), A.cols()*B.cols());
    for(unsigned int i = 0; i < A.rows(); ++i) {
        for(unsigned int j = 0; j < A.cols(); ++j) {
            C.block(i*B.rows(),j*B.cols(), B.rows(), B.cols()) = A(i,j)*B;
        }
    }
    return C;
}

//! \brief Implements a Runge-Kutta implicit solver for a given Butcher tableau for autonomous ODEs

class implicit_RKIntegrator {
public:
    //! \brief Constructor for the implicit RK method.
    //! Performs size checks and copies A and b into internal storage
    //! \param[in] A matrix containing coefficents of Butcher tableau, must be (strictly) lower triangular (no check)
    //! \param[in] b vector containing coefficients of lower part of Butcher tableau
    implicit_RKIntegrator(const Eigen::MatrixXd & A, const Eigen::VectorXd & b)
        : A(A), b(b), s(b.size()) {
        assert( A.cols() == A.rows() && "Matrix must be square.");
        assert( A.cols() == b.size() && "Incompatible matrix/vector size.");
    }

    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using an implicit RK scheme given in the Butcher tableau provided in the
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function. Must have Eigen::VectorXd operator()(Eigen::VectorXd x)
    //! \tparam Function2 type for function implementing the Jacobian of f. Must have Eigen::MatrixXd operator()(Eigen::VectorXd x)
    //! \param[in] f function handle for rhs in y' = f(y), e.g. implemented using lambda funciton
    //! \param[in] Jf function handle for Jf, e.g. implemented using lambda funciton
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function, class Function2>
    std::vector<Eigen::VectorXd> solve(const Function &f, const Function2 &Jf, double T, const Eigen::VectorXd & y0, unsigned int N) const {
        // Iniz step size
        double h = T / N;

        // Will contain all steps, reserve memory for efficiency
        std::vector<Eigen::VectorXd> res;
        res.reserve(N+1);

        // Store initial data
        res.push_back(y0);

        // Initialize some memory to store temporary values
        Eigen::VectorXd ytemp1 = y0;
        Eigen::VectorXd ytemp2 = y0;
        // Pointers to swap previous value
        Eigen::VectorXd * yold = &ytemp1;
        Eigen::VectorXd * ynew = &ytemp2;

        // Loop over all fixed steps
        for(unsigned int k = 0; k < N; ++k) {
            // Compute, save and swap next step
            step(f, Jf, h, *yold, *ynew);
            res.push_back(*ynew);
            std::swap(yold, ynew);
        }
        return res;
    }

private:

    //! \brief Perform a single step of the RK method for the solution of the autonomous ODE
    //! Compute a single explicit RK step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs. Must have Eigen::VectorXd operator()(Eigen::VectorXd x)
    //! \tparam Function2 type for function implementing the Jacobian of f. Must have Eigen::MatrixXd operator()(Eigen::VectorXd x)
    //! \param[in] f function handle for ths f, s.t. y' = f(y)
    //! \param[in] Jf function handle for Jf, e.g. implemented using lambda funciton
    //! \param[in] h step size
    //! \param[in] y0 initial Eigen::VectorXd
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function, class Function2>
    void step(const Function &f, const Function2 &Jf, double h, const Eigen::VectorXd & y0, Eigen::VectorXd & y1) const {

        int d = y0.size();

        // Handle for the function F describing the equation satisfied by the stages g
        auto F = [y0, h, d, this, f] (Eigen::VectorXd gv) {
            Eigen::VectorXd Fv = gv;
            for (int j = 0; j < s; j++)
                Fv = Fv - h*kron(A.col(j),Eigen::MatrixXd::Identity(d,d))*f(y0+gv.segment(j*d,d));
            return Fv;
        };

        // Handle for the Jacobian of F.
        auto JF = [y0, h, d, Jf, this] (Eigen::VectorXd gv) {
            Eigen::MatrixXd DF(s*d,s*d);
            for (int j = 0; j < s; j++)
                DF.block(0,j*d,s*d,d) = kron(A.col(j),Eigen::MatrixXd::Identity(d,d))*Jf(y0+gv.segment(j*d,d));
            DF = Eigen::MatrixXd::Identity(s*d,s*d) - h*DF;
            return DF;
        };

        // Obtain stages with damped Newton method
        Eigen::VectorXd gv = Eigen::VectorXd::Zero(s*d);
        dampnewton(F, JF, gv);

        // Calculate y1
        Eigen::MatrixXd K(d,s);
        for (int j = 0; j < s; j++) K.col(j) = f(y0+gv.segment(j*d,d));
        y1 = y0 + h*K*b;
    }

    //! Matrix A in Butcher scheme
    const Eigen::MatrixXd A;
    //! Vector b in Butcher scheme
    const Eigen::VectorXd b;
    //! Size of Butcher matrix and vector A and b
    unsigned int s;
};

Nummerical Methods for CSE/PS14/solutions_ps14/implicit_rkintegrator.hpp


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use the general purpose non-linear
    least squares optimization routines from the dlib C++ Library.

    This example program will demonstrate how these routines can be used for data fitting.
    In particular, we will generate a set of data and then use the least squares
    routines to infer the parameters of the model which generated the data.
*/

#include <dlib/optimization.h>
#include <iostream>
#include <vector>

using namespace std;
using namespace dlib;

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

typedef matrix<double,2,1> input_vector;
typedef matrix<double,3,1> parameter_vector;

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

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
    const input_vector& input,
    const parameter_vector& params
)
{
    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = input(0);
    const double i1 = input(1);

    const double temp = p0*i0 + p1*i1 + p2;

    return temp*temp;
}

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

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

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

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    const double temp = p0*i0 + p1*i1 + p2;

    der(0) = i0*2*temp;
    der(1) = i1*2*temp;
    der(2) = 2*temp;

    return der;
}

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

int main()
{
    try
    {
        // randomly pick a set of parameters to use in this example
        const parameter_vector params = 10*randm(3,1);
        cout << "params: " << trans(params) << endl;

        // Now let's generate a bunch of input/output pairs according to our model.
        std::vector<std::pair<input_vector, double> > data_samples;
        input_vector input;
        for (int i = 0; i < 1000; ++i)
        {
            input = 10*randm(2,1);
            const double output = model(input, params);

            // save the pair
            data_samples.push_back(make_pair(input, output));
        }

        // Before we do anything, let's make sure that our derivative function defined above matches
        // the approximate derivative computed using central differences (via derivative()).
        // If this value is big then it means we probably typed the derivative function incorrectly.
        cout << "derivative error: " << length(residual_derivative(data_samples[0], params) -
                                               derivative(residual)(data_samples[0], params) ) << endl;

        // Now let's use the solve_least_squares_lm() routine to figure out what the
        // parameters are based on just the data_samples.
        parameter_vector x;
        x = 1;

        cout << "Use Levenberg-Marquardt" << endl;
        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               residual_derivative,
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = 1;
        cout << "Use Levenberg-Marquardt, approximate derivatives" << endl;
        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               derivative(residual),
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = 1;
        cout << "Use Levenberg-Marquardt/quasi-newton hybrid" << endl;
        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        solve_least_squares(objective_delta_stop_strategy(1e-7).be_verbose(),
                            residual,
                            residual_derivative,
                            data_samples,
                            x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;

    }
    catch (std::exception& e)
    {
        cout << e.what() << endl;
    }
}

// Example output:
/*
params: 8.40188 3.94383 7.83099

derivative error: 9.78267e-06
Use Levenberg-Marquardt
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.39172e+10
iteration: 3   objective: 1.57036e+09
iteration: 4   objective: 2.66917e+07
iteration: 5   objective: 4741.9
iteration: 6   objective: 0.000238674
iteration: 7   objective: 7.8815e-19
iteration: 8   objective: 0
inferred parameters: 8.40188 3.94383 7.83099

solution error:      0

Use Levenberg-Marquardt, approximate derivatives
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.39172e+10
iteration: 3   objective: 1.57036e+09
iteration: 4   objective: 2.66917e+07
iteration: 5   objective: 4741.87
iteration: 6   objective: 0.000238701
iteration: 7   objective: 1.0571e-18
iteration: 8   objective: 4.12469e-22
inferred parameters: 8.40188 3.94383 7.83099

solution error:      5.34754e-15

Use Levenberg-Marquardt/quasi-newton hybrid
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.3917e+10
iteration: 3   objective: 1.5572e+09
iteration: 4   objective: 2.74139e+07
iteration: 5   objective: 5135.98
iteration: 6   objective: 0.000285539
iteration: 7   objective: 1.15441e-18
iteration: 8   objective: 3.38834e-23
inferred parameters: 8.40188 3.94383 7.83099

solution error:      1.77636e-15
*/


#include "Math/math_3dh.hpp"
#include <Eigen/Dense>
using Eigen::MatrixXf;
using Eigen::VectorXf;

namespace math_3dh {
float multiquadric_rbf(float r) {
  float R = 1.0f; // smoothing parameter
  float e = 3.0f; // shape parameter
  return -sqrtf(R + powf(e * r, 2));
}
float rbf_interp(glm::vec2 point, std::vector<glm::vec3> data,
                 float (*rbf)(float)) {
  // Construct A
  size_t n = data.size();
  MatrixXf A(n, n);
  for (size_t j = 0; j < n; j++) {
    for (size_t i = 0; i < n; i++) {
      if (i == j) {
        A(i, j) = 0.0f;
      } else {
        A(i, j) = rbf(glm::length(glm::vec2(data[i]) - glm::vec2(data[j])));
      }
    }
  }
  // Construct z
  VectorXf z(n);
  for (size_t i = 0; i < n; i++) {
    z(i) = data[i].z;
  }
  // Construct p
  VectorXf p(n);
  for (size_t i = 0; i < n; i++) {
    p(i) = rbf(glm::length(point - glm::vec2(data[i])));
  }
  // elevation(point) = p*(A^(-1)z) = p*w
  VectorXf w = A.partialPivLu().solve(z);
  return p.dot(w);
}
} // namespace math_3dh


#include <cmath>
#include <iostream>
#include <fstream>
#include <OpenEXR/ImathVec.h>
#include <boost/format.hpp>

/*!
 * see http://cubic.org/docs/hermite.htm
 *
 */
namespace interpolate
{
	template <typename T> void hermite(const Imath::Vec3<T>& P1,
									   const Imath::Vec3<T>& T1,
									   const Imath::Vec3<T>& P2,
									   const Imath::Vec3<T>& T2,
									   T s,
									   Imath::Vec3<T>& P)
	{
		T h1 =  2.0*std::pow(s,3.0) - 3.0*std::pow(s,2.0) + 1.0; // calculate basis function 1
		T h2 = -2.0*std::pow(s,3.0) + 3.0*std::pow(s,2.0)      ; // calculate basis function 2
		T h3 =  std::pow(s,3.0)     - 2.0*std::pow(s,2.0) + s  ; // calculate basis function 3
		T h4 =  std::pow(s,3.0)     - std::pow(s,2.0)          ; // calculate basis function 4

		P = h1*P1 + // multiply and sum all funtions
			h2*P2 + // together to build the interpolated
			h3*T1 + // point along the curve.
			h4*T2;
	}
}

// == Emacs ================
// -------------------------
// Local variables:
// tab-width: 4
// indent-tabs-mode: t
// c-basic-offset: 4
// end:
//
// == vi ===================
// -------------------------
// Format block
// ex:ts=4:sw=4:expandtab
// -------------------------


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

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

#include <iostream>

#include <boost/array.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/ring.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/util/write_dsv.hpp>

struct triangle : public boost::array<boost::geometry::model::d2::point_xy<double>, 4>
{
    inline void close()
    {
        (*this)[3] = (*this)[0];
    }
};

// Register triangle as a ring
BOOST_GEOMETRY_REGISTER_RING(triangle)

// Specializations of algorithms, where useful. If not specialized the default ones
// (for linear rings) will be used for triangle. Which is OK as long as the triangle
// is closed, that means, has 4 points (the last one being the first).
namespace boost { namespace geometry {

template<>
inline double area<triangle>(const triangle& t)
{
    /*         C
              / \
             /   \
            A-----B

           ((Bx - Ax) * (Cy - Ay)) - ((Cx - Ax) * (By - Ay))
           -------------------------------------------------
                                   2
    */

    return 0.5 * ((t[1].x() - t[0].x()) * (t[2].y() - t[0].y())
                - (t[2].x() - t[0].x()) * (t[1].y() - t[0].y()));
}

}} // namespace boost::geometry

int main()
{
    triangle t;

    t[0].x(0);
    t[0].y(0);
    t[1].x(5);
    t[1].y(0);
    t[2].x(2.5);
    t[2].y(2.5);

    t.close();

    std::cout << "Triangle: " << boost::geometry::dsv(t) << std::endl;
    std::cout << "Area: " << boost::geometry::area(t) << std::endl;

    boost::geometry::model::d2::point_xy<double> c;
    boost::geometry::centroid(t, c);
    std::cout << "Centroid: " << boost::geometry::dsv(c) << std::endl;

    return 0;
}


#include <iostream>
#include <fstream>
#include <vector>

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

using Matrix3d = Eigen::Matrix3d;
using Vector3d = Eigen::Vector3d;

Matrix3d euler_to_mat( const double roll,
                        const double pitch,
                        const double yaw )
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q.matrix();
}

Matrix3d qua_integration(const Matrix3d &R0, const std::vector<Vector3d> &w_vec)
{
    std::cout << "============= qua_integration =============" << std::endl;

    Eigen::Quaterniond q(R0);

    for(int i = 0; i < w_vec.size(); ++i) {
        Vector3d omega_measured_ = w_vec[i];
        constexpr double DT = 1.0;

        Eigen::Quaterniond q_new;
        Eigen::Quaterniond q_add;

        // Method 1
        // Eigen::Quaterniond q_omega;
        // q_omega.w() = 0;
        // q_omega.vec() = omega_measured_ * DT * 0.5;
        // q_add = q_omega * q;
        // q_new.w() = q.w() + q_add.w();
        // q_new.vec() = q.vec() + q_add.vec();

        // Method 2
        Eigen::Vector3d rotated = omega_measured_ * DT;
        double angle = rotated.norm();
        Eigen::Vector3d axis = rotated.normalized();
        q_add = Eigen::AngleAxisd(angle, axis);
        q_new = q * q_add;

        q = q_new;

        std::cout << "i:" << i << " w:" << omega_measured_.transpose() << "\nR:" << q.matrix() << std::endl;
    }
    return q.matrix();
}

inline Eigen::Matrix3d skewm(const Eigen::Vector3d& v)
{
    double x = v(0);
    double y = v(1);
    double z = v(2);

    Eigen::Matrix3d S;
    S << 0.0, -z, y, z, 0.0, -x, -y, x, 0.0;

    return S;
}

inline Eigen::Matrix3d exp_hat_so3(const Eigen::Vector3d& v)
{
    const double theta = v.norm();
    if (theta < 1e-10)
    {
        return Matrix3d::Identity();
    }
    const Vector3d w = v / theta;
    Matrix3d W = skewm(w);
    // NOTE(Ning): W*W = -I + w * w^T
    // NOTE(Ning): Rodrigues rotation formula
    Matrix3d SO3 = Matrix3d::Identity() + std::sin(theta) * W + (1 - std::cos(theta)) * W * W;
    return SO3;
}

Matrix3d mat_integration(const Matrix3d &R0, const std::vector<Vector3d> &w_vec) {
    std::cout << "============= mat_integration =============" << std::endl;
    Eigen::Matrix3d R = R0;
    for(int i = 0; i < w_vec.size(); ++i) {
        Vector3d w = w_vec[i];
        R = R * exp_hat_so3(w);
        std::cout << "i:" << i << " w:" << w.transpose() << "\nR:" << R << std::endl;
    }

    return R;
}

void test1()
{
    std::cout << "test1" << std::endl;
    std::vector<Eigen::Vector3d> w_vec =
        {
            {0,0,0},
            {1,0,0},
            {0,1,0},
            {0,0,1},
            {1,1,1},
            {-1,-2,-3}
        };

    Matrix3d R0 = euler_to_mat(0.1,0.2,-0.1);

    mat_integration(R0, w_vec);

    qua_integration(R0, w_vec);

    std::cout << std::endl;
}

void test2()
{
    std::cout << "test2" << std::endl;

    std::vector<Eigen::Vector3d> w_vec =
    {
        {0,0,0},
        {0,0,0}
    };

    Matrix3d R0 = euler_to_mat(0.0,0.0,0.0);

    mat_integration(R0, w_vec);

    qua_integration(R0, w_vec);

    std::cout << std::endl;
}

void test3()
{
    std::cout << "test3" << std::endl;

    std::vector<Eigen::Vector3d> w_vec =
    {
        {0.01,0.01,0.01},
        {0.01,0.01,0.01},
        {0.01,0.01,0.01},
        {0.01,0.01,0.01}
    };

    Matrix3d R0 = euler_to_mat(0.0,0.0,0.0);

    mat_integration(R0, w_vec);

    qua_integration(R0, w_vec);

    std::cout << std::endl;
}

int main() {
    test1();
    test2();
    test3();
}


#include "writer.hpp"
#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>
#include <tuple>
#include <utility>

// Simpson integrator
inline double integrate(std::function<double(double)> f, double a, double b) {
	return (b-a) / 6 * (f(a) + 4 * f((a + b) / 2) + f(b));
}

//----------------GodunovBegin----------------
/// @param[in] N the number of grid points, NOT INCLUDING BOUNDARY POINTS
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[out] u solution at time T
/// @param[out] X Grid Points
void Godunov(int N, double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, Eigen::VectorXd &u, Eigen::VectorXd &X) {
	// Create space discretization for interval [-2,2]
	// (write your solution here)
	u.resize(N + 1);
	X.setLinSpaced(N + 2, -2, 2);
	double dx = (X.maxCoeff() - X.minCoeff()) / (N + 1);

	// The Godunov flux
	// (write your solution here)
	// $F_{j + \frac{1}{2}}^n = \max(f(max(U_j^n, \omega)), f(min(U_{j + 1}^n, \omega)))$
	// where $\omega$ minimum of $f$
	auto F = [&f](double a, double b) -> double {
		using std::max;
		using std::min;
		// TODO: find minimum of f
		double omega{0};
		return max(f(max(a, omega)), f(min(b, omega)));
	};

	//setup vectors to store (old) solution
	// (write your solution here)
	for (int i = 0; i < N + 1; ++i) {
		u(i) = 1 / dx * integrate(u0, X(i), X(i + 1));
	}
	Eigen::VectorXd u_old{u};

	// choose dt such that if obeys CFL condition
	// (write your solution here)
	double t = 0;

	//Please uncomment the next line:
	while (t < T) {
		// Update dT according to current CFL condition
		// (write your solution here)
		// $\increment t = \frac{\increment x}{2 \max_j \abs{f'(U_j^n)}$
		double dt = std::min(T - t, dx / (2 * u.unaryExpr([&df](double x) -> double {
			                       return std::abs(df(x));
			                     }).maxCoeff()));

		// Update current time
		// (write your solution here)
		t += dt;

		// Update the internal values of u
		// (write your solution here)
		// $U_j^{n + 1} = U_j^n - \frac{\increment t}{\increment x} \left( F_{j + \frac{1}{2}}^n - F_{j - \frac{1}{2}}^n \right)$
		std::swap(u, u_old);
		for (int i = 1; i < N; ++i) {
			u(i) = u_old(i) - dt / dx * (F(u_old(i), u_old(i + 1)) - F(u_old(i - 1), u_old(i)));
		}

		// Update boundary with non-reflecting Neumann bc
		// (write your solution here)
		u(0) = u(1);
		u(N) = u(N - 1);

		//Please uncomment the next line:
	}
}
//----------------GodunovEnd----------------

//----------------convGodBegin----------------
//! Computes error for a range of cell lengths and stores them to error vectors
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[in] uex exact solution
/// @param[in] baseName string containing name to save the computed errors and resolution
void GodunovConvergence(double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, const std::function<double(double, double)> &uex, const std::string &baseName) {
	std::vector<int>    resolutions = {100, 200, 400, 800, 1600};
	std::vector<double> L1_errors;
	std::vector<double> Linf_errors;

	for (auto &N : resolutions) {
		// find approximate solution using Godunov scheme
		// (write your solution here)
		Eigen::VectorXd u;
		Eigen::VectorXd X;
		Godunov(N, T, f, df, u0, u, X);

		// compute errors and push them bach to the corresponfing error vectors
		// (write your solution here)
		double L1_error{0};
		double Linf_error{0};
		for (int i = 0; i < N + 1; ++i) {
			double dx = X(i + 1) - X(i);
			double error = std::abs(u(i) - 1.0 / dx * integrate([&](double x) -> double {return uex(x, T);}, X(i), X(i + 1)));
			L1_error += dx * error;
			Linf_error = std::max(Linf_error, error);
		}
		L1_errors.emplace_back(L1_error);
		Linf_errors.emplace_back(Linf_error);
	}

	writeToFile(baseName + "_L1errors_Godunov.txt", L1_errors);
	writeToFile(baseName + "_Linferrors_Godunov.txt", Linf_errors);
	writeToFile(baseName + "_resolutions.txt", resolutions);
}
//----------------convGodEnd----------------

//----------------LFBegin----------------
/// @param[in] N the number of grid points, NOT INCLUDING BOUNDARY POINTS
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[out] u solution at time T
/// @param[out] X Grid Points
void LaxFriedrichs(int N, double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, Eigen::VectorXd &u, Eigen::VectorXd &X) {
	std::ignore = df;
	// Create space discretization for interval [-2,2]
	// (write your solution here)
	double CFL = 0.5;
	X.setLinSpaced(N + 2, -2, 2);
	double dx  = (X.maxCoeff() - X.minCoeff()) / (N + 1);

	//setup vectors to store solution
	// (write your solution here)
	u.resize(N + 1);
	for (int i = 0; i < N + 1; ++i) {
		u(i) = 1 / dx * integrate(u0, X(i), X(i + 1));
	}
	Eigen::VectorXd u_old{u};

	// choose dt such that if obeys CFL condition
	// (write your solution here)
	double t  = 0;
	double dt;

	// The Lax-Friedrichs flux
	// (write your solution here)
	auto F = [&](double a, double b) -> double {
		return (f(a) + f(b)) / 2 - dx / (2 * dt) * (b - a);
	};

	//Please uncomment the next line:
	while (t < T) {
		// Update dT according to current CFL condition
		// (write your solution here)
		dt = std::min(T - t, CFL * dx / u.unaryExpr([&df](double x) -> double {
			                       return std::abs(df(x));
			                     }).maxCoeff());

		// Update current time
		// (write your solution here)
		t += dt;

		// Update the internal values of u
		// (write your solution here)
		std::swap(u, u_old);
		for (int i = 1; i < N; ++i) {
			u(i) = u_old(i) - dt / dx * (F(u_old(i), u_old(i + 1)) - F(u_old(i - 1), u_old(i)));
		}

		// Update boundary with non-reflecting Neumann bc
		// (write your solution here)
		u(0) = u(1);
		u(N) = u(N - 1);

		//Please uncomment the next line:
	}
}
//----------------LFEnd----------------

//----------------convLFBegin----------------
//! Computes error for a range of cell lengths and stores them to error vectors
/// @param[in] T the final time at which to compute the solution
/// @param[in] f flux function, as function
/// @param[in] df derivative of flux function, as function
/// @param[in] u0 the initial conditions, as function
/// @param[in] uex exact solution
/// @param[in] baseName string containing name to save the computed errors and resolution
void LFConvergence(double T, const std::function<double(double)> &f, const std::function<double(double)> &df, const std::function<double(double)> &u0, const std::function<double(double, double)> &uex, const std::string &baseName) {
	std::vector<int>    resolutions = {100, 200, 400, 800, 1600};
	std::vector<double> L1_errors;
	std::vector<double> Linf_errors;

	for (auto &N : resolutions) {
		// find approximate solution using Lax-Friedrichs scheme
		// (write your solution here)
		Eigen::VectorXd u;
		Eigen::VectorXd X;
		LaxFriedrichs(N, T, f, df, u0, u, X);

		// compute errors and push them bach to the corresponfing error vectors
		// (write your solution here)
		double L1_error{0};
		double Linf_error{0};
		for (int i = 0; i < N + 1; ++i) {
			double dx = X(i + 1) - X(i);
			double error = std::abs(u(i) - 1.0 / dx * integrate([&](double x) -> double {return uex(x, T);}, X(i), X(i + 1)));
			L1_error += dx * error;
			Linf_error = std::max(Linf_error, error);
		}
		L1_errors.emplace_back(L1_error);
		Linf_errors.emplace_back(Linf_error);
	}

	writeToFile(baseName + "_L1errors_LF.txt", L1_errors);
	writeToFile(baseName + "_Linferrors_LF.txt", Linf_errors);
	writeToFile(baseName + "_resolutions.txt", resolutions);
}
//----------------convLFEnd----------------

/* Fluxes for Burgers' equation */
double fBurgers(double u) {
	return std::pow(u, 2) / 2.;
}

double dfBurgers(double u) {
	return u;
}

/* Initial data and exact solutions for Burgers' equation  */
// i)
double U0i(double x) {
	if (x < 0.)
		return 1.;
	else
		return 0.;
}
double Uexi(double x, double t) {
	if (x < 0.5 * t)
		return 1.;
	else
		return 0.;
}

// ii)
double U0ii(double x) {
	if (x < 0.)
		return 0.;
	else
		return -2.;
}
double Uexii(double x, double t) {
	if (x < -t)
		return 0.;
	else
		return -2.;
}

// iii)
double U0iii(double x) {
	if (x < 0.)
		return 0.;
	else
		return 1.;
}
double Uexiii(double x, double t) {
	if (x < 0)
		return 0.;
	else if (x < t && t < 2)
		return x / t;
	else
		return 1.;
}

// iv)
double U0iv(double x) {
	if (0. < x && x < 1.)
		return 1.;
	else
		return 0.;
}
double Uexiv(double x, double t) {
	if (x <= 0)
		return 0.;
	else if ((x <= t && t <= 2) || (x < std::sqrt(2 * t) && t > 2))
		return x / t;
	else if (t <= 2 && t < x && (x < 1 + t / 2.))
		return 1.;
	else
		return 0.;
}

/* Flux for Buckley-Leverett equation*/
double fBL(double u) {
	return std::pow(u, 2) / (std::pow(u, 2) + std::pow(1 - u, 2));
}

double dfBL(double u) {
	return (2 * u * (u * u + (1 - u) * (1 - u)) - u * u * (2 * u - 2 * (1 - u))) / std::pow(u * u + (1 - u) * (1 - u), 2);
}

/* Initial data for Buckley-Leverett */
double U0BL(double x) {
	if (x < 0.)
		return 0.1;
	else
		return 0.9;
}

int main(int, char **) {
	double T = 0.8;
	int    N = 400;

	Eigen::VectorXd u, X;
	// Test for Burgers with initiald data i
	Godunov(N, T, fBurgers, dfBurgers, U0i, u, X);
	writeToFile("uBi_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0i, u, X);
	writeToFile("uBi_LF.txt", u);

	Godunov(N, T, fBurgers, dfBurgers, U0ii, u, X);
	writeToFile("uBii_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0ii, u, X);
	writeToFile("uBii_LF.txt", u);

	Godunov(N, T, fBurgers, dfBurgers, U0iii, u, X);
	writeToFile("uBiii_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0iii, u, X);
	writeToFile("uBiii_LF.txt", u);

	Godunov(N, T, fBurgers, dfBurgers, U0iv, u, X);
	writeToFile("uBiv_G.txt", u);
	LaxFriedrichs(N, T, fBurgers, dfBurgers, U0iv, u, X);
	writeToFile("uBiv_LF.txt", u);

	// compute convergence for Burgers with initial data i
	GodunovConvergence(T, fBurgers, dfBurgers, U0i, Uexi, "Burgers1");
	LFConvergence(T, fBurgers, dfBurgers, U0i, Uexi, "Burgers1");

	// compute convergence for Burgers with initial data ii
	GodunovConvergence(T, fBurgers, dfBurgers, U0ii, Uexii, "Burgers2");
	LFConvergence(T, fBurgers, dfBurgers, U0ii, Uexii, "Burgers2");

	// compute convergence for Burgers with initial data iii
	GodunovConvergence(T, fBurgers, dfBurgers, U0iii, Uexiii, "Burgers3");
	LFConvergence(T, fBurgers, dfBurgers, U0iii, Uexiii, "Burgers3");

	// compute convergence for Burgers with initial data iv
	GodunovConvergence(T, fBurgers, dfBurgers, U0iv, Uexiv, "Burgers4");
	LFConvergence(T, fBurgers, dfBurgers, U0iv, Uexiv, "Burgers4");

	// Test for Buckley-Leverett
	Godunov(N, T, fBL, dfBL, U0BL, u, X);
	writeToFile("uBL_G.txt", u);
	LaxFriedrichs(N, T, fBL, dfBL, U0BL, u, X);
	writeToFile("uBL_LF.txt", u);
}


#pragma once
#include "H1_norm.hpp"
#include "L2_norm.hpp"
#include "fem_solve.hpp"
#include "writer.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <functional>
#include <igl/readMESH.h>
#include <vector>

//----------------convBegin----------------
//! Compute the L2 and H1-error for different meshes using quadratic FEM
//!
//! @param baseMeshName the basename for the mesh (eg. Square).
//!                     the method will automatically append _<n>.stl
//!
//! @param maxLevel the max mesh level to use. Will read up to and including
//!                 baseMeshName_<maxLevel>.stl
//!
//! @param f the function f as in the exercise (LHS)
//! @param exactSol the exact solution (found by eg. hand computation or googling).
//! @param exactSol_grad the gradient of the exact solution.
void convergenceAnalysis(const std::string &baseMeshName, int maxLevel, const std::function<double(double, double)> f, const std::function<double(double, double)> exactSol, const std::function<Eigen::Vector2d(double, double)> exactSol_grad) {
	std::vector<double> differences_L2;
	std::vector<double> differences_H1;
	std::vector<int>    numberOfDegreesOfFreedom;
	for (int i = 0; i <= maxLevel; i++) {
		Vector          u;
		Eigen::MatrixXd vertices;
		Eigen::MatrixXi triangles;
		Eigen::MatrixXi tetrahedra;

		std::stringstream basenameSS;
		basenameSS << baseMeshName << "_" << i;
		std::string basename = basenameSS.str();

		igl::readMESH(std::string(NPDE_DATA_PATH)
		                  + basename
		                  + ".mesh",
		              vertices,
		              tetrahedra,
		              triangles);

		// Initialize quadratic Dofs
		QDofs quadraticDofs(vertices, triangles);
		// get dofs
		Eigen::MatrixXi dofs;
		quadraticDofs.get_dofs(dofs);

		std::cout << "Computing convergence. At: " << basename << std::endl;
		// solve finite element system
		// (write your solution here)
		numberOfDegreesOfFreedom.push_back(solveFiniteElement(u, quadraticDofs, f));

		//compute L2-error and save it in the corresponding vector
		// (write your solution here)
		differences_L2.push_back(computeL2Difference(vertices, dofs, u, exactSol));

		//compute H1-error and save it in the corresponding vector
		// (write your solution here)
		differences_H1.push_back(computeH1Difference(vertices, dofs, u, exactSol_grad));

		// store number of dofs in vector
		// (write your solution here)

		writeToFile(basename + "_values.txt", u);
		writeMatrixToFile(basename + "_vertices.txt", vertices);
		writeMatrixToFile(basename + "_triangles.txt", triangles);
	}

	std::stringstream errorL2Filename;
	errorL2Filename << baseMeshName << "_errors.txt";

	std::stringstream errorH1Filename;
	errorH1Filename << baseMeshName << "_errorsH1.txt";

	writeToFile(errorL2Filename.str(), differences_L2);
	writeToFile(errorH1Filename.str(), differences_H1);
	writeToFile(baseMeshName + "_resolutions.txt", numberOfDegreesOfFreedom);
}
//----------------convEnd----------------


/*
 * test.cpp

 *
 *  Created on: Oct 14, 2017
 *      Author: nmsutton
 */

#include "test.h"
//#include <iostream>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

/*
 * uncomment out below lines to get plotting working
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
 */

test::test()
{

}

test::~test()
{
}

/* we solve the simple ODE x' = 3/(2t^2) + x/(2t)
 * with initial condition x(1) = 0.
 * Analytic solution is x(t) = sqrt(t) - 1/t
 */

void test::rhs( const double x , double &dxdt , const double t )
{
    dxdt = 3.0/(2.0*t*t) + x/(2.0*t);
}

void test::write_cout( const double &x , const double t )
{
    cout << t << '\t' << x << endl;
    //x_data.push_back(x[0]);
}

// state_type = double
typedef runge_kutta_dopri5< double > stepper_type;

void test::run_test()
{
    double x = 0.0;
    runge_kutta_dopri5< double > rk2;
    //integrate_adaptive( make_controlled( 1E-12 , 1E-12 , stepper_type() ) , rhs , x , 1.0 , 10.0 , 0.1 , write_cout );
	double t = 1.0;
	const double dt = 0.1;//0.0025;
	int time_span = 90;

	//integrate_const( rk2 , rhs , x , 1.0 , 10.0 , 0.1, write_cout );
	for (int i = 0; i < time_span; i++) {
		t += dt;
		x_data.push_back(x);
		//y_data.push_back(0);
		integrate_const( rk2 , rhs , x , t , (t+dt) , dt);//, write_cout );
	}
	/*
	 * uncomment out below lines to get plotting working
    plt::plot(x_data);
    plt::show();
    */
}


#include <cstdlib>
#include <tuple>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>

namespace LinearRegression
{
	//h(X₁,X₂)=θ₁X₁+θ₂X₂
	//θ₁=10,θ₂=150,X₂=1
	std::tuple<Eigen::MatrixX2d, Eigen::VectorXd> RandomGenterateTrainSet(size_t counts)
	{
		Eigen::MatrixX2d train_input(counts, 2);
		Eigen::VectorXd train_output(counts);
		for (size_t i = 0; i < counts; i++)
		{
			float X₁ = std::rand() % 100 + 20, X₂ = 1;
			train_input(i, 0) = X₁;
			train_input(i, 1) = X₂;
			float θ₁ = 10, θ₂ = 150;
			float error = std::rand() % 11 - 5;
			float hx = θ₁ * X₁ + θ₂ * X₂ + error;
			train_output[i] = hx;
		}
		return { train_input/*.normalized()*/, train_output };
	}

	//              i      i
	//1/2m * ∑(h(x) - y(x) )²
	double LossFunction(const Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output)
	{
		auto hx_sub_jx = train_output - train_input * model;
		return hx_sub_jx.array().pow(2).sum() / (train_input.rows() * 2);
	}

	//                            i      i     i
	//θ = θ - α * 1/m * ∑(h(x) - y(x) ) * x
	// j    j                                  j
	void GradientDescent(Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output, const Eigen::VectorXd& learning_rate, double limit)
	{
		size_t batch_size = 0;
		Eigen::MatrixXd update = Eigen::MatrixXd::Zero(model.rows(), model.cols());
		do
		{
			batch_size++;
			Eigen::MatrixXd last_update;
			for (size_t train_index = 0; train_index < train_input.rows(); train_index++)
			{
				auto hx_sub_yx = train_input * model - train_output;
				//std::cout << hx_sub_yx << "\n\n";
				Eigen::MatrixXd duplicate_line(model.cols(), train_input.cols());
				auto&& reference = duplicate_line << train_input.row(train_index);
				for (size_t line = 0; line < model.cols() - 1; line++)
				{
					reference, train_input.row(train_index);
				}
				last_update = update;
				update = learning_rate.array() * (1.0 / train_input.rows() * hx_sub_yx.row(train_index) * duplicate_line).transpose().array();
				//std::cout << duplicate_line << "\n\n";
				//std::cout << update << "\n\n";
				model -= update;
				std::cout << model << "\n\n";
			}

			if (last_update.minCoeff() != 0)
			{
				//non-convergence
				if ((update.array().abs() > last_update.array().abs()).sum())
					break;
				//convergence
				if ((update.array() / last_update.array()).abs().maxCoeff() < limit)
					break;
			}
		} while (true);
		std::cout << "batch_size:" << batch_size << "\n\n";
	}

	void GradientDescent(Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output, const Eigen::VectorXd& learning_rate, size_t batch_size)
	{
		for (size_t i = 0; i < batch_size; i++)
		{
			for (size_t train_index = 0; train_index < train_input.rows(); train_index++)
			{
				auto hx_sub_yx = train_input * model - train_output;
				//std::cout << hx_sub_yx << "\n\n";
				Eigen::MatrixXd duplicate_line(model.cols(), train_input.cols());
				auto&& reference = duplicate_line << train_input.row(train_index);
				for (size_t line = 0; line < model.cols() - 1; line++)
				{
					reference, train_input.row(train_index);
				}
				Eigen::MatrixXd update = learning_rate.array() * (1.0 / train_input.rows() * hx_sub_yx.row(train_index) * duplicate_line).transpose().array();
				//std::cout << duplicate_line << "\n\n";
				//std::cout << update << "\n\n";
				model -= update;
				std::cout << model << "\n\n";
			}
		}
	}

	//  T   -1    T
	//(X *X)   * X  * y
	//\note This matrix must be invertible, otherwise the result is undefined.
	Eigen::MatrixXd NormalEquation(const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output)
	{
		if ((train_input.transpose() * train_input).fullPivLu().isInvertible())
		{
			return (train_input.transpose() * train_input).inverse() * train_input.transpose() * train_output;
		}
		else
		{
			std::cout << "Not invertible!" << std::endl;
			return {};
		}
	}
}

int main_()
{
	std::cout << std::boolalpha;

	auto [train_input, train_output] = LinearRegression::RandomGenterateTrainSet(100);
	//std::cout << train_input << std::endl;
	//std::cout << train_output << std::endl;

	Eigen::MatrixXd model = Eigen::Vector2d(1, 200);
	Eigen::Vector2d learning_rate(0.005, 1);
	double limit = 0.05;
	size_t batch_size = 25;
	LinearRegression::GradientDescent(model, train_input, train_output, learning_rate, limit/*batch_size*/);
	std::cout << LinearRegression::NormalEquation(train_input, train_output) << std::endl;
	return 0;
}


#ifndef GRAVITY_HPP
# define GRAVITY_HPP

#include <Eigen/Dense>

#include <cmath>

/** @brief Parameters for gravity model */
class GravBody {
protected:
  double mu;         /* (m3/s2) gravitational constant of planet */
  double eq_radius;  /* (m)     mean equatorial radius of planet */
public:
  GravBody(const double& mu_, const double& r_eq)
    : mu(mu_),
      eq_radius(r_eq)
  { }

  /** @brief Compute gravitational acceleration and gravity gradient
   *         (change in acceleration with respect to change in position)
   *
   * This function treats the planet as a point mass.
   *
   * @param[in]  r_pcpf   position in planet-centered, planet-fixed
   *                      coordinates; should have same units as
   *                      params->mu (meters, usually)
   * @param[out] a_pcpf   acceleration due to gravity in PCPF frame
   * @param[out] da_dr    partial of change in acceleration with respect
   *                      to change in position
   */
  void accel(const Eigen::Vector3d& r_pcpf,
	     Eigen::Vector3d& a_pcpf,
	     Eigen::Matrix3d& da_dr) const {
    double r2 = r_pcpf.squaredNorm();
    double r3 = r2 * sqrt(r2);
    double mu_over_r3 = mu / r3;

    // Compute acceleration vector
    a_pcpf = r_pcpf * -mu_over_r3;

    // Compute gravity gradient
    da_dr = (r_pcpf * r_pcpf.transpose() * 3.0 / r2 - Eigen::Matrix3d::Identity()) * mu_over_r3;
  }
};

#endif


#pragma once

#include <cstdio>
#include <dlib/optimization.h>
#include <cmath>
#include <iostream>

#define DIM 3

using namespace dlib;

class CovIntersection
{
public:
	void loadData(matrix<double, DIM, DIM> covA, matrix<double, DIM, DIM> covB, matrix<double, DIM, 1> posA, matrix<double, DIM, 1> posB)
	{
		CA = covA;
		CB = covB;

		ca = posA;
		cb = posB;
	}

	static double function(double x)
	{
		// double value = sum(diag(inv(inv(CA) + inv(CB) - inv(x*CA + (1-x)*CB))));
		return sum(diag(inv(inv(CA) + inv(CB) - inv(x*CA + (1 - x)*CB))));
		// return value;
	}

	static matrix<double, DIM, DIM> CA, CB;
	static matrix<double, DIM, 1> ca, cb;

	void optimize()
	{
		minValue = find_min_single_variable(&CovIntersection::function, starting_point, begin, end, eps, max_iter, initial_search_radius);
		minX = starting_point;
	}

	void computeFusedValues()
	{
		covFused = inv(inv(CA) + inv(CB) - inv(minX*CA + (1 - minX)*CB));

		matrix <double, DIM, DIM> KICI, LICI;
		KICI = covFused * (inv(CA) - minX * inv(minX*CA + (1 - minX)*CB));
		LICI = covFused * (inv(CB) - (1 - minX) * inv(minX*CA + (1 - minX)*CB));

		poseFused = KICI * ca + LICI * cb;
	}

	double minValue;
	double minX;

	matrix <double, DIM, DIM> covFused;
	matrix <double, DIM, 1> poseFused;

private:
	const double begin = 0.0;
	const double end = 1.0;
	double starting_point = 0.0;
	const double eps = 1e-3;
	const long max_iter = 100;
	const double initial_search_radius = 0.01;
	// print variables
};

matrix <double, DIM, DIM> CovIntersection::CA;
matrix <double, DIM, DIM> CovIntersection::CB;
matrix <double, DIM, 1> CovIntersection::ca;
matrix <double, DIM, 1> CovIntersection::cb;


#include <Eigen/Dense>
#include <cmath>
#include "nv_core.h"
#include "nv_num.h"
#include "nv_ml_gaussian.h"

// ガウス分布

float nv_gaussian_log_predict(const nv_cov_t *cov, const nv_matrix_t *x, int xm)
{
	Eigen::VectorXf X = Eigen::Map<Eigen::VectorXf>(&x->v[xm*x->n], x->n) - Eigen::Map<Eigen::VectorXf>(cov->u->v, x->n);
	Eigen::Map<Eigen::MatrixXf> Sigma(cov->cov->v, x->n, x->n);
	return log(1 / sqrt(pow(2 * acos(-1), x->n) * Sigma.determinant())) - X.dot(Sigma.llt().solve(X)) / 2;
}


#include "ode45.hpp"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Sparse>

using namespace Eigen;
using namespace std;

// Compute the maps Phi and W at time T, for initial data given by u0 and v0.
pair<Vector2d,Matrix2d> PhiAndW(double u0, double v0, double T) {

    auto f = [] (const VectorXd & w) {
        Eigen::VectorXd temp(6);
        temp(0) = (2. - w(1))*w(0);
        temp(1) = (w(0) - 1.)*w(1);
        temp(2) = (2. - w(1))*w(2) - w(0)*w(3);
        temp(3) = w(1)*w(2) + (w(0) - 1.)*w(3);
        temp(4) = (2. - w(1))*w(4) - w(0)*w(5);
        temp(5) = w(1)*w(4) + (w(0) - 1.)*w(5);
        return temp;
    };

    Eigen::VectorXd w0(6);
    w0 << u0, v0, 1., 0, 0, 1.;

    ode45<Eigen::VectorXd> O(f);
    O.options.rtol = 1e-14;
    O.options.atol = 1e-12;
    auto sol = O.solve(w0, T);
    VectorXd wT = sol.back().first;

    pair<Vector2d,Matrix2d> PaW;
    PaW.first << wT(0), wT(1);
    PaW.second << wT(2), wT(4), wT(3), wT(5);
    return PaW;
}

// Apply the Newton method to find initial data giving solutions with period equal to 5.
int main(){
    Vector2d y;
    y << 3, 2;
    double T = 5;
    pair<Vector2d,Matrix2d> PaW = PhiAndW(y(0), y(1), T);
    Vector2d F = PaW.first - y;
    Matrix2d DF;

    while (F.norm() > 1e-5) {
        PaW = PhiAndW(y(0), y(1), T);
        F = PaW.first - y;
        DF = PaW.second - MatrixXd::Identity(2,2);
        y = y - DF.lu().solve(F);
    }

    cout << "The obtained initial condition is: " << endl << y << endl;
    PaW = PhiAndW(y(0), y(1), 100);

    cout << "y(100) = " << endl << PaW.first << endl;
}

Nummerical Methods for CSE/PS13/solutions_ps13/LV.cpp


//
//  main.cpp
//  Exercise 5
//
//  Created by Zhehao Li on 2020/4/17.
//  Copyright © 2020 Zhehao Li. All rights reserved.
//
//  Exercise 5 Statistical Functions

#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions.hpp>             // For non-member functions of distributions

#include <vector>
#include <iostream>
using namespace std;

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

    using namespace boost::math;

    // 1. Create exponential distribution object
    double scaleParameter = 0.5;
    exponential_distribution<> myExponential(scaleParameter);          // Default type is 'double'
    cout << "Mean: " << mean(myExponential) << ", standard deviation: " << standard_deviation(myExponential) << endl;

    // 1.1 Distributional properties
    double x = 3.6;
    cout << "pdf: " << pdf(myExponential, x) << endl;
    cout << "cdf: " << cdf(myExponential, x) << endl;

    // 1.2 Choose precision
    cout.precision(10); // Number of values behind the comma

    // 1.3 Other properties
    cout << "\n*** Poisson Distribution ***\n";
    cout << "mean: "                    << mean(myExponential)              << endl;
    cout << "variance: "                << variance(myExponential)          << endl;
    cout << "median: "                  << median(myExponential)            << endl;
    cout << "mode: "                    << mode(myExponential)              << endl;
    cout << "kurtosis excess: "         << kurtosis_excess(myExponential)   << endl;
    cout << "kurtosis: "                << kurtosis(myExponential)          << endl;

    // 2. Poisson distribution
    double lmbda = 3.0;                                 // Mean
    poisson_distribution<> myPoisson(lmbda);            // Default type is 'double'

    double val = 13.0;
    cout << "Exponential pdf: " << pdf(myPoisson, val) << endl;
    cout << "Exponential cdf: " << cdf(myPoisson, val) << endl;

    vector<double> pdfList;
    vector<double> cdfList;

    double start = 0.0;
    double end = 10.0;
    long   N = 30;        // Number of subdivisions

    val = 0.0;
    double h = (end - start) / double(N);

    // Push the pdf and cdf value into vectors
    for (long j = 1; j <= N; ++j){
        pdfList.push_back(pdf(myPoisson, val));
        cdfList.push_back(cdf(myPoisson, val));

        val += h;
    }

    cout << "\n********\n" << endl;

    // Print the vector of pdf
    cout << "Poisson pdf:" << endl;
    for (long j = 0; j < pdfList.size(); ++j){
        cout << pdfList[j] << ", ";
    }

    cout << "\n********\n" << endl;

    // Print the vector of cdf
    cout << "Poisson cdf:" << endl;
    for (long j = 0; j < cdfList.size(); ++j){
        cout << cdfList[j] << ", ";
    }

    return 0;
}


#ifndef LQR_CONTROLLER_HPP
#define LQR_CONTROLLER_HPP

#include <armadillo>
#include <cmath>
class LQRController {

public:

    arma::mat A;
    arma::mat B;
    arma::mat Q;
    arma::mat R;
    arma::mat P;
    arma::mat Klqr;

    int maxIter;
    float eps = 1e-3;

    LQRController(const arma::mat& _A, const arma::mat& _B,
                const arma::mat& _Q, const arma::mat& _R,
                int _maxIter = 2000, float _eps = 1e-3) {

        maxIter = _maxIter;
        eps = _eps;

        compute_LQR_gain(_A, _B, _Q, _R);
    }

    void compute_LQR_gain(const arma::mat& _A, const arma::mat& _B, const arma::mat& _Q, const arma::mat& _R) {

        A = _A;
        B = _B;
        Q = _Q;
        R = _R;
        P = _Q;

        arma::mat Pold = P;
        arma::mat delta;

        for (int i = 0; i < maxIter; i++) {
            P = A.t() * P * A - (A.t() * P * B) * arma::inv(R + B.t() * P * B) * B.t() * P * A + Q;
            delta = Pold - P;
            if (std::abs(delta.max()) < eps) {
                break;
            }
            Pold = P;
        }

        Klqr = arma::inv(R + B.t() * P * B) * B.t() * P * A;
    }

    arma::vec get_control(const arma::vec & x) {
        return -Klqr * x;
    }

};

#endif


#include "utils.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/integer.hpp>
using namespace std;
using Bint = boost::multiprecision::cpp_int;

string FermatFactorizer_cppfunc(string s){
    Bint n(s);
    if(n%2 == 0){
        return "2";
    }
    Bint x = sqrt(n);
    if(pow(x,2)==n){
        return x.str();
    }
    x+=1;
    Bint y = sqrt(pow(x,2)-n);
    Bint w = pow(x,2)-n-pow(y,2);
    for(;;){
        if(w==0){
            Bint retval = x-y;
            return retval.str();
        }
        else if(w>0){
            y+=1;
        }
        else{
            x+=1;
        }
        w = pow(x,2)-n-pow(y,2);
    }
}


#include <fstream>
#include <cmath>
#include <boost/config.hpp>
#include <boost/version.hpp>

// CGAL headers
#include <CGAL/Cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Min_circle_2.h>
#include <CGAL/Min_circle_2_traits_2.h>
#include <CGAL/Min_ellipse_2.h>
#include <CGAL/Min_ellipse_2_traits_2.h>
#include <CGAL/convex_hull_2.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/min_quadrilateral_2.h>
#include <CGAL/rectangular_p_center_2.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsEllipseItem>
#include <QGraphicsRectItem>

// GraphicsView items and event filters (input classes)

#include <CGAL/Qt/PointsGraphicsItem.h>
#include <CGAL/Qt/PolygonGraphicsItem.h>

// for viewportsBbox
#include <CGAL/Qt/utility.h>

#include <CGAL/Qt/GraphicsViewPolylineInput.h>

// the two base classes
#include "ui_Bounding_volumes.h"
#include <CGAL/Qt/DemosMainWindow.h>

#include "Ellipse.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Vector_2 Vector_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;

typedef CGAL::Polygon_2<K> Polygon_2;

typedef CGAL::Min_circle_2<CGAL::Min_circle_2_traits_2<K> > Min_circle;
typedef CGAL::Min_ellipse_2<CGAL::Min_ellipse_2_traits_2<K> > Min_ellipse;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Bounding_volumes
{
  Q_OBJECT

private:
  Polygon_2 convex_hull, min_rectangle, min_parallelogram;
  Min_circle mc;
  Min_ellipse me;
  QGraphicsScene scene;

  std::vector<Point_2> points;
  CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> > * pgi;
  CGAL::Qt::PolygonGraphicsItem<Polygon_2> * convex_hull_gi;
  CGAL::Qt::PolygonGraphicsItem<Polygon_2> * min_rectangle_gi;
  CGAL::Qt::PolygonGraphicsItem<Polygon_2> * min_parallelogram_gi;
  QGraphicsEllipseItem *cgi, *egi;

  const std::size_t P;
  QGraphicsRectItem *p_center[3];
  Iso_rectangle_2 p_center_iso_rectangle[3];
  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

public:
  MainWindow();

public Q_SLOTS:

  void update();

  void update_from_points();

  void processInput(CGAL::Object o);

  void on_actionShowMinCircle_toggled(bool checked);

  void on_actionShowMinEllipse_toggled(bool checked);

  void on_actionShowMinRectangle_toggled(bool checked);

  void on_actionShowMinParallelogram_toggled(bool checked);

  void on_actionShowConvexHull_toggled(bool checked);

  void on_actionShowPCenter_toggled(bool checked);

  void on_actionInsertPoint_toggled(bool checked);

  void on_actionInsertRandomPoints_triggered();

  void on_actionLoadPoints_triggered();

  void on_actionSavePoints_triggered();

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  virtual void open(QString fileName);

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow(), P(3)
{
  setupUi(this);

  QObject::connect(this, SIGNAL(changed()), this, SLOT(update()));

  // Add a GraphicItem for the Min_circle
  cgi = new QGraphicsEllipseItem;
  cgi->setPen(QPen(Qt::red, 0, Qt::SolidLine));
  cgi->hide();
  scene.addItem(cgi);

  egi = new QGraphicsEllipseItem;
  egi->setPen(QPen(Qt::magenta, 0, Qt::SolidLine));
  egi->hide();
  scene.addItem(egi);

  for(std::size_t i =0; i < P; i++){
    p_center[i] = new QGraphicsRectItem;
    p_center[i]->setPen(QPen(Qt::cyan, 0, Qt::SolidLine));
    p_center[i]->hide();
    scene.addItem(p_center[i]);
  }

  // Graphics Item for the input point set
  pgi = new CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> >(&points);

  QObject::connect(this, SIGNAL(changed()),
		   pgi, SLOT(modelChanged()));
  pgi->setVerticesPen(QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(pgi);

  // Graphics Item for the convex hull
  convex_hull_gi = new CGAL::Qt::PolygonGraphicsItem<Polygon_2>(&convex_hull);

  QObject::connect(this, SIGNAL(changed()),
		   convex_hull_gi, SLOT(modelChanged()));
  convex_hull_gi->setEdgesPen(QPen(Qt::black, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(convex_hull_gi);

  // Graphics Item for the min rectangle
  min_rectangle_gi = new CGAL::Qt::PolygonGraphicsItem<Polygon_2>(&min_rectangle);

  QObject::connect(this, SIGNAL(changed()),
		   min_rectangle_gi, SLOT(modelChanged()));
  min_rectangle_gi->setEdgesPen(QPen(Qt::green, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(min_rectangle_gi);

  // Graphics Item for the min parallelogram
  min_parallelogram_gi = new CGAL::Qt::PolygonGraphicsItem<Polygon_2>(&min_parallelogram);

  QObject::connect(this, SIGNAL(changed()),
		   min_parallelogram_gi, SLOT(modelChanged()));
  min_parallelogram_gi->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(min_parallelogram_gi);

  // Setup input handlers. They get events before the scene gets them
  // and the input they generate is passed to the triangulation with
  // the signal/slot mechanism
  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 1);

  scene.installEventFilter(pi);

  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
		   this, SLOT(processInput(CGAL::Object)));

  //
  // Manual handling of actions
  //

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
		   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Turn the vertical axis upside down
  this->graphicsView->matrix().scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Bounding_volumes.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
	  this, SLOT(open(QString)));
}

void
MainWindow::update()
{
  if(this->actionShowConvexHull->isChecked()){
    convex_hull_gi->show();
  }else {
    convex_hull_gi->hide();
  }

  if(this->actionShowMinRectangle->isChecked()){
    min_rectangle_gi->show();
  }else {
    min_rectangle_gi->hide();
  }

  if(this->actionShowMinParallelogram->isChecked()){
    min_parallelogram_gi->show();
  }else {
    min_parallelogram_gi->hide();
  }

  CGAL::Qt::Converter<K> convert;

  if(this->actionShowPCenter->isChecked() && convex_hull.size()>=3){
    for(std::size_t i=0; i< P; i++){
      p_center[i]->setRect(convert(p_center_iso_rectangle[i]));
      p_center[i]->show();
    }
  }

  if (mc.is_degenerate() || (! this->actionShowMinCircle->isChecked())){
    cgi->hide();
  } else {
    K::Circle_2 c;
    if (mc.number_of_support_points() == 2)
      c = K::Circle_2(mc.support_point(0), mc.support_point(1));
    else
      c = K::Circle_2(mc.support_point(0), mc.support_point(1), mc.support_point(2));

    cgi->setRect(convert(c.bbox()));
    cgi->show();
  }

  if (me.is_degenerate()  || (! this->actionShowMinEllipse->isChecked()) ){
    egi->hide();
  } else {
    if (me.number_of_support_points() == 2) {
    } else {
      Ellipse_2<K> e(me);
      double half_width = sqrt(e.va() * e.va());
      double half_height = sqrt(e.vb() * e.vb());
      double angle = std::atan2( e.va().y(), e.va().x() ) * 180.0/CGAL_PI;
      Vector_2 wh(half_width, half_height);

      Iso_rectangle_2 isor(e.center()+ wh, e.center()-wh);
      egi->setRect(convert(isor));
      // Rotate an item 45 degrees around (x, y).
      double x = e.center().x();
      double y = e.center().y();
      egi->setTransform(QTransform().translate(x, y).rotate(angle).translate(-x, -y));
      egi->show();
    }
  }
}

void
MainWindow::update_from_points()
{
    convex_hull.clear();
    CGAL::convex_hull_2(points.begin(), points.end(), std::back_inserter(convex_hull));

    min_rectangle.clear();
    CGAL::min_rectangle_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_rectangle));

    min_parallelogram.clear();
    CGAL::min_parallelogram_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_parallelogram));

    std::vector<Point_2> center;
    double radius;

    CGAL::rectangular_p_center_2 (points.begin(), points.end(), std::back_inserter(center), radius, static_cast<int>(P));
    Vector_2 rvec(radius, radius);

    for(std::size_t i = 0; i < center.size(); i++){
      p_center_iso_rectangle[i] = Iso_rectangle_2(center[i]-rvec, center[i]+rvec);
    }
}

void
MainWindow::processInput(CGAL::Object o)
{
  std::list<Point_2> input;
  if(CGAL::assign(input, o)){
    Point_2 p = input.front();

    mc.insert(p);
    me.insert(p);
    points.push_back(p);

    convex_hull.push_back(p);
    Polygon_2 tmp;
    CGAL::convex_hull_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(tmp));
    convex_hull = tmp;

    min_rectangle.clear();
    CGAL::min_rectangle_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_rectangle));

    min_parallelogram.clear();
    CGAL::min_parallelogram_2(convex_hull.vertices_begin(), convex_hull.vertices_end(), std::back_inserter(min_parallelogram));

    std::vector<Point_2> center;
    double radius;
    if (points.size()>=P){
      CGAL::rectangular_p_center_2 (points.begin(), points.end(), std::back_inserter(center), radius, static_cast<int>(P));
      Vector_2 rvec(radius, radius);

      for(std::size_t i=0; i < center.size(); i++){
        p_center_iso_rectangle[i] = Iso_rectangle_2(center[i]-rvec, center[i]+rvec);
      }
    }
  }
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */
void
MainWindow::on_actionInsertPoint_toggled(bool checked)
{
  if(checked){
    scene.installEventFilter(pi);
  } else {
    scene.removeEventFilter(pi);
  }
}

void
MainWindow::on_actionShowMinCircle_toggled(bool checked)
{
  cgi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowMinEllipse_toggled(bool checked)
{
  egi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowMinRectangle_toggled(bool checked)
{
  min_rectangle_gi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowMinParallelogram_toggled(bool checked)
{
  min_parallelogram_gi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowConvexHull_toggled(bool checked)
{
  convex_hull_gi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowPCenter_toggled(bool checked)
{
  for(std::size_t i =0; i < P; i++){
    p_center[i]->setVisible(checked);
  }
  Q_EMIT( changed());
}

void
MainWindow::on_actionClear_triggered()
{
  mc.clear();
  me.clear();
  points.clear();
  convex_hull.clear();
  min_rectangle.clear();
  min_parallelogram.clear();
  for(std::size_t i=0; i < P;i++){
    p_center[i]->hide();
  }
  Q_EMIT( changed());
}

void
MainWindow::on_actionInsertRandomPoints_triggered()
{
  QRectF rect = CGAL::Qt::viewportsBbox(&scene);
  CGAL::Qt::Converter<K> convert;
  Iso_rectangle_2 isor = convert(rect);
  CGAL::Random_points_in_iso_rectangle_2<Point_2> pg((isor.min)(), (isor.max)());
  bool ok = false;

  const int number_of_points =
    QInputDialog::getInt(this,
                             tr("Number of random points"),
                             tr("Enter number of random points"),
			     100,
			     0,
			     (std::numeric_limits<int>::max)(),
			     1,
			     &ok);

  if(!ok) {
    return;
  }

  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  for(int i = 0; i < number_of_points; ++i){
    Point_2 p = *pg++;
    mc.insert(p);
    me.insert(p);
    points.push_back(p);
  }

  update_from_points();

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

void
MainWindow::on_actionLoadPoints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
						  tr("Open Points file"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.WKT *.wkt);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    CGAL::read_multi_point_WKT(ifs, points);
    for(K::Point_2 p : points)
    {
      mc.insert(p);
      me.insert(p);
    }
#endif
  }
  else
  {
    K::Point_2 p;
    while(ifs >> p) {
      mc.insert(p);
      me.insert(p);
      points.push_back(p);
    }
  }
  update_from_points();

  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  actionRecenter->trigger();
  Q_EMIT( changed());

}

void
MainWindow::on_actionSavePoints_triggered()
{
  QString fileName = QFileDialog::getSaveFileName(this,
						  tr("Save points"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.WKT *.wkt);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));
    if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
    {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      std::vector<K::Point_2> out_pts;
      out_pts.reserve(std::distance(mc.points_begin(),
                                    mc.points_end()));
      for(Min_circle::Point_iterator pit = mc.points_begin();
          pit != mc.points_end(); ++pit)
        out_pts.push_back(*pit);
      CGAL::write_multi_point_WKT(ofs, out_pts);
#endif
    }
    else
    {
      for(Min_circle::Point_iterator
          vit = mc.points_begin(),
          end = mc.points_end();
          vit!= end; ++vit)
      {
        ofs << *vit << std::endl;
      }
    }
  }
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(cgi->boundingRect());
  this->graphicsView->fitInView(cgi->boundingRect(), Qt::KeepAspectRatio);
}

#include "Bounding_volumes.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Bounding_volumes demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


#ifndef QUADRATIC_FORM_HPP
#define QUADRATIC_FORM_HPP

#include <Eigen/Dense>

// quadraticform in three variables
struct QuadraticForm{
    // x1, x2, x3
    // A*x1*x1 + B*x1*x2 + C*x2*x2 + D*x1*x3 + E*x2*x3 + F*x3*x3 = 0
    double A, B, C, D, E, F;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
    Eigen::Matrix3d mat;

    QuadraticForm(double a, double b, double c, double d, double e, double f) :
        A(a), B(b), C(c), D(d), E(e), F(f) {
        mat << A, B/2, D/2,
               B/2, C, E/2,
               D/2, E/2, F;
        eigensolver = Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>(mat);
    }

    QuadraticForm& operator*=(double k) {
        A*=k; B*=k; C*=k; D*=k; E*=k; F*=k;
        Eigen::Matrix3d _mat;
        _mat << A, B/2, D/2,
                B/2, C, E/2,
                D/2, E/2, F;
        mat = _mat;
        eigensolver = Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>(mat);
        return *this;
    }

    double evaluate(const Eigen::Vector3d& vec) {
        return vec.transpose()*mat*vec;
    }

};

#endif // QUADRATIC_FORM_HPP