/***************************************************************************
/* 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 <blitz/array.h>
#include <blitz/array/stencil-et.h>

BZ_USING_NAMESPACE(blitz)

// This example illustrates a simple use of stencils.
// It applies a Laplacian operator to an array which is
// all zero, except for a single 1 in the centre.
// This results in the stencil pattern (although reversed
// and upside-down).

int main()
{
    const int N = 9;

    // Create some arrays
    Array<float,2> A(N,N), B(N,N), C(N,N), D(N,N);

    // Initialize A to 1 in the centre, 0 elsewhere
    A = 0;
    A(N/2,N/2) = 1.0;

    // Get the interior of the arrays.  Since central42 is the
    // stencil [ 1 -4 6 -4 1 ], we have to leave a padding of 2
    // cells around the interior.  The range 2..N-3 does this.
    Range I(2,N-3);
    Array<float,2> Ai = A(I,I), Bi = B(I,I), Ci = C(I,I), Di = D(I,I);

    // These declarations let us call the first dimension "x"
    // and the second dimension "y".
    const int x = firstDim, y = secondDim;

    // Apply the stencils.  Note that Laplacian2D is identical to
    // central22(Ai,x) + central22(Ai,y).

    Bi = Laplacian2D(Ai);
    Ci = central42(Ai,x) + central42(Ai,y);
    Di = central22(Ai,x) + central22(Ai,y);

    // Display some of the results

    Range middle(N/4,3*N/4);

    cout << "Laplacian2D: " << B(middle,middle) << endl
         << "central42: " << C(middle,middle) << endl
         << "central22: " << D(middle,middle) << endl;

    return 0;
}


/*
Неэффективная реализация этой формулы:
https://i.imgur.com/SZNcbva.png
Для сходящихся знакочередующихся рядов справедлив следующий критерий:
члены следует суммировать до тех пор, пока они по модулю больше заданной точности.
*/

#include <iostream>
#include <iomanip>

#include <boost/multiprecision/cpp_dec_float.hpp>

#include "../include/calcE.h"

using namespace boost::multiprecision;
//namespace mp = boost::multiprecision; // mp::pow

const long long ACCUR = 10000; // Кол-во знаков после запятой
const size_t DIGNUM = ACCUR+10;

typedef number< cpp_dec_float<DIGNUM> > cpp_dec_float_DIGNUM;

void calcE()
{
    const cpp_dec_float_DIGNUM one = 1;
    cpp_dec_float_DIGNUM e;
    cpp_dec_float_DIGNUM xi;
    cpp_dec_float_DIGNUM buff;
    cpp_dec_float_DIGNUM fac;

    buff = 10;
    buff = pow(buff, -ACCUR);
    const cpp_dec_float_DIGNUM accur = buff;

    size_t i = 2;
    fac = 2;
    e = 0;
    int iter = 0;
    do
    {
        xi = one / fac;
        if(iter == 0) // чет
            e += xi;
        else // нечет
            e -= xi;

        ++i;
        iter = (iter == 0?1:0);

        buff = i;
        fac *= buff;
    }while(xi >= accur);

    buff = 1;
    e = buff / e;

    //std::cout << std::setprecision(std::numeric_limits<number< cpp_dec_float<ACCUR> >>::max_digits10) << "e = " << e << std::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;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  typedef Matrix<float,Dynamic,2> DataMatrix;
// let's generate some samples on the 3D plane of equation z = 2x+3y (with some noise)
DataMatrix samples = DataMatrix::Random(12,2);
VectorXf elevations = 2*samples.col(0) + 3*samples.col(1) + VectorXf::Random(12)*0.1;
// and let's solve samples * [x y]^T = elevations in least square sense:
Matrix<float,2,1> xy
 = (samples.adjoint() * samples).llt().solve((samples.adjoint()*elevations));
cout << xy << endl;

  return 0;
}


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


#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include "stiffness_matrix.hpp"

//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;

//----------------AssembleMatrixBegin----------------
//! Assemble the stiffness matrix
//! for the linear system
//!
//! @param[out] A will at the end contain the Galerkin matrix
//! @param[in] vertices a list of triangle vertices
//! @param[in] dofs a list of the dofs' indices in each triangle
template <class Matrix>
void assembleStiffnessMatrix(Matrix &A, const Eigen::MatrixXd &vertices, const Eigen::MatrixXi &dofs, const int &N) {
	const int numberOfElements = dofs.rows();
	A.resize(N, N);

	std::vector<Triplet> triplets;

	triplets.reserve(numberOfElements * 6 * 10);
	// (write your solution here)
	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = dofs.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));
		const auto &c = vertices.row(indexSet(2));

		Eigen::MatrixXd stiffnessMatrix;
		computeStiffnessMatrix(stiffnessMatrix, a, b, c);

		for (int n = 0; n < 6; ++n) {
			for (int m = 0; m < 6; ++m) {
				auto triplet = Triplet(indexSet(n), indexSet(m), stiffnessMatrix(n, m));
				triplets.push_back(triplet);
			}
		}
	}
	A.setFromTriplets(triplets.begin(), triplets.end());
}
//----------------AssembleMatrixEnd----------------


/**
 * @file kalman_filter.h
 * @brief Kalman Filter.
 *
 */

#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

#include <Eigen/Dense>

namespace filter
{

class KalmanFilter
{
    public:
        /**
         * @brief Create a Kalman filter.
         *
         * @param C Output matrix
         * @param Q Process noise covariance
         * @param R Measurement noise covariance
         * @param P Estimate error covariance
         */
        KalmanFilter(
            const Eigen::MatrixXd& A,
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& Q,
            const Eigen::MatrixXd& R,
            const Eigen::MatrixXd& P
        );
        ~KalmanFilter() {};

        /**
         * @brief Initialize the filter with initial states as zero.
         *
         */
        void init();

        /**
         * @brief Initialize the filter with an estimation on the initial states.
         * @param t0 initial timestamp.
         * @param x0 estimated state of the system.
         *
         */
        void init(double t0, const Eigen::VectorXd& x0);

        /**
         * @brief An adaptive Kalman filter.
         * @brief Update the estimated state based on measured values, using the given time step and dynamics matrix.
         * @param y measurements.
         * @param dt timestamp.
         *
         */
        Eigen::VectorXd compute(const Eigen::VectorXd& y, double dt);

    private:
        // Matrices for computation.
        Eigen::MatrixXd A, C, Q, R, P, K, P0;

        // Systems dimension.
        int m; // Number of measurements.
        int n; // Number of states.

        Eigen::MatrixXd I; // Identity matrix.
        Eigen::VectorXd x_hat, x_hat_new; // Estimated states.

        // Initial timer.
        double t0;

        // Flag.
        bool initialized;
};

} // namespace controller

#endif /* KALMAN_FILTER_H */


/******************************************************************************
 * 反射衛星砲: 数学モデルユーティリティ関数
 *****************************************************************************/

#ifndef __MATHEMATIC_UTILS_HPP__
#define __MATHEMATIC_UTILS_HPP__

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

typedef std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > Vector3dSet;

// 点P1,P2の間の距離の2乗を求める
double distance2(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2);

// ベクトルX を正規化したベクトルを求める
Eigen::Vector3d normalize(const Eigen::Vector3d& X);

// ベクトルX が ベクトルa を軸として p[rad]回転したときのベクトルを求める
Eigen::Vector3d rotate(const Eigen::Vector3d& X, const Eigen::Vector3d& a, const double p);

// ベクトルX が 法線ベクトルn の面に入射したときの反射ベクトルを求める
Eigen::Vector3d reflect(const Eigen::Vector3d& X, const Eigen::Vector3d& n);

// 点の集合P={p1,p2,...} のうち、点Oに最も近い点の集合(⊆P)を求める
Vector3dSet neighoring(const Eigen::Vector3d& O, const Vector3dSet& P);

// 直線L(P,v) が楕円体E(O,R)の表面を通過,接触する座標を求める
// ただし実数解のみ
Vector3dSet intersection(const Eigen::Vector3d& P, const Eigen::Vector3d& v,
                       const Eigen::Vector3d& O, const Eigen::Vector3d& R);

// 点P が楕円体E(O,R)の中にあるかどうか調べる
bool inner_elipsoid(const Eigen::Vector3d& P, const Eigen::Vector3d& O, const Eigen::Vector3d& R);

#endif // __MATHEMATIC_UTILS_HPP__


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2014 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Non-Negative Matrix Factorization                                        *
***************************************************************************/

#ifndef NNMF_HPP
#define NNMF_HPP

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <cmath>
#include <algorithm>

#define SQRTEPS 1.490116119384766e-08

// [1] C. Boutsidis, E. Gallopoulos. SVD based initialization:A head start for nonnegative matrix factorization. Pattern Recognition 41 (2008) 1350–1362

using namespace Eigen;

class NNMF
{
public:
	enum Algorithm { ALS, MUR };

	NNMF(const int maxIterations = 200, const double threshold = 1e-4, const double tolerance = 1e-4, const bool verbose = false);

	template <class Derived>
	void compute(const MatrixBase<Derived> &dataset, const int K, const Algorithm algorithm = ALS);
	template <class Derived>
	void compute(const MatrixBase<Derived> &dataset, const MatrixXd &W0, const MatrixXd &H0, const Algorithm algorithm = ALS);

	MatrixXd W() const { return m_W; }
	MatrixXd H() const { return m_H; }

private:
	template <bool AlternatingLeastSquares = true>
	void nnmf(const MatrixXd &dataset);

	template <class Derived>
	void SVDInitialization(const MatrixBase<Derived> &dataset, const int K);
	void standard_form();

	MatrixXd m_W;
	MatrixXd m_H;
	int m_components;
	int m_maxIterations;
	double m_threshold;
	double m_tolerance;
	bool m_verbose;
};

/***************************** Implementation *****************************/

NNMF::NNMF(const int maxIterations, const double threshold, const double tolerance, const bool verbose)
: m_W(MatrixXd()), m_H(MatrixXd()), m_components(0), m_maxIterations(maxIterations), m_threshold(threshold), m_tolerance(tolerance), m_verbose(verbose)
{
}

template<class Derived>
void NNMF::compute(const MatrixBase<Derived> &dataset, const int K, const Algorithm algorithm)
{
	if (K >= dataset.cols())
	{
		m_W = dataset.derived().cast<double>();
		m_H = MatrixXd::Identity(K, K);
		return;
	}

	if (K >= dataset.rows())
	{
		m_W = MatrixXd::Identity(K, K);
		m_H = dataset.derived().cast<double>();
		return;
	}

	// Set components
	m_components = K;

	// Singular Value Decomposition initialization
	SVDInitialization(dataset, K);

	// Run Non-Negative Matrix Factorization decomposition
	if (algorithm == ALS)
		nnmf<true>(dataset.derived().cast<double>());
	else if (algorithm == MUR)
		nnmf<false>(dataset.derived().cast<double>());
	else
		nnmf<true>(dataset.derived().cast<double>());
}

template<class Derived>
void NNMF::compute(const MatrixBase<Derived> &dataset, const MatrixXd &W0, const MatrixXd &H0, const Algorithm algorithm)
{
	// Set components
	m_components = W0.cols();

	// Initialize W and H matrices
	m_W = W0;
	m_H = H0;

	// Run Non-Negative Matrix Factorization decomposition
	if (algorithm == ALS)
		nnmf<true>(dataset.derived().cast<double>());
	else if (algorithm == MUR)
		nnmf<false>(dataset.derived().cast<double>());
	else
		nnmf<true>(dataset.derived().cast<double>());
}

template <bool AlternatingLeastSquares>
void NNMF::nnmf(const MatrixXd &dataset)
{
	// Set number of elements
	const double NM = (double) dataset.size();

	// Declare previous iteration matrices and error
	MatrixXd W0 = m_W;
	MatrixXd H0 = m_H;
	double dnorm0 = 0;

	if (m_verbose)
	{
		mexPrintf("Iteration\t\tMax iterations\t\tRMSE\t\t\tRatio\t\t\tThreshold\t\tDelta\t\t\tTolerance\n");
		mexPrintf("-------------------------------------------------------------------------------------------------------------\n");
	}

	// Constant matrices
	const MatrixXd H_MUR_SQRTEPS = MatrixXd::Constant(m_components, dataset.cols(), SQRTEPS);
	const MatrixXd W_MUR_SQRTEPS = MatrixXd::Constant(dataset.rows(), m_components, SQRTEPS);

	for (int i = 0; i < m_maxIterations; ++i)
	{
		if (AlternatingLeastSquares)
		{
			// Update H
			m_H = W0.fullPivHouseholderQr().solve(dataset).cwiseMax(0);
			// Update W
			m_W = m_H.transpose().fullPivHouseholderQr().solve(dataset.transpose()).transpose().cwiseMax(0);
		}
		else
		{
			// Update H
			MatrixXd numer = W0.transpose() * dataset;
			m_H = H0.cwiseProduct(numer.cwiseQuotient(((W0.transpose() * W0) * H0) + H_MUR_SQRTEPS)).cwiseMax(0);
			// Update W
			numer = dataset * m_H.transpose();
			m_W = W0.cwiseProduct(numer.cwiseQuotient((W0 * (m_H * m_H)) + W_MUR_SQRTEPS)).cwiseMax(0);
		}

		// Compute squared error
		const ArrayXXd d = dataset - (m_W * m_H);
		const double dnorm = std::sqrt((d * d).sum() / NM);
		const double dW = ((m_W - W0).cwiseAbs() / (SQRTEPS + W0.cwiseAbs().maxCoeff())).maxCoeff();
		const double dH = ((m_H - H0).cwiseAbs() / (SQRTEPS + H0.cwiseAbs().maxCoeff())).maxCoeff();
		const double delta = std::max(dW, dH);

		if (m_verbose)
		{
			mexPrintf("%d\t\t\t\t%d\t\t\t\t\t%.2e\t\t%.2e\t\t%.2e\t\t%.2e\t\t%.2e\n", i+1, m_maxIterations, dnorm, (dnorm0 - dnorm), (m_threshold * std::max(1.0, dnorm0)), delta, m_tolerance);
			mexEvalString("drawnow");
		}

		if (i > 0)
		{
			if (delta <= m_tolerance)
			{
				standard_form();
				return;
			}
			if (dnorm0 - dnorm <= (m_threshold * std::max(1.0, dnorm0)))
			{
				standard_form();
				return;
			}
		}

		dnorm0 = dnorm;
		W0 = m_W;
		H0 = m_H;
	}
}

template <class Derived>
void NNMF::SVDInitialization(const MatrixBase<Derived> &dataset, const int K)
{
	// Allocate memory
	m_W.resize(dataset.rows(), K);
	m_H.resize(K, dataset.cols());

	// Perform SVD decomposition
	JacobiSVD<MatrixXd> svd(dataset, ComputeThinU | ComputeThinV);
	MatrixXd U = svd.matrixU();
	MatrixXd V = svd.matrixV();
	VectorXd S = svd.singularValues();

	// Retain only the largest K eigen values and eigen vectors
	S = S.segment(0, K).eval();
	U = U.block(0, 0, U.rows(), K).eval();
	V = V.block(0, 0, V.rows(), K).eval();

	// Perform initialization as suggested in [1]
	m_W.col(0) = std::sqrt(S(0)) * U.col(0);
	m_H.row(0) = std::sqrt(S(0)) * V.col(0).transpose();

	for (int i = 1; i < K; ++i)
	{
		const VectorXd Uip = U.col(i).cwiseMax(0);
		const VectorXd Uin = U.col(i).cwiseMin(0).cwiseAbs();

		const VectorXd Vip = V.col(i).cwiseMax(0);
		const VectorXd Vin = V.col(i).cwiseMin(0).cwiseAbs();

		const double UipNorm = Uip.norm();
		const double VipNorm = Vip.norm();

		const double UinNorm = Uin.norm();
		const double VinNorm = Vin.norm();

		const double mp = UipNorm * VipNorm;
		const double mn = UinNorm * VinNorm;

		VectorXd u;
		VectorXd v;
		double sigma;
		if (mp > mn)
		{
			u = Uip / UipNorm;
			v = Vip / VipNorm;
			sigma = mp;
		}
		else
		{
			u = Uin / UinNorm;
			v = Vin / VinNorm;
			sigma = mn;
		}
		m_W.col(i) = std::sqrt(S(i) * sigma) * u;
		m_H.row(i) = std::sqrt(S(i) * sigma) * v.transpose();
	}
}

void NNMF::standard_form()
{
	// Put in standard form
	RowVectorXd Hlen = m_H.cwiseProduct(m_H).rowwise().sum().cwiseSqrt();

	for (int i = 0; i < Hlen.size(); ++i)
		Hlen(i) = Hlen(i) == 0 ? 1.0 : Hlen(i);

	for (int i = 0; i < m_W.rows(); ++i)
		m_W.row(i) = m_W.row(i).cwiseProduct(Hlen);

	for (int i = 0; i < m_H.cols(); ++i)
		m_H.col(i) = m_H.col(i).cwiseQuotient(Hlen.transpose());

	const RowVectorXd magnitude = m_W.cwiseProduct(m_W).colwise().sum();
	std::vector<int> y(magnitude.size());
    int n = 0;
    std::generate(std::begin(y), std::end(y), [&] { return n++; });
	std::sort(std::begin(y), std::end(y), [&](int i1, int i2) { return magnitude(i1) > magnitude(i2); });

	const MatrixXd Wcopy = m_W;
	const MatrixXd Hcopy = m_H;
	for (int i = 0; i < magnitude.size(); ++i)
	{
		m_W.col(i) = Wcopy.col(y[i]);
		m_H.row(i) = Hcopy.row(y[i]);
	}
}

#endif


/**
 * @file odesolve.cc
 * @brief NPDE homework ODESolve code
 * @author ?, Philippe Peter
 * @date 18.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "odesolve.h"

#include <Eigen/Core>
#include <cmath>
#include <iostream>
#include <vector>

#include "../../../lecturecodes/helperfiles/polyfit.h"

namespace ODESolve {

/* SAM_LISTING_BEGIN_2 */
double TestCvpExtrapolatedEuler() {
  double conv_rate;
  double T = 1.0;
  double y0 = 0.0;
  auto f = [](double y) -> double { return 1.0 + y * y; };

  auto Psi = [&f](double h, double y0) -> double { return y0 + h * f(y0); };
  unsigned p = 1;

  // lambda corresponding to \tilde{\psi}
  auto Psi_tilde = [&Psi, &f, &p](double h, double y0) -> double {
    return PsiTilde(Psi, p, h, y0);
  };

  // exact value
  double y_ex1 = std::tan(T);

  // values for convergence study
  Eigen::ArrayXd err(11);
  Eigen::ArrayXd M(11);

  std::cout << "Error table for equidistant steps:" << std::endl;
  std::cout << "M"
            << "\t"
            << "Error" << std::endl;

  for (int i = 0; i < 11; ++i) {
    M(i) = std::pow(2, i + 2);
    double yT = OdeIntEqui(Psi_tilde, T, y0, M(i)).back();
    err(i) = std::abs(yT - y_ex1);
    std::cout << M(i) << "\t" << err(i) << std::endl;
  }
  // compute fitted rate
  Eigen::VectorXd coeffs = polyfit(M.log(), err.log(), 1);
  conv_rate = -coeffs(0);
  return conv_rate;
}

/* SAM_LISTING_BEGIN_4 */
std::pair<std::vector<double>, std::vector<double>> SolveTangentIVP() {
  auto f = [](double y) -> double { return 1.0 + y * y; };
  double y0 = 0.0;

  double T = 1.5;
  unsigned p = 1;
  double h0 = 1. / 100.;

  auto Psi = [&f](double h, double y0) -> double { return y0 + h * f(y0); };
  // run adaptive algoritm
  return OdeIntSsCtrl(Psi, p, y0, T, h0, 10e-4, 10e-6, 10e-5);

}
/* SAM_LISTING_END_4 */

}  // namespace ODESolve


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

using namespace std;
using namespace Eigen;

#define PRINT_MAT(X) cout << #X << ":\n" << X << endl

void Example1(void)
{ /*
  This Function I almost copied from following link.
  https://gist.github.com/AtsushiSakai/5175898#file-geometrysample-cpp-L23
  And here is the copy right of this function.
  FileName: GeometrySample.cpp Line 23 to Line 37
  Discription: EigenのGeometry関連の関数のサンプル
  Author: Atsushi Sakai
  Update: 2013/03/16
  */

    cout<<"====1. 2次元空間における回転行列を使用した点の回転====="<<endl;
  //変換したい点を作成
  VectorXf point_in(2), point_out(2);
  point_in<<1,1;
  PRINT_MAT(point_in);

  //回転行列の作成
  Matrix2f rot;
  rot=Rotation2Df(M_PI);  //90度反時計回りに回転
  PRINT_MAT(rot);

  //回転行列をかけて回転
  point_out=rot*point_in;
  PRINT_MAT(point_out);
}

void Example2(void)
{
  cout << "====2. 2次元空間における同時変換行列の作成===== "<< endl;
  // 同時変換行列
  Affine2f homogeneous;
  // 回転行列と並進ベクトル
  Rotation2Df rot = Rotation2Df(M_PI/6.);
  Translation2f trans(0.5, 4);

  // 同時変換行列を生成
  homogeneous = trans * rot;

  cout << "homogeneous.rotation():" << endl;
  cout << homogeneous.rotation() << endl;
  cout << "homogeneous.translation():" << endl;
  cout << homogeneous.translation() << endl;
}

void Example3(void)
{
  cout << "====3. 2次元ベクトルにおける内積と外積=====" << endl;
  Vector2d vec1(2,3);
  Vector2d vec2(4,5);
  Matrix2d m;
  m << vec1, vec2;
  auto dot = vec1.dot(vec2);
  auto cross = m.determinant();

  cout << "vec1" << endl;
  cout << vec1.transpose() << endl;
  cout << "vec2" << endl;
  cout << vec2.transpose() << endl;
  cout << "dot product" << endl;
  cout << dot << endl;
  cout << "cross product" << endl;
  cout << cross << endl;
}

void Example4(void)
{
  Matrix<double,4,4> m;// = MatrixXd::Ones(4,4);

  m <<
    1,2,3,4,
    5,6,7,8,
    9,10,11,12,
    13,14,15,16;

  cout << m << endl;

  Vector2d v1 = m.block(0,3,2,1);
  Vector2d v2 = m.block(2,0,2,1);

  cout << typeid(v1).name() << endl;

  cout << v1.transpose() << endl;
  cout << v2.transpose() << endl;
  cout << (v1 + v2).transpose() << endl;
  cout << v1.lpNorm<2>() << endl;

  cout << ((v1+v2)/2.).transpose() << endl;
  cout << ((v1+v2)/2.).lpNorm<1>() << endl;
}

class cltst{
public:
  void b(const double& a)
  { b_ = a; };

  void c(const vector<double>& c)
  { c_ = c; };

  double b(void) const
  { return b_; };

  vector<double> c(void) const
  { return c_; };

  vector<double>& c(void)
  { return c_; };

private:
  double b_;
  vector<double> c_;
};

void test(void)
{
  cltst t;
  double a = 3;
  vector<double> b{4,5,6};

  t.b(a);
  t.c(b);
  cout << t.b() << endl;
  for(size_t i=0; i<t.c().size(); i++)
    cout << t.c()[i] << ", ";
  cout << endl;

  a = 4;
  b = {7,8,9};
  cout << t.b() << endl;
  for(size_t i=0; i<t.c().size(); i++)
    cout << t.c()[i] << ", ";
  cout << endl;

  vector<double>& u = t.c();
  for(size_t i=0; i<u.size(); i++)
    cout << u[i] << ", ";
  cout << endl;
  u = {7,8,9};
  for(size_t i=0; i<t.c().size(); i++)
    cout << t.c()[i] << ", ";
  cout << endl;
}

void func(const vector<double>& a)
{
  for(auto i=a.begin(); i!=a.end(); i++)
    cout << *i << ", ";
  cout << endl;
}

void test2(void)
{
  func(vector<double>{2,3,4});
}

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

  // Example1();
  // Example2();
  // Example3();
  Example4();
  // test();
  test2();

  return 0;
}


#ifndef HELP_FUNCTIONS_HPP
#define HELP_FUNCTIONS_HPP
#include <Eigen/Sparse>
#include <vector>
#include <QImage>
typedef Eigen::SparseMatrix<double> SpMat; // declares a column-major sparse matrix type of double
typedef Eigen::Triplet<double> T;

void insertCoefficient(int id, int i, int j, double w, std::vector<T> &coeffs,
                       Eigen::VectorXd &b, const Eigen::VectorXd &boundary)
{
        int n = int(boundary.size());
        int id1 = i + j * n;
        if (i == -1 || i == n)
                b(id) -= w * boundary(j); // constrained coefficient
        else if (j == -1 || j == n)
                b(id) -= w * boundary(i); // constrained coefficient
        else
                coeffs.push_back(T(id, id1, w)); // unknown coefficient
}
void buildProblem(std::vector<T> &coefficients, Eigen::VectorXd &b, int n)
{
        b.setZero();
        Eigen::ArrayXd boundary = Eigen::ArrayXd::LinSpaced(n, 0, M_PI).sin().pow(2);
        for (int j = 0; j < n; ++j)
        {
                for (int i = 0; i < n; ++i)
                {
                        int id = i + j * n;
                        insertCoefficient(id, i - 1, j, -1, coefficients, b, boundary);
                        insertCoefficient(id, i + 1, j, -1, coefficients, b, boundary);
                        insertCoefficient(id, i, j - 1, -1, coefficients, b, boundary);
                        insertCoefficient(id, i, j + 1, -1, coefficients, b, boundary);
                        insertCoefficient(id, i, j, 4, coefficients, b, boundary);
                }
        }
}
void saveAsBitmap(const Eigen::VectorXd &x, int n, const char *filename)
{
        Eigen::Array<unsigned char, Eigen::Dynamic, Eigen::Dynamic> bits = (x * 255).cast<unsigned char>();
        QImage img(bits.data(), n, n, QImage::Format_Indexed8);
        img.setColorCount(256);
        for (int i = 0; i < 256; i++)
                img.setColor(i, qRgb(i, i, i));
        img.save(filename);
}

#endif


// Kernel PCA using the Eigen library, by Tim Nugent 2014

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

using namespace Eigen;
using namespace std;

class PCA{

public:
	PCA() : components(2), kernel_type(1), normalise(0), gamma(0.001), constant(1.0), order(2.0) {}
	explicit PCA(MatrixXd& d) : components(2), kernel_type(1), normalise(0), gamma(0.001), constant(1.0), order(2.0) {X = d;}
	void load_data(const char* data, char sep = ',');
	void set_components(const int i){components = i;};
	void set_kernel(const int i){kernel_type = i;};
	void set_normalise(const int i){normalise = i;};
	void set_gamma(const double i){gamma = i;};
	void set_constant(const double i){constant = i;};
	void set_order(const double i){order = i;};
	MatrixXd& get_transformed(){return transformed;}
	void run_pca();
	void run_kpca();
	void print();
	void write_transformed(string);
	void write_eigenvectors(string);
private:
	double kernel(const VectorXd& a, const VectorXd& b);
	MatrixXd X, Xcentered, C, K, eigenvectors, transformed;
	VectorXd eigenvalues, cumulative;
	unsigned int components, kernel_type, normalise;
	double gamma, constant, order;

};

void PCA::load_data(const char* data, char sep){

	// Read data
	unsigned int row = 0;
	ifstream file(data);
	if(file.is_open()){
		string line,token;
		while(getline(file, line)){
			stringstream tmp(line);
			unsigned int col = 0;
			while(getline(tmp, token, sep)){
				if(X.rows() < row+1){
					X.conservativeResize(row+1,X.cols());
				}
				if(X.cols() < col+1){
					X.conservativeResize(X.rows(),col+1);
				}
				X(row,col) = atof(token.c_str());
				col++;
			}
			row++;
		}
		file.close();
		Xcentered.resize(X.rows(),X.cols());
	}else{
		cout << "Failed to read file " << data << endl;
	}

}

double PCA::kernel(const VectorXd& a, const VectorXd& b){

	/*
		Kernels
		1 = RBF
		2 = Polynomial
		TODO - add some of these these:
		http://crsouza.blogspot.co.uk/2010/03/kernel-functions-for-machine-learning.html

	*/
	switch(kernel_type){
	    case 2  :
	    	return(pow(a.dot(b)+constant,order));
	    default :
	    	return(exp(-gamma*((a-b).squaredNorm())));
	}

}

void PCA::run_kpca(){

	// Fill kernel matrix
	K.resize(X.rows(),X.rows());
	for(unsigned int i = 0; i < X.rows(); i++){
		for(unsigned int j = i; j < X.rows(); j++){
			K(i,j) = K(j,i) = kernel(X.row(i),X.row(j));
			//printf("k(%i,%i) = %f\n",i,j,K(i,j));
		}
	}
	//cout << endl << K << endl;

	EigenSolver<MatrixXd> edecomp(K);
	eigenvalues = edecomp.eigenvalues().real();
	eigenvectors = edecomp.eigenvectors().real();
	cumulative.resize(eigenvalues.rows());
	vector<pair<double,VectorXd> > eigen_pairs;
	double c = 0.0;
	for(unsigned int i = 0; i < eigenvectors.cols(); i++){
		if(normalise){
			double norm = eigenvectors.col(i).norm();
			eigenvectors.col(i) /= norm;
		}
		eigen_pairs.push_back(make_pair(eigenvalues(i),eigenvectors.col(i)));
	}
	// http://stackoverflow.com/questions/5122804/sorting-with-lambda
	sort(eigen_pairs.begin(),eigen_pairs.end(), [](const pair<double,VectorXd> a, const pair<double,VectorXd> b) -> bool {return (a.first > b.first);} );
	for(unsigned int i = 0; i < eigen_pairs.size(); i++){
		eigenvalues(i) = eigen_pairs[i].first;
		c += eigenvalues(i);
		cumulative(i) = c;
		eigenvectors.col(i) = eigen_pairs[i].second;
	}
	transformed.resize(X.rows(),components);

	for(unsigned int i = 0; i < X.rows(); i++){
		for(unsigned int j = 0; j < components; j++){
			for (int k = 0; k < K.rows(); k++){
                transformed(i,j) += K(i,k) * eigenvectors(k,j);
		 	}
		}
	}

	/*
	cout << "Input data:" << endl << X << endl << endl;
	cout << "Centered data:"<< endl << Xcentered << endl << endl;
	cout << "Centered kernel matrix:" << endl << Kcentered << endl << endl;
	cout << "Eigenvalues:" << endl << eigenvalues << endl << endl;
	cout << "Eigenvectors:" << endl << eigenvectors << endl << endl;
	*/
	cout << "Sorted eigenvalues:" << endl;
	for(unsigned int i = 0; i < eigenvalues.rows(); i++){
		if(eigenvalues(i) > 0){
			cout << "PC " << i+1 << ": Eigenvalue: " << eigenvalues(i);
			printf("\t(%3.3f of variance, cumulative =  %3.3f)\n",eigenvalues(i)/eigenvalues.sum(),cumulative(i)/eigenvalues.sum());
		}
	}
	cout << endl;
	//cout << "Sorted eigenvectors:" << endl << eigenvectors << endl << endl;
	//cout << "Transformed data:" << endl << transformed << endl << endl;
}

void PCA::run_pca(){

	// http://stackoverflow.com/questions/15138634/eigen-is-there-an-inbuilt-way-to-calculate-sample-covariance
	Xcentered = X.rowwise() - X.colwise().mean();
	C = (Xcentered.adjoint() * Xcentered) / double(X.rows());
	EigenSolver<MatrixXd> edecomp(C);
	eigenvalues = edecomp.eigenvalues().real();
	eigenvectors = edecomp.eigenvectors().real();
	cumulative.resize(eigenvalues.rows());
	vector<pair<double,VectorXd> > eigen_pairs;
	double c = 0.0;
	for(unsigned int i = 0; i < eigenvectors.cols(); i++){
		if(normalise){
			double norm = eigenvectors.col(i).norm();
			eigenvectors.col(i) /= norm;
		}
		eigen_pairs.push_back(make_pair(eigenvalues(i),eigenvectors.col(i)));
	}
	// http://stackoverflow.com/questions/5122804/sorting-with-lambda
	sort(eigen_pairs.begin(),eigen_pairs.end(), [](const pair<double,VectorXd> a, const pair<double,VectorXd> b) -> bool {return (a.first > b.first);} );
	for(unsigned int i = 0; i < eigen_pairs.size(); i++){
		eigenvalues(i) = eigen_pairs[i].first;
		c += eigenvalues(i);
		cumulative(i) = c;
		eigenvectors.col(i) = eigen_pairs[i].second;
	}
	transformed = Xcentered * eigenvectors;

}

void PCA::print(){

	cout << "Input data:" << endl << X << endl << endl;
	cout << "Centered data:"<< endl << Xcentered << endl << endl;
	cout << "Covariance matrix:" << endl << C << endl << endl;
	cout << "Eigenvalues:" << endl << eigenvalues << endl << endl;
	cout << "Eigenvectors:" << endl << eigenvectors << endl << endl;
	cout << "Sorted eigenvalues:" << endl;
	for(unsigned int i = 0; i < eigenvalues.rows(); i++){
		if(eigenvalues(i) > 0){
			cout << "PC " << i+1 << ": Eigenvalue: " << eigenvalues(i);
			printf("\t(%3.3f of variance, cumulative =  %3.3f)\n",eigenvalues(i)/eigenvalues.sum(),cumulative(i)/eigenvalues.sum());
		}
	}
	cout << endl;
	cout << "Sorted eigenvectors:" << endl << eigenvectors << endl << endl;
	cout << "Transformed data:" << endl << X * eigenvectors << endl << endl;
	//cout << "Transformed centred data:" << endl << transformed << endl << endl;

}

void PCA::write_transformed(string file){

	ofstream outfile(file);
	for(unsigned int i = 0; i < transformed.rows(); i++){
		for(unsigned int j = 0; j < transformed.cols(); j++){
			outfile << transformed(i,j);
			if(j != transformed.cols()-1) outfile << ",";
		}
		outfile << endl;
	}
	outfile.close();
	cout << "Written file " << file << endl;

}

void PCA::write_eigenvectors(string file){

	ofstream outfile(file);
	for(unsigned int i = 0; i < eigenvectors.rows(); i++){
		for(unsigned int j = 0; j < eigenvectors.cols(); j++){
			outfile << eigenvectors(i,j);
			if(j != eigenvectors.cols()-1) outfile << ",";
		}
		outfile << endl;
	}
	outfile.close();
	cout << "Written file " << file << endl;

}

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

	/*
	if(argc < 2){
		cout << "Usage:\n" << argv[0] << " <DATA>" << endl;
		cout << "File format:\nX1,X2, ... Xn\n";
		return(0);
	}
	*/

	PCA* P = new PCA();
	P->load_data("data/test.data");
	P->run_pca();
	cout << "Regular PCA (data/test.data):" << endl;
	P->run_pca();
	P->print();
	delete P;

	P = new PCA();
	P->load_data("data/wikipedia.data");
	cout << "Kernel PCA (data/wikipedia.data) - RBF kernel, gamma = 0.001:" << endl;
	P->run_kpca();
	P->write_eigenvectors("data/eigenvectors_RBF_data.csv");
	P->write_transformed("data/transformed_RBF_data.csv");
	cout << endl;
	delete P;

	P = new PCA();
	P->load_data("data/wikipedia.data");
	P->set_kernel(2);
	P->set_constant(1);
	P->set_order(2);
	cout << "Kernel PCA (data/wikipedia.data) - Polynomial kernel, order = 2, constant = 1:" << endl;
	P->run_kpca();
	P->write_eigenvectors("data/eigenvectors_Polynomial_data.csv");
	P->write_transformed("data/transformed_Polynomial_data.csv");
	cout << endl;
	delete P;

	return(0);

}


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

class NatCSI{
public:
	NatCSI(const std::vector<double> & t, const std::vector<double> & y):
	t_(t),
	y_(y),
	h(t.size()-1),
	c(t.size()) {
		assert ((t_.size() == y_.size()) && "size missmatch (t & y)");
		m=t.size();//n+1
		for (int i = 0; i<(m-1); i++){
			h(i)=t_[i+1]-t_[i];
			assert( ( h(i) > 0 ) && "Error: array t must be sorted!");
		}

		Eigen::SparseMatrix<double> A(m,m);
        Eigen::VectorXd b(m);

        A.reserve(3);

		A.coeffRef(0,0)=2/h(0);
		A.coeffRef(0,1)=1/h(0);
		A.coeffRef(m-1,m-1)=2/h(m-2);
		A.coeffRef(m-1,m-2)=1/h(m-2);
		double bold =(y[1]-y[0])/(h(0)*h(0));
		b(0)= 3*bold;

		for (int i=1;i<m; ++i){
			//A
			A.coeffRef(i,i-1)=1/h(i);
			A.coeffRef(i,i)=2/h(i)+2/h(i-1);
			A.coeffRef(i,i+1)=1/h(i);/// not exactly matching with the theory!
			//b
			double bnew =(y[i+1]-y[i])/(h(i)*h(i));
			b(i)= 3*(bnew+bold);
			bold=bnew;

		}
		b(m-1) =3*bold;
		A.makeCompressed();
		std::cout << A << std::endl;

		Eigen::SparseLU<Eigen::SparseMatrix<double> > lu;
		lu.compute(A);
		c = lu.solve(b);

	}

	double operator () (double x) const {
		assert( (x>=t_[0] && x<=t_[m-1]) && "Error: x is not in [t(0),t(n)]");

		auto j = ((std::lower_bound(t_.begin(), t_.end(), x)-t_.begin())-1);
		if (j==-1) j++;

		double tau = x - t_[j]/h(j);
		double tau2 = tau*tau;
		double tau3 = tau2*tau;
		return y_[j]  *  (1.-3.*tau2 + 2* tau3) +
				y_[j+1] *(3*tau2-2*tau3) +
				h(j)*c(j) *(tau-2*tau2+tau3) +
				h(j)*c(j+1)*(-tau2+tau3);
	};

private:
	std::vector<double> t_, y_;
	Eigen::VectorXd h, c;
	size_t m; //m=n+1

};

int main() {

    int n = 8, m = 100;
    std::vector<double> t;
    std::vector<double> y;
    t.resize(n);
    y.resize(n);
    for(int i = 0; i < n; ++i) {
        t[i] = (double) i / (n-1);
        y[i] = cos(t[i]);
    }

    NatCSI N(t,y);

    Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(m, 0, 1);
    for(int i = 0; i < x.size(); ++i) {
        x(i) = N(x(i));
    }
    std::cout << x << std::endl;
}

Nummerical Methods for CSE/PS8/NatCSI.cpp


#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>

#include <cmath>

#include <iomanip>
#include <iostream>

#define PI  M_PI
#define PI_HALF  M_PI_2

using vector = Eigen::VectorXd;

//! \brief Golub-Welsh implementation 5.3.35
//! \param[in] n number of Gauss nodes
//! \param[out] w weights
//! \param[out] x nodes for interval [-1,1]
void golubwelsh(int n, vector & w, vector & x) {
    w.resize(n);
    x.resize(n);
    if( n == 0 ) {
        x(0) = 0;
        w(0) = 2;
    } else {
        vector b(n-1);
        Eigen::MatrixXd J = Eigen::MatrixXd::Zero(n,n);

        for(int i = 1; i < n; ++i) {
            double d = (i) / sqrt(4. * i * i - 1.);
            J(i,i-1) = d;
            J(i-1,i) = d;
        }

        Eigen::EigenSolver<Eigen::MatrixXd> eig(J);

        x = eig.eigenvalues().real();
        w = 2 * eig.eigenvectors().real().topRows<1>().cwiseProduct(eig.eigenvectors().real().topRows<1>());
    }
}

//! \brief Compute \int_a^b f(x) dx \approx \sum w_i f(x_i) (with scaling of w and x)
//! \tparam func template type for function handle f (e.g. lambda func.)
//! \param[in] f integrand
//! \param[in] w weights
//! \param[in] x nodes for interval [-1,1]
//! \param[in] a left boundary in [a,b]
//! \param[in] b right boundary in [a,b]
//! \return Approximation of integral \int_a^b f(x) dx
template <class func>
double quad(func&& f, const vector & w, const vector & x, double a, double b) {
    double I = 0;
    for(int i = 0; i < w.size(); ++i) {
        I += f( (x(i) + 1) * (b - a) / 2 + a ) * w(i);
    }
    return I * (b - a) / 2.;
}

//! \brief Compute \int_{-infty}^\infty f(x) dx using transformation x = cot(t)
//! \tparam func template type for function handle f (e.g. lambda func.)
//! \param[in] n number of Gauss points
//! \param[in] f integrand
//! \return Approximation of integral \int_{-infty}^\infty f(x) dx
template <class func>
double quadinf(int n, func&& f) {
    vector w, x;
    golubwelsh(n, w, x);
    // NOTE: no function cot available in c++, need to resort to trigonometric identities
    //! Both below are valid, the first computes two trigonometric functions
    auto ftilde = [&f] (double x) { return f(cos(x)/sin(x)) / pow(sin(x),2); };
//     auto ftilde = [&f] (double x) { double cot = tan(PI_HALF - x); return f(cot) * (1. + pow(cot,2)); };
    return quad(ftilde, w, x, 0, PI);
}

int main() {
    // Number of max Gauss pts.
    double N = 100;

    // Integrand and exact integral
    auto f = [] (double t) { return exp(-pow((t-1),2)); };
    double I_ex = sqrt(PI);

    // NOTE: We observe exponential convergence
    int sep = 12;
    std::cout << std::setw(sep) << "Nodes" << std::setw(sep) << "Quadrature" << std::setw(sep) << "Exact" << std::setw(sep) << "Error" << std::endl;
    for(int n = 1; n < N; ++n) {
        vector w, x;
        double QS = quadinf(n, f);
//         std::cout << std::setw(sep) << n << std::setw(sep) << QS << std::setw(sep) << I_ex << std::setw(sep) << std::abs(QS - I_ex) << std::endl;
        std::cout << std::setw(sep) << " " << std::abs(QS - I_ex) << std::endl;
    }
}

Nummerical Methods for CSE/solutions/solutions_ps10/quadinf.cpp


#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    double ar = 1.0, br = 2.0, cr = 1.0;
    double ae = 2.0, be = -1.0, ce = 5.0;
    int N = 100;
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;  // Opencv随机数产生器

    // 生成数据
    vector<double> x_data, y_data;
    for (int i=0; i<N; i++)
    {
        double x = i / 100.0;
        x_data.push_back(x);
        double y = exp(ar*x*x + br*x + cr) + rng.gaussian(w_sigma*w_sigma);
        y_data.push_back(y);
    }

    // 开始Gauss-Newton迭代
    int iter;
    double cost=0, lastCost = 0;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    while(1)
    {
        Matrix3d H = Matrix3d::Zero();
        Vector3d b = Vector3d::Zero();
        cost = 0;

        for (int i=0; i<N; i++)
        {
            double x = x_data[i];
            double y = y_data[i];

            double error = y - exp(ae*x*x + be*x + ce);

            double de_da = - x*x*exp(ae*x*x + be*x +ce);
            double de_db = - x*exp(ae*x*x + be*x +ce);
            double de_dc = - exp(ae*x*x + be*x +ce);

            Vector3d J;
            J[0] = de_da;
            J[1] = de_db;
            J[2] = de_dc;

            H = H + J * J.transpose();
            b = b - J * error;

            cost = cost + error*error;
        }

        // 求解线性方程 Hx=b
        Vector3d dx= H.ldlt().solve(b);
        if(isnan(dx[0]))
        {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost)
        {
            cout << "cost >= lastCost, break!" << endl;
            break;
        }

        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        cout << "iter = " << iter << ", total cost = " << cost << ", \t\t update = " << dx.transpose()
             << ", \t\t estimated paras = " << ae << ", " << be << ", " << ce << endl;

        if (fabs(cost - lastCost) < 1e-8)
        {
            break;
        }

        lastCost = cost;
        iter += 1;
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2-t1);

    cout << endl << "Estimated paras a, b, c = " << ae << ", " << be << ", " << ce << endl;
    cout << endl << "Solve time cost = " << time_used.count() << "s" << endl;

    return 0;
}


// https://projecteuler.net/problem=56
/*
Powerful digit sum

A googol (10^100) is a massive number:
one followed by one-hundred zeros;

100^100 is almost unimaginably large:
one followed by two-hundred zeros.

Despite their size, the sum of the digits in each number is only 1.

Considering natural numbers of the form, a^b, where a, b < 100,
what is the maximum digital sum?

Solution:

*/

#include <iostream>
#include <cstdint>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>
#include <chrono>

auto digitCount(uint32_t a, uint32_t b) {
	using namespace boost;
	multiprecision::cpp_int p = a;
	p = multiprecision::pow(p, b);
	auto ans = p.str();
	uint32_t sum = 0;
	for (const auto &c : ans)
		sum += c - '0';
	return sum;
}

auto compute() {
	constexpr uint32_t limit = 100;

	uint32_t max = 0;
	for (uint32_t a = 1; a < limit; ++a)
		for (uint32_t b = 1, c; b < limit; ++b)
			if (max < (c = digitCount(a, b)))
				max = c;

	return max;
}

#ifdef _MSC_VER
	template <class T>
	inline void DoNotOptimize(const T &value) {
		__asm { lea ebx, value }
	}
#else
	template <class T>
	__attribute__((always_inline)) inline void DoNotOptimize(const T &value) {
		asm volatile("" : "+m"(const_cast<T &>(value)));
	}
#endif

int main() {
	using namespace std;
	using namespace chrono;
	auto start = high_resolution_clock::now();
	auto result = compute();
	DoNotOptimize(result);
	cout << "Done in "
		<< duration_cast<nanoseconds>(high_resolution_clock::now() - start).count() / 1e6
		<< " miliseconds." << endl;
	cout << result << endl;
}


#include <iostream>

#include <Eigen/Dense>
#include "src/rk_implementer.hpp"
#include "src/rk_solvers.hpp"

int main() {

  /**
   *  Setting global variables, the time we are integrating over
   *  and the number of integration steps we take in this time.
   *
   *  We also define the function f representing our ODE
   *  and the initial conditiond y0.
   */

  unsigned int steps = 100;
  unsigned int time = 20;

  auto f = [] (Eigen::VectorXd y) {
    Eigen::VectorXd df(2);
    df << y(0)*(3-0.7*y(1)) , -y(1)*(0.7-0.8*y(0));
    return df;
  };

  Eigen::VectorXd y0(2);
  y0 << 6,2;

  /**
   * Solve using built in solver:
   */

  std::vector<Eigen::VectorXd> y1Vec = ExplicitRKSolvers::classical4thOrderRuleIntegrator(f, time,y0, steps);

  std::cout << "The classical 4th order Runge-Kutta method gives us" << y1Vec.back().transpose() << std::endl;

  for(int i = 0; i < y1Vec.size(); i++){
    std::cout << y1Vec.at(i).transpose() << " \n";
  }

  /**
   * Solve explicitly using custom Butcher's table:
   */

  Eigen::MatrixXd A(3,3);
  A << 0, 0, 0,
       1, 0, 0,
       1.0/4,1.0/4,0;
  Eigen::VectorXd b(3);
  b << 1.0/6, 1.0/6, 2.0/3;

  ExplicitRungeKuttaIntegrator<Eigen::VectorXd> Solver(A,b);
  std::vector<Eigen::VectorXd> y2Vec = Solver.solve(f,2,y0,10);

  std::cout << "SSPRK3 3rd order Runge-Kutta method gives us = " << y2Vec[10].transpose() << std::endl;

}


#include <Eigen/Dense>
#include "simpleVecOp.hpp"
#include <stdexcept>

using namespace Eigen;

VectorXd vectorOp::weightedSum(VectorXd x, VectorXd y, VectorXd z) {
    if (x.size() != y.size() || y.size() != z.size()) {
        throw std::runtime_error("Input vectors does not have equal size!");
    }

    return a*x + b*y + c*z;
}

void vectorOp::setScalar(double a0, double b0, double c0) {
    a = a0;
    b = b0;
    c = c0;
}