#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

// Generic functor
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
    // Information that tells the caller the numeric type (eg. double) and size (input / output dim)
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
};
// Tell the caller the matrix sizes associated with the input, output, and jacobian
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

// Local copy of the number of inputs
int m_inputs, m_values;

// Two constructors:
Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

// Get methods for users to determine function input and output dimensions
int inputs() const { return m_inputs; }
int values() const { return m_values; }

};

///////////////////////// Levenberg Marquardt Example (1) f(x) = a x + b ///////////////////////////////

typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > Point2DVector;

Point2DVector GeneratePoints();

struct SimpleLineFunctor : Functor<double>
{
    int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const
    {
        // "a" in the model is z(0), and "b" is z(1)
        double x_i,y_i,a,b;
        for(unsigned int i = 0; i < this->Points.size(); ++i)
        {
            y_i=this->Points[i](1);
            x_i=this->Points[i](0);
            a=z(0);
            b=z(1);
            fvec(i) =y_i-(a*x_i +b);
        }

        return 0;
    }

  Point2DVector Points;

  int inputs() const { return 2; } // There are two parameters of the model
  int values() const { return this->Points.size(); } // The number of observations
};

struct SimpleLineFunctorNumericalDiff : Eigen::NumericalDiff<SimpleLineFunctor> {};

Point2DVector GeneratePoints(const unsigned int numberOfPoints)
{
    Point2DVector points;
    // Model y = 2*x + 5 with some noise (meaning that the resulting minimization should be about (2,5)
    for(unsigned int i = 0; i < numberOfPoints; ++i)
    {
        double x = static_cast<double>(i);
        Eigen::Vector2d point;
        point(0) = x;
        point(1) = 2.0 * x + 5.0 + drand48()/10.0;
        points.push_back(point);
    }

  return points;
}

void testSimpleLineFunctor()
{
    std::cout << "Testing f(x) = a x + b function..." << std::endl;

    unsigned int numberOfPoints = 50;
    Point2DVector points = GeneratePoints(numberOfPoints);

    Eigen::VectorXd x(2);
    x.fill(2.0f);

    SimpleLineFunctorNumericalDiff functor;
    functor.Points = points;
    Eigen::LevenbergMarquardt<SimpleLineFunctorNumericalDiff> lm(functor);

    Eigen::LevenbergMarquardtSpace::Status status = lm.minimize(x);
    std::cout << "status: " << status << std::endl;
    std::cout << "x that minimizes the function: " << std::endl << x << std::endl;
    std::cout <<"lm.parameters.epsfcn: " <<lm.parameters.epsfcn << std::endl;
    std::cout <<"lm.parameters.factor: " <<lm.parameters.factor << std::endl;
    std::cout <<"lm.parameters.ftol: " << lm.parameters.ftol << std::endl;
    std::cout <<"lm.parameters.gtol: " <<lm.parameters.gtol << std::endl;
    std::cout <<"lm.parameters.maxfev: " <<lm.parameters.maxfev << std::endl;
    std::cout <<"lm.parameters.xtol: " <<lm.parameters.xtol << std::endl;

    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

}

///////////////////////// Levenberg Marquardt Example (2) f(x) = ax² + bx + c ///////////////////////////////

// Ref: https://medium.com/@sarvagya.vaish/levenberg-marquardt-optimization-part-2-5a71f7db27a0

struct QuadraticFunctor
{
    // Compute 'm' errors, one for each data point, for the given parameter values in 'x'
    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
    {
        // 'x' has dimensions n x 1
        // It contains the current estimates for the parameters.

        // 'fvec' has dimensions m x 1
        // It will contain the error for each data point.

        double aParam = x(0);
        double bParam = x(1);
        double cParam = x(2);

        for (int i = 0; i < values(); i++)
        {
            double xValue = measuredValues(i, 0);
            double yValue = measuredValues(i, 1);

            fvec(i) = yValue - (aParam * xValue * xValue + bParam * xValue + cParam);
        }
    }

    // Compute the jacobian of the errors
    int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac) const
    {
        // 'x' has dimensions n x 1
        // It contains the current estimates for the parameters.

        // 'fjac' has dimensions m x n
        // It will contain the jacobian of the errors, calculated numerically in this case.

        float epsilon;
        epsilon = 1e-5f;

        for (int i = 0; i < x.size(); i++)
        {
            Eigen::VectorXd xPlus(x);
            xPlus(i) += epsilon;
            Eigen::VectorXd xMinus(x);
            xMinus(i) -= epsilon;

            Eigen::VectorXd fvecPlus(values());
            operator()(xPlus, fvecPlus);

            Eigen::VectorXd fvecMinus(values());
            operator()(xMinus, fvecMinus);

            Eigen::VectorXd fvecDiff(values());
            fvecDiff = (fvecPlus - fvecMinus) / (2.0f * epsilon);

            fjac.block(0, i, values(), 1) = fvecDiff;
        }
    }

    // Number of data points, i.e. values.
    int m;

    // Returns 'm', the number of values.
    int values() const { return m; }

    // The number of parameters, i.e. inputs.
    int n;

    // Returns 'n', the number of inputs.
    int inputs() const { return n; }

    Eigen::MatrixXd measuredValues;
};

void quadraticPointsGenerator(std::vector<double> &x_values, std::vector<double> &y_values,double a=-1, double b=0.6, double c=2, unsigned int numberOfPoints=50 )
{
    //point are x_start=-4, x_end=3
    double x_start=-4;
    double x_end=3;
    double x,y;

    for(unsigned int i = 0; i < numberOfPoints; ++i)
    {
        x = x_start+ static_cast<double>(i)* ( x_end- x_start)/numberOfPoints;
        y = a*pow(x,2)+b*x+c + drand48()/10.0;
        x_values.push_back(x);
        y_values.push_back(y);
    }

}

void testQuadraticFunctor()
{
    std::cout << "Testing the f(x) = ax² + bx + c function..." << std::endl;
    std::vector<double> x_values;
    std::vector<double> y_values;

    double a=-1;
    double b=0.6;
    double c=2;

    quadraticPointsGenerator(x_values, y_values,a,b,c);

    // 'm' is the number of data points.
    int m = x_values.size();

    // Move the data into an Eigen Matrix.
    // The first column has the input values, x. The second column is the f(x) values.
    Eigen::MatrixXd measuredValues(m, 2);
    for (int i = 0; i < m; i++) {
        measuredValues(i, 0) = x_values[i];
        measuredValues(i, 1) = y_values[i];
    }

    // 'n' is the number of parameters in the function.
    // f(x) = a(x^2) + b(x) + c has 3 parameters: a, b, c
    int n = 3;

    // 'parameters' is vector of length 'n' containing the initial values for the parameters.
    // The parameters 'x' are also referred to as the 'inputs' in the context of LM optimization.
    // The LM optimization inputs should not be confused with the x input values.
    Eigen::VectorXd parameters(n);
    parameters(0) = 0.0;             // initial value for 'a'
    parameters(1) = 0.0;             // initial value for 'b'
    parameters(2) = 0.0;             // initial value for 'c'

    //
    // Run the LM optimization
    // Create a LevenbergMarquardt object and pass it the functor.
    //

    QuadraticFunctor functor;
    functor.measuredValues = measuredValues;
    functor.m = m;
    functor.n = n;

    Eigen::LevenbergMarquardt<QuadraticFunctor, double> lm(functor);
    int status = lm.minimize(parameters);
    std::cout << "LM optimization status: " << status << std::endl;

    //
    // Results
    // The 'x' vector also contains the results of the optimization.
    //
    std::cout << "Optimization results" << std::endl;
    std::cout << "\ta: " << parameters(0) << std::endl;
    std::cout << "\tb: " << parameters(1) << std::endl;
    std::cout << "\tc: " << parameters(2) << std::endl;

    std::cout << "Actual values are" << std::endl;
    std::cout << "\ta: " << a << std::endl;
    std::cout << "\tb: " << b << std::endl;
    std::cout << "\tc: " << c << std::endl;
    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

}

///////////////////////// Levenberg Marquardt Example (3) f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2 ///////////////////////////////

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Booth Function
// Implement f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2
struct BoothFunctor : Functor<double>
{
    // Simple constructor
    BoothFunctor(): Functor<double>(2,2) {}

    // Implementation of the objective function
    int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const
    {
        double x = z(0);   double y = z(1);
        /*
        * Evaluate the Booth function.
        * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
        * of squared terms. The algorithm takes this into account: do not do it yourself.
        * In other words: objFun = sum(fvec(i)^2)
        */
        fvec(0) = x + 2*y - 7;
        fvec(1) = 2*x + y - 5;
        return 0;
    }
};

void testBoothFunctor() {
    std::cout << "Testing the f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2 function..." << std::endl;
    Eigen::VectorXd zInit(2); zInit << 1.87, 2.032;
    std::cout << "zInit: " << zInit.transpose() << std::endl;
    Eigen::VectorXd zSoln(2); zSoln << 1.0, 3.0;
    std::cout << "zSoln: " << zSoln.transpose() << std::endl;

    BoothFunctor functor;
    Eigen::NumericalDiff<BoothFunctor> numDiff(functor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<BoothFunctor>,double> lm(numDiff);
    lm.parameters.maxfev = 1000;
    lm.parameters.xtol = 1.0e-10;
    std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
    std::cout << "x tol: " << lm.parameters.xtol << std::endl;

    Eigen::VectorXd z = zInit;
    int ret = lm.minimize(z);
    std::cout << "iter count: " << lm.iter << std::endl;
    std::cout << "return status: " << ret << std::endl;
    std::cout << "zSolver: " << z.transpose() << std::endl;
    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

///////////////////////// Levenberg Marquardt Example (4) f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2 ///////////////////////////////

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Himmelblau's Function
// Implement f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2
struct HimmelblauFunctor : Functor<double>
{
    // Simple constructor
    HimmelblauFunctor(): Functor<double>(2,2) {}

    // Implementation of the objective function
    int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const
    {
        double x = z(0);   double y = z(1);
        /*
        * Evaluate Himmelblau's function.
        * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
        * of squared terms. The algorithm takes this into account: do not do it yourself.
        * In other words: objFun = sum(fvec(i)^2)
        */
        fvec(0) = x * x + y - 11;
        fvec(1) = x + y * y - 7;
        return 0;
    }
};

void testHimmelblauFunctor()
{
    std::cout << "Testing f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2 function..." << std::endl;
    // Eigen::VectorXd zInit(2); zInit << 0.0, 0.0;  // soln 1
    // Eigen::VectorXd zInit(2); zInit << -1, 1;  // soln 2
    // Eigen::VectorXd zInit(2); zInit << -1, -1;  // soln 3
    Eigen::VectorXd zInit(2); zInit << 1, -1;  // soln 4
    std::cout << "zInit: " << zInit.transpose() << std::endl;
    std::cout << "soln 1: [3.0, 2.0]" << std::endl;
    std::cout << "soln 2: [-2.805118, 3.131312]" << std::endl;
    std::cout << "soln 3: [-3.77931, -3.28316]" << std::endl;
    std::cout << "soln 4: [3.584428, -1.848126]" << std::endl;

    HimmelblauFunctor functor;
    Eigen::NumericalDiff<HimmelblauFunctor> numDiff(functor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<HimmelblauFunctor>,double> lm(numDiff);
    lm.parameters.maxfev = 1000;
    lm.parameters.xtol = 1.0e-10;
    std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
    std::cout << "x tol: " << lm.parameters.xtol << std::endl;

    Eigen::VectorXd z = zInit;
    int ret = lm.minimize(z);
    std::cout << "iter count: " << lm.iter << std::endl;
    std::cout << "return status: " << ret << std::endl;
    std::cout << "zSolver: " << z.transpose() << std::endl;
    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

int main()
{
    testSimpleLineFunctor();
    testQuadraticFunctor();
    testBoothFunctor();
    testHimmelblauFunctor();
}

/*

Refs
https://github.com/daviddoria/Examples/blob/master/c%2B%2B/Eigen/LevenbergMarquardt/CurveFitting.cpp
https://stackoverflow.com/questions/18509228/how-to-use-the-eigen-unsupported-levenberg-marquardt-implementation
https://stackoverflow.com/questions/48213584/understanding-levenberg-marquardt-enumeration-returns
https://www.ultimatepp.org/reference$Eigen_demo$en-us.html
https://ethz-adrl.github.io/ct/ct_doc/doc/html/core_tut_linearization.html
https://robotics.stackexchange.com/questions/20673/why-with-the-pseudo-inverse-it-is-possible-to-invert-the-jacobian-matrix-even-in
http://users.ics.forth.gr/~lourakis/
https://mathoverflow.net/questions/257699/gauss-newton-vs-gradient-descent-vs-levenberg-marquadt-for-least-squared-method
https://math.stackexchange.com/questions/1085436/gauss-newton-versus-gradient-descent
https://stackoverflow.com/questions/34701160/how-to-set-levenberg-marquardt-damping-using-eigen
http://www.netlib.org/minpack/lmder.f
https://en.wikipedia.org/wiki/Test_functions_for_optimization
https://github.com/daviddoria/Examples/blob/master/c%2B%2B/Eigen/LevenbergMarquardt/NumericalDerivative.cpp
https://stackoverflow.com/questions/18509228/how-to-use-the-eigen-unsupported-levenberg-marquardt-implementation
*/


#include <Eigen/Core>
#include <Eigen/LU>
#include <vector>
#include <iostream>
#include "writer.hpp"
#include <assert.h>

/// Uses the explicit Euler method to compute y from time 0 to time T
/// where y is a 2x1 vector solving the linear system of ODEs as in the exercise
///
/// @param[out] yT at the end of the call, this will have vNext
/// @param[in] y0 the initial conditions
/// @param[in] zeta the damping factor (see exercise)
/// @param[in] h the step size
/// @param[in] T the final time at which to compute the solution.
///
/// The first component of y (the position) will be stored to y1, the second component (the velocity) to y2. The i-th entry of y1 (resp. y2) will contain the first (resp. second) component of y at time i*h.
///

//----------------explicitEulerBegin----------------
void explicitEuler(std::vector<double> & y1, std::vector<double> &y2, std::vector<double> & time,
	const Eigen::Vector2d& y0,
		   double zeta, double h, double T) {

	y1.push_back(y0(0));
	y2.push_back(y0(1));
	time.push_back(0);

	for(int k = 0; k < (T / h); k++) {
		double y1_k = y1.back();
		double y2_k = y2.back();

		//y1[k + 1] = y1_k + h * y2_k;
		//y2[k + 1] = y2_k + h * (-y1_k - (2 * zeta * y2_k));

		//time[k + 1] = time[k] + h;

		y1.push_back(y1_k + h * y2_k);
		y2.push_back(y2_k + h * (-y1_k - (2 * zeta * y2_k)));

		time.push_back(time.back() + h);
	}
}
//----------------explicitEulerEnd----------------

// Implements the implicit Euler. Analogous to explicit Euler, same input and output parameters
//----------------implicitEulerBegin----------------
void implicitEuler(std::vector<double> & y1, std::vector<double> & y2, std::vector<double> & time,
	const Eigen::Vector2d& y0,
		   double zeta, double h, double T) {

	int nSteps = T / h;
	//y1.resize(nSteps + 1);
	//y2.resize(nSteps + 1);
	//time.resize(nSteps + 1);

	y1[0] = y0(0);
	y2[0] = y0(1);
	time[0] = 0;

	Eigen::MatrixXd A(2, 2);
	A << 1, -h,
			 h, (1 + 2*zeta*h);

	Eigen::FullPivLU<Eigen::MatrixXd> lu = A.fullPivLu();

	for(int k = 0; k < nSteps + 1; k++) {
		Eigen::Vector2d y_prev = { y1[k], y2[k] };
		Eigen::VectorXd y_next = lu.solve(y_prev);
		y1[k + 1] = y_next(0);
		y2[k + 1] = y_next(1);
		time[k + 1] = time[k] + h;
	}
}
//----------------implicitEulerEnd----------------

//----------------energyBegin----------------
// Energy computation given the velocity. Assume the energy vector to be already initialized with the correct size.
void Energy(const std::vector<double> & v, std::vector<double> & energy)
{
  assert(v.size()==energy.size());

	for(int i = 0; i < v.size(); i++) {
		energy[i] = 0.5 * v[i] * v[i];
	}
}
//----------------energyEnd----------------

int main() {

	double T = 20.0;
	double h = 0.5; // Change this for explicit / implicit time stepping comparison
	const Eigen::Vector2d y0(1,0);
	double zeta=0.2;
	std::vector<double> y1;
	std::vector<double> y2;
	std::vector<double> time;
	explicitEuler(y1,y2,time,y0,zeta,h,T);
	writeToFile("position_expl.txt", y1);
	writeToFile("velocity_expl.txt",y2);
	writeToFile("time_expl.txt",time);
	std::vector<double> energy(y2.size());
	Energy(y2,energy);
	writeToFile("energy_expl.txt",energy);
	y1.assign(y1.size(),0);
	y2.assign(y2.size(),0);
	time.assign(time.size(),0);
	energy.assign(energy.size(),0);
	implicitEuler(y1,y2,time,y0,zeta,h,T);
	writeToFile("position_impl.txt", y1);
	writeToFile("velocity_impl.txt",y2);
	writeToFile("time_impl.txt",time);
	Energy(y2,energy);
	writeToFile("energy_impl.txt",energy);

	return 0;
}


#include <toynet/math.h>
#include <algorithm>
#include <cmath>
#include <functional>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace toynet {

ublas::vector<double> softmax(const ublas::vector<double>& v)
{
    return stable_softmax(v);
}

ublas::vector<double> naive_softmax(const ublas::vector<double>& v)
{
    double sum = 0;
    for (auto x : v)
        sum += exp(x);
    ublas::vector<double> ret(v.size());
    for (int i = 0;  i < v.size();  ++i)
        ret[i] = exp(v[i]) / sum;
    return ret;
}

ublas::vector<double> stable_softmax(const ublas::vector<double> &v)
{
    double max = v.empty() ? 0.0 : *std::max_element(v.begin(), v.end());
    double sum = 0;
    for (auto x : v)
        sum += exp(x - max);
    ublas::vector<double> ret(v.size());
    for (int i = 0;  i < v.size();  ++i)
        ret[i] = exp(v[i] - max) / sum;
    return ret;
}

double magnitude(const ublas::vector<double>& v)
{
    return ublas_magnitude(v);
}

double naive_magnitude(const ublas::vector<double>& v)
{
    double ret = 0.0;
    for (auto x : v)
        ret += x * x;
    return sqrt(ret);
}

double ublas_magnitude(const ublas::vector<double>& v)
{
    return ublas::norm_2(v);
}

double dot_product(const ublas::vector<double>& v1, const ublas::vector<double>& v2)
{
    return ublas_dot_product(v1, v2);
}

double naive_dot_product(const ublas::vector<double>& v1, const ublas::vector<double>& v2)
{
    double ret = 0.0;
    for (int i = 0;  i < v1.size();  ++i)
        ret += v1[i] * v2[i];
    return ret;
}

double ublas_dot_product(const ublas::vector<double>& v1, const ublas::vector<double>& v2)
{
    return ublas::inner_prod(v1, v2);
}

double cosine_distance(const ublas::vector<double>& v1, const ublas::vector<double>& v2)
{
    return dot_product(v1, v2) / (magnitude(v1) * magnitude(v2));
}

std::vector<int> nearest_neighbors(const ublas::vector<double>& v, const std::vector<ublas::vector<double>>& points)
{
    std::vector<std::pair<double, int>> distances(points.size());
    for (int i = 0;  i < points.size();  ++i)
        distances[i] = std::make_pair(cosine_distance(v, points[i]), i);
    std::sort(distances.begin(), distances.end(), std::greater<>());
    std::vector<int> ret(points.size());
    for (int i = 0;  i < points.size();  ++i)
        ret[i] = distances[i].second;
    return ret;
}

void add(std::vector<ublas::vector<double>>& to, const std::vector<ublas::vector<double>>& other)
{
    for (int i = 0;  i < to.size();  ++i)
        to[i] += other[i];
}

void add(std::vector<ublas::matrix<double>>& to, const std::vector<ublas::matrix<double>>& other)
{
    for (int i = 0;  i < to.size();  ++i)
        to[i] += other[i];
}

void normalize(std::vector<ublas::vector<double>>& to, double denom)
{
    for (auto& d : to)
        d /= denom;
}

void normalize(std::vector<ublas::matrix<double>>& to, double denom)
{
    for (auto& d : to)
        d /= denom;
}

} // namespace toynet


/*
 * Copyright Nick Thompson, 2020
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#include <iostream>
#include <boost/core/demangle.hpp>
#include <boost/hana/for_each.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>

#include <boost/multiprecision/float128.hpp>
#include <boost/math/special_functions/daubechies_scaling.hpp>
#include <boost/math/tools/ulps_plot.hpp>
#include <quicksvg/graph_fn.hpp>

using boost::multiprecision::float128;
constexpr const int GRAPH_WIDTH = 300;

template<typename Real, int p>
void plot_phi(int grid_refinements = -1)
{
    auto phi = boost::math::daubechies_scaling<Real, p>();
    if (grid_refinements >= 0)
    {
        phi = boost::math::daubechies_scaling<Real, p>(grid_refinements);
    }
    Real a = 0;
    Real b = phi.support().second;
    std::string title = "Daubechies " + std::to_string(p) + " scaling function";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_scaling.svg";
    int samples = 1024;
    quicksvg::graph_fn daub(a, b, title, filename, samples, GRAPH_WIDTH);
    daub.set_gridlines(8, 2*p-1);
    daub.set_stroke_width(1);
    daub.add_fn(phi);
    daub.write_all();
}

template<typename Real, int p>
void plot_dphi(int grid_refinements = -1)
{
    auto phi = boost::math::daubechies_scaling<Real, p>();
    if (grid_refinements >= 0)
    {
        phi = boost::math::daubechies_scaling<Real, p>(grid_refinements);
    }
    Real a = 0;
    Real b = phi.support().second;
    std::string title = "Daubechies " + std::to_string(p) + " scaling function derivative";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_scaling_prime.svg";
    int samples = 1024;
    quicksvg::graph_fn daub(a, b, title, filename, samples, GRAPH_WIDTH);
    daub.set_stroke_width(1);
    daub.set_gridlines(8, 2*p-1);
    auto dphi = [phi](Real x)->Real { return phi.prime(x); };
    daub.add_fn(dphi);
    daub.write_all();
}

template<typename Real, int p>
void plot_convergence()
{
    auto phi0 = boost::math::daubechies_scaling<Real, p>(0);
    Real a = 0;
    Real b = phi0.support().second;
    std::string title = "Daubechies " + std::to_string(p) + " scaling at 0 (green), 1 (orange), 2 (red), and 24 (blue) grid refinements";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_scaling_convergence.svg";

    quicksvg::graph_fn daub(a, b, title, filename, 1024, 900);
    daub.set_stroke_width(1);
    daub.set_gridlines(8, 2*p-1);

    daub.add_fn(phi0, "green");
    auto phi1 = boost::math::daubechies_scaling<Real, p>(1);
    daub.add_fn(phi1, "orange");
    auto phi2 = boost::math::daubechies_scaling<Real, p>(2);
    daub.add_fn(phi2, "red");

    auto phi21 = boost::math::daubechies_scaling<Real, p>(21);
    daub.add_fn(phi21);

    daub.write_all();
}

template<typename Real, int p>
void plot_condition_number()
{
    using std::abs;
    using std::log;
    static_assert(p >= 3, "p = 2 is not differentiable, so condition numbers cannot be effectively evaluated.");
    auto phi = boost::math::daubechies_scaling<Real, p>();
    Real a = std::sqrt(std::numeric_limits<Real>::epsilon());
    Real b = phi.support().second - 1000*std::sqrt(std::numeric_limits<Real>::epsilon());
    std::string title = "log10 of condition number of function evaluation for Daubechies " + std::to_string(p) + " scaling function.";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_scaling_condition_number.svg";

    quicksvg::graph_fn daub(a, b, title, filename, 2048, GRAPH_WIDTH);
    daub.set_stroke_width(1);
    daub.set_gridlines(8, 2*p-1);

    auto cond = [&phi](Real x)
    {
        Real y = phi(x);
        Real dydx = phi.prime(x);
        Real z = abs(x*dydx/y);
        using std::isnan;
        if (z==0)
        {
            return Real(-1);
        }
        if (isnan(z))
        {
            // Graphing libraries don't like nan's:
            return Real(1);
        }
        return log10(z);
    };
    daub.add_fn(cond);
    daub.write_all();
}

template<typename CoarseReal, typename PreciseReal, int p, class PhiPrecise>
void do_ulp(int coarse_refinements, PhiPrecise phi_precise)
{
    auto phi_coarse = boost::math::daubechies_scaling<CoarseReal, p>(coarse_refinements);

    std::string title = std::to_string(p) + " vanishing moment ULP plot at " + std::to_string(coarse_refinements) + " refinements and " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    title = "";

    std::string filename = "daubechies_" + std::to_string(p) + "_" + boost::core::demangle(typeid(CoarseReal).name()) + "_" + std::to_string(coarse_refinements) + "_refinements.svg";
    int samples = 20000;
    int clip = 10;
    int horizontal_lines = 8;
    int vertical_lines = 2*p - 1;
    auto [a, b] = phi_coarse.support();
    auto plot = boost::math::tools::ulps_plot<decltype(phi_precise), PreciseReal, CoarseReal>(phi_precise, a, b, samples);
    plot.clip(clip).width(GRAPH_WIDTH).horizontal_lines(horizontal_lines).vertical_lines(vertical_lines).ulp_envelope(false);

    plot.background_color("white").font_color("black");
    plot.add_fn(phi_coarse);
    plot.write(filename);
}

int main()
{
    boost::hana::for_each(std::make_index_sequence<18>(), [&](auto i){ plot_phi<double, i+2>(); });
    boost::hana::for_each(std::make_index_sequence<17>(), [&](auto i){ plot_dphi<double, i+3>(); });
    boost::hana::for_each(std::make_index_sequence<17>(), [&](auto i){ plot_condition_number<double, i+3>(); });
    boost::hana::for_each(std::make_index_sequence<18>(), [&](auto i){ plot_convergence<double, i+2>(); });

    using PreciseReal = float128;
    using CoarseReal = double;
    int precise_refinements = 23;
    constexpr const int p = 8;
    std::cout << "Computing precise scaling function in " << boost::core::demangle(typeid(PreciseReal).name()) << " precision.\n";
    auto phi_precise = boost::math::daubechies_scaling<PreciseReal, p>(precise_refinements);
    std::cout << "Beginning comparison with functions computed in " << boost::core::demangle(typeid(CoarseReal).name()) << " precision.\n";
    for (int i = 7; i <= precise_refinements-1; ++i)
    {
        std::cout << "\tCoarse refinement " << i << "\n";
        do_ulp<CoarseReal, PreciseReal, p>(i, phi_precise);
    }
}


#include "polynomial.h"
#include <Eigen/Eigen>

namespace Chaf
{
	PolynomialInterpolation::PolynomialInterpolation(const std::vector<glm::f64vec2>& points)
	{
		// Ax=y, we want x
		size_t dim = points.size();
		Eigen::MatrixXd A(dim, dim);
		Eigen::VectorXd y(dim);
		m_params.resize(dim);

		for (uint32_t i = 0; i < points.size(); i++)
		{
			for (uint32_t j = 0; j < points.size(); j++)
			{
				A(i, j) = pow(points[i].x, (double)j);
			}

			y(i) = points[i].y;
		}

		Eigen::VectorXd x = A.colPivHouseholderQr().solve(y);

		for (uint32_t i = 0; i < points.size(); i++)
		{
			m_params[i] = x(i);
		}
	}

	double PolynomialInterpolation::evaluate(double x)
	{
		double result = 0.f;

		for (uint32_t i = 0; i < m_params.size(); i++)
		{
			result += m_params[i] * std::pow(x, static_cast<double>(i));
		}

		return result;
	}

	std::vector<double> PolynomialInterpolation::evaluate(const std::vector<double>& x)
	{
		std::vector<double> result(x.size());

		for (uint32_t i = 0; i < result.size(); i++)
		{
			result[i] = evaluate(x[i]);
		}
		return result;
	}
}


/*
 *  (C) Copyright Nick Thompson 2018.
 *  (C) Copyright Matt Borland 2021.
 *  Use, modification and distribution are subject to the
 *  Boost Software License, Version 1.0. (See accompanying file
 *  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <iomanip>
#include <vector>
#include <array>
#include <forward_list>
#include <algorithm>
#include <random>
#include <tuple>
#include <cmath>
#include "math_unit_test.hpp"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/statistics/bivariate_statistics.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_complex.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>

using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_complex_50;

/*
 * Test checklist:
 * 1) Does it work with multiprecision?
 * 2) Does it work with .cbegin()/.cend() if the data is not altered?
 * 3) Does it work with ublas and std::array? (Checking Eigen and Armadillo will make the CI system really unhappy.)
 * 4) Does it work with std::forward_list if a forward iterator is all that is required?
 * 5) Does it work with complex data if complex data is sensible?
 */

using  boost::math::statistics::means_and_covariance;
using  boost::math::statistics::covariance;

#ifndef BOOST_NO_CXX17_HDR_EXECUTION
#include <execution>

template<typename Real, typename ExecutionPolicy>
void test_covariance(ExecutionPolicy&& exec)
{
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10+1);
    Real tol = std::numeric_limits<Real>::epsilon();
    using std::abs;

    // Covariance of a single thing is zero:
    std::array<Real, 1> u1{8};
    std::array<Real, 1> v1{17};
    std::tuple<Real, Real, Real> temp = means_and_covariance(exec, u1, v1);
    Real mu_u1 = std::get<0>(temp);
    Real mu_v1 = std::get<1>(temp);
    Real cov1 = std::get<2>(temp);

    CHECK_LE(abs(cov1), tol);
    CHECK_LE(abs(mu_u1 - 8), tol);
    CHECK_LE(abs(mu_v1 - 17), tol);

    std::array<Real, 2> u2{8, 4};
    std::array<Real, 2> v2{3, 7};
    temp = means_and_covariance(exec, u2, v2);
    Real mu_u2 = std::get<0>(temp);
    Real mu_v2 = std::get<1>(temp);
    Real cov2 = std::get<2>(temp);

    CHECK_LE(abs(cov2+4), tol);
    CHECK_LE(abs(mu_u2 - 6), tol);
    CHECK_LE(abs(mu_v2 - 5), tol);

    std::vector<Real> u3{1,2,3};
    std::vector<Real> v3{1,1,1};

    temp = means_and_covariance(exec, u3, v3);
    Real mu_u3 = std::get<0>(temp);
    Real mu_v3 = std::get<1>(temp);
    Real cov3 = std::get<2>(temp);

    // Since v is constant, covariance(u,v) = 0 against everything any u:
    CHECK_LE(abs(cov3), tol);
    CHECK_LE(abs(mu_u3 - 2), tol);
    CHECK_LE(abs(mu_v3 - 1), tol);
    // Make sure we pull the correct symbol out of means_and_covariance:
    cov3 = covariance(exec, u3, v3);
    CHECK_LE(abs(cov3), tol);

    cov3 = covariance(exec, v3, u3);
    // Covariance is symmetric: cov(u,v) = cov(v,u)
    CHECK_LE(abs(cov3), tol);

    // cov(u,u) = sigma(u)^2:
    cov3 = covariance(exec, u3, u3);
    Real expected = Real(2)/Real(3);

    CHECK_LE(abs(cov3 - expected), tol);

    std::mt19937 gen(15);
    // Can't template standard library on multiprecision, so use double and cast back:
    std::uniform_real_distribution<double> dis(-1.0, 1.0);
    std::vector<Real> u(500);
    std::vector<Real> v(500);
    for(size_t i = 0; i < u.size(); ++i)
    {
        u[i] = (Real) dis(gen);
        v[i] = (Real) dis(gen);
    }

    Real mu_u = boost::math::statistics::mean(u);
    Real mu_v = boost::math::statistics::mean(v);
    Real sigma_u_sq = boost::math::statistics::variance(u);
    Real sigma_v_sq = boost::math::statistics::variance(v);

    temp = means_and_covariance(exec, u, v);
    Real mu_u_ = std::get<0>(temp);
    Real mu_v_ = std::get<1>(temp);
    Real cov_uv = std::get<2>(temp);

    CHECK_LE(abs(mu_u - mu_u_), tol);
    CHECK_LE(abs(mu_v - mu_v_), tol);

    // Cauchy-Schwartz inequality:
    CHECK_LE(cov_uv*cov_uv, sigma_u_sq*sigma_v_sq);
    // cov(X, X) = sigma(X)^2:
    Real cov_uu = covariance(exec, u, u);
    CHECK_LE(abs(cov_uu - sigma_u_sq), tol);
    Real cov_vv = covariance(exec, v, v);
    CHECK_LE(abs(cov_vv - sigma_v_sq), tol);
}

template<typename Z, typename ExecutionPolicy>
void test_integer_covariance(ExecutionPolicy&& exec)
{
    std::cout << std::setprecision(std::numeric_limits<double>::digits10+1);
    double tol = std::numeric_limits<double>::epsilon();
    using std::abs;

    // Covariance of a single thing is zero:
    std::array<Z, 1> u1{8};
    std::array<Z, 1> v1{17};
    std::tuple<double, double, double> temp = means_and_covariance(exec, u1, v1);
    double mu_u1 = std::get<0>(temp);
    double mu_v1 = std::get<1>(temp);
    double cov1 = std::get<2>(temp);

    CHECK_LE(abs(cov1), tol);
    CHECK_LE(abs(mu_u1 - 8), tol);
    CHECK_LE(abs(mu_v1 - 17), tol);

    std::array<Z, 2> u2{8, 4};
    std::array<Z, 2> v2{3, 7};
    temp = means_and_covariance(exec, u2, v2);
    double mu_u2 = std::get<0>(temp);
    double mu_v2 = std::get<1>(temp);
    double cov2 = std::get<2>(temp);

    CHECK_LE(abs(cov2+4), tol);
    CHECK_LE(abs(mu_u2 - 6), tol);
    CHECK_LE(abs(mu_v2 - 5), tol);

    std::vector<Z> u3{1,2,3};
    std::vector<Z> v3{1,1,1};

    temp = means_and_covariance(exec, u3, v3);
    double mu_u3 = std::get<0>(temp);
    double mu_v3 = std::get<1>(temp);
    double cov3 = std::get<2>(temp);

    // Since v is constant, covariance(u,v) = 0 against everything any u:
    CHECK_LE(abs(cov3), tol);
    CHECK_LE(abs(mu_u3 - 2), tol);
    CHECK_LE(abs(mu_v3 - 1), tol);
    // Make sure we pull the correct symbol out of means_and_covariance:
    cov3 = covariance(exec, u3, v3);
    CHECK_LE(abs(cov3), tol);

    cov3 = covariance(exec, v3, u3);
    // Covariance is symmetric: cov(u,v) = cov(v,u)
    CHECK_LE(abs(cov3), tol);

    // cov(u,u) = sigma(u)^2:
    cov3 = covariance(exec, u3, u3);
    double expected = double(2)/double(3);

    CHECK_LE(abs(cov3 - expected), tol);

    std::mt19937 gen(15);
    // Can't template standard library on multiprecision, so use double and cast back:
    std::uniform_real_distribution<double> dis(-1.0, 1.0);
    std::vector<Z> u(500);
    std::vector<Z> v(500);
    for(size_t i = 0; i < u.size(); ++i)
    {
        u[i] = (Z) dis(gen);
        v[i] = (Z) dis(gen);
    }

    double mu_u = boost::math::statistics::mean(u);
    double mu_v = boost::math::statistics::mean(v);
    double sigma_u_sq = boost::math::statistics::variance(u);
    double sigma_v_sq = boost::math::statistics::variance(v);

    temp = means_and_covariance(exec, u, v);
    double mu_u_ = std::get<0>(temp);
    double mu_v_ = std::get<1>(temp);
    double cov_uv = std::get<2>(temp);

    CHECK_LE(abs(mu_u - mu_u_), tol);
    CHECK_LE(abs(mu_v - mu_v_), tol);

    // Cauchy-Schwartz inequality:
    CHECK_LE(cov_uv*cov_uv, sigma_u_sq*sigma_v_sq);
    // cov(X, X) = sigma(X)^2:
    double cov_uu = covariance(exec, u, u);
    CHECK_LE(abs(cov_uu - sigma_u_sq), tol);
    double cov_vv = covariance(exec, v, v);
    CHECK_LE(abs(cov_vv - sigma_v_sq), tol);
}

template<typename Real, typename ExecutionPolicy>
void test_correlation_coefficient(ExecutionPolicy&& exec)
{
    using boost::math::statistics::correlation_coefficient;
    using std::abs;
    using std::sqrt;

    Real tol = std::numeric_limits<Real>::epsilon();
    std::vector<Real> u{1};
    std::vector<Real> v{1};
    Real rho_uv = correlation_coefficient(exec, u, v);
    CHECK_NAN(rho_uv);

    u = {1,1};
    v = {1,1};
    rho_uv = correlation_coefficient(exec, u, v);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {1, 2, 3};
    rho_uv = correlation_coefficient(exec, u, v);
    CHECK_LE(abs(rho_uv - 1), tol);

    u = {1, 2, 3};
    v = {-1, -2, -3};
    rho_uv = correlation_coefficient(exec, u, v);
    CHECK_LE(abs(rho_uv + 1), tol);

    rho_uv = correlation_coefficient(exec, v, u);
    CHECK_LE(abs(rho_uv + 1), tol);

    u = {1, 2, 3};
    v = {0, 0, 0};
    rho_uv = correlation_coefficient(exec, v, u);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {0, 0, 3};
    rho_uv = correlation_coefficient(exec, v, u);
    // mu_u = 2, sigma_u^2 = 2/3, mu_v = 1, sigma_v^2 = 2, cov(u,v) = 1.
    CHECK_LE(abs(rho_uv - sqrt(Real(3))/Real(2)), tol);
}

template<typename Z, typename ExecutionPolicy>
void test_integer_correlation_coefficient(ExecutionPolicy&& exec)
{
    using boost::math::statistics::correlation_coefficient;
    using std::abs;
    using std::sqrt;

    double tol = std::numeric_limits<double>::epsilon();
    std::vector<Z> u{1};
    std::vector<Z> v{1};
    double rho_uv = correlation_coefficient(exec, u, v);
    CHECK_NAN(rho_uv);

    u = {1,1};
    v = {1,1};
    rho_uv = correlation_coefficient(exec, u, v);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {1, 2, 3};
    rho_uv = correlation_coefficient(exec, u, v);
    CHECK_LE(abs(rho_uv - 1.0), tol);

    rho_uv = correlation_coefficient(exec, v, u);
    CHECK_LE(abs(rho_uv - 1.0), tol);

    u = {1, 2, 3};
    v = {0, 0, 0};
    rho_uv = correlation_coefficient(exec, v, u);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {0, 0, 3};
    rho_uv = correlation_coefficient(exec, v, u);
    // mu_u = 2, sigma_u^2 = 2/3, mu_v = 1, sigma_v^2 = 2, cov(u,v) = 1.
    CHECK_LE(abs(rho_uv - sqrt(double(3))/double(2)), tol);
}

int main()
{
    test_covariance<float>(std::execution::seq);
    test_covariance<float>(std::execution::par);
    test_covariance<double>(std::execution::seq);
    test_covariance<double>(std::execution::par);
    test_covariance<long double>(std::execution::seq);
    test_covariance<long double>(std::execution::par);
    test_covariance<cpp_bin_float_50>(std::execution::seq);
    test_covariance<cpp_bin_float_50>(std::execution::par);

    test_integer_covariance<int>(std::execution::seq);
    test_integer_covariance<int>(std::execution::par);
    test_integer_covariance<int32_t>(std::execution::seq);
    test_integer_covariance<int32_t>(std::execution::par);
    test_integer_covariance<int64_t>(std::execution::seq);
    test_integer_covariance<int64_t>(std::execution::par);
    test_integer_covariance<uint32_t>(std::execution::seq);
    test_integer_covariance<uint32_t>(std::execution::par);

    test_correlation_coefficient<float>(std::execution::seq);
    test_correlation_coefficient<float>(std::execution::par);
    test_correlation_coefficient<double>(std::execution::seq);
    test_correlation_coefficient<double>(std::execution::par);
    test_correlation_coefficient<long double>(std::execution::seq);
    test_correlation_coefficient<long double>(std::execution::par);
    test_correlation_coefficient<cpp_bin_float_50>(std::execution::seq);
    test_correlation_coefficient<cpp_bin_float_50>(std::execution::par);

    test_integer_correlation_coefficient<int>(std::execution::seq);
    test_integer_correlation_coefficient<int>(std::execution::par);
    test_integer_correlation_coefficient<int32_t>(std::execution::seq);
    test_integer_correlation_coefficient<int32_t>(std::execution::par);
    test_integer_correlation_coefficient<int64_t>(std::execution::seq);
    test_integer_correlation_coefficient<int64_t>(std::execution::par);
    test_integer_correlation_coefficient<uint32_t>(std::execution::seq);
    test_integer_correlation_coefficient<uint32_t>(std::execution::par);

    return boost::math::test::report_errors();
}

#else

template<typename Real>
void test_covariance()
{
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10+1);
    Real tol = std::numeric_limits<Real>::epsilon();
    using std::abs;

    // Covariance of a single thing is zero:
    std::array<Real, 1> u1{8};
    std::array<Real, 1> v1{17};
    std::tuple<Real, Real, Real> temp = means_and_covariance(u1, v1);
    Real mu_u1 = std::get<0>(temp);
    Real mu_v1 = std::get<1>(temp);
    Real cov1 = std::get<2>(temp);

    CHECK_LE(abs(cov1), tol);
    CHECK_LE(abs(mu_u1 - 8), tol);
    CHECK_LE(abs(mu_v1 - 17), tol);

    std::array<Real, 2> u2{8, 4};
    std::array<Real, 2> v2{3, 7};
    temp = means_and_covariance(u2, v2);
    Real mu_u2 = std::get<0>(temp);
    Real mu_v2 = std::get<1>(temp);
    Real cov2 = std::get<2>(temp);

    CHECK_LE(abs(cov2+4), tol);
    CHECK_LE(abs(mu_u2 - 6), tol);
    CHECK_LE(abs(mu_v2 - 5), tol);

    std::vector<Real> u3{1,2,3};
    std::vector<Real> v3{1,1,1};

    temp = means_and_covariance(u3, v3);
    Real mu_u3 = std::get<0>(temp);
    Real mu_v3 = std::get<1>(temp);
    Real cov3 = std::get<2>(temp);

    // Since v is constant, covariance(u,v) = 0 against everything any u:
    CHECK_LE(abs(cov3), tol);
    CHECK_LE(abs(mu_u3 - 2), tol);
    CHECK_LE(abs(mu_v3 - 1), tol);
    // Make sure we pull the correct symbol out of means_and_covariance:
    cov3 = covariance(u3, v3);
    CHECK_LE(abs(cov3), tol);

    cov3 = covariance(v3, u3);
    // Covariance is symmetric: cov(u,v) = cov(v,u)
    CHECK_LE(abs(cov3), tol);

    // cov(u,u) = sigma(u)^2:
    cov3 = covariance(u3, u3);
    Real expected = Real(2)/Real(3);

    CHECK_LE(abs(cov3 - expected), tol);

    std::mt19937 gen(15);
    // Can't template standard library on multiprecision, so use double and cast back:
    std::uniform_real_distribution<double> dis(-1.0, 1.0);
    std::vector<Real> u(500);
    std::vector<Real> v(500);
    for(size_t i = 0; i < u.size(); ++i)
    {
        u[i] = (Real) dis(gen);
        v[i] = (Real) dis(gen);
    }

    Real mu_u = boost::math::statistics::mean(u);
    Real mu_v = boost::math::statistics::mean(v);
    Real sigma_u_sq = boost::math::statistics::variance(u);
    Real sigma_v_sq = boost::math::statistics::variance(v);

    temp = means_and_covariance(u, v);
    Real mu_u_ = std::get<0>(temp);
    Real mu_v_ = std::get<1>(temp);
    Real cov_uv = std::get<2>(temp);

    CHECK_LE(abs(mu_u - mu_u_), tol);
    CHECK_LE(abs(mu_v - mu_v_), tol);

    // Cauchy-Schwartz inequality:
    CHECK_LE(cov_uv*cov_uv, sigma_u_sq*sigma_v_sq);
    // cov(X, X) = sigma(X)^2:
    Real cov_uu = covariance(u, u);
    CHECK_LE(abs(cov_uu - sigma_u_sq), tol);
    Real cov_vv = covariance(v, v);
    CHECK_LE(abs(cov_vv - sigma_v_sq), tol);
}

template<typename Z>
void test_integer_covariance()
{
    std::cout << std::setprecision(std::numeric_limits<double>::digits10+1);
    double tol = std::numeric_limits<double>::epsilon();
    using std::abs;

    // Covariance of a single thing is zero:
    std::array<Z, 1> u1{8};
    std::array<Z, 1> v1{17};
    std::tuple<double, double, double> temp = means_and_covariance(u1, v1);
    double mu_u1 = std::get<0>(temp);
    double mu_v1 = std::get<1>(temp);
    double cov1 = std::get<2>(temp);

    CHECK_LE(abs(cov1), tol);
    CHECK_LE(abs(mu_u1 - 8), tol);
    CHECK_LE(abs(mu_v1 - 17), tol);

    std::array<Z, 2> u2{8, 4};
    std::array<Z, 2> v2{3, 7};
    temp = means_and_covariance(u2, v2);
    double mu_u2 = std::get<0>(temp);
    double mu_v2 = std::get<1>(temp);
    double cov2 = std::get<2>(temp);

    CHECK_LE(abs(cov2+4), tol);
    CHECK_LE(abs(mu_u2 - 6), tol);
    CHECK_LE(abs(mu_v2 - 5), tol);

    std::vector<Z> u3{1,2,3};
    std::vector<Z> v3{1,1,1};

    temp = means_and_covariance(u3, v3);
    double mu_u3 = std::get<0>(temp);
    double mu_v3 = std::get<1>(temp);
    double cov3 = std::get<2>(temp);

    // Since v is constant, covariance(u,v) = 0 against everything any u:
    CHECK_LE(abs(cov3), tol);
    CHECK_LE(abs(mu_u3 - 2), tol);
    CHECK_LE(abs(mu_v3 - 1), tol);
    // Make sure we pull the correct symbol out of means_and_covariance:
    cov3 = covariance(u3, v3);
    CHECK_LE(abs(cov3), tol);

    cov3 = covariance(v3, u3);
    // Covariance is symmetric: cov(u,v) = cov(v,u)
    CHECK_LE(abs(cov3), tol);

    // cov(u,u) = sigma(u)^2:
    cov3 = covariance(u3, u3);
    double expected = double(2)/double(3);

    CHECK_LE(abs(cov3 - expected), tol);

    std::mt19937 gen(15);
    // Can't template standard library on multiprecision, so use double and cast back:
    std::uniform_real_distribution<double> dis(-1.0, 1.0);
    std::vector<Z> u(500);
    std::vector<Z> v(500);
    for(size_t i = 0; i < u.size(); ++i)
    {
        u[i] = (Z) dis(gen);
        v[i] = (Z) dis(gen);
    }

    double mu_u = boost::math::statistics::mean(u);
    double mu_v = boost::math::statistics::mean(v);
    double sigma_u_sq = boost::math::statistics::variance(u);
    double sigma_v_sq = boost::math::statistics::variance(v);

    temp = means_and_covariance(u, v);
    double mu_u_ = std::get<0>(temp);
    double mu_v_ = std::get<1>(temp);
    double cov_uv = std::get<2>(temp);

    CHECK_LE(abs(mu_u - mu_u_), tol);
    CHECK_LE(abs(mu_v - mu_v_), tol);

    // Cauchy-Schwartz inequality:
    CHECK_LE(cov_uv*cov_uv, sigma_u_sq*sigma_v_sq);
    // cov(X, X) = sigma(X)^2:
    double cov_uu = covariance(u, u);
    CHECK_LE(abs(cov_uu - sigma_u_sq), tol);
    double cov_vv = covariance(v, v);
    CHECK_LE(abs(cov_vv - sigma_v_sq), tol);
}

template<typename Real>
void test_correlation_coefficient()
{
    using boost::math::statistics::correlation_coefficient;
    using std::abs;
    using std::sqrt;

    Real tol = std::numeric_limits<Real>::epsilon();
    std::vector<Real> u{1};
    std::vector<Real> v{1};
    Real rho_uv = correlation_coefficient(u, v);
    CHECK_NAN(rho_uv);

    u = {1,1};
    v = {1,1};
    rho_uv = correlation_coefficient(u, v);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {1, 2, 3};
    rho_uv = correlation_coefficient(u, v);
    CHECK_LE(abs(rho_uv - 1), tol);

    u = {1, 2, 3};
    v = {-1, -2, -3};
    rho_uv = correlation_coefficient(u, v);
    CHECK_LE(abs(rho_uv + 1), tol);

    rho_uv = correlation_coefficient(v, u);
    CHECK_LE(abs(rho_uv + 1), tol);

    u = {1, 2, 3};
    v = {0, 0, 0};
    rho_uv = correlation_coefficient(v, u);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {0, 0, 3};
    rho_uv = correlation_coefficient(v, u);
    // mu_u = 2, sigma_u^2 = 2/3, mu_v = 1, sigma_v^2 = 2, cov(u,v) = 1.
    CHECK_LE(abs(rho_uv - sqrt(Real(3))/Real(2)), tol);
}

template<typename Z>
void test_integer_correlation_coefficient()
{
    using boost::math::statistics::correlation_coefficient;
    using std::abs;
    using std::sqrt;

    double tol = std::numeric_limits<double>::epsilon();
    std::vector<Z> u{1};
    std::vector<Z> v{1};
    double rho_uv = correlation_coefficient(u, v);
    CHECK_NAN(rho_uv);

    u = {1,1};
    v = {1,1};
    rho_uv = correlation_coefficient(u, v);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {1, 2, 3};
    rho_uv = correlation_coefficient(u, v);
    CHECK_LE(abs(rho_uv - 1.0), tol);

    rho_uv = correlation_coefficient(v, u);
    CHECK_LE(abs(rho_uv - 1.0), tol);

    u = {1, 2, 3};
    v = {0, 0, 0};
    rho_uv = correlation_coefficient(v, u);
    CHECK_NAN(rho_uv);

    u = {1, 2, 3};
    v = {0, 0, 3};
    rho_uv = correlation_coefficient(v, u);
    // mu_u = 2, sigma_u^2 = 2/3, mu_v = 1, sigma_v^2 = 2, cov(u,v) = 1.
    CHECK_LE(abs(rho_uv - sqrt(double(3))/double(2)), tol);
}

int main()
{
    test_covariance<float>();
    test_covariance<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_covariance<long double>();
#endif
    test_covariance<cpp_bin_float_50>();

    test_integer_covariance<int>();
    test_integer_covariance<int32_t>();
    test_integer_covariance<int64_t>();
    test_integer_covariance<uint32_t>();

    test_correlation_coefficient<float>();
    test_correlation_coefficient<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_correlation_coefficient<long double>();
#endif
    test_correlation_coefficient<cpp_bin_float_50>();

    test_integer_correlation_coefficient<int>();
    test_integer_correlation_coefficient<int32_t>();
    test_integer_correlation_coefficient<int64_t>();
    test_integer_correlation_coefficient<uint32_t>();

    return boost::math::test::report_errors();
}

#endif


#include <iostream>

#include <Eigen/Core>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

TEST_CASE("Test finite difference hessian of quadratic", "[hessian]")
{
    int n = GENERATE(1, 2, 4, 10, 25);

    // f(x) = xᵀAx + bᵀx
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(n, n);
    Eigen::VectorXd b = Eigen::VectorXd::Random(n);

    const auto f = [&](const Eigen::VectorXd x) -> double {
        return (x.transpose() * A * x + b.transpose() * x)(0);
    };

    Eigen::VectorXd x = Eigen::VectorXd::Random(n);

    Eigen::MatrixXd hess = A + A.transpose();

    Eigen::MatrixXd fhess;
    fd::finite_hessian(x, f, fhess);

    CAPTURE(n);
    CHECK(fd::compare_hessian(hess, fhess));
}

TEST_CASE("Test finite difference hessian of Rosenbrock", "[hessian]")
{
    const auto f = [](const Eigen::VectorXd x) {
        double t1 = 1 - x[0];
        double t2 = (x[1] - x[0] * x[0]);
        return t1 * t1 + 100 * t2 * t2;
    };

    Eigen::VectorXd x = Eigen::Vector2d::Random();

    Eigen::MatrixXd hess(2, 2);
    hess(0, 0) = 1200 * x[0] * x[0] - 400 * x[1] + 2;
    hess(0, 1) = -400 * x[0];
    hess(1, 0) = -400 * x[0];
    hess(1, 1) = 200;

    Eigen::MatrixXd fhess;
    fd::finite_hessian(x, f, fhess);

    CHECK(fd::compare_hessian(hess, fhess));
}

TEST_CASE("Test finite difference hessian of trig", "[hessian]")
{
    int n = GENERATE(1, 2, 4, 10, 25);

    const auto f = [&](const Eigen::VectorXd x) -> double {
        return x.array().sin().matrix().squaredNorm();
    };

    Eigen::VectorXd x = Eigen::VectorXd::Random(n);

    Eigen::ArrayXd sin_x = x.array().sin(), cos_x = x.array().cos();
    Eigen::MatrixXd hess = Eigen::MatrixXd::Zero(n, n);
    hess.diagonal() = 2 * (cos_x * cos_x) - 2 * (sin_x * sin_x);

    Eigen::MatrixXd fhess;
    fd::finite_hessian(x, f, fhess);

    CHECK(fd::compare_hessian(hess, fhess));
}


/**
 * Write a function 'filter()' that implements a multi-
 *   dimensional Kalman Filter for the example given
 */

#include <iostream>
#include <vector>
#include <Eigen>

using std::cout;
using std::endl;
using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;

// Kalman Filter variables
VectorXd x;	// object state
MatrixXd P;	// object covariance matrix
VectorXd u;	// external motion
MatrixXd F; // state transition matrix
MatrixXd H;	// measurement matrix
MatrixXd R;	// measurement covariance matrix
MatrixXd I; // Identity matrix
MatrixXd Q;	// process covariance matrix

vector<VectorXd> measurements;
void filter(VectorXd &x, MatrixXd &P);

int main() {
  /**
   * Code used as example to work with Eigen matrices
   */
  // design the KF with 1D motion
  x = VectorXd(2);
  x << 0, 0;
  P = MatrixXd(2, 2);
  P << 1000, 0, 0, 1000;
  u = VectorXd(2);
  u << 0, 0;
  F = MatrixXd(2, 2);
  F << 1, 1, 0, 1;
  H = MatrixXd(1, 2);
  H << 1, 0;
  R = MatrixXd(1, 1);
  R << 1;
  I = MatrixXd::Identity(2, 2);
  Q = MatrixXd(2, 2);
  Q << 0, 0, 0, 0;
  // create a list of measurements
  VectorXd single_meas(1);
  single_meas << 1;
  measurements.push_back(single_meas);
  single_meas << 2;
  measurements.push_back(single_meas);
  single_meas << 3;
  measurements.push_back(single_meas);

  // call Kalman filter algorithm
  filter(x, P);

  return 0;
}

void filter(VectorXd &x, MatrixXd &P) {

  for (unsigned int n = 0; n < measurements.size(); ++n) {

    VectorXd z = measurements[n];
    // TODO: YOUR CODE HERE

    // KF Measurement update step
    x=x+(P*H.transpose()*(H*P*H.transpose()+R).inverse())*(z-H*x);
    P=(I-(P*H.transpose()*(H*P*H.transpose()+R).inverse())*H)*P;
    // new state

    // KF Prediction step
    x=F*x+u;
    P=F*P*F.transpose();
    cout << "x=" << endl <<  x << endl;
    cout << "P=" << endl <<  P << endl;
  }
}

//x=
//0.999001
//       0
//P=
//1001 1000
//1000 1000
//x=
//   2.998
//0.999002
//P=
//4.99002 2.99302
//2.99302 1.99501
//x=
//3.99967
//      1
//P=
// 2.33189 0.999168
//0.999168 0.499501


// Arguments: Ints, Ints, Doubles
#include <stan/math/prim.hpp>
#include <boost/math/special_functions/binomial.hpp>

using stan::math::var;
using std::numeric_limits;
using std::vector;

class AgradCdfLogBinomial : public AgradCdfLogTest {
 public:
  void valid_values(vector<vector<double> >& parameters,
                    vector<double>& cdf_log) {
    vector<double> param(3);

    param[0] = 17;   // Successes
    param[1] = 45;   // Trials
    param[2] = 0.5;  // Probability
    parameters.push_back(param);
    cdf_log.push_back(std::log(0.06757822542283530020679));  // expected cdf_log
  }

  void invalid_values(vector<size_t>& index, vector<double>& value) {
    // N (Trials)
    index.push_back(1U);
    value.push_back(-1);

    // p (Probability
    index.push_back(2U);
    value.push_back(-1e-4);

    index.push_back(2U);
    value.push_back(1 + 1e-4);
  }

  bool has_lower_bound() { return false; }

  bool has_upper_bound() { return false; }

  template <typename T_n, typename T_N, typename T_prob, typename T3,
            typename T4, typename T5>
  stan::return_type_t<T_prob> cdf_log(const T_n& n, const T_N& N,
                                      const T_prob& theta, const T3&, const T4&,
                                      const T5&) {
    return stan::math::binomial_cdf_log(n, N, theta);
  }

  template <typename T_n, typename T_N, typename T_prob, typename T3,
            typename T4, typename T5>
  stan::return_type_t<T_prob> cdf_log_function(const T_n& n, const T_N& N,
                                               const T_prob& theta, const T3&,
                                               const T4&, const T5&) {
    using boost::math::binomial_coefficient;
    using std::exp;
    using std::log;

    stan::return_type_t<T_prob> cdf(0);

    for (int i = 0; i <= n; i++) {
      cdf += binomial_coefficient<double>(N, i)
             * exp(i * log(theta) + (N - i) * log(1 - theta));
    }

    return log(cdf);
  }
};


// Copyright Christopher Kormanyos 2013.
// Copyright Paul A. Bristow 2013.
// Copyright John Maddock 2013.

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt).

#ifdef _MSC_VER
#  pragma warning (disable : 4512) // assignment operator could not be generated.
#  pragma warning (disable : 4996) // assignment operator could not be generated.
#endif

#include <iostream>
#include <limits>
#include <vector>
#include <algorithm>
#include <iomanip>
#include <iterator>

// Weisstein, Eric W. "Bessel Function Zeros." From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/BesselFunctionZeros.html
// Test values can be calculated using [@wolframalpha.com WolframAplha]
// See also http://dlmf.nist.gov/10.21

//[airy_zeros_example_1

/*`This example demonstrates calculating zeros of the Airy functions.
It also shows how Boost.Math and Boost.Multiprecision can be combined to provide
a many decimal digit precision. For 50 decimal digit precision we need to include
*/

  #include <boost/multiprecision/cpp_dec_float.hpp>

/*`and a `typedef` for `float_type` may be convenient
(allowing a quick switch to re-compute at built-in `double` or other precision)
*/
  typedef boost::multiprecision::cpp_dec_float_50 float_type;

//`To use the functions for finding zeros of the functions we need

  #include <boost/math/special_functions/airy.hpp>

/*`This example shows obtaining both a single zero of the Airy functions,
and then placing multiple zeros into a container like `std::vector` by providing an iterator.
The signature of the single-value Airy Ai function is:

  template <class T>
  T airy_ai_zero(unsigned m); // 1-based index of the zero.

The signature of multiple zeros Airy Ai function is:

  template <class T, class OutputIterator>
  OutputIterator airy_ai_zero(
                           unsigned start_index, // 1-based index of the zero.
                           unsigned number_of_zeros, // How many zeros to generate.
                           OutputIterator out_it); // Destination for zeros.

There are also versions which allows control of the __policy_section for error handling and precision.

  template <class T, class OutputIterator, class Policy>
  OutputIterator airy_ai_zero(
                           unsigned start_index, // 1-based index of the zero.
                           unsigned number_of_zeros, // How many zeros to generate.
                           OutputIterator out_it, // Destination for zeros.
                           const Policy& pol);  // Policy to use.
*/
//] [/airy_zeros_example_1]

int main()
{
  try
  {
//[airy_zeros_example_2

/*`[tip It is always wise to place code using Boost.Math inside `try'n'catch` blocks;
this will ensure that helpful error messages are shown when exceptional conditions arise.]

First, evaluate a single Airy zero.

The precision is controlled by the template parameter `T`,
so this example has `double` precision, at least 15 but up to 17 decimal digits
(for the common 64-bit double).
*/
    double aiz1 = boost::math::airy_ai_zero<double>(1);
    std::cout << "boost::math::airy_ai_zero<double>(1) = " << aiz1 << std::endl;
    double aiz2 = boost::math::airy_ai_zero<double>(2);
    std::cout << "boost::math::airy_ai_zero<double>(2) = " << aiz2 << std::endl;
    double biz3 = boost::math::airy_bi_zero<double>(3);
    std::cout << "boost::math::airy_bi_zero<double>(3) = " << biz3 << std::endl;

/*`Other versions of `airy_ai_zero` and `airy_bi_zero`
allow calculation of multiple zeros with one call,
placing the results in a container, often `std::vector`.
For example, generate and display the first five `double` roots
[@http://mathworld.wolfram.com/AiryFunctionZeros.html Wolfram Airy Functions Zeros].
*/
    unsigned int n_roots = 5U;
    std::vector<double> roots;
    boost::math::airy_ai_zero<double>(1U, n_roots, std::back_inserter(roots));
    std::cout << "airy_ai_zeros:" << std::endl;
    std::copy(roots.begin(),
              roots.end(),
              std::ostream_iterator<double>(std::cout, "\n"));

/*`The first few real roots of Ai(x) are approximately -2.33811, -4.08795, -5.52056, -6.7867144, -7.94413, -9.02265 ...

Or we can use Boost.Multiprecision to generate 50 decimal digit roots.

We set the precision of the output stream, and show trailing zeros to display a fixed 50 decimal digits.
*/
    std::cout.precision(std::numeric_limits<float_type>::digits10); // float_type has 50 decimal digits.
    std::cout << std::showpoint << std::endl; // Show trailing zeros too.

    unsigned int m = 1U;
    float_type r = boost::math::airy_ai_zero<double>(1U); // 1st root.
    std::cout << "boost::math::airy_bi_zero<float_type>(" << m << ")  = " << r << std::endl;
    r = boost::math::airy_ai_zero<float_type>(1U); // 1st root.
    std::cout << "boost::math::airy_bi_zero<float_type>(" << m << ")  = " << r << std::endl;
    m = 7U;
    r = boost::math::airy_bi_zero<float_type>(7U); // 7th root.
    std::cout << "boost::math::airy_bi_zero<float_type>(" << m << ")  = " << r << std::endl;

    std::vector<float_type> zeros;
    boost::math::airy_ai_zero<float_type>(1U, 3, std::back_inserter(zeros));
    std::cout << "airy_ai_zeros:" << std::endl;
    // Print the roots to the output stream.
    std::copy(zeros.begin(), zeros.end(),
              std::ostream_iterator<float_type>(std::cout, "\n"));
//] [/airy_zeros_example_2]
  }
  catch (std::exception ex)
  {
    std::cout << "Thrown exception " << ex.what() << std::endl;
  }

 } // int main()

/*

 Output:

  Description: Autorun "J:\Cpp\big_number\Debug\airy_zeros_example.exe"
  boost::math::airy_ai_zero<double>(1) = -2.33811
  boost::math::airy_ai_zero<double>(2) = -4.08795
  boost::math::airy_bi_zero<double>(3) = -4.83074

  airy_ai_zeros:
  -2.33811
  -4.08795
  -5.52056
  -6.78671
  -7.94413

  boost::math::airy_bi_zero<float_type>(1)  = -2.3381074104597665552773833042010664939880371093750
  boost::math::airy_bi_zero<float_type>(1)  = -2.3381074104597670384891972524467354406385502908783
  boost::math::airy_bi_zero<float_type>(7)  = -9.5381943793462388866329885451560196208390720763825

  airy_ai_zeros:
  -2.3381074104597670384891972524467354406385502908783
  -4.0879494441309706166369887014573910602247646991085
  -5.5205598280955510591298555129312935737972142806175

*/


#include "perceptron.hpp"
#include <cmath>
#include <time.h>
#include <random>
#include <fstream>
#include <stdio.h>
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

mt19937 rng;

perceptron::perceptron()
{
	num_epochs = 5;
}

void perceptron::fit(MatrixXf& X_train, VectorXi& y_train)
{
	cout << "fit" << endl;
	int n = X_train.rows(), dim = X_train.cols();

	theta0 = 0.0;
	theta = VectorXf::Zero(dim, 1);

	uniform_int_distribution<int> unif(0, n-1);
	int k = 1;
	for (int epoch = 0; epoch < num_epochs; ++epoch)
	{
		for (int i = 0; i < n; ++i)
		{
			int idx = unif(rng); //sample random point
			if (y_train[idx] * (theta.dot(X_train.row(idx)) + theta0) <= 1.0) //hinge loss
			{
				float eta = pow(k + 1, -1); k++;  //update learning rate
				theta = theta + eta * y_train[idx] * X_train.row(idx).transpose();  //update theta
				theta0 = theta0 + eta * y_train[idx]; //update intercept
				cout << "eta: " << eta << endl;
			}
		}
		cout << "epoch: " << epoch << endl;
		cout << "theta: " << endl << theta << endl << " theta0: " << theta0 << endl;
	}
}
void perceptron::predict(MatrixXf& X_test, VectorXi& y_test)
{
	cout << "predict" << endl;
	int n = X_test.rows(), dim = X_test.cols();

	int num_errors = 0;
	for (int i = 0; i < n; ++i)
	{
		if (y_test[i] * (theta.dot(X_test.row(i)) + theta0) < 0)
		{
			num_errors++;
		}
	}
	float yerr = (float)num_errors / float(n);
	cout << "Perceptron classification error: " << yerr << endl;
}

void load_iris(MatrixXf& X_train, VectorXi& y_train)
{
	//read-in labels
	int cnt = 0;
	string line;
	ifstream fin1("./data/iris_labels.txt");
	if (fin1.is_open())
	{
		while (getline(fin1, line))
		{
			y_train(cnt) = stoi(line);
			cnt++;
		}
		fin1.close();
	}
	else cout << "Unable to open fin1\n";
	cout << "y = " << endl << y_train << endl;
	cout << "y.size = " << y_train.size() << endl;

	//read-in data
	cnt = 0;
	int nrows = 0;
	int ncols = 0;
	ifstream fin2("./data/iris_data.txt");
	if (fin2.is_open())
	{
		fin2 >> nrows >> ncols;
		for (int row = 0; row < nrows; row++)
			for (int col = 0; col < ncols; col++)
			{
				float num = 0.0;
				fin2 >> num;
				X_train(row, col) = num;
			}
		fin2.close();
	}
	else cout << "Unable to open fin2\n";
	cout << "X = " << endl << X_train << endl;
	cout << "X.size = " << X_train.rows() << " x " << X_train.cols() << endl;
}

int main()
{
    cout << "Perceptron Algorithm" << endl;

	//load iris
	int nrows = 0, ncols = 0;
	string line;
	ifstream fin1("./data/iris_data.txt");
	if (fin1.is_open())
	{
		fin1 >> nrows >> ncols;
		fin1.close();
	}
	else cout << "Unable to open data file\n";
	cout << "nrows = " << nrows << endl;
	cout << "ncols = " << ncols << endl;

	MatrixXf X_train = MatrixXf::Zero(nrows, ncols); //training data n x d
	VectorXi y_train = VectorXi::Zero(nrows);        //training labels n x 1

	load_iris(X_train, y_train);

	perceptron P = perceptron();
	P.fit(X_train, y_train);
	P.predict(X_train, y_train); //TODO: predict on test data

	return 0;
}


#include <Eigen/Dense>

#include <iostream>
#include <iomanip>

#include <cmath>

//! \brief Solve the autonomous IVP y' = f(y), y(0) = y0 using Rosenbrock method
//! Use semi-implicit Rosenbrock method using Jacobian evaluation. Equidistant steps of size T/N.
//! \tparam Func function type for r.h.s. f
//! \tparam DFunc function type for Jacobian df
//! \tparam StateType type of solution space y and initial data y0
//! \param[in] f r.h.s. func f
//! \param[in] df Jacobian df of f
//! \param[in] y0 initial data y(0)
//! \param[in] N number of equidistant steps
//! \param[in] T final time
//! \return vector of y_k for each step k from 0 to N
template <class Func, class DFunc, class StateType>
std::vector<StateType> solveRosenbrock(const Func & f, const DFunc & df,
                                       const StateType & y0, unsigned int N, double T) {
    const double h = T/N;
    const double a = 1. / (std::sqrt(2) + 2.);

    // Will contain all time steps
    std::vector<StateType> res(N+1);
    // Push initial data
    res.at(0) = y0;

    // Some temporary
    StateType k1, k2;
    Eigen::Matrix2d J, W;

    // Main loop: *up to N (performs N steps)*
    for(unsigned int i = 1; i <= N; ++i) {
        StateType & yprev = res.at(i-1);

        // Jacobian computation
        J = df(yprev);
        W = Eigen::Matrix2d::Identity() - a*h*J;

        // Reuse factorization for each step
        auto W_lu = W.partialPivLu();

        // Increments
        k1 = W_lu.solve(f(yprev));
        k2 = W_lu.solve(f(yprev+0.5*h*k1) - a*h*J*k1);

        // Push new step
        res.at(i) = yprev + h*k2;
    }

    return res;
}

int main() {
    // Final time
    const double T = 10;
    // All mesh sizes
    const std::vector<int> N = {16, 32, 64, 128, 256, 512, 1024, 2048, 4096};
    // Reference mesh size
    const int N_ref = 16384;
    // Initial data
    Eigen::Vector2d y0;
    y0 << 1., 1.;
    // Parameter and useful matrix for f
    const double lambda = 1;
    Eigen::Matrix2d R;
    R << 0., -1., 1., 0.;

    // Function and his Jacobian
    auto f =  [&R, &lambda] (const Eigen::Vector2d & y) { return R*y + lambda*(1. - std::pow(y.norm(),2))*y; };
    auto df = [&lambda] (const Eigen::Vector2d & y) {
        double x = 1 - std::pow(y.norm(), 2);
        Eigen::Matrix2d J;
        J << lambda*x - 2*lambda*y(0)*y(0),
        -1 - 2*lambda*y(1)*y(0),
        1 - 2*lambda*y(1)*y(0),
        lambda*x - 2*lambda*y(1)*y(1);
        return J;
    };

    // Reference solution
    auto solref = solveRosenbrock(f, df, y0, N_ref, T);

    std::cout << std::setw(15) << "n" << std::setw(15) << "maxerr" << std::setw(15) << "rate" << std::endl;
    // Used to compute rate at each step:
    double errold = 0;
    // Main loop: loop over all meshes
    for(unsigned int i = 0; i < N.size(); ++i) {
        int n = N.at(i);

        // Get solution
        auto sol = solveRosenbrock(f, df, y0, n, T);

        // Compute error
        double maxerr = 0;
        for(unsigned int j = 0; j < sol.size(); ++j) {
            maxerr = std::max(maxerr, (sol.at(j) - solref.at((j*N_ref)/n)).norm());
        }

        // Do some I/O
        std::cout << std::setw(15) << n << std::setw(15) << maxerr;
        if(i > 0) std::cout << std::setw(15) << std::log2(errold / maxerr);
        std::cout << std::endl;

        // Save old error (for rate)
        errold = maxerr;
    }
}

Nummerical Methods for CSE/PS14/solutions_ps14/rosenbrock.cpp


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

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

#include <sophus/se3.hpp>
#include <sophus/so3.hpp>

using Sophus::SO3d;
using Sophus::SE3d;

int main(int argc, char** argv) {
  // 沿Z轴转90度的旋转矩阵
  Eigen::Matrix3d R =
      Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();

  SO3d SO3_R(R);  // SO(3)可以直接从旋转矩阵构造
  SO3d SO3_v(SO3d::exp({0, 0, M_PI / 2}));  // 亦可从旋转向量构造
  Eigen::Quaterniond q(R);                  // 或者四元数
  SO3d SO3_q(q);
  // 上述表达方式都是等价的
  // 输出SO(3)时，以so(3)形式输出
  cout << "SO(3) from matrix: " << SO3_R.log() << endl;
  cout << "SO(3) from vector: " << SO3_v.log() << endl;
  cout << "SO(3) from quaternion :" << SO3_q.log() << endl;

  // 使用对数映射获得它的李代数
  Eigen::Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵
  cout << "so3 hat=\n" << SO3d::hat(so3) << endl;
  // 相对的，vee为反对称到向
  cout << "so3 hat vee= " << SO3d::vee(SO3d::hat(so3)).transpose()
       << endl;  // transpose纯粹是为了输出美观一些

  // 增量扰动模型的更新
  Eigen::Vector3d update_so3(1e-4, 0, 0);  //假设更新量为这么多
  SO3d SO3_updated = SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = " << SO3_updated.log() << endl;

  /********************萌萌的分割线*****************************/
  cout << "*************************" << endl;
  // 对SE(3)操作大同小异
  Eigen::Vector3d t(1, 0, 0);  // 沿X轴平移1
  SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
  SE3d SE3_qt(q, t);           // 从q,t构造SE(3)
  cout << "SE3 from R,t= " << endl << SE3_Rt.log() << endl;
  cout << "SE3 from q,t= " << endl << SE3_qt.log() << endl;
  // 李代数se(3) 是一个六维向量，方便起见先typedef一下
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  // 同样的，有hat和vee两个算符
  cout << "se3 hat = " << endl << SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << SE3d::vee(SE3d::hat(se3)).transpose() << endl;

  // 最后，演示一下更新
  Vector6d update_se3;  //更新量
  update_se3.setZero();
  update_se3(0, 0) = 1e-4d;
  SE3d SE3_updated = SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}


//
// Created by vasy1 on 2/14/2019.
//
#include <Eigen/Core>
#include <Eigen/QR>
#include <vector>
#include <iostream>
#include <opencv2/opencv.hpp>

namespace utilities
{
    static constexpr double pi() { return M_PI; }

    inline double deg2rad(double x) { return x * pi() / 180; }

    inline double rad2deg(double x) { return x * 180 / pi(); }

// Evaluate a polynomial.
    static double polyeval(Eigen::VectorXd coeffs, double x)
    {
        double result = 0.0;
        for (int i = 0; i < coeffs.size(); i++)
        {
            result += coeffs[i] * pow(x, i);
        }
        return result;
    }

    static Eigen::VectorXd polyfit(const Eigen::VectorXd &xvals, const Eigen::VectorXd &yvals,
                            int order)
    {
        assert(xvals.size() == yvals.size());
        assert(order >= 1 && order <= xvals.size() - 1);
        Eigen::MatrixXd A(xvals.size(), order + 1);

        for (int i = 0; i < xvals.size(); i++)
        {
            A(i, 0) = 1.0;
        }

        for (int j = 0; j < xvals.size(); j++)
        {
            for (int i = 0; i < order; i++)
            {
                A(j, i + 1) = A(j, i) * xvals(j);
            }
        }

        auto Q = A.householderQr();
        auto result = Q.solve(yvals);
        return result;
    }

    static std::vector<Eigen::Map<Eigen::VectorXd>> rotate_coords(const std::vector<cv::Point2d> &coords)
    {
        //adjust points in ref to be in front of car, apply angle transformation
        const double sin_psi = sin(deg2rad(-90));
        const double cos_psi = cos(deg2rad(-90));
        std::vector<double> waypoints_x;
        std::vector<double> waypoints_y;
        for (auto &coord : coords)
        {
            waypoints_x.push_back(coord.x * cos_psi - coord.y * sin_psi);
            waypoints_y.push_back(coord.x * sin_psi + coord.y * cos_psi);
        }

        double *ptrx = &waypoints_x[0];
        double *ptry = &waypoints_y[0];
        Eigen::Map<Eigen::VectorXd> waypoints_x_eig(ptrx, 6);
        Eigen::Map<Eigen::VectorXd> waypoints_y_eig(ptry, 6);
        return {waypoints_x_eig, waypoints_y_eig};
    }
}


#include "rubbishrsa/maths.hpp"
#include "rubbishrsa/log.hpp"

// Rubbish rng
#include <boost/random/mersenne_twister.hpp>
// Crypto rng
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <atomic>
#include <thread>

namespace rubbishrsa {
  bigint modinv(const bigint& a, const bigint& n) {
    auto res = egcd(a, n);
    if (res.gcd != 1)
      throw std::invalid_argument("Cannot compute modular inverse of non-coprime numbers!");

    // The second coefficient would represent the number of multiples of the modulus,
    // but we don't care about that
    //
    // In addition to this, the egcd will may return a negative number in the same
    // congruence class. We will normalise this
    if (res.coefficients.first >= 0)
      return res.coefficients.first;
    else
      return n + res.coefficients.first;
  }

  // lcm(a,b) = a/gcd(a,b) * b
  bigint lcm(const bigint& a, const bigint& b) {
    return (a / egcd(a, b).gcd) * b;
  }

  using egcd_matrix_t = std::array<std::array<bigint, 3>, 2>;

  // Pretty-prints the array
  void egcd_log_matrix(const egcd_matrix_t& state) {
    std::array<std::array<std::string, 3>, 2> string_matrix;
    std::array<size_t, 3> max_len = { 0, 0, 0 };
    // Pass 1: get the maximum length, and generate the strings
    for (size_t row_i = 0; row_i < 2; ++row_i) {
      for (size_t column_i = 0; column_i < 3; ++column_i) {
        auto& res = (string_matrix[row_i][column_i] = state[row_i][column_i].str());
        auto& target = max_len[column_i];
        if (res.size() > target)
          target = res.size();
      }
    }

    std::cerr << "| "
              << std::setw(max_len[0]) << string_matrix[0][0] << "  "
              << std::setw(max_len[1]) << string_matrix[0][1]
              << " | " << std::setw(max_len[2]) << string_matrix[0][2] << " |"
              << std::endl
              << "| "
              << std::setw(max_len[0]) << string_matrix[1][0] << "  "
              << std::setw(max_len[1]) << string_matrix[1][1]
              << " | " << std::setw(max_len[2]) << string_matrix[1][2] << " |"
              << std::endl
              << std::endl;
  }

  egcd_result egcd(const bigint& a, const bigint& b) {
    // This is basically a code version of the lecture notes on the topic

    RUBBISHRSA_LOG_TRACE(std::cerr << "Calculating GCD of " << a.str() << " and " << b.str() << std::endl);

    if (a <= 0 || b <= 0)
      throw std::invalid_argument("GCD cannot be computed with non-positive argument!");

    std::array<bigint, 3>col[2];

    egcd_matrix_t matrix = {{
      {1, 0, a},
      {0, 1, b}
    }};

    RUBBISHRSA_LOG_TRACE(egcd_log_matrix(matrix));

    egcd_result return_value;

    while (true) {
      // Calculate the number of multiples of the b cell that fits into the a cell
      auto a_fit = matrix[0][2] / matrix[1][2];

      // Subtract that from the a row
      for (size_t i = 0; i < 3; ++i) {
        matrix[0][i] -= matrix[1][i] * a_fit;
      }

      RUBBISHRSA_LOG_TRACE(egcd_log_matrix(matrix));

      // Check the a cell to see if we're done
      if (matrix[0][2] == 0)
        return {
          .gcd = matrix[1][2],
          .coefficients = {matrix[1][0], matrix[1][1]}
        };

      // Now we do the same thing for the b row
      auto b_fit = matrix[1][2] / matrix[0][2];
      for (size_t i = 0; i < 3; ++i) {
        matrix[1][i] -= matrix[0][i] * b_fit;
      }

      RUBBISHRSA_LOG_TRACE(egcd_log_matrix(matrix));

      // Check the b cell to see if we're done
      if (matrix[1][2] == 0)
        return {
          .gcd = matrix[0][2],
          .coefficients = {matrix[0][0], matrix[0][1]}
        };
    }
  }

  // I will use boost random for this stuff, and stl's one doesn't support the bigint
  bool is_prime(const bigint& candidate, uint_fast8_t certainty_log_4) {
    // This in an implementation of the Miller-Rabin probabilistic primality test
    bigint odd = candidate;
    size_t exponent = 0;

    // We keep the the non-power of 2 in i
    while (odd % 2 != 0) {
      ++exponent;
      // A quicker way of doing i = 2
      odd >>= 1;
    }

    bigint min = 2;
    bigint max = candidate - 2;
    // We don't need a crypto rng here
    thread_local boost::random::mt19937 rng;
    const boost::random::uniform_int_distribution<bigint> dist(std::move(min), std::move(max));

    // We do this a lot, so precompute it
    bigint candidate_minus_1 = candidate - 1;

    bigint x;
    for (uint_fast8_t iter = 0; iter < certainty_log_4; ++iter) {
      bigint a = dist(rng);
      x = bmp::powm(a, odd, candidate);
      // If we already have a congruence, then we have passed this iter
      if (x == 1 || x == candidate_minus_1)
        continue;

      for (decltype(exponent) i = 1; i < exponent; ++i) {
        x = bmp::powm(x, 2, candidate);
        if (x == candidate_minus_1)
          // Only way to quickly leave a nested loop
          //
          // Please don't just take away marks because I used a goto!
          //
          // gotos to escape nested loops are perfectly fine, and make
          // the code _far_ more readable than a separate func, or a
          // boolean check at the end (which would also be slower)
          goto next_iter;
      }
      // If we failed all of our subiterations, then we must exit out of the loop
      return false;
next_iter: {}
    }

    return true;
  }

  bigint generate_prime(uint_fast16_t bits) {
    // (1 << n) means 2^n, giving us a range of 2^(n-2) to 2^(n-1) inclusive
    //
    // The reason for keeping this half of the desired values is that 2 is the only even prime,
    // so we can save a lot of composite candidates by multiplying by 2 and adding 1
    bigint min = 1; min <<= (bits - 2);
    bigint max = min; max <<= 1;

    RUBBISHRSA_LOG_TRACE(
          std::cerr << "Generating prime candidate bases between " << min.str()
                    << " and " << max.str() << std::endl
    );

    // Our distribution is that of all numbers with the given bit count
    const boost::random::uniform_int_distribution<bigint> dist(std::move(min), std::move(max));

    // TO speed up prime generation, we run on each core of the cpu until we find a prime
    std::vector<std::thread> pool;
    bigint ret;
    std::atomic<bool> stop = false;
    for (unsigned int i = 0; i < std::thread::hardware_concurrency(); ++i) {
      pool.emplace_back([&, i]() {
        // Removes warnings about i not being used
        (void)i;
        // (Almost) always a cryptographically secure rng
        thread_local boost::random::random_device rng;
        // Moving this outside may create some nebulous speed improvement
        bigint candidate;
        // We stop looping when a single thread has found a result, and marked stop as true
        while (!stop) {
          // Get a random number, and make it odd
          candidate = dist(rng) * 2 + 1;
          // We will only log the candidates of one thread so that we keep the output synchronised
          RUBBISHRSA_LOG_TRACE(if (i == 0) std::cerr << "\tPrime candidate " << candidate.str() << std::endl);
          // Check if we have a prime, and check if we are the first thread to have one
          if (is_prime(candidate, 128) && !stop.exchange(true)) {
            ret = std::move(candidate);
          }
        }
      });
    }

    // Wait for each thread to finish
    for (auto& thread : pool) thread.join();

    RUBBISHRSA_LOG_TRACE(std::cerr << "Chose " << ret << " as prime" << std::endl);

    return ret;
  }

  bigint pollard_rho(const bigint& n) {
    std::vector<std::thread> pool;
    std::atomic<bool> found = false;
    bigint result;

    // Using primes will minimise the chance of collision, which means that threads are less likely to do redundant work
    // I have hand removed 5, as it is the second term of 2's sequence
    constexpr static std::array<int, 128> primes{2, 3, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53, 59, 61, 67, 71, 73, 79, 83, 89, 97, 101, 103, 107, 109, 113, 127, 131, 137, 139, 149, 151, 157, 163, 167, 173, 179, 181, 191, 193, 197, 199, 211, 223, 227, 229, 233, 239, 241, 251, 257, 263, 269, 271, 277, 281, 283, 293, 307, 311, 313, 317, 331, 337, 347, 349, 353, 359, 367, 373, 379, 383, 389, 397, 401, 409, 419, 421, 431, 433, 439, 443, 449, 457, 461, 463, 467, 479, 487, 491, 499, 503, 509, 521, 523, 541, 547, 557, 563, 569, 571, 577, 587, 593, 599, 601, 607, 613, 617, 619, 631, 641, 643, 647, 653, 659, 661, 673, 677, 683, 691, 701, 709, 719};
    auto max_threads = std::min(static_cast<size_t>(std::thread::hardware_concurrency()), primes.size());
    // Do Pollard's rho algorithm with each thread, each with a different polynomial
    for (size_t i = 0; i < max_threads; ++i) {
      pool.emplace_back([&, i]() {
        bigint x = primes[i];
        bigint y = x;
        bigint gcd;

        while (!found) {
          // We are trying to find two elements in the sequence u_n such that u_i is congruent to u_j (mod p), but u_n is not equal to u_i
          //
          // With two such elements, we have (as a result of the remainder property of moduli) gcd(|u_i - u_j|, n) is not 1.
          //
          // This means that there is some common divisor between them, and the result of this gcd is a factor of n
          //
          // We step one position (x) forward by 1, and the other (y) by 2, to increase the size of the tested cycle.
          //
          // For some unknown reason, If we pick u_n = u_n^2 + a (mod n) as our random generator, we will find a result quicker.

          // 1 iter for x
          x = (x*x + 1) % n;
          // 2 iters for y
          y = (y*y + 1) % n;
          y = (y*y + 1) % n;

          // We don't need to worry about both elements being equal (unless it is prime),
          // as we will happen upon a factor cycle far before that (with high probability)
          gcd = egcd(bmp::abs(x - y), n).gcd;
          // If we found something with a non-trivial gcd, that's a factor
          if (gcd != 1 && !found.exchange(true))
            result = gcd;
        }
      });
    }

    for (auto& thread : pool)
      thread.join();

    return result;
  }

  // TODO: implement
//  bigint quadratic_sieve(const bigint& n) {
//    abort();

//    // Not a perfect calcuation, but easy to do
//    bmp::mpf_float log_n = bmp::log(bmp::mpf_float{n});
//    bmp::mpf_float bound = bmp::exp(bmp::mpf_float{1/2} * log_n * bmp::log(log_n));

//    bigint prime_count_approx{bmp::ceil(bmp::mpf_float{n} / log_n)};

//    for (bigint i = 0; i < bound; ++i) {

//    }
//  }

  std::pair<bigint, bigint> factorise_semiprime(const bigint& semiprime) {
    size_t bits = floor_log2(semiprime);

    // Pollard takes a bit too long when bits >= 83 on my system, and I'll knock off a few "Windows points"
    if (bits < 70) {
      auto p = pollard_rho(semiprime);
      auto q = semiprime / p;
      return {p, q};
    }
    // TODO: make this use quadratic sieve
    else {
      auto p = pollard_rho(semiprime);
      auto q = semiprime / p;
      return {p, q};
    }
  }

  bigint ascii2bigint(std::string_view str) {
    bigint data = 0;
    for (auto i : str) {
      // Cast the character to an unsigned integer type, and add it to the end of the number
      data <<= 8;
      data += static_cast<unsigned char>(i);
    }
    return data;
  }

  bigint ascii2bigint(std::istream& in) {
    bigint data = 0;
    // Keep going until the end of the file
    while (!in.eof()) {
      data <<= 8;
      data += in.get();
    }
    return data;
  }

  std::string bigint2ascii(bigint data) {
    std::string str;
    str.reserve(2048);
    // Read the string backwards
    while (data) {
      str.push_back(static_cast<char>(data & 0xFF));
      data >>= 8;
    }
    std::reverse(str.begin(), str.end());
    return str;
  }

  bigint hex2bigint(std::string_view str) {
    // A slow way of doing it, but this is not a bottleneck
    std::string con_str{"0x"};
    con_str += str;
    return bigint{con_str};
  }
}


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

using namespace Eigen;
using namespace std;

#define MATRIX_SIZE 100

// 编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序
int main(int argc, char const *argv[])
{
    // 创建随机矩阵 A[100x100] 并输出
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> A
    = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    // cout << "Matrix A is: \n" << A << endl;

    // 创建随机
    Matrix<double, MATRIX_SIZE, 1> B = MatrixXd::Random(MATRIX_SIZE, 1);
    // cout << "Matrix B is: \n" << B << endl;

    // 求解 Ax = B
    // 1. QR 分解
    auto x = A.colPivHouseholderQr().solve(B);
    cout << "QR decompostion: \n x = " << x << endl << endl;

    // 2. Cholesky 分解
    auto y = A.ldlt().solve(B);
    cout << "Cholesky decompostion: \n x = " << y << endl;

    return 0;
}