#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;
	}
}


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


// 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;
}


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2020 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Math                                                                     *
***************************************************************************/

#ifndef MATH_HPP
#define MATH_HPP

#include <Eigen/Dense>
#include <cmath>
#include <cassert>
#include <stdexcept>
#include <algorithm>
#include <utility>
#include <tuple>
#include <omp.h>

#define MATH_FLINTMAX 9.0072e+15
#define MATH_EPS 2.220446049250313e-16
#define MATH_INF 1e300

#if _WIN32
#define __PRETTY_FUNCTION__  std::string(__FUNCTION__)
#endif

using namespace Eigen;

template<typename T>
int round(const T value)
{
    return (int) std::ceil(value - 0.5);
}

template<typename T>
int sign(const T value)
{
    return value == 0 ? 0 : value > 0 ? 1 : -1;
}

/***************************** Histogram Edges class *****************************/

class Edges
{
public:
    template<typename T>
    static VectorXd binpicker(const T minx, const T maxx, const double rawBinWidth);
    template<typename Derived>
    static VectorXd autorule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits);
    template<typename Derived>
    static VectorXd scottsrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits);
    template<typename Derived>
    static VectorXd integerrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits);
    template<typename Derived>
    static VectorXd sturgesrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits);
    template<typename Derived>
    static VectorXd sqrtrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits);
};

/***************************** Math class *****************************/

class Math
{
public:
    enum Order { ASCENDING, DESCENDING };
    enum Interpolation { PCHIP, MAKIMA };
    enum Kernel { NORMAL, BOX };

    template<typename T>
    static bool isClose(const T x, const T y, const double rtol = 1.e-5, const double atol = 1.e-8);
    // Sort, reorder, permute, align
    template<typename Derived>
    static typename Derived::PlainObject sort(const DenseBase<Derived>& x, const Order order = Math::ASCENDING);
    template<typename Derived1, typename Derived2>
    static typename Derived1::PlainObject sort(const DenseBase<Derived1>& x, DenseBase<Derived2> &indices, const Order order = Math::ASCENDING);
    // Basic statistics
    template<typename Derived>
    static double mean(const DenseBase<Derived>& x);
    template<typename Derived>
    static double median(const DenseBase<Derived>& x);
    template<typename Derived>
    static typename Derived::Scalar mode(const DenseBase<Derived>& x);
    template<typename Derived>
    static double var(const DenseBase<Derived>& x);
    template<typename Derived>
    static double std(const DenseBase<Derived>& x);
    template<typename Derived>
    static double mad(const DenseBase<Derived>& x);
    template<typename Derived>
    static double percentile(const DenseBase<Derived>& x, const double p);
    template<typename Derived>
    static double percentile(const DenseBase<Derived>& x, const double p, int& index);
    // Linspace
    template<typename T>
    static VectorXd linspace(const T minx, const T maxx, const int N = 100);
    // Discrete integral and derivatives
    template<typename Derived1, typename Derived2>
    static double integral(const DenseBase<Derived1>& x, const DenseBase<Derived2>& y);
    template<typename Derived>
    static double integral(const DenseBase<Derived>& y, const double step = 1);
    template<typename Derived>
    static Matrix<typename Derived::Scalar, Dynamic, 1> diff(const DenseBase<Derived>& x);
    template<typename Derived>
    static VectorXd gradient(const DenseBase<Derived>& y);
    template<typename Derived1, typename Derived2>
    static VectorXd gradient(const DenseBase<Derived1>& x, const DenseBase<Derived2>& y);
    // Distribution density estimation
    template<typename Derived>
    static std::tuple<VectorXi, VectorXd, VectorXi> histogram(const DenseBase<Derived>& x);
    template<typename Derived>
    static std::tuple<VectorXi, VectorXd, VectorXi> histogram(const DenseBase<Derived>& x, const VectorXd& edges);
    template<typename Derived>
    static std::tuple<VectorXd, VectorXd, VectorXi> cdf(const DenseBase<Derived>& x);
    template<typename Derived>
    static std::tuple<VectorXd, VectorXd, VectorXi> cdf(const DenseBase<Derived>& x, const VectorXd& edges);
    // Convolutions
    template<typename Derived>
    static VectorXd conv1d(const DenseBase<Derived>& x, const Kernel type = Math::NORMAL, const int size = 5, const double alpha = 2.5);
    template<typename Derived>
    static VectorXd conv1d(const DenseBase<Derived>& x, const VectorXd &k);
    // Interpolation
    template<typename Derived1, typename Derived2, typename Derived3>
    static VectorXd interp1(const DenseBase<Derived1>& x, const DenseBase<Derived2>& y, const DenseBase<Derived3> &xq, const Interpolation type = Math::MAKIMA);
};

/***************************** Implementation *****************************/
template<typename T>
bool Math::isClose(const T x, const T y, const double rtol, const double atol)
{
    return std::abs(x - y) <= (atol + rtol * std::abs(y));
}

template<typename Derived>
typename Derived::PlainObject Math::sort(const DenseBase<Derived>& x, const Order order)
{
    if (order != ASCENDING && order != DESCENDING)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << ": Unsupported <order> parameter." << std::endl;
        throw std::runtime_error(s.str());
    }

    typename Derived::PlainObject y = x.derived();

    std::sort(y.data(), y.data() + y.size());
    return order == Math::ASCENDING ? y : y.reverse();
}

template<typename Derived1, typename Derived2>
typename Derived1::PlainObject Math::sort(const DenseBase<Derived1>& x, DenseBase<Derived2> &indices, const Order order)
{
    if (order != ASCENDING && order != DESCENDING)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << ": Unsupported <order> parameter." << std::endl;
        throw std::runtime_error(s.str());
    }

    typename Derived1::PlainObject y = x.derived();
    typename Derived2::PlainObject& z = indices.derived();

    z.resize(y.rows(), y.cols());
#pragma omp parallel for
    for (Index i = 0; i < z.size(); ++i)
        z(i) = i;

    std::sort(z.data(), z.data() + z.size(), [&](size_t a, size_t b) { return y(a) < y(b); });

#pragma omp parallel for
    for (Index i = 0; i < z.size(); ++i)
        y(i) = x((Index) z(i));

    return order == Math::ASCENDING ? y : y.reverse();
}

template<typename Derived>
double Math::mean(const DenseBase<Derived>& x)
{
    const typename Derived::PlainObject& y = x.derived();
    return (double) y.template cast<double>().mean();
}

template<typename Derived>
double Math::median(const DenseBase<Derived>& x)
{
    typename Derived::PlainObject y = x.derived();

    std::sort(y.data(), y.data() + y.size());
    if (((int) y.size() % 2) == 0)
        return (double) (y((Index)(y.size() / 2)) + y((Index)(y.size() / 2) - 1)) / 2.0;
    else
        return (double) y((Index)(y.size() / 2));
}

template<typename Derived>
typename Derived::Scalar Math::mode(const DenseBase<Derived>& x)
{
    typename Derived::PlainObject y = x.derived();

    std::sort(y.data(), y.data() + y.size());

    int max = 1;
    int counter = 1;
    typename Derived::Scalar mode = y(0);
    for (Index i = 1; i < y.size(); ++i)
    {
        if (y(i) == y(i - 1))
        {
            ++counter;
            if (counter > max)
            {
                max = counter;
                mode = y(i);
            }
        }
        else
        {
            counter = 1;
        }
    }
    return mode;
}

template<typename Derived>
double Math::var(const DenseBase<Derived>& x)
{
    const double mu = Math::mean(x);
    double diff = 0;
    for (Index i = 0; i < x.size(); ++i)
        diff += (((double) x(i) - mu) * ((double) x(i) - mu));
    return diff / ((double) x.size() - 1.0);
}

template<typename Derived>
double Math::std(const DenseBase<Derived>& x)
{
    return std::sqrt(Math::var(x));
}

template<typename Derived>
double Math::mad(const DenseBase<Derived>& x)
{
    const typename Derived::PlainObject& y = x.derived();
    return Math::median((y - Math::median(y)).abs());
}

template<typename Derived>
double Math::percentile(const DenseBase<Derived>& x, const double p)
{
    int index = 0;
    return Math::percentile(x, p, index);
}

template<typename Derived>
double Math::percentile(const DenseBase<Derived>& x, const double p, int& index)
{
    if (p < 0 || p > 100)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << ": Percentile must be in the range [0, 100]" << std::endl;
        throw std::runtime_error(s.str());
    }

    if (x.size() == 1)
        return (double) x(0);

    VectorXi indices;
    const typename Derived::PlainObject& y = Math::sort(x, indices);

    if (p == 50)
    {
        index = indices((int) (y.size() / 2));
        if (((int) y.size() % 2) == 0)
            return (double) (y((Index) (y.size() / 2)) + y((Index)(y.size() / 2) - 1)) / 2.0;
        else
            return (double) y((Index) (y.size() / 2));
    }

    // Get index corresponding to p-percentile
    double r = (p / 100.0) * (double) (y.size() - 1);
    const int k = (int) std::floor(r);
    const int kp1 = k + 1 >= (int) y.size() ? (int) y.size() - 1 : k + 1;
    r = r - k;

    // Set index
    index = indices(k);

    // By interpolation
    return r == 0 ? (double) y(k) : (r * (double) y(kp1)) + ((1.0 - r) * (double) y(k));
}

template<typename T>
VectorXd Math::linspace(const T minx, const T maxx, const int N)
{
    if (N < 3)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << ": Number of space points must be greater or equal than 3" << std::endl;
        throw std::runtime_error(s.str());
    }

    VectorXd y(N);

    const int N1 = N - 1;
    const T c = (maxx - minx) * (T)(N1 - 1);
    if (!std::isfinite(c))
    {
        if (!std::isfinite(maxx - minx))
        {
            for (Index i = 0; i < N; ++i)
                y(i) = (double) minx + ((double) maxx / (double) N1) * i - ((double) minx / (double) N1) * i;
        }
        else
        {
            for (Index i = 0; i < N; ++i)
                y(i) = (double) minx + i * ((double) (maxx - minx) / (double) N1);
        }
    }
    else
    {
        for (Index i = 0; i < N; ++i)
            y(i) = (double) minx + i * ((double) (maxx - minx) / (double) N1);
    }
    if (y.size() > 0 )
    {
        y(0) = (double) minx;
        y(N1) = (double) maxx;
    }
    return y;
}

template<typename Derived1, typename Derived2>
double Math::integral(const DenseBase<Derived1>& x, const DenseBase<Derived2>& y)
{
    if (x.size() != y.size())
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x and y must be of the same size." << std::endl;
        throw std::runtime_error(s.str());
    }

    double result = 0;
    for (Index i = 0; i < y.size() - 1; ++i)
        result += (double) (x(i+1) - x(i)) * (y(i+1) + y(i));
    return result / 2.0;
}

template<typename Derived>
double Math::integral(const DenseBase<Derived>& y, const double step)
{
    double result = 0;
    for (Index i = 0; i < y.size() - 1; ++i)
        result += (double) step * (y(i+1) + y(i));
    return result / 2.0;
}

template<typename Derived>
Matrix<typename Derived::Scalar, Dynamic, 1> Math::diff(const DenseBase<Derived>& x)
{
    if (x.rows() != 1 && x.cols() != 1)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x must be a row or column vector/array." << std::endl;
        throw std::runtime_error(s.str());
    }

    Matrix<typename Derived::Scalar, Dynamic, 1> d(x.size() - 1);
#pragma omp parallel for
    for (Index i = 0; i < x.size() - 1; ++i)
        d(i) = x(i+1) - x(i);
    return d;
}

template<typename Derived>
VectorXd Math::gradient(const DenseBase<Derived> &y)
{
    if (y.rows() != 1 && y.cols() != 1)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x must be row or column vector/array." << std::endl;
        throw std::runtime_error(s.str());
    }

    const int N = y.size();
    if (N == 1)
        return y.derived().template cast<double>();

    VectorXd g(y.size());
    // Forward differences on left and right edges
    g(0) = y(1) - y(0);
    g(N - 1) = y(N - 1) - y(N - 2);
    // Centered differences on interior points
    if (N > 2)
    {
#pragma omp parallel for
        for (Index i = 1; i < N - 1; ++i)
            g(i) = (double) (y(i+1) - y(i-1)) / 2.0;
    }
    return g;
}

template<typename Derived1, typename Derived2>
VectorXd Math::gradient(const DenseBase<Derived1>& x, const DenseBase<Derived2>& y)
{
    if (x.size() != y.size())
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x and y must be of the same size." << std::endl;
        throw std::runtime_error(s.str());
    }

    if (y.rows() != 1 && y.cols() != 1)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x and y must be row or column vectors/arrays." << std::endl;
        throw std::runtime_error(s.str());
    }

    const int N = y.size();
    if (N == 1)
        return y.derived().template cast<double>();

    VectorXd g(y.size());
    // Forward differences on left and right edges
    g(0) = (y(1) - y(0)) / (x(1) - x(0));
    g(N - 1) = (y(N - 1) - y(N - 2)) / (x(N - 1) - x(N - 2));
    // Centered differences on interior points
    if (N > 2)
    {
#pragma omp parallel for
        for (Index i = 1; i < N - 1; ++i)
            g(i) = (double) (y(i+1) - y(i-1)) / (x(i+1) - x(i-1));
    }
    return g;
}

template<typename Derived>
std::tuple<VectorXi, VectorXd, VectorXi> Math::histogram(const DenseBase<Derived>& x)
{
    return Math::histogram(x, Edges::autorule(x, x.minCoeff(), x.maxCoeff(), false));
}

template<typename Derived>
std::tuple<VectorXi, VectorXd, VectorXi> Math::histogram(const DenseBase<Derived>& x, const VectorXd& edges)
{
    if (x.size() < 3)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x must have at least more than 3 elements." << std::endl;
        throw std::runtime_error(s.str());
    }

    if (!Math::isClose(std::abs(edges(1) - edges(0)), std::abs(edges(2) - edges(1))) ||
        !Math::isClose(std::abs(edges(2) - edges(1)), std::abs(edges(3) - edges(2))))
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> Unsupported binwidth. <histogram> function only prepared to deal with equal binwidths" << std::endl;
        throw std::runtime_error(s.str());
    }

    const int numEqualBins = (int) edges.size() - 1;
    const double firstEdge = edges(0);
    const double lastEdge = edges(numEqualBins);
    const double norm = (double) numEqualBins / std::abs(lastEdge - firstEdge);
    VectorXi hist = VectorXi::Zero(numEqualBins);
    VectorXi bin(x.size());
    for (Index i = 0; i < x.size(); ++i)
    {
        if (x(i) < firstEdge || x(i) > lastEdge)
            continue;

        int pos = (int) (std::abs(x(i) - firstEdge) * norm);
        pos = pos == numEqualBins ? pos - 1 : pos;
        pos = x(i) < edges(pos) ? pos - 1 : pos;
        pos = (x(i) >= edges(pos + 1)) && (pos != (numEqualBins - 1)) ? pos + 1 : pos;
        hist(pos)++;
        bin(i) = pos;
    }

    return std::make_tuple(hist, edges, bin);
}

template<typename Derived>
std::tuple<VectorXd, VectorXd, VectorXi> Math::cdf(const DenseBase<Derived>& x)
{
    return Math::cdf(x, Edges::autorule(x, x.minCoeff(), x.maxCoeff(), false));
}

template<typename Derived>
std::tuple<VectorXd, VectorXd, VectorXi> Math::cdf(const DenseBase<Derived>& x, const VectorXd& edges)
{
    const auto H = Math::histogram(x, edges);
    const VectorXi hist = std::get<0>(H);
    const VectorXi bin = std::get<2>(H);

    VectorXd cum = VectorXd::Zero(hist.size());
    cum(0) = hist(0);
    for (Index i = 1; i < cum.size(); ++i)
    {
        cum(i) = (double) (cum(i-1) + hist(i));
    }

    const double sum = (double) hist.sum();
#pragma omp parallel for
    for (Index i = 0; i < cum.size(); ++i)
    {
        cum(i) /= sum;
    }

    return std::make_tuple(cum, edges, bin);
}

template<typename Derived>
VectorXd Math::conv1d(const DenseBase<Derived>& x, const Kernel type, const int size, const double alpha)
{
    VectorXd k(size);

    const int vOffset = (int) (size / 2);
    switch (type)
    {
        case Math::NORMAL:
        {
            const double N = (double) (size - 1) / 2.0;
            for (Index i = 0, j = -vOffset; i < size; ++i, ++j)
                k(i) = std::exp(-0.5 * ((alpha * j) / N) * ((alpha * j) / N));
            k = k / k.sum();
        }
        break;
        case Math::BOX:
        {
            for (Index i = 0; i < size; ++i)
                k(i) = 1.0 / size;
        }
        break;
        default:
        {
            const double N = ((double) size - 1.0) / 2.0;
            for (Index i = 0, j = -vOffset; i < size; ++i, ++j)
                k(i) = std::exp(-0.5 * ((alpha * j) / N) * ((alpha * j) / N));
            k = k / k.sum();
        }
        break;
    }
    return Math::conv1d(x, k);
}

template<typename Derived>
VectorXd Math::conv1d(const DenseBase<Derived>& x, const VectorXd& k)
{
    if (x.rows() != 1 && x.cols() != 1)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x must be a row or column vector/array." << std::endl;
        throw std::runtime_error(s.str());
    }

    if ((k.size() % 2) == 0)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> Kernel must have an odd number of elements." << std::endl;
        throw std::runtime_error(s.str());
    }

    if (k.sum() < 1.0 - MATH_EPS || k.sum() > 1.0 + MATH_EPS)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << "==> Kernel does not sum 1." << std::endl;
        throw std::runtime_error(s.str());
    }

    const int size = (int) k.size();
    const int offset = (int) (size / 2);

    VectorXd y(x.size() + (offset * 2));
#pragma omp parallel for
    for (Index i = 0; i < x.size(); ++i)
        y(i + offset) = x(i);
    for (Index i = 0, j = offset; i < offset; ++i, --j)
        y(i) = x(j);
    for (Index i = y.size() - 1, j = x.size() - offset - 1; i >= y.size() - offset; --i, ++j)
        y(i) = x(j);

    VectorXd z(x.size());
#pragma omp parallel for
    for (int i = 0; i < z.size(); ++i)
        z(i) = y.segment(i, size).transpose() * k;

    return z;
}

inline ArrayXd _pchipSlopes(const ArrayXd& h, const ArrayXd& delta)
{
    assert(h.size() == delta.size());
    const int N = h.size();

    ArrayXi k(N);
#pragma omp parallel for
    for (Index i = 0; i < N-1; ++i)
        k(i) = sign(delta(i))*sign(delta(i+1)) > 0 ? i : -1;

    ArrayXd hs(N);
#pragma omp parallel for
    for (Index i = 0; i < N; ++i)
    {
        if (k(i) == -1)
        {
            hs(i) = 0;
            continue;
        }
        hs(i) = h(k(i)) + h(k(i)+1);
    }
    ArrayXd w1(N);
    ArrayXd w2(N);
    ArrayXd dmax(N);
    ArrayXd dmin(N);
#pragma omp parallel for
    for (Index i = 0; i < N; ++i)
    {
        if (k(i) == -1)
        {
            w1(i) = 0.0;
            w2(i) = 0.0;
            dmin(i) = 0.0;
            dmax(i) = 0.0;
            continue;
        }
        w1(i) = (h(k(i)) + hs(i)) / (3.0 * hs(i));
        w2(i) = (h(k(i)+1) + hs(i)) / (3.0 * hs(i));
        dmax(i) = std::abs(delta(k(i)));
        dmin(i) = std::abs(delta(k(i)+1));
        if (dmax(i) < dmin(i))
        {
            dmin(i) = dmax(i);
            dmax(i) = std::abs(delta(k(i)+1));
        }
    }
    ArrayXd slopes(N+1);
#pragma omp parallel for
    for (Index i = 0; i < N; ++i)
    {
        if (k(i) == -1)
        {
            slopes(i) = 0;
            continue;
        }
        slopes(k(i)+1) = dmin(i) / ((w1(i) * delta(k(i)) / dmax(i)) + (w2(i) * delta(k(i)+1) / dmax(i)));
    }
    slopes(0) = ((2.0 * h(0) + h(1)) * delta(0) - h(0) * delta(1)) / (h(0) + h(1));
    if (sign(slopes(0)) != sign(delta(0)))
        slopes(0) = 0;
    else if ((sign(delta(0)) != sign(delta(1))) && (std::abs(slopes(0)) > std::abs(3.0 * delta(0))))
        slopes(0) = 3.0 * delta(0);
    slopes(N) = ((2.0 * h(N-1) + h(N-2)) * delta(N-1) - h(N-1) * delta(N-2)) / (h(N-1) + h(N-2));
    if (sign(slopes(N)) != sign(delta(N-1)))
        slopes(N) = 0;
    else if ((sign(delta(N-1)) != sign(delta(N-2))) && (std::abs(slopes(N)) > std::abs(3.0 * delta(N-1))))
        slopes(N) = 3.0 * delta(N-1);

    return slopes;
}

inline ArrayXd _mAkimaSlopes(const ArrayXd& del)
{
    const int N = del.size();

    const double delta_0 = 2.0 * del(0) - del(1);
    const double delta_m1 = 2.0 * delta_0 - del(0);
    const double delta_n = 2.0 * del(N-1) - del(N-2);
    const double delta_n1 = 2.0 * delta_n - del(N-1);

    ArrayXd delta(N+4);
    delta(0) = delta_m1;
    delta(1) = delta_0;
    delta.segment(2, N) = del;
    delta(N+2) = delta_n;
    delta(N+3) = delta_n1;

    const ArrayXd ddelta = Math::diff(delta);
    ArrayXd weights(N+3);
#pragma omp parallel for
    for (Index i = 0; i < N+3; ++i)
        weights(i) = std::abs(ddelta(i)) + std::abs((delta(i) + delta(i+1)) / 2.0);

    ArrayXd slopes(N+1);
#pragma omp parallel for
    for (Index i = 0; i < N+1; ++i)
    {
        const double w = weights(i) + weights(i+2);
        slopes(i) = w == 0 ? 0 : (delta(i+1) * weights(i+2) / w) + (delta(i+2) * weights(i) / w);
    }

    return slopes;
}

inline MatrixXd _pwch(const ArrayXd& x, const ArrayXd& y, const ArrayXd& slopes, const ArrayXd& h, const ArrayXd& delta)
{
    const int N = x.size();

    ArrayXd dzzdx(N-1);
    ArrayXd dzdxdx(N-1);
#pragma omp parallel for
    for (Index i = 0; i < N-1; ++i)
    {
        dzzdx(i) = (delta(i) - slopes(i)) / h(i);
        dzdxdx(i) = (slopes(i+1) - delta(i)) / h(i);
    }
    MatrixXd pp(N-1, 4);
#pragma omp parallel for
    for (Index i = 0; i < N-1; ++i)
    {
        pp(i, 0) = (dzdxdx(i) - dzzdx(i)) / h(i);
        pp(i, 1) = 2.0 * dzzdx(i) - dzdxdx(i);
        pp(i, 2) = slopes(i);
        pp(i, 3) = y(i);
    }
    return pp;
}

inline ArrayXd _ppval(const MatrixXd& pp, const ArrayXd& x, const ArrayXd& xq)
{
    const auto H = Math::histogram(xq, x);
    const VectorXi bin = std::get<2>(H);

    assert(bin.size() == xq.size());

    const int N = xq.size();
    ArrayXd xs(N);
    ArrayXd yq(N);
#pragma omp parallel for
    for (Index i = 0; i < N; ++i)
    {
        xs(i) = xq(i) - x(bin(i));
        yq(i) = pp(bin(i), 0);
    }

    for (Index j = 1; j < 4; ++j)
    {
#pragma omp parallel for
        for (Index i = 0; i < N; ++i)
            yq(i) = xs(i) * yq(i) + pp(bin(i), j);
    }
    return yq;
}

template<typename Derived1, typename Derived2, typename Derived3>
VectorXd Math::interp1(const DenseBase<Derived1>& x, const DenseBase<Derived2>& y, const DenseBase<Derived3> &xq, const Interpolation type)
{
    if (x.size() != y.size())
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x and y must be of the same size." << std::endl;
        throw std::runtime_error(s.str());
    }

    if (x.size() < 3)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> x and y must have more than 2 elements." << std::endl;
        throw std::runtime_error(s.str());
    }

    if (xq.size() < 4)
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> xq must have more than 3 elements." << std::endl;
        throw std::runtime_error(s.str());
    }

    const ArrayXd h = Math::diff(x);
    const ArrayXd delta = Math::diff(y).array() / h;
    const ArrayXd slopes = type == Math::PCHIP ? _pchipSlopes(h, delta) : _mAkimaSlopes(delta);

    return _ppval(_pwch(x, y, slopes, h, delta), x, xq);
}

/***************************** Implementation of Histogram Edges class *****************************/
/***************************** must be here with the declaration first *****************************/

template<typename T>
VectorXd Edges::binpicker(const T minx, const T maxx, const double rawBinWidth)
{
    const double xscale = (double) std::max(std::abs((double) minx), std::abs((double) maxx));
    const double xrange = (double) (maxx - minx);

    const double powOfTen = std::pow(10.0, std::floor(std::log10(rawBinWidth)));
    const double relSize = rawBinWidth / powOfTen;
    double binWidth;
    if (relSize < 1.5)
        binWidth = 1 * powOfTen;
    else if (relSize < 2.5)
        binWidth = 2 * powOfTen;
    else if (relSize < 4)
        binWidth = 3 * powOfTen;
    else if (relSize < 7.5)
        binWidth = 5 * powOfTen;
    else
        binWidth = 10 * powOfTen;

    const double leftEdge = std::min(binWidth * std::floor((double) minx / binWidth), (double) minx);
    const int nbinsActual = std::max(1, (int) std::ceil((double) (maxx - leftEdge) / binWidth));
    const double rightEdge = std::max(leftEdge + nbinsActual * binWidth, (double) maxx);

    VectorXd y(nbinsActual + 1);
    for (Index i = 0; i < nbinsActual; ++i)
        y(i) = leftEdge + ((double) i * binWidth);
    y(nbinsActual) = rightEdge;
    return y;
}

template<typename Derived>
VectorXd Edges::autorule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits)
{
    const typename Derived::Scalar xrange = maxx - minx;
    if (x.size() > 0 && std::is_integral<typename Derived::Scalar>::value && xrange <= 50 && maxx <= (MATH_FLINTMAX / 2.0) && minx >= -(MATH_FLINTMAX / 2.0))
        return Edges::integerrule(x, minx, maxx, hardlimits);
    else
        return Edges::scottsrule(x, minx, maxx, hardlimits);
}

template<typename Derived>
VectorXd Edges::scottsrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits)
{
    const double sigma = Math::std(x);
    const double binwidth = 3.5 * sigma / std::pow((double) x.size(), 1.0 / 3.0);
    if (hardlimits)
    {
        const int nbins = (int) std::ceil((double) (maxx - minx) / binwidth);
        return Math::linspace(minx, maxx, nbins);
    }
    else
    {
        return Edges::binpicker(minx, maxx, binwidth);
    }
}

template<typename Derived>
VectorXd Edges::integerrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits)
{
    if (maxx > (MATH_FLINTMAX / 2.0) || minx < -(MATH_FLINTMAX / 2.0))
    {
        std::stringstream s;
        s << __PRETTY_FUNCTION__ << " ==> Input out of integer range." << std::endl;
        throw std::runtime_error(s.str());
    }

    const int maximumBins = 65536;
    const typename Derived::Scalar xscale = x.derived().array().abs().maxCoeff();
    const typename Derived::Scalar xrange = x.maxCoeff() - x.minCoeff();

    double binwidth = 1;
    if (xrange > maximumBins)
        binwidth = std::pow(10.0, std::ceil(std::log10((double) xrange / (double) maximumBins)));

    if (hardlimits)
    {
        const double minxi = binwidth * std::ceil((double) minx / binwidth)  + 0.5;
        const double maxxi = binwidth * std::floor((double) maxx / binwidth) - 0.5;
        const int N = (int) std::floor(((maxxi - minxi) / binwidth) + 3);
        VectorXd y(N);
        int j = 1;
        for (double i = minxi; i <= maxxi; i += binwidth)
        {
            y(j) = i;
            j++;
        }
        y(0) = minx;
        y(N-1) = maxx;
        return y;
    }
    else
    {
        const double minxi = std::floor(binwidth * round((double) minx / binwidth)) - 0.5 * binwidth;
        const double maxxi = std::ceil(binwidth * round((double) maxx / binwidth)) + 0.5 * binwidth;
        const int N = (int) std::floor(((maxxi - minxi) / binwidth) + 1);
        VectorXd y(N);
        int j = 0;
        for (double i = minxi; i <= maxxi; i += binwidth)
        {
            y(j) = i;
            j++;
        }
        return y;
    }
}

template<typename Derived>
VectorXd Edges::sturgesrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits)
{
    const int nbins = std::max(std::ceil(std::log2(x.size()) + 1), 1);
    if (hardlimits)
    {
        return Math::linspace(minx, maxx, nbins);
    }
    else
    {
        return Edges::binpicker(minx, maxx, (double)(maxx - minx) / (double) nbins);
    }
}

template<typename Derived>
VectorXd Edges::sqrtrule(const DenseBase<Derived>& x, const typename Derived::Scalar minx, const typename Derived::Scalar maxx, const bool hardlimits)
{
    const int nbins = std::max(std::ceil(std::sqrt(x.size())), 1);
    if (hardlimits)
    {
        return Math::linspace(minx, maxx, nbins);
    }
    else
    {
        return Edges::binpicker(minx, maxx, (double)(maxx - minx) / (double) nbins);
    }
}

#endif


#ifndef _RANDOM_DISCRIMINANT_
#define _RANDOM_DISCRIMINANT_

#include <math.h>

#include <Eigen/Dense>

struct DiscriminantModel {
  unsigned dim;
  Eigen::VectorXd projection_line;
  double decision_boundary;
};

void
InitDiscriminantModel(
  DiscriminantModel* self,
  unsigned dimensionality,
  const Eigen::MatrixXd* class_0_data,
  const Eigen::MatrixXd* class_1_data) {
/**
 *
 */
  self->dim = dimensionality;

  // get a random projection line (unit vector for easy projections)
  self->projection_line.resize(self->dim);
  unsigned i;
  for (i = 0; i < self->dim; ++i) {
    self->projection_line(i) = (double) (rand() % 100);
  }
  self->projection_line.normalize();

  Eigen::VectorXd class_0_projections = *class_0_data * self->projection_line;
  Eigen::VectorXd class_1_projections = *class_1_data * self->projection_line;

  double mu_0 = class_0_projections.mean();
  double mu_1 = class_1_projections.mean();
  double sigma_0 = 0.0;
  double sigma_1 = 0.0;
  for (i = 0; i < class_0_projections.size(); ++i) {
    sigma_0 += (class_0_projections(i) - mu_0) *
               (class_0_projections(i) - mu_0);
    sigma_1 += (class_1_projections(i) - mu_1) *
               (class_1_projections(i) - mu_1);
  }
  sigma_0 /= class_0_projections.size();
  sigma_1 /= class_1_projections.size();

  self->decision_boundary = 0.5 * (mu_1 - mu_0); // TODO: do actual boundary!!!!!!!!!!!
}

Eigen::VectorXi
GetDiscriminantPredictions(
  const DiscriminantModel* self,
  const Eigen::MatrixXd* observations) {
/**
 *
 */
  Eigen::VectorXd projections = *observations * self->projection_line;
std::cout << projections << std::endl;

  Eigen::VectorXi predictions(projections.size());
  unsigned i;
  for (i = 0; i < predictions.size(); ++i) {
    predictions(i) = (projections(i) >= self->decision_boundary) ? 0 : 1;
  }

  return predictions;
}

void
PrintDiscriminantModel(
  const DiscriminantModel* self,
  FILE* stream,
  const bool show_projection_line) {
/**
 *
 */
  printf("Dimensionality:    %u -> 1\n", self->dim);
  printf("Decision Boundary: %lf\n", self->decision_boundary);
  printf("Projection Line:\n");
  if (show_projection_line) {
    std::cout << self->projection_line << std::endl;
  }
}

#endif // define _RANDOM_DISCRIMINANT_


#include <Eigen/Dense>
#include <iostream>
int main(int argc, char **argv) {
  using namespace std;

  // Solve the equation:
  // A*X = Y where:
  // A=  1.0   2.0
  //     4.0   9.0
  //
  // Y= 1
  //    3

  Eigen::VectorXd Y(2);
  Y(0)= 1.0;
  Y(1)= 3.0;
  Eigen::MatrixXd A(2,2);
  A(0,0)= 1.0; A(0,1)=2.0;
  A(1,0)= 4.0; A(1,1)=9.0;

  cout << "Determinant = " << A.determinant() << endl;

  Eigen::MatrixXd AInv= A.inverse();
  cout << "Solution is \n" << AInv*Y << endl;

}


#include <iostream>
#include <tuple>
#include <Eigen/Dense>
#include "linear_algebra_addon.hpp"
using namespace Eigen;
using namespace std;

MatrixXcd bl_bicgstab_rq(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
  //Originally derived based on
  // Rashedi et al. 2016, On short recurrence Krylov type methods for linear systems with many right-hand sides, J. Comput. Appl. Math.
  // Guennouni et al 2003, A block version of BICGSTAB for linear systems with multiple right-hand sides, Electronic Transaction on Numerical Anaysis

  int N= B.rows();
  int L= B.cols();
  MatrixXcd thinQ(MatrixXcd::Identity(N,L));
  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(N,L); // Initial guess of X (zeros)
  MatrixXcd R0= B-A*X;
  MatrixXcd R;
  MatrixXcd C;
  tie(R,C)= qr_reduced(R0);
  MatrixXcd Y0= R0;
  double norm0= R0.norm();
  double normr= norm0;
  MatrixXcd P= R;

  for(int k= 0; k < itermax; ++k){
    MatrixXcd AP= A*P;
    FullPivLU<MatrixXcd> lu(Y0.adjoint()*AP);
    MatrixXcd alpha= lu.solve(Y0.adjoint()*R);
    MatrixXcd S= R-AP*alpha;
    MatrixXcd T= A*S;
    MatrixXcd SC= S*C;
    MatrixXcd TC= T*C;
    complex<double> w= (SC.adjoint()*TC).trace()/(TC.adjoint()*TC).trace();
    X= X+P*alpha*C+w*SC;
    R= S-w*T;
    MatrixXcd U;
    tie(R,U)= qr_reduced(R);
    C= U*C;

    double err= C.norm()/Bnorm;
    cout << "bl_bicgstab_rq: " << "iter= " << k << " relative err= " << err << endl;
    if(err < tol) break;

    MatrixXcd beta= -lu.solve(Y0.adjoint()*T*U.inverse());
    P= R+(P-AP*w)*beta;

  }
  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_bicgstab_rq did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}


#include <Eigen/Dense>
#include <iostream>
using namespace std;
int main() {
  Eigen::MatrixXf matrix = Eigen::MatrixXf::Random(3, 3);
  Eigen::MatrixXf matrix_c = Eigen::MatrixXf::Constant(3, 3, 2.f);
  Eigen::Vector3f vector(3), vector1(3);
  vector << 1, 2, 3;
  vector1 << 4, 5, 6;
  cout << "初始矩阵=\n" << matrix << endl;
  cout << "矩阵乘法(非elementwise)=\n" << matrix * matrix_c << "\n";
  cout << "矩阵和标量的乘法=\n" << matrix * 2.f << "\n";
  cout << "矩阵的转置=\n" << matrix.transpose() << "\n";
  matrix.transposeInPlace();
  cout << "矩阵的Inplace转置=\n" << matrix << "\n";
  cout << "矩阵和向量的乘法=\n" << matrix * vector << "\n";
  cout << "矩阵的reduce操作\n";
  cout << "所有元素求和=" << matrix.sum() << "\n";
  cout << "矩阵的元素累乘=" << matrix.prod() << "\n";
  cout << "矩阵的均值=" << matrix.mean() << "\n";
  cout << "矩阵的最小元素=" << matrix.minCoeff() << "\n";
  cout << "矩阵的最大元素=" << matrix.maxCoeff() << "\n";
  cout << "矩阵的迹=" << matrix.trace() << "\n";

  cout << "向量的点乘=\n" << vector.dot(vector1) << endl;
  cout << "向量的叉乘(只支持三维向量)=\n" << vector.cross(vector1) << endl;
}


/**
 * @file engquistoshernumericalflux.cc
 * @brief NPDE homework "EngquistOsherNumericalFlux" code
 * @author Oliver Rietmann
 * @date 23.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "engquistoshernumericalflux.h"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>

namespace EngquistOsherNumericalFlux {

/* SAM_LISTING_BEGIN_1 */
double EngquistOsherNumFlux(double v, double w) {
  double result;
#if SOLUTION
  auto Fv = [](double v) { return 0 <= v ? std::cosh(v) - 0.5 : 0.5; };
  result = Fv(v) + Fv(-w);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd solveCP(double a, double b, Eigen::VectorXd u0, double T) {
  // Find the maximal speed of propagation
  double A = u0.minCoeff();
  double B = u0.maxCoeff();
  double K = std::max(std::abs(std::sinh(A)), std::abs(std::sinh(B)));
  // Set uniform timestep according to CFL condition
  int N = u0.size();
  double h = (b - a) / N;
  double tau_max = h / K;
  double timesteps = std::ceil(T / tau_max);
  double tau = T / timesteps;

  // Main timestepping loop
#if SOLUTION
  Eigen::VectorXd mu(N);
  for (int i = 0; i < timesteps; ++i) {
    mu.swap(u0);
    double F_minus;
    double F_plus = EngquistOsherNumFlux(mu(0), mu(0));
    for (int j = 0; j < N - 1; ++j) {
      F_minus = F_plus;
      F_plus = EngquistOsherNumFlux(mu(j), mu(j + 1));
      u0(j) = mu(j) - tau / h * (F_plus - F_minus);
    }
    F_minus = F_plus;
    F_plus = EngquistOsherNumFlux(mu(N - 1), mu(N - 1));
    u0(N - 1) = mu(N - 1) - tau / h * (F_plus - F_minus);
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return u0;
}
/* SAM_LISTING_END_2 */

}  // namespace EngquistOsherNumericalFlux


/**
* gaussian.hpp
* @author : koide
* 13/06/12
* 13/06/20 add GMM
* 13/08/03 rename
* 13/11/16 add univariate gauss distibution and univariate gaussian mixture
* 14/10/13 add KL divergence
* 14/10/16 add univariate L2 distance
* 14/11/11 IncrementalGaussianDistribution
* 15/02/13 make IncrementalGaussianDistribution faster using PartialPivLu
* 15/08/20 GaussianEstimater, IndependentGaussianEstimater
**/
#ifndef KKL_GAUSSIAN_HPP
#define KKL_GAUSSIAN_HPP

//#define _USE_BOOST_ERF

#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>

#ifdef _USE_BOOST_ERF
#include <boost/math/special_functions/erf.hpp>
#endif

namespace kkl{
	namespace math{

    /**
     * @brief univariate gaussian PDF
     */
		template<typename T>
		T gaussianProbUni(T mean, T var, T x){
			const T dif = x - mean;
			return 1.0 / sqrt(2.0 * M_PI * var) * exp(-(dif * dif) / (2 * var));
		}

    /**
     * @brief multivariate gaussian PDF
     */
		template<typename T, int p>
		T gaussianProbMul(const Eigen::Matrix<T, p, 1>& mean, const Eigen::Matrix<T, p, p>& cov, const Eigen::Matrix<T, p, 1>& x) {
			const T sqrtDet = sqrt(cov.determinant());
			const Eigen::Matrix<T, p, 1> dif = x - mean;
			const T lhs = 1.0 / (pow(2.0 * M_PI, p / 2.0) * sqrtDet);
			const T rhs = exp(-0.5 * ((dif.transpose() * cov.inverse() * dif))(0, 0));
			return lhs * rhs;
		}

#ifdef _USE_BOOST_ERF
    /**
     * @brief univariate cumulative probability function
     */
		template<typename T>
		T gaussianCumulativeProbUni(T mean, T var, T x) {
			return 0.5 * (1 + boost::math::erf((x - mean) / sqrt(2 * var)));
		};
#endif

    /**
     * @brief compute an error ellipse
     */
    template<typename T, typename Matrix>
    Eigen::Matrix<T, 3, 1> errorEllipse(const Matrix& cov, double kai) {
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, 2, 2>> solver(cov);

      Eigen::Matrix<T, 3, 1> params;
      params[0] = std::sqrt(kai * kai * solver.eigenvalues()[1]);
      params[1] = std::sqrt(kai * kai * solver.eigenvalues()[0]);
      params[2] = std::atan2(solver.eigenvectors()(0, 2), solver.eigenvectors()(1, 2));

      return params;
    }

    /**
     * @brief squared mahalanobis distance
     */
		template<typename T, int p>
		T squaredMahalanobisDistance(const Eigen::Matrix<T, p, 1>& mean, const Eigen::Matrix<T, p, p>& cov, const Eigen::Matrix<T, p, 1>& x){
			const Eigen::Matrix<T, p, 1> diff = x - mean;
			double distance = diff.transpose() * cov.inverse() * diff;
			return distance;
		}

    /**
     * @brief univariate squared mahalanobis distance
     */
		template<typename T>
		T squaredMahalanobisDistanceUni(T mean, T var, T x) {
			T diff = mean - x;
			return diff * diff / var;
		}

  }
}

#endif


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

// Copyright Paul A. Bristow 2015.
// Copyright Christopher Kormanyos 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing a polynomial approximation of sin(negatable).

// Below are snippets of code that are included into Quickbook file fixed_point.qbk.

#include <iomanip>
#include <iostream>

#include <boost/fixed_point/fixed_point.hpp>

namespace local
{
  template<typename NumericType>
  NumericType sin(const NumericType& x)
  {
    // This subroutine computes sin(x) using an order 5 polynomial
    // approximation that achieves about 4 decimal digits of precision
    // in the range -pi/2 < x < +pi/2.

    // The coefficients in the polynomial approximation originate
    // from C. Kormanyos, Real-Time C++ (Springer, Heidelberg, 2013).
    // See Sect. 13.3, in particular Eqs. 13.4 and 13.5 therein.

    // Use the scaled argument chi = x / (pi/2).
    const NumericType chi  = (x * 2) / NumericType(3.14159265358979323846F);

    // Compute chi^2, as this is also used in the polynomial approximation.
    const NumericType chi2 = chi * chi;

    // Perform the polynomial approximation using a coefficient
    // expansion via the method of Horner.
    return ((         0.0722739F
             * chi2 - 0.6425639F)
             * chi2 + 1.5704128F)
             * chi;
  }
}

int main()
{
  // This example computes approximations of sin(x) in the
  // range -pi/2 < x < pi/2. Comparison are made between
  // fixed-point calculations and floating-point calculations.
  // A 16-bit fixed-point type is used, as may be well-suited
  // for a tiny bare-metal embedded system.

  // The arguments used in the comparisons are:
  //   x = (approx.) 1/4, 1/2, 3/4, 1, 5/4, 3/2.

  // Here we compute sin(n/4) for 1 <= n < 7 for
  // a fixed-point negatable type.
  std::cout << "fixed-point:   ";
  for(int n = 1; n < 7; ++n)
  {
    typedef boost::fixed_point::negatable<5, -10> fixed_point_type;

    const fixed_point_type x = fixed_point_type(n) / 4;

    std::cout << std::setprecision(4)
              << std::fixed
              << local::sin(x)
              << "  ";
  }
  std::cout << std::endl;

  // Compare with the control values of sin(n/4)
  // computed with a built-in floating-point type.
  std::cout << "float-point:   ";
  for(int n = 1; n < 7; ++n)
  {
    using std::sin;

    const float x = float(n) / 4;

    std::cout << std::setprecision(4)
              << std::fixed
              << sin(x)
              << "  ";
  }
  std::cout << std::endl;
}


// Exercise 5.1.5 - Statistical Functions
//
// by Scott Sidoli
//
// 5-30-19
//
// Main.cpp
//
// In this exercise, we modify the code in TestNormalDistribution.cpp to work with the exponential distribution instead of the
// normal distribution and the poisson distribution instead of the gamma distribution.

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

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

int main()
{
	// Don't forget to tell compiler which namespace
	using namespace boost::math;

	//=============================== Normal Distribution ===============================

	normal_distribution<> myNormal(1.0, 10.0); // Default type is 'double'
	cout << "Mean: " << mean(myNormal) << ", standard deviation: " << standard_deviation(myNormal) << endl;

	// Distributional properties
	double x = 10.25;

	cout << "pdf: " << pdf(myNormal, x) << endl;
	cout << "cdf: " << cdf(myNormal, x) << endl;

	// Choose another data type and now a N(0,1) variate
	normal_distribution<float> myNormal2;
	cout << "Mean: " << mean(myNormal2) << ", standard deviation: " << standard_deviation(myNormal2) << endl;

	cout << "pdf: " << pdf(myNormal2, x) << endl;
	cout << "cdf: " << cdf(myNormal2, x) << endl;

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

	// Other properties
	cout << "\n***normal distribution: \n";
	cout << "mean: " << mean(myNormal) << endl;
	cout << "variance: " << variance(myNormal) << endl;
	cout << "median: " << median(myNormal) << endl;
	cout << "mode: " << mode(myNormal) << endl;
	cout << "kurtosis excess: " << kurtosis_excess(myNormal) << endl;
	cout << "kurtosis: " << kurtosis(myNormal) << endl;
	cout << "characteristic function: " << chf(myNormal, x) << endl;
	cout << "hazard: " << hazard(myNormal, x) << endl;

	//=============================== Exponential Distribution ===============================

	double scaleParameter = 0.5;

	// Default type is 'double'
	exponential_distribution<> myExponential(scaleParameter);

	cout << "Mean: " << mean(myExponential) << ", standard deviation: " << standard_deviation(myExponential) << endl;

	// PDF and CDF
	cout << "pdf: " << pdf(myExponential, x) << endl;
	cout << "cdf: " << cdf(myExponential, x) << endl;

	// Using another data type and a N(0,1) variate
	exponential_distribution<float> myExponential2(1.0);
	cout << "Mean: " << mean(myExponential2) << ", standard deviation: " << standard_deviation(myExponential2) << endl;

	// PDF and CDF
	cout << "pdf: " << pdf(myExponential2, x) << endl;
	cout << "cdf: " << cdf(myExponential2, x) << endl;

	// Other properties
	cout << "\n***exponential 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;
	cout << "characteristic function: " << chf(myExponential, x) << endl;
	cout << "hazard: " << hazard(myExponential, x) << endl;

	//=============================== Gamma Distribution ===============================
	double alpha = 3.0; // Shape parameter, k
	double beta = 0.5;	// Scale parameter, theta
	gamma_distribution<double> myGamma(alpha, beta);
	cout << "everything that follows is for the gamma distribution" << endl;
	double val = 13.0;
	cout << endl << "pdf: " << pdf(myGamma, val) << endl;
	cout << "cdf: " << cdf(myGamma, 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);

	for (long j = 1; j <= N; ++j)
	{
		pdfList.push_back(pdf(myGamma, val));
		cdfList.push_back(cdf(myGamma, val));

		val += h;
	}

	for (long j = 0; j < pdfList.size(); ++j)
	{
		cout << pdfList[j] << ", " << endl;

	}

	cout << "***" << endl;

	for (long j = 0; j < cdfList.size(); ++j)
	{
		cout << cdfList[j] << ", " << endl;

	}

	//=============================== Poisson Distribution ===============================
	cout << "everything that follows is for the Poisson distribution" << endl;
	double mean = 3.0;
	poisson_distribution<double> myPoisson(mean);

	val = 3.5;
	cout << "pdf: " << pdf(myPoisson, val) << endl;
	cout << "cdf: " << cdf(myPoisson, val) << endl;

	vector<double> pdfList2;
	vector<double> cdfList2;

	double start2 = 0.0;
	double end2 = 6.0;
	long N2 = 30;		// Number of subdivisions

	val = 0.0;
	double h2 = (end2 - start2) / double(N2);

	for (long j = 1; j <= N2; ++j)
	{
		pdfList2.push_back(pdf(myPoisson, val));
		cdfList2.push_back(cdf(myPoisson, val));

		val += h2;
	}

	for (long j = 0; j < pdfList2.size(); ++j)
	{
		cout << pdfList2[j] << ", "<< endl;

	}

	cout << "***" << endl;

	for (long j = 0; j < cdfList2.size(); ++j)
	{
		cout << cdfList[j] << ", " << endl;

	}
	return 0;
}

Scott_Sidoli Level 8 HW Submission/Section 5.1/Exercise515/Exercise515/Main.cpp


#include "utils.h"
#include "bshelper.h"
#include <cmath>
#include <boost/math/distributions/normal.hpp>

using boost::math::normal;

const double getD1(double asset_price, double strike, double rate, double volatility, double time_to_maturity) {
    double unadjusted_d1 = log(asset_price / strike) + (rate + pow(volatility, 2) / 2) * time_to_maturity;
    double result = unadjusted_d1 / (volatility * sqrt(time_to_maturity));
    return result;
}

const double getD2(double asset_price, double strike, double rate, double volatility, double time_to_maturity) {
    double unadjusted_d2 = log(asset_price / strike) + (rate - pow(volatility, 2) / 2) * time_to_maturity;
    double result = unadjusted_d2 / (volatility * sqrt(time_to_maturity));
    return result;
}

const double weightAsset(double asset_price, double option_d1) {
    normal normalDistInstance;
    double d1_probability = cdf(normalDistInstance, option_d1);
    return asset_price * d1_probability;
}

const double weightStrike(double option_strike, double option_d2, double rate, double time_to_maturity) {
    normal normalDistInstance;
    double d2_probability = cdf(normalDistInstance, option_d2);
    double discount_factor = continuous_discount_factor(rate, time_to_maturity);
    return option_strike * discount_factor * d2_probability;
}

const double
evaluateCall(double option_strike, double time_to_maturity, double rate, double volatility, double asset_price) {
    double d1_value = getD1(asset_price, option_strike, rate, volatility, time_to_maturity);
    double d2_value = getD2(asset_price, option_strike, rate, volatility, time_to_maturity);

    double weighted_asset = weightAsset(asset_price, d1_value);
    double weighted_strike = weightStrike(option_strike, d2_value, rate, time_to_maturity);

    return weighted_asset - weighted_strike;
}

const double
evaluatePut(double option_strike, double time_to_maturity, double rate, double volatility, double asset_price) {
    double d1_value = getD1(asset_price, option_strike, rate, volatility, time_to_maturity);
    double d2_value = getD2(asset_price, option_strike, rate, volatility, time_to_maturity);

    double weighted_asset = weightAsset(asset_price, -d1_value);
    double weighted_strike = weightStrike(option_strike, -d2_value, rate, time_to_maturity);

    return weighted_strike - weighted_asset;
}

const double
blackScholes(enum OptionType option_type, double option_strike, double time_to_maturity, double rate, double volatility,
             double asset_price) {
    switch (option_type) {
        case call:
            return evaluateCall(option_strike, time_to_maturity, rate, volatility, asset_price);
        case put:
            return evaluatePut(option_strike, time_to_maturity, rate, volatility, asset_price);
    }
    return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <string>
#include <fstream>

using Eigen::AngleAxisf;
using Eigen::Matrix3f;
using Eigen::Vector3f;
using Eigen::Quaternionf;
using Eigen::Transform;

int main()
{
    const float angle_x = 30.0f * M_PI / 180.0f;
    const float angle_y = 20.0f * M_PI / 180.0f;
    const float angle_z = 10.0f * M_PI / 180.0f;
    const Vector3f target(1, 2, 3);
    const Vector3f bias(3, 2, 1);
    Matrix3f fix_axis_m;
    Matrix3f unfix_axis_m;
    std::ofstream file;

    file.open("transform_wcm.txt");

    // 计算固定坐标系下的旋转矩阵
    {
        // Eigen 自带的轴角表示系统，传入坐标轴和旋转角度即可
        // 由于 AngleAxisf 重载了乘法运算，所以合并多个旋转矩阵只要用乘法就行
        // http://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html
        Matrix3f m = AngleAxisf(angle_z, Vector3f::UnitZ())
            * AngleAxisf(angle_y, Vector3f::UnitY())
            * AngleAxisf(angle_x, Vector3f::UnitX()).toRotationMatrix();

        std::cout << "\n固定坐标系下的旋转矩阵：" << std::endl
            << m << std::endl;
        fix_axis_m = m;

        AngleAxisf res;
        // 从旋转矩阵中得到轴角表示
        // http://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html#a2e35689645f69ba886df1a0a14b76ffe
        res.fromRotationMatrix(fix_axis_m);
        file << "固定坐标系：" << std::endl
             << "旋转轴：(" << res.axis().transpose() << "), 旋转角" << res.angle() * 180.0f / M_PI << std::endl
             << "四元数表示：[" << Quaternionf(res).vec().transpose() << "]" << std::endl;
    }

    // 计算不固定坐标系下的旋转矩阵
    {
        Vector3f axis_x = Vector3f::UnitX();
        Vector3f axis_y = Vector3f::UnitY();
        Vector3f axis_z = Vector3f::UnitZ();
        Matrix3f m = Matrix3f::Identity();

        // 这里的 lambda 函数对三个坐标轴都进行了旋转
        // 注意函数的捕获列表，& 表示捕获作用域内所有变量的引用
        // 所以在函数内我们可以直接修改函数外变量的值
        auto rotate_axises = [&](const AngleAxisf rotate) {
            axis_x = rotate * axis_x;
            axis_y = rotate * axis_y;
            axis_z = rotate * axis_z;
            m = rotate * m;
        };

        // 从 x 开始旋转
        rotate_axises(AngleAxisf(angle_x, axis_x));
        rotate_axises(AngleAxisf(angle_y, axis_y));
        rotate_axises(AngleAxisf(angle_z, axis_z));

        std::cout << "\n不固定坐标系下的旋转矩阵：" << std::endl
                  << m << std::endl;
        unfix_axis_m = m;

        AngleAxisf res;
        res.fromRotationMatrix(unfix_axis_m);
        file << "\n不固定坐标系：" << std::endl
             << "旋转轴：(" << res.axis().transpose() << "), 旋转角" << res.angle() * 180.0f / M_PI << std::endl
             << "四元数表示：[" << Quaternionf(res).vec().transpose() << "]" << std::endl;
    }

    // 计算向量的齐次变换
    {
        // 使用 Transform 表示齐次变换
        // http://eigen.tuxfamily.org/dox/group__TutorialGeometry.html#title2
        Transform<float, 3, Eigen::TransformTraits::Affine> t;
        t = AngleAxisf(angle_z, Vector3f::UnitZ());
        // 等价于 t = t * AngleAxisf(angle_y, Vector3f::UnitY())
        t.rotate(AngleAxisf(angle_y, Vector3f::UnitY()));
        t.rotate(AngleAxisf(angle_x, Vector3f::UnitX()));
        // 等价于 t = t*() + bias
        // 括号表示要旋转的向量
        t.pretranslate(bias);
        std::cout << std::endl << t.matrix() << std::endl;

        std::cout << "\n固定坐标下，齐次变换目标向量结果为：" << std::endl
            << t*target << std::endl;

        file << "\n固定坐标下，齐次变换目标向量结果为：("
             << (t * target).transpose() << ")" << std::endl;

        // 从旋转矩阵构造四元数
        // http://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#title30
        Quaternionf q(unfix_axis_m);
        std::cout << "\n不固定坐标系下，齐次变换目标向量结果为：" << std::endl
            << (q.toRotationMatrix()*target) + bias << std::endl;

        file << "\n不固定坐标下，齐次变换目标向量结果为：("
             << ((q.toRotationMatrix() * target) + bias).transpose() << ")" << std::endl;
    }
    file.close();

    // 手搓一个固定坐标系下的旋转矩阵，证明 Eigen 库的正确性
    {
        using std::sin;
        using std::cos;
        Matrix3f m = Matrix3f::Identity();
        Matrix3f tmp;

        // 绕 x 轴旋转
        tmp << 1, 0, 0,
            0, cos(angle_x), -sin(angle_x),
            0, sin(angle_x), cos(angle_x);
        m = tmp * m;

        // 绕 y 轴旋转
        tmp << cos(angle_y), 0, sin(angle_y),
            0, 1, 0,
            -sin(angle_y), 0, cos(angle_y);
        m = tmp * m;

        // 绕 z 轴旋转
        tmp << cos(angle_z), -sin(angle_z), 0,
            sin(angle_z), cos(angle_z), 0,
            0, 0, 1;
        m = tmp*m;
        std::cout << std::endl << m*target + bias << std::endl;
    }

    return 0;
}


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

using namespace Eigen;

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

    MatrixXd A = MatrixXd::Zero(2,2);
    MatrixXd B = MatrixXd::Zero(2,2);

    A(0, 0) = 1;
    A(0, 1) = 2;
    A(1, 0) = 3;
    A(1, 1) = 4;

    B(0, 0) = 5;
    B(0, 1) = 6;
    B(1, 0) = 7;
    B(1, 1) = 8;

    cout << "Same Dimension Matrix Multiply" << endl;
    cout << A.rows() << " " << A.cols() << endl;
    cout << A*B << endl; // 行列の積
    cout << A.array() * B.array() << endl; // 要素積

    MatrixXd X = MatrixXd::Zero(2,3);
    MatrixXd Y = MatrixXd::Zero(3,2);

    X << 1,2,3, 4,5,6;
    Y << 1,2, 3,4, 5,6;

    cout << "Different Dimension Matrix Multiply" << endl;
    cout << "X shape: " << X.rows() << " " << X.cols() << endl;
    cout << "Y shape: " << Y.rows() << " " << Y.cols() << endl;
    cout << X * Y << endl;
    cout << X.array() * Y.transpose().array() << endl;

    return 0;
}