//-----------Written by ZhangYu HIT-------------------
#include <iostream>
#include <cmath>
#include <vector>
#include <fstream>
#include <sstream>
#include <string>
#include <ctime>

#include <Eigen/Dense>
#include <Eigen/Sparse>

using namespace Eigen;
using namespace std;

int get_NodeCondition(const int ii, const int ij, const int N2);

inline int coordinate(const int x, const int y, const int NX)
{
    return (x + y * NX);
}

template <typename T>
inline T p2(const T x)
{
    return (x * x);
}

namespace Basic
{
    const int N = 80 + 1;
    const double MAX_ERR = 1e-6;

    // ---------------------------------------
    const bool uni_mesh = 1;

    double S_x = 1.0 / 0.98;
    double S_y = 0.98;

    //--------------------Node Spaces-----------------
    vector<double> X_delta(N - 1, 0);
    vector<double> Y_delta(N - 1, 0);

    //--------------------Node Positions--------------
    vector<double> X_position(N, 0);
    vector<double> Y_position(N, 0);

}

namespace Solve_equ
{
    using Basic::N;

    double L2_error;

    // ----------------- FD Coeffieient Matrix
    //MatrixXd A = MatrixXd::Constant(N * N, N * N, 0);

    SparseMatrix<double, RowMajor> A(N *N, N *N);

    SparseMatrix<double, RowMajor> A1(p2(N - 2), p2(N - 2));

    // -----------------[A][X] = [B]
    VectorXd B = VectorXd::Constant(N * N, 0);

    VectorXd B1 = VectorXd::Constant(p2(N - 2), 0);

    VectorXd X = VectorXd::Constant(N * N, 1);

    VectorXd X1 = VectorXd::Constant(p2(N - 2), 1);

    VectorXd phi_ana = VectorXd::Constant(N * N, 0);
}

namespace MultiGrids
{

}

void get_Bacis_Mesh(const bool uni_mesh, const int N, double S_x, double S_y,
                    vector<double> &X_delta, vector<double> &Y_delta,
                    vector<double> &X_position, vector<double> &Y_position)
{
    // using namespace Basic;

    if (uni_mesh != 1)
    {
        S_x = 1.0 / S_x;
        S_y = 1.0 / S_y;
    }
    else
    {
        S_x = S_y = 1.0;
    }

    double x0, y0;

    if (uni_mesh == 1)
    {
        x0 = 1.0 / (N - 1);
        y0 = 1.0 / (N - 1);
    }
    else
    {
        x0 = 1.0 * (1 - S_x) / (1 - pow(S_x, N - 1));
        y0 = 1.0 * (1 - S_y) / (1 - pow(S_y, N - 1));
    }

    X_delta[0] = x0;
    Y_delta[0] = y0;

    cout << "-----------N = " << N << "   deltax   deltay----------" << endl;
    for (int i = 1; i < N - 1; ++i)
    {
        X_delta[i] = X_delta[i - 1] * S_x;
        Y_delta[i] = Y_delta[i - 1] * S_y;
        cout << X_delta[i] << " " << Y_delta[i] << endl;
    }

    cout << "-----------N = " << N << "    positionx   positiony----------" << endl;
    for (int i = 1; i < N; ++i)
    {
        X_position[i] = X_position[i - 1] + X_delta[i - 1];
        Y_position[i] = Y_position[i - 1] + Y_delta[i - 1];
        cout << X_position[i] << " " << Y_position[i] << endl;
    }
}

inline double anaSolu(const double X_, const double Y_)
{
    //double ana = (5000000*p2(Y_) + 5000000*p2(X_ - 1) - 100000)*exp(-50*p2(Y_) - 50*p2(1 - X_));
    double ana = 500 * exp(-50 * (p2(1.0 - X_) + p2(Y_))) + 100 * X_ * (1 - Y_);
    return ana;
}

void get_Analytic_Solution()
{

    using Basic::N;
    using Basic::X_position;
    using Basic::Y_position;

    using Solve_equ::phi_ana;

    for (int j = 0; j < N; ++j)
    {
        for (int i = 0; i < N; ++i)
        {
            const int index = coordinate(i, j, N);
            const double X_ = X_position[i];
            const double Y_ = Y_position[j];
            //phi_ana(index) = 500 * exp(-50 * (p2(1.0 - X_) + p2(Y_))) + 100 * X_ * (1 - Y_);
            phi_ana(index) = anaSolu(X_, Y_);
        }
    }
}

int get_NodeCondition(const int ii, const int ij, const int N2)
{

    int NodeCondition;
    // ------------6 2 5
    // ------------3 0 1
    // ------------7 4 8
    if (ii != 0 && ij != 0 && ii != N2 - 1 && ij != N2 - 1)
    {
        NodeCondition = 0;
    }
    else if (ij == 0 && ii == 0)
    {
        //NodeCondition = "LeftBottom";
        NodeCondition = 7;
    }
    else if (ij == 0 && ii != 0 && ii != N2 - 1)
    {
        //NodeCondition = "Bottom";
        NodeCondition = 4;
    }
    else if (ij == 0 && ii == N2 - 1)
    {
        //NodeCondition = "RightBottom";
        NodeCondition = 8;
    }
    else if (ii == 0 && ij != 0 && ij != N2 - 1)
    {
        //NodeCondition = "Left";
        NodeCondition = 3;
    }
    else if (ii == N2 - 1 && ij != 0 && ij != N2 - 1)
    {
        //NodeCondition = "Right";
        NodeCondition = 1;
    }
    else if (ii == N2 - 1 && ij == N2 - 1)
    {
        //NodeCondition = "RightTop";
        NodeCondition = 5;
    }
    else if (ij == N2 - 1 && ii != 0 && ii != N2 - 1)
    {
        //NodeCondition = "Top";
        NodeCondition = 2;
    }
    else if (ii == 0 && ij == N2 - 1)
    {
        //NodeCondition = "LeftTop";
        NodeCondition = 6;
    }
    return NodeCondition;
}

inline double SRight(const double X_, const double Y_)
{
    double S = (5000000 * p2(Y_) + 5000000 * p2(X_ - 1) - 100000) * exp(-50 * p2(Y_) - 50 * p2(1 - X_));
    return S;
}

void get_Right_term_resi(const int N, const double *X_delta, const double *Y_delta,
                         VectorXd &r, VectorXd &B)
{
    const int N2 = N - 2;

    for (int i = 0; i < p2(N2); ++i)
    {

        int ii = i % ((int)N2);
        int ij = i / ((int)N2);

        int NodeCondition = get_NodeCondition(ii, ij, N2);

        double dx1 = X_delta[1 + ii - 1];
        double dx2 = X_delta[1 + ii];
        double dy1 = Y_delta[1 + ij - 1];
        double dy2 = Y_delta[1 + ij];

        double B_N, B_S, B_W, B_E;

        switch (NodeCondition)
        {
        case 1:
            B_E = 0;
            B(i) = r(i) - 2.0 * B_E / (dx2 * (dx1 + dx2)); //---E
            break;
        case 2:
            B_N = 0;
            B(i) = r(i) - 2.0 * B_N / (dy2 * (dy1 + dy2)); //---N
            break;
        case 3:
            B_W = 0;
            B(i) = r(i) - 2.0 * B_W / (dx1 * (dx1 + dx2)); //---W
            break;
        case 4:
            B_S = 0;
            B(i) = r(i) - 2.0 * B_S / (dy1 * (dy1 + dy2)); //---S
            break;
        case 5:
            B_N = 0;
            B_E = 0;
            B(i) = r(i) - (2.0 * B_E / (dx2 * (dx1 + dx2)) + 2.0 * B_N / (dy2 * (dy1 + dy2))); //---NE
            break;
        case 6:
            B_N = 0;
            B_W = 0;
            B(i) = r(i) - (2.0 * B_W / (dx1 * (dx1 + dx2)) + 2.0 * B_N / (dy2 * (dy1 + dy2))); //---NW
            break;
        case 7:
            B_S = 0;
            B_W = 0;
            B(i) = r(i) - (2.0 * B_W / (dx1 * (dx1 + dx2)) + 2.0 * B_S / (dy1 * (dy1 + dy2))); //---SW
            break;
        case 8:
            B_S = 0;
            B_E = 0;
            B(i) = r(i) - (2.0 * B_E / (dx2 * (dx1 + dx2)) + 2.0 * B_S / (dy1 * (dy1 + dy2))); //---SE
            break;
        default:
            B(i) = r(i);
            break;
        }
    }
}

void get_Right_term_interior(const int N, const double *X_delta, const double *Y_delta,
                             vector<double> &X_position, vector<double> &Y_position,
                             VectorXd &B1)
{
    const int N2 = N - 2;

    for (int i = 0; i < p2(N2); ++i)
    {

        int ii = i % ((int)N2);
        int ij = i / ((int)N2);

        int NodeCondition = get_NodeCondition(ii, ij, N2);

        double dx1 = X_delta[1 + ii - 1];
        double dx2 = X_delta[1 + ii];
        double dy1 = Y_delta[1 + ij - 1];
        double dy2 = Y_delta[1 + ij];

        double *X_ = &X_position[0] + ii + 1;
        double *Y_ = &Y_position[0] + ij + 1;

        double B_N, B_S, B_W, B_E;

        B1(i) = SRight(*X_, *Y_);
        switch (NodeCondition)
        {
        case 1:
            B_E = anaSolu(*(X_ + 1), *Y_);
            B1(i) -= 2.0 * B_E / (dx2 * (dx1 + dx2)); //---E
            break;
        case 2:
            B_N = anaSolu(*X_, *(Y_ + 1));
            B1(i) -= 2.0 * B_N / (dy2 * (dy1 + dy2)); //---N
            break;
        case 3:
            B_W = anaSolu(*(X_ - 1), *Y_);
            B1(i) -= 2.0 * B_W / (dx1 * (dx1 + dx2)); //---W
            break;
        case 4:
            B_S = anaSolu(*X_, *(Y_ - 1));
            B1(i) -= 2.0 * B_S / (dy1 * (dy1 + dy2)); //---S
            break;
        case 5:
            B_N = anaSolu(*X_, *(Y_ + 1));
            B_E = anaSolu(*(X_ + 1), *Y_);
            B1(i) -= (2.0 * B_E / (dx2 * (dx1 + dx2)) + 2.0 * B_N / (dy2 * (dy1 + dy2))); //---NE
            break;
        case 6:
            B_N = anaSolu(*X_, *(Y_ + 1));
            B_W = anaSolu(*(X_ - 1), *Y_);
            B1(i) -= (2.0 * B_W / (dx1 * (dx1 + dx2)) + 2.0 * B_N / (dy2 * (dy1 + dy2))); //---NW
            break;
        case 7:
            B_S = anaSolu(*X_, *(Y_ - 1));
            B_W = anaSolu(*(X_ - 1), *Y_);
            B1(i) -= (2.0 * B_W / (dx1 * (dx1 + dx2)) + 2.0 * B_S / (dy1 * (dy1 + dy2))); //---SW
            break;
        case 8:
            B_S = anaSolu(*X_, *(Y_ - 1));
            B_E = anaSolu(*(X_ + 1), *Y_);
            B1(i) -= (2.0 * B_E / (dx2 * (dx1 + dx2)) + 2.0 * B_S / (dy1 * (dy1 + dy2))); //---SE
            break;
        default:
            //B1(i) = SRight(*X_, *Y_);
            break;
        }
    }
    //cout << B1 << endl;
}

void get_iter_coeff(VectorXd &A_c, const double *X_delta, const double *Y_delta, const int N)
{
    int N2 = N - 2;
    int N22 = p2(N2);

    for (int i = 0; i < N22; ++i)
    {

        int ii = i % ((int)N2);
        int ij = i / ((int)N2);

        int im5 = i * 5;

        double dx1 = X_delta[1 + ii - 1];
        double dx2 = X_delta[1 + ii];
        double dy1 = Y_delta[1 + ij - 1];
        double dy2 = Y_delta[1 + ij];

        int NodeCondition = get_NodeCondition(ii, ij, N2);
        A_c(im5) = -(2.0 / (dx1 * dx2) + 2.0 / (dy1 * dy2));

        switch (NodeCondition)
        {
            // center - left - bottom - right - top
        case 0:
            A_c(im5 + 1) = 2.0 / (dx1 * (dx1 + dx2)); //---W
            A_c(im5 + 2) = 2.0 / (dy1 * (dy1 + dy2)); //---S
            A_c(im5 + 3) = 2.0 / (dx2 * (dx1 + dx2)); //---E
            A_c(im5 + 4) = 2.0 / (dy2 * (dy1 + dy2)); //---N
            break;
        case 1:
            A_c(im5 + 1) = 2.0 / (dx1 * (dx1 + dx2)); //---W
            A_c(im5 + 2) = 2.0 / (dy1 * (dy1 + dy2)); //---S
            A_c(im5 + 4) = 2.0 / (dy2 * (dy1 + dy2)); //---N
            break;
        case 2:
            A_c(im5 + 1) = 2.0 / (dx1 * (dx1 + dx2)); //---W
            A_c(im5 + 2) = 2.0 / (dy1 * (dy1 + dy2)); //---S
            A_c(im5 + 3) = 2.0 / (dx2 * (dx1 + dx2)); //---E
            break;
        case 3:
            A_c(im5 + 2) = 2.0 / (dy1 * (dy1 + dy2)); //---S
            A_c(im5 + 3) = 2.0 / (dx2 * (dx1 + dx2)); //---E
            A_c(im5 + 4) = 2.0 / (dy2 * (dy1 + dy2)); //---N
            break;
        case 4:
            A_c(im5 + 1) = 2.0 / (dx1 * (dx1 + dx2)); //---W
            A_c(im5 + 3) = 2.0 / (dx2 * (dx1 + dx2)); //---E
            A_c(im5 + 4) = 2.0 / (dy2 * (dy1 + dy2)); //---N
            break;
        case 5:
            A_c(im5 + 1) = 2.0 / (dx1 * (dx1 + dx2)); //---W
            A_c(im5 + 2) = 2.0 / (dy1 * (dy1 + dy2)); //---S
            break;
        case 6:
            A_c(im5 + 2) = 2.0 / (dy1 * (dy1 + dy2)); //---S
            A_c(im5 + 3) = 2.0 / (dx2 * (dx1 + dx2)); //---E
            break;
        case 7:
            A_c(im5 + 3) = 2.0 / (dx2 * (dx1 + dx2)); //---E
            A_c(im5 + 4) = 2.0 / (dy2 * (dy1 + dy2)); //---N
            break;
        case 8:
            A_c(im5 + 1) = 2.0 / (dx1 * (dx1 + dx2)); //---W
            A_c(im5 + 4) = 2.0 / (dy2 * (dy1 + dy2)); //---N
            break;

        default:
            break;
        }
    }
}

void Gauss_Seidel(VectorXd &X, VectorXd &Ac, VectorXd &B, const int N)
{
    for (int j = 0; j < N; ++j)
    {
        for (int i = 0; i < N; ++i)
        {
            const int index = coordinate(i, j, N);

            const int i_L = coordinate(i - 1, j, N);
            const int i_B = coordinate(i, j - 1, N);
            const int i_R = coordinate(i + 1, j, N);
            const int i_T = coordinate(i, j + 1, N);

            const int im5 = 5 * index;

            int NodeCondition = get_NodeCondition(i, j, N);

            switch (NodeCondition)
            {
            case 0:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 1) * X(i_L) + Ac(im5 + 2) * X(i_B) + Ac(im5 + 3) * X(i_R) + Ac(im5 + 4) * X(i_T)));
                break;
            case 1:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 1) * X(i_L) + Ac(im5 + 2) * X(i_B) + Ac(im5 + 4) * X(i_T)));
                break;
            case 2:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 1) * X(i_L) + Ac(im5 + 2) * X(i_B) + Ac(im5 + 3) * X(i_R)));
                break;
            case 3:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 2) * X(i_B) + Ac(im5 + 3) * X(i_R) + Ac(im5 + 4) * X(i_T)));
                break;
            case 4:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 1) * X(i_L) + Ac(im5 + 3) * X(i_R) + Ac(im5 + 4) * X(i_T)));
                break;
            case 5:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 1) * X(i_L) + Ac(im5 + 2) * X(i_B)));
                break;
            case 6:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 2) * X(i_B) + Ac(im5 + 3) * X(i_R)));
                break;
            case 7:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 3) * X(i_R) + Ac(im5 + 4) * X(i_T)));
                break;
            case 8:
                X(index) = 1.0 / Ac(im5) * (B(index) - (Ac(im5 + 1) * X(i_L) + Ac(im5 + 4) * X(i_T)));
                break;

            default:
                break;
            }
        }
    }
}

void from_X1_to_X(VectorXd &X, VectorXd &X1, int N)
{
    using Solve_equ::phi_ana;

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

            const int index = coordinate(i, j, N);

            int index_in;

            int NodeCondition = get_NodeCondition(i, j, N);

            switch (NodeCondition)
            {
            case 0:
                index_in = coordinate(i - 1, j - 1, N - 2);
                X(index) = X1(index_in);
                break;
            //case 1:
            //    X(index) = phi_ana(index);
            //    break;
            //case 2:
            //    X(index) = phi_ana(index);
            //    break;
            //case 3:
            //    X(index) = phi_ana(index);
            //    break;
            //case 4:
            //    X(index) = phi_ana(index);
            //    break;
            //case 5:
            //    X(index) = phi_ana(index);
            //    break;
            //case 6:
            //    X(index) = phi_ana(index);
            //    break;
            //case 7:
            //    X(index) = phi_ana(index);
            //    break;
            //case 8:
            //    X(index) = phi_ana(index);
            //    break;
            default:
                X(index) = phi_ana(index);
                break;
            }
        }
    }
}

void Gauss_Seidel_Iteration(VectorXd &X, VectorXd &X1, VectorXd &B,
                            vector<double> &X_delta, vector<double> &Y_delta,
                            vector<double> &X_position, vector<double> &Y_position, const int N)
{
    // ********** The Gauss Seidel Iteration don't include Boundary nodes
    const int N2 = N - 2;
    // FD coefficient----------------
    VectorXd A_coeff = VectorXd::Constant(p2(N2) * 5, 0);

    get_iter_coeff(A_coeff, &X_delta[0], &Y_delta[0], N);

    get_Right_term_interior(N, &X_delta[0], &Y_delta[0], X_position, Y_position, B);

    //Gauss_Seidel(X1, A_coeff, B, N);

    double err_sum = 1.0;
    int iter_num = 0;

    while (err_sum > 1e-8)
    {
        VectorXd X_old = X1;
        Gauss_Seidel(X1, A_coeff, B, N2);

        VectorXd error = X1 - X_old;
        err_sum = error.lpNorm<2>();

        ++iter_num;
    }

    cout << "Iter steps is " << iter_num << ", "
         << "iter max err is " << err_sum << endl;

    from_X1_to_X(X, X1, N);
}

void residual(VectorXd &rh, const int N,
              const MatrixXd &Ah_coeff, const VectorXd &Xh, const VectorXd &Bh)
{
    for (int j = 0; j < N; ++j)
    {
        for (int i = 0; i < N; ++i)
        {
            const int index = coordinate(i, j, N);

            const int i_L = coordinate(i - 1, j, N);
            const int i_B = coordinate(i, j - 1, N);
            const int i_R = coordinate(i + 1, j, N);
            const int i_T = coordinate(i, j + 1, N);

            const int im5 = 5 * index;

            int NodeCondition = get_NodeCondition(i, j, N);

            switch (NodeCondition)
            {
            case 0:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 1) * Xh(i_L) + Ah_coeff(im5 + 2) * Xh(i_B) + Ah_coeff(im5 + 3) * Xh(i_R) + Ah_coeff(im5 + 4) * Xh(i_T));
                break;
            case 1:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 1) * Xh(i_L) + Ah_coeff(im5 + 2) * Xh(i_B) + Ah_coeff(im5 + 4) * Xh(i_T));
                break;
            case 2:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 1) * Xh(i_L) + Ah_coeff(im5 + 2) * Xh(i_B) + Ah_coeff(im5 + 3) * Xh(i_R));
                break;
            case 3:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 2) * Xh(i_B) + Ah_coeff(im5 + 3) * Xh(i_R) + Ah_coeff(im5 + 4) * Xh(i_T));
                break;
            case 4:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 1) * Xh(i_L) + Ah_coeff(im5 + 3) * Xh(i_R) + Ah_coeff(im5 + 4) * Xh(i_T));
                break;
            case 5:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 1) * Xh(i_L) + Ah_coeff(im5 + 2) * Xh(i_B));
                break;
            case 6:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 2) * Xh(i_B) + Ah_coeff(im5 + 3) * Xh(i_R));
                break;
            case 7:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 3) * Xh(i_R) + Ah_coeff(im5 + 4) * Xh(i_T));
                break;
            case 8:
                rh(index) = Bh(index) - (Ah_coeff(im5) * Xh(index) + Ah_coeff(im5 + 1) * Xh(i_L) + Ah_coeff(im5 + 4) * Xh(i_T));
                break;

            default:
                break;
            }
        }
    }
}

void Restriction(VectorXd &r1, const VectorXd &r0, const int N1, const int N0)
{

    // ------------------- simply injection
    for (int i = 0; i < N1; ++i)
    {
        for (int j = 0; j < N1; ++j)
        {
            const int index1 = coordinate(i, j, N1);
            const int index0 = coordinate(i * 2 + 1, j * 2 + 1, N0);
            r1(index1) = r0(index0);
        }
    }
}

void Prolongation(const VectorXd &eh1, VectorXd &rh0, const int N1, const int N0,
                  vector<double> &X_weight, vector<double> &Y_weight)
{
    // Bilinear interpolation -----------------------
    for (int i = 0; i < N1 - 1; ++i)
    {
        for (int j = 0; j < N1 - 1; ++j)
        {
            const int ip1 = i + 1;
            const int jp1 = j + 1;
            const int im2p1 = i * 2 + 1;
            const int im2p2 = i * 2 + 2;
            const int jm2p1 = j * 2 + 1;
            const int jm2p2 = j * 2 + 2;

            const int index1 = coordinate(i, j, N1);
            const int index1_i = coordinate(ip1, j, N1);
            const int index1_j = coordinate(i, jp1, N1);
            const int index1_ij = coordinate(ip1, jp1, N1);

            const int index0 = coordinate(im2p1, jm2p1, N0);
            const int index0_i = coordinate(im2p2, jm2p1, N0);
            const int index0_j = coordinate(im2p1, jm2p2, N0);
            const int index0_ij = coordinate(im2p2, jm2p2, N0);

            rh0(index0) = eh1(index1);
            rh0(index0_i) = X_weight[im2p2] * eh1(index1) + X_weight[im2p2 + 1] * eh1(index1_i);
            rh0(index0_j) = Y_weight[jm2p2] * eh1(index1) + Y_weight[jm2p2 + 1] * eh1(index1_j);
            rh0(index0_ij) = Y_weight[jm2p2] * (X_weight[im2p2] * eh1(index1) + X_weight[im2p2 + 1] * eh1(index1_i)) + Y_weight[jm2p2 + 1] * (X_weight[im2p2] * eh1(index1_j) + X_weight[im2p2 + 1] * eh1(index1_ij));
        }
    }

    const int N1_1 = N1 - 1;

    for (int i = 0; i < N1_1; ++i)
    {
        const int ip1 = i + 1;
        const int im2p1 = i * 2 + 1;
        const int im2p2 = i * 2 + 2;

        const int index1 = coordinate(i, N1_1, N1);
        const int index1_1 = coordinate(ip1, N1_1, N1);

        const int index0 = coordinate(im2p1, N1_1 * 2 + 1, N0);
        const int index0_1 = coordinate(im2p2, N1_1 * 2 + 1, N0);

        rh0(index0) = eh1(index1);
        rh0(index0_1) = X_weight[im2p2] * eh1(index1) + X_weight[im2p2 + 1] * eh1(index1_1);

        const int index0_pj = coordinate(im2p1, N1_1 * 2 + 2, N0);
        const int index0_1pj = coordinate(im2p2, N1_1 * 2 + 2, N0);
        rh0(index0_pj) = Y_weight[N1_1 * 2 + 2] * rh0(index0) + Y_weight[N1_1 * 2 + 3] * 0;
        rh0(index0_1pj) = Y_weight[N1_1 * 2 + 2] * rh0(index0_1) + Y_weight[N1_1 * 2 + 3] * 0;

        const int index0_0mj = coordinate(im2p1, 1, N0);
        const int index0_01mj = coordinate(im2p2, 1, N0);
        rh0(coordinate(im2p1, 0, N0)) = Y_weight[1] * rh0(coordinate(im2p1, 1, N0)) + Y_weight[0] * 0;
        rh0(coordinate(im2p2, 0, N0)) = Y_weight[1] * rh0(coordinate(im2p2, 1, N0)) + Y_weight[0] * 0;
    }

    for (int j = 0; j < N1_1; ++j)
    {
        const int jp1 = j + 1;
        const int jm2p1 = j * 2 + 1;
        const int jm2p2 = j * 2 + 2;

        const int index1 = coordinate(N1_1, j, N1);
        const int index1_1 = coordinate(N1_1, jp1, N1);

        const int index0 = coordinate(N1_1 * 2 + 1, jm2p1, N0);
        const int index0_1 = coordinate(N1_1 * 2 + 1, jm2p2, N0);

        rh0(index0) = eh1(index1);
        rh0(index0_1) = Y_weight[jm2p2] * eh1(index1) + Y_weight[jm2p2 + 1] * eh1(index1_1);

        const int index0_pi = coordinate(N1_1 * 2 + 2, jm2p1, N0);
        const int index0_1pi = coordinate(N1_1 * 2 + 2, jm2p2, N0);
        rh0(index0_pi) = X_weight[N1_1 * 2 + 2] * rh0(index0) + X_weight[N1_1 * 2 + 3] * 0;
        rh0(index0_1pi) = X_weight[N1_1 * 2 + 2] * rh0(index0_1) + X_weight[N1_1 * 2 + 3] * 0;

        const int index0_0mi = coordinate(1, jm2p1, N0);
        const int index0_01mi = coordinate(1, jm2p2, N0);
        rh0(coordinate(0, jm2p1, N0)) = X_weight[1] * rh0(coordinate(1, jm2p1, N0)) + X_weight[0] * 0;
        rh0(coordinate(0, jm2p2, N0)) = X_weight[1] * rh0(coordinate(1, jm2p2, N0)) + X_weight[0] * 0;
    }

    rh0(coordinate(2 * N1_1 + 1, 2 * N1_1 + 1, N0)) = eh1(coordinate(N1_1, N1_1, N1));

    rh0(coordinate(2 * N1_1 + 1, 0, N0)) = Y_weight[1] * rh0(coordinate(2 * N1_1 + 1, 1, N0)) + Y_weight[0] * 0;
    rh0(coordinate(2 * N1_1 + 1, 2 * N1_1 + 2, N0)) = Y_weight[N1_1 * 2 + 2] * rh0(coordinate(2 * N1_1 + 1, 2 * N1_1 + 1, N0)) + Y_weight[N1_1 * 2 + 3] * 0;

    rh0(coordinate(0, 2 * N1_1 + 1, N0)) = X_weight[1] * rh0(coordinate(1, 2 * N1_1 + 1, N0)) + Y_weight[0] * 0;
    rh0(coordinate(2 * N1_1 + 2, 2 * N1_1 + 1, N0)) = X_weight[N1_1 * 2 + 2] * rh0(coordinate(2 * N1_1 + 1, 2 * N1_1 + 1, N0)) + Y_weight[N1_1 * 2 + 3] * 0;

    rh0(coordinate(0, 0, N0)) = X_weight[1] * rh0(coordinate(1, 0, N0));
    rh0(coordinate(0, N0 - 1, N0)) = X_weight[1] * rh0(coordinate(1, N0 - 1, N0));
    rh0(coordinate(N0 - 1, 0, N0)) = X_weight[N0 - 1] * rh0(coordinate(N0 - 2, 0, N0));
    rh0(coordinate(N0 - 1, N0 - 1, N0)) = X_weight[N0 - 1] * rh0(coordinate(N0 - 2, N0 - 1, N0));
}

void get_Weight_Interpolation(vector<double> &weight, const int N_c,
                              const vector<double> &X_f, const vector<double> &X_c)
{
    for (int i = 0; i < N_c; ++i)
    {
        const int i2 = 2 * i;
        //const double inv_delta = 1.0 / X_c[i];
        const double inv_delta = 1.0 / (X_f[i2 + 1] + X_f[i2]);

        weight[i2] = X_f[i2 + 1] * inv_delta;
        weight[i2 + 1] = X_f[i2 + 0] * inv_delta;
    }
}
void get_resi_mesh(const int N1, vector<double> &X1_delta, vector<double> &Y1_delta,
                   vector<double> &X1_position, vector<double> &Y1_position,
                   vector<double> &X0_delta, vector<double> &Y0_delta,
                   vector<double> &X0_position, vector<double> &Y0_position)
{
    for(int i = 0; i < N1 - 1; ++i){
        X1_delta[i] = X0_delta[2 * i] + X0_delta[2 * i + 1];
        Y1_delta[i] = Y0_delta[2 * i] + Y0_delta[2 * i + 1];
    }

    X1_position[0] = X0_position[0];
    Y1_position[0] = Y0_position[0];
    for (int i = 1; i < N1; ++i)
    {
        X1_position[i] = X1_position[i - 1] + X1_delta[i - 1];
        Y1_position[i] = Y1_position[i - 1] + Y1_delta[i - 1];
        // cout << X_position[i] << " " << Y_position[i] << endl;
    }
}

void MultiGrid_Iter(const int N, VectorXd &X, VectorXd &Xh0, VectorXd &Bh0,
                    const bool unimesh, const double S_x, const double S_y,
                    vector<double> &Xh0_delta, vector<double> &Yh0_delta,
                    vector<double> &Xh0_position, vector<double> &Yh0_position)
{
    const int N0 = N - 2;
    const int N1 = (N0 - 1) / 2;
    const int N2 = (N1 - 1) / 2;
    const int N3 = (N2 - 1) / 2;

    //--------------------Node Spaces-----------------
    vector<double> Xh1_delta(N1 + 2 - 1, 0);
    vector<double> Yh1_delta(N1 + 2 - 1, 0);
    vector<double> Xh2_delta(N2 + 2 - 1, 0);
    vector<double> Yh2_delta(N2 + 2 - 1, 0);
    vector<double> Xh3_delta(N3 + 2 - 1, 0);
    vector<double> Yh3_delta(N3 + 2 - 1, 0);

    vector<double> X01_weight(N0 + 2 - 1, 0);
    vector<double> Y01_weight(N0 + 2 - 1, 0);
    vector<double> X12_weight(N1 + 2 - 1, 0);
    vector<double> Y12_weight(N1 + 2 - 1, 0);
    vector<double> X23_weight(N2 + 2 - 1, 0);
    vector<double> Y23_weight(N2 + 2 - 1, 0);

    //--------------------Node Positions--------------
    vector<double> Xh1_position(N1 + 2, 0);
    vector<double> Yh1_position(N1 + 2, 0);
    vector<double> Xh2_position(N2 + 2, 0);
    vector<double> Yh2_position(N2 + 2, 0);
    vector<double> Xh3_position(N3 + 2, 0);
    vector<double> Yh3_position(N3 + 2, 0);

    get_resi_mesh(N1 + 2, Xh1_delta, Yh1_delta, Xh1_position, Yh1_position, Xh0_delta, Yh0_delta, Xh0_position, Yh0_position);
    get_resi_mesh(N2 + 2, Xh2_delta, Yh2_delta, Xh2_position, Yh2_position, Xh1_delta, Yh1_delta, Xh1_position, Yh1_position);
    get_resi_mesh(N3 + 2, Xh3_delta, Yh3_delta, Xh3_position, Yh3_position, Xh2_delta, Yh2_delta, Xh2_position, Yh2_position);
    // get_Bacis_Mesh(unimesh, N1 + 2, S_x, S_y, Xh1_delta, Yh1_delta, Xh1_position, Yh1_position);
    // get_Bacis_Mesh(unimesh, N2 + 2, S_x, S_y, Xh2_delta, Yh2_delta, Xh2_position, Yh2_position);
    // get_Bacis_Mesh(unimesh, N3 + 2, S_x, S_y, Xh3_delta, Yh3_delta, Xh3_position, Yh3_position);

    get_Weight_Interpolation(X01_weight, N1 + 1, Xh0_delta, Xh1_delta);
    get_Weight_Interpolation(Y01_weight, N1 + 1, Yh0_delta, Yh1_delta);
    get_Weight_Interpolation(X12_weight, N2 + 1, Xh1_delta, Xh2_delta);
    get_Weight_Interpolation(Y12_weight, N2 + 1, Yh1_delta, Yh2_delta);
    get_Weight_Interpolation(X23_weight, N3 + 1, Xh2_delta, Xh3_delta);
    get_Weight_Interpolation(Y23_weight, N3 + 1, Yh2_delta, Yh3_delta);

    VectorXd rh0 = VectorXd::Constant(p2(N0), 0);
    VectorXd eh0 = VectorXd::Constant(p2(N0), 0);
    VectorXd rh1 = VectorXd::Constant(p2(N1), 0);
    VectorXd eh1 = VectorXd::Constant(p2(N1), 0);
    VectorXd rh2 = VectorXd::Constant(p2(N2), 0);
    VectorXd eh2 = VectorXd::Constant(p2(N2), 0);
    VectorXd rh3 = VectorXd::Constant(p2(N3), 0);
    VectorXd eh3 = VectorXd::Constant(p2(N3), 0);

    VectorXd Bh1 = VectorXd::Constant(p2(N1), 1);
    VectorXd Bh2 = VectorXd::Constant(p2(N2), 1);
    VectorXd Bh3 = VectorXd::Constant(p2(N3), 1);

    VectorXd Ah0_coeff = VectorXd::Constant(p2(N0) * 5, 0);
    VectorXd Ah1_coeff = VectorXd::Constant(p2(N1) * 5, 0);
    VectorXd Ah2_coeff = VectorXd::Constant(p2(N2) * 5, 0);
    VectorXd Ah3_coeff = VectorXd::Constant(p2(N3) * 5, 0);

    get_iter_coeff(Ah0_coeff, &Xh0_delta[0], &Yh0_delta[0], N0 + 2);
    get_Right_term_interior(N0 + 2, &Xh0_delta[0], &Yh0_delta[0], Xh0_position, Yh0_position, Bh0);

    get_iter_coeff(Ah1_coeff, &Xh1_delta[0], &Yh1_delta[0], N1 + 2);
    get_iter_coeff(Ah2_coeff, &Xh2_delta[0], &Yh2_delta[0], N2 + 2);
    get_iter_coeff(Ah3_coeff, &Xh3_delta[0], &Yh3_delta[0], N3 + 2);

    double err_sum = 1.0;
    int iter_num = 0;
    VectorXd X_old = Xh0;
    VectorXd error = Xh0;

    std::ofstream err_out("err.txt");
    for (int step = 0; step < 20000; ++step)
    //for (int step = 0; step < 1; ++step)
    {
        X_old = Xh0;
        eh1.setZero();
        eh2.setZero();
        eh3.setZero();

        for (int steph0 = 0; steph0 < 5; ++steph0)
        {
            Gauss_Seidel(Xh0, Ah0_coeff, Bh0, N0);
            error = Xh0 - X_old;
            err_sum = error.lpNorm<2>();
        }

        if (err_sum < 1e-6)
        {
            cout << "Iterations of MG = " << step << " x 5" << endl;
            break;
        }

        err_out << step << " " << err_sum << endl;

        residual(rh0, N0, Ah0_coeff, Xh0, Bh0);

        Restriction(rh1, rh0, N1, N0);
        get_Right_term_resi(N1 + 2, &Xh1_delta[0], &Yh1_delta[0], rh1, Bh1);

        for (int steph0 = 0; steph0 < 10; ++steph0)
        {
            Gauss_Seidel(eh1, Ah1_coeff, rh1, N1);
        }

        residual(rh1, N1, Ah1_coeff, eh1, Bh1);
        Restriction(rh2, rh1, N2, N1);
        get_Right_term_resi(N2 + 2, &Xh2_delta[0], &Yh2_delta[0], rh2, Bh2);
        for (int steph0 = 0; steph0 < 5; ++steph0)
        {
            Gauss_Seidel(eh2, Ah2_coeff, Bh2, N2);
        }

        residual(rh2, N2, Ah2_coeff, eh2, Bh2);
        Restriction(rh3, rh2, N3, N2);
        get_Right_term_resi(N3 + 2, &Xh3_delta[0], &Yh3_delta[0], rh3, Bh3);
        for (int steph0 = 0; steph0 < 20; ++steph0)
        {
            Gauss_Seidel(eh3, Ah3_coeff, Bh3, N3);
        }
        //   cout << eh3 << endl;

        //   -------------------------------------------------------
        Prolongation(eh3, rh2, N3, N2, X23_weight, Y23_weight);
        eh2 += rh2;
        for (int steph0 = 0; steph0 < 5; ++steph0)
        {
            Gauss_Seidel(eh2, Ah2_coeff, Bh2, N2);
        }

        Prolongation(eh2, rh1, N2, N1, X12_weight, Y12_weight);
        eh1 += rh1;
        for (int steph0 = 0; steph0 < 5; ++steph0)
        {
            Gauss_Seidel(eh1, Ah1_coeff, Bh1, N1);
        }

        Prolongation(eh1, rh0, N1, N0, X01_weight, Y01_weight);
        Xh0 += rh0;
    }

    err_out.close();

    rh0.setZero();
    VectorXd ph1 = VectorXd::Constant(p2(N1), 500);
    Prolongation(ph1, rh0, N1, N0, X01_weight, Y01_weight);

    cout << "------------------------------------------------------------------ll" << endl;
    from_X1_to_X(X, Xh0, N0 + 2);
}

void out_tec()
{

    using Basic::N;

    using Basic::X_position;
    using Basic::Y_position;

    using Solve_equ::phi_ana;
    using Solve_equ::X;

    VectorXd error_num2ana = VectorXd::Constant(p2(N), 0);
    VectorXd error_num2ana_L2 = VectorXd::Constant(p2(N), 0);

    std::ostringstream name;
    //name << "Phi_" << N << "_.dat";
    if (Basic::uni_mesh == 1)
    {
        name << "UniMesh_Phi_" << N << "_.dat";
    }
    else
    {
        name << "Un_uniMesh_Phi_" << N << "_.dat";
    }
    std::ofstream out(name.str().c_str());
    out << "Title= \"Poisson_\"\n"
        << "VARIABLES = \"X\", \"Y\", \"phi_num\", \"phi_ana\", \"error\", \"rela_error\" \n";
    out << "ZONE T= \"BOX\",I=" << N - 2 << ",J=" << N - 2 << ", F = POINT" << endl;
    for (int j = 1; j < N - 1; ++j)
    {
        for (int i = 1; i < N - 1; ++i)
        {

            const int index = coordinate(i, j, N);

            const int NodeCondition = get_NodeCondition(i, j, N);

            switch (NodeCondition)
            {
            case 0:
                //error_num2ana(index) = sqrt(p2(X(index) - phi_ana(index)));
                error_num2ana = (X - phi_ana).cwiseAbs();
                error_num2ana_L2(index) = error_num2ana(index) / phi_ana(index);
                break;
            default:
                break;
            }

            out << X_position[i] << " " << Y_position[j] << " "
                << X(index) << " "
                << phi_ana(index) << " "
                << error_num2ana(index) << " "
                << error_num2ana_L2(index) << " "
                //<< phi_ana(index) << " "
                << endl;
        }
    }
    out.close();
}

int main()
{
    cout << "Hello wo!" << endl;

    get_Bacis_Mesh(Basic::uni_mesh, Basic::N, Basic::S_x, Basic::S_y,
                   Basic::X_delta, Basic::Y_delta, Basic::X_position, Basic::Y_position);
    get_Analytic_Solution();

    clock_t start = clock();
    //// --------------- Solve AX = B directly -------------
    //solve_AX_B();

    // Gauss_Seidel_Iteration(Solve_equ::X, Solve_equ::X1, Solve_equ::B1,
    //                        Basic::X_delta, Basic::Y_delta, Basic::X_position, Basic::Y_position, Basic::N);

    MultiGrid_Iter(Basic::N, Solve_equ::X, Solve_equ::X1, Solve_equ::B1,
                   Basic::uni_mesh, Basic::S_x, Basic::S_y,
                   Basic::X_delta, Basic::Y_delta,
                   Basic::X_position, Basic::Y_position);

    clock_t end = clock();

    Solve_equ::L2_error = ((Solve_equ::phi_ana - Solve_equ::X).lpNorm<1>()) / Solve_equ::phi_ana.lpNorm<1>();

    cout << "N = " << Basic::N << ", ";
    cout << "Time : " << double(end - start) << " ms ,";
    cout << "L1 error = " << Solve_equ::L2_error << endl;

    std::ofstream time_out("Steepest_descent.txt", ios::app);
    time_out << "N = " << Basic::N << ", ";
    time_out << "Time : " << double(end - start) << " ms ,";
    time_out << "L1 error = " << Solve_equ::L2_error << endl;
    time_out.close();

    out_tec();
}


#ifndef CANNON_ML_BFGS_H
#define CANNON_ML_BFGS_H

/*!
 * \file cannon/ml/bfgs.hpp
 * \brief File containing BFGSOptimizer class definition.
 */

#include <functional>

#include <Eigen/Dense>

#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

using RealFunc = std::function<double(const VectorXd&)>;
using MultiFunc = std::function<VectorXd(const VectorXd&)>;

namespace cannon {
  namespace ml {

    CANNON_CLASS_FORWARD(OptimizationResult);

    /*!
     * \brief Class representing a BFGS optimizer, which is a quasi-Newton
     * unconstrained optimization method for nonlinear problems. See
     * https://en.wikipedia.org/wiki/Broyden%E2%80%93Fletcher%E2%80%93Goldfarb%E2%80%93Shanno_algorithm
     */
    class BFGSOptimizer {
      public:
        BFGSOptimizer() = delete;

        /*!
         * \brief Constructor taking a function to optimize and a function
         * providing gradients for that function.
         */
        BFGSOptimizer(RealFunc f, MultiFunc f_grad) : f_(f), f_grad_(f_grad) {}

        /*!
         * \brief Minimize the function stored by this object, beginning from
         * the input start state.
         *
         * \param start Initial state for optimization
         * \param eps Small number used to detect convergence
         * \param iterations Maximum number of iterations to perform
         *
         * \returns The result of the optimization.
         */
        OptimizationResult optimize(const VectorXd& start, double eps=1e-4, unsigned
            int iterations=100);

      private:
        RealFunc f_; //!< The function to minimize
        MultiFunc f_grad_; //!< Gradient function for the function to minimize

    };

  } // namespace ml
} // namespace cannon

#endif /* ifndef CANNON_ML_BFGS_H */


#define BOOST_TEST_MODULE classic hamiltonian
#define BOOST_TEST_DYN_LINK
#include <cmath>
#include <random>
#include <functional>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/test/unit_test.hpp>
#include <mpp/chains/mcmc_chain.hpp>
#include <mpp/hamiltonian/classic_hamiltonian.hpp>
#include <mpp/dists/multivariate_normal.hpp>

template<typename real_scalar_type>
void test_classic_hamiltonian(std::string const & chn_file_name) {
    using namespace mpp::hamiltonian;
    using namespace mpp::chains;
    using namespace mpp::dists;
    using namespace boost::numeric::ublas;

    typedef diag_multivar_normal<real_scalar_type> diag_multivar_normal_type;
    typedef hmc_sampler<real_scalar_type> hmc_sampler_type;
    typedef vector<real_scalar_type> real_vector_type;
    typedef std::normal_distribution<real_scalar_type> normal_distribution_type;
    typedef std::mt19937 rng_type;
    typedef mcmc_chain<real_scalar_type> chain_type;
    typedef typename hmc_sampler_type::log_post_func_type log_post_func_type;
    typedef typename hmc_sampler_type::grad_log_post_func_type grad_log_post_func_type;

    size_t const num_dims(10);
    real_vector_type mean(num_dims);
    real_vector_type var(num_dims);
    for(size_t i=0;i<num_dims;++i) {
        mean(i) = real_scalar_type(0);
        var(i) = real_scalar_type(1);
    }
    diag_multivar_normal_type dmn(mean,var);
    using std::placeholders::_1;
    log_post_func_type log_posterior
        = std::bind (&diag_multivar_normal_type::log_posterior,&dmn,_1);

    grad_log_post_func_type grad_log_posterior
        = std::bind (&diag_multivar_normal_type::grad_log_posterior,&dmn,_1);
    size_t const max_num_steps(10);
    real_scalar_type const max_eps(1);
    real_vector_type inv_mass_mat(num_dims);
    for(size_t i=0;i<num_dims;++i) {
        inv_mass_mat(i) = real_scalar_type(1);
    }

    hmc_sampler_type hmc_spr(
        log_posterior,
        grad_log_posterior,
        num_dims,
        max_num_steps,
        max_eps,
        inv_mass_mat
    );

    size_t const num_samples(100);
    rng_type rng;
    normal_distribution_type nrm_dist;
    real_vector_type q_0(num_dims);
    for(size_t i=0;i<num_dims;++i) {
        q_0(i) = nrm_dist(rng);
    }
    chain_type chn = hmc_spr.run_sampler(num_samples,q_0);
    chn.write_samples_to_csv(chn_file_name);

}

BOOST_AUTO_TEST_CASE(classic_hamiltonian) {
    test_classic_hamiltonian<float>(std::string("float.chain"));
    test_classic_hamiltonian<double>(std::string("double.chain"));
    test_classic_hamiltonian<long double>(std::string("long-double.chain"));
}


// ALGOLAB BGL Tutorial 2
// Flow example demonstrating
// - interior graph properties for flow algorithms
// - custom edge adder

// Compile and run with one of the following:
// g++ -std=c++11 -O2 flows.cpp -o flows ./flows
// g++ -std=c++11 -O2 -I path/to/boost_1_58_0 flows.cpp -o flows; ./flows

// Includes
// ========
// STL includes
#include <iostream>
#include <vector>
#include <algorithm>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
// Namespaces
using namespace std;
using namespace boost;

// BGL Graph definitions
// =====================
// Graph Type with nested interior edge properties for Flow Algorithms
typedef	adjacency_list_traits<vecS, vecS, directedS> Traits;
typedef adjacency_list<vecS, vecS, directedS, no_property,
	property<edge_capacity_t, long,
		property<edge_residual_capacity_t, long,
			property<edge_reverse_t, Traits::edge_descriptor> > > >	Graph;
// Interior Property Maps
typedef	property_map<Graph, edge_capacity_t>::type		EdgeCapacityMap;
typedef	property_map<Graph, edge_residual_capacity_t>::type	ResidualCapacityMap;
typedef	property_map<Graph, edge_reverse_t>::type		ReverseEdgeMap;
typedef	graph_traits<Graph>::vertex_descriptor			Vertex;
typedef	graph_traits<Graph>::edge_descriptor			Edge;

// Custom Edge Adder Class, that holds the references
// to the graph, capacity map and reverse edge map
// ===================================================
class EdgeAdder {
	Graph &G;
	EdgeCapacityMap	&capacitymap;
	ReverseEdgeMap	&revedgemap;

public:
	// to initialize the Object
	EdgeAdder(Graph & G, EdgeCapacityMap &capacitymap, ReverseEdgeMap &revedgemap):
		G(G), capacitymap(capacitymap), revedgemap(revedgemap){}

	// to use the Function (add an edge)
	void addEdge(int from, int to, long capacity) {
		Edge e, reverseE;
		bool success;
		tie(e, success) = add_edge(from, to, G);
		tie(reverseE, success) = add_edge(to, from, G);
		capacitymap[e] = capacity;
		capacitymap[reverseE] = 0;
		revedgemap[e] = reverseE;
		revedgemap[reverseE] = e;
	}
};

// Functions
// =========
// Function for an individual testcase
void testcases() {
	// Create Graph and Maps
	Graph G(4);
	EdgeCapacityMap capacitymap = get(edge_capacity, G);
	ReverseEdgeMap revedgemap = get(edge_reverse, G);
	ResidualCapacityMap rescapacitymap = get(edge_residual_capacity, G);
	EdgeAdder eaG(G, capacitymap, revedgemap);

	// Add edges
	eaG.addEdge(0, 1, 1); // from, to, capacity
	eaG.addEdge(0, 3, 1);
	eaG.addEdge(2, 1, 1);
	eaG.addEdge(2, 3, 1);

	// Add source and sink
	// Careful: The names 'source' and 'target' are already used for BGL's
	// functions to get the two endpoints of an edge, use 'src' and 'sink'.
	Vertex src = add_vertex(G);
	Vertex sink = add_vertex(G);
	eaG.addEdge(src, 0, 2);
	eaG.addEdge(src, 2, 1);
	eaG.addEdge(1, sink, 2);
	eaG.addEdge(3, sink, 1);

	// Calculate flow
	// If not called otherwise, the flow algorithm uses the interior properties
	// - edge_capacity, edge_reverse (read access),
	// - edge_residual_capacity (read and write access).
	long flow1 = push_relabel_max_flow(G, src, sink);
	long flow2 = edmonds_karp_max_flow(G, src, sink);
	cout << flow1 << " == " << flow2 << endl;
}

// Main function to loop over the testcases
int main() {
	ios_base::sync_with_stdio(false);
	int T;	T = 1;
	for (; T > 0; --T)	testcases();
	return 0;
}


//------------------------------------------------------------------------------
// \file Algorithms_tests.cpp
//------------------------------------------------------------------------------

#include <boost/test/unit_test.hpp>

#include <algorithm> // std::transform
#include <array>
#include <cctype> // std::toupper
#include <cstdio> // std::printf
#include <cstdlib> // std::atoi
#include <iostream>
#include <list>
#include <numeric>
#include <optional>
#include <string>
#include <vector>

BOOST_AUTO_TEST_SUITE(Cpp)
BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(Algorithms_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(StdForEachPrintsEachValue)
{
	{
		constexpr std::size_t N {100};

		// Initializer list doesn't work to make std::list have size of N.
		std::list<std::size_t> range_list (N);
		std::iota(range_list.begin(), range_list.end(), 0);

		std::size_t counter {0};
		for (const auto& l_i : range_list)
		{
			BOOST_TEST_REQUIRE(l_i == counter);
			counter++;
		}
		BOOST_TEST_REQUIRE(counter == N);
		BOOST_TEST_REQUIRE(range_list.size() == N);

		// Initializer list just initializes to 2 values if done like this:
		// {N, '0'} or this {{N, '0'}}
		std::vector<unsigned char> test_values (N, '0');
		BOOST_TEST_REQUIRE(test_values.size() == N);

		std::transform(
			range_list.begin(),
			range_list.end(),
			test_values.begin(),
			[](const auto& l_i) -> unsigned char
			{
				return 'a' + l_i;
			});

	  std::for_each(
	  	range_list.begin(),
	    range_list.end(),
	    [&test_values](const auto& index)
	    {
	      std::printf("%01x ", test_values[index]);
	    });
	}
}

BOOST_AUTO_TEST_SUITE_END() // Algorithms_tests
BOOST_AUTO_TEST_SUITE_END() // Algorithms
BOOST_AUTO_TEST_SUITE_END() // Cpp


/*
 *  vector_map_view_test_2.cpp
 *  MTL
 *
 *	Test rscaled_view and divide_by_view
 *
 *  Created by Hui Li (huil@Princeton.EDU)
 *
 */

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/rscale.hpp>
#include <boost/numeric/mtl/operation/divide_by.hpp>

#if 0
#include <boost/numeric/mtl/operation/hermitian.hpp>
#endif

using std::cout;  using std::complex;

typedef complex<double> ct;

double value(double)
{
    return 7.0;
}

complex<double> value(complex<double>)
{
    return ct(7.0, 1.0);
}

// rscaled value
double rsvalue(double)
{
    return 14.0;
}

ct rsvalue(ct)
{
    return ct(14.0, 2.0);
}

// complex rscaled value
ct crsvalue(double)
{
    return ct(0.0, 7.0);
}

ct crsvalue(ct)
{
    return ct(-1.0, 7.0);
}

template <typename Vector>
void test(Vector& vector, const char* name)
{
    set_to_zero(vector);
    typename Vector::value_type ref(0);

#if 1
    vector[2]= value(ref);
    vector[4]= value(ref) + 1.0;
    vector[5]= value(ref) + 2.0;

#else // When sparse vectors are used there should be an inserter class for vectors too
    {
		inserter<Vector>  ins(vector);
		ins(2) << value(ref);
		ins(4) << value(ref) + 1.0;
		ins(5) << value(ref) + 2.0;
    }
#endif

    cout << "\n\n" << name << "\n";
    cout << "Original vector:\n" << vector << "\n";

	// test rscaled_view
    mtl::vec::rscaled_view<Vector,double>  rscaled_vector(vector,2.0);
    cout << "vector right scaled with 2.0\n" << rscaled_vector << "\n";
    MTL_THROW_IF(rscaled_vector(2) != rsvalue(ref), mtl::runtime_error("right scaling wrong"));

    mtl::vec::rscaled_view<Vector,ct>  crscaled_vector(vector,ct(0.0, 1.0));
    cout << "vector right scaled with i (complex(0, 1))\n" << crscaled_vector << "\n";
    MTL_THROW_IF(crscaled_vector(2) != crsvalue(ref), mtl::runtime_error("complex right scaling wrong"));

    cout << "vector right scaled with 2.0 (free function)\n" << rscale(vector,2.0) << "\n";
    MTL_THROW_IF(rscale(vector,2.0)(2) != rsvalue(ref), mtl::runtime_error("right scaling wrong"));

    cout << "vector right scaled with i (complex(0, 1)) (free function)\n" << rscale(vector,ct(0.0, 1.0)) << "\n";
    MTL_THROW_IF(rscale(vector,ct(0.0, 1.0))(2) != crsvalue(ref), mtl::runtime_error("complex right scaling wrong"));

	// test divide_by_view
    mtl::vec::divide_by_view<Vector,double>  div_vector(vector,0.5);
    cout << "vector divide by 0.5\n" << div_vector << "\n";
    MTL_THROW_IF(div_vector(2) != rsvalue(ref), mtl::runtime_error("divide_by wrong"));

    mtl::vec::divide_by_view<Vector,ct>  cdiv_vector(vector,ct(0.0, -1.0));
    cout << "vector divide by -i (complex(0, -1))\n" << cdiv_vector << "\n";
    MTL_THROW_IF(cdiv_vector(2) != crsvalue(ref), mtl::runtime_error("complex divide_by wrong"));

    cout << "vector divide by 0.5 (free function)\n" << divide_by(vector,0.5) << "\n";
    MTL_THROW_IF(divide_by(vector,0.5)(2) != rsvalue(ref), mtl::runtime_error("divide_by wrong"));

    cout << "vector divide by -i (complex(0, -1)) (free function)\n" << divide_by(vector,ct(0.0, -1.0)) << "\n";
    MTL_THROW_IF(divide_by(vector,ct(0.0, -1.0))(2) != crsvalue(ref), mtl::runtime_error("complex divide_by wrong"));

}

int main(int argc, char* argv[])
{
    unsigned size= 7;
    if (argc > 1) size= atoi(argv[1]);

    mtl::dense_vector<double>                                 dv(size);
    mtl::dense_vector<complex<double> >                       drc(size);

    test(dv, "Dense double vector");
    test(drc, "Dense complex vector");

    return 0;
}


// -*- mode: C++ -*-
/**
   Dietrich Bollmann, Kamakura, 2015/01/01

   FEM-test.h

   Unit tests for class: FEM

   Copyright (c) 2015 Dietrich Bollmann

   This software may be modified and distributed under the terms
   of the MIT license.  See the LICENSE file for details.
*/

#define BOOST_TEST_DYN_LINK
#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE Main
#endif

#include <boost/test/unit_test.hpp>

#include <cmath>

#include "Spring.h"

#include "FEM.h"

namespace nsl {

BOOST_AUTO_TEST_SUITE(TestSuite_FEM)

/**
   Add node.
*/
void addNode(FEM &fem, std::vector<double> node)
{
  // Add node
  int id = node[0];
  fem.addNode(id);

  for (size_t i = 1; i < node.size() - 1; i += 2)
    {
      // Add displacement, when given
      if (node[i] == 'd')
	{
	  double displacement = node[i+1];
	  fem.addDisplacement(id, displacement);
	}

      // Add force, when given
      if (node[i] == 'f')
	{
	  double force = node[i+1];
	  fem.addForce(id, force);
	}
    }
}

/**
  Add spring.
 */
void addSpring(FEM &fem, std::vector<double> spring)
{
  // Unpack spring data
  int id = spring[0];
  int node1 = spring[1];
  int node2 = spring[2];
  double springConstant = spring[3];

  // Add spring
  fem.addSpring(id, node1, node2, springConstant);
}

/**
   Test stiffness matrix.
 */
void testStiffnessMatrix(std::vector<std::vector<double> > nodes,
			 std::vector<std::vector<double> > springs,
			 std::vector<std::vector<double> > stiffnessMatrix)
{
  FEM fem;

  // Assemble model
  for (auto node : nodes) addNode(fem, node);
  for (auto spring : springs) addSpring(fem, spring);

  // Solve model
  fem.solve();

  DMatrix expectedStiffnessMatrix(stiffnessMatrix);
  DMatrix calculatedStiffnessMatrix = fem.getGlobalStiffnessMatrix();

  double distance = euclideanDistance(expectedStiffnessMatrix, calculatedStiffnessMatrix);
  bool testResult = distance < 1e-20;

  BOOST_CHECK( testResult );

  if (!testResult)
    std::cout
      << ">>> Stiffness matrix:" << std::endl << std::endl
      << "expected:"   << std::endl << expectedStiffnessMatrix << std::endl
      << "calculated:" << std::endl << calculatedStiffnessMatrix << std::endl
      << "euclidian distance: " << distance << std::endl
      ;
}

BOOST_AUTO_TEST_CASE(Test_stiffnessMatrix)
{
  // Test case 1
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.1, page 40.
  //
  testStiffnessMatrix(// Nodes:
		      // node id ['d', <displacement>] ['f', <force>]
		      {{1, 'd', 0},
		       {2, 'd', 0},
		       {3},
		       {4, 'f', 5000}
		      },
		      // Springs:
		      // spring id  node1 node2  spring constant
		      {{1,  1, 3,  1000},
		       {2,  3, 4,  2000},
		       {3,  4, 2,  3000}
		      },
		      // Stiffness matrix:
		      {{ 1000,     0, -1000,     0},
		       {    0,  3000,     0, -3000},
		       {-1000,     0,  3000, -2000},
		       {    0, -3000, -2000,  5000},
		      }
		      );

  // Test case 2
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.2, page 43.
  //
  testStiffnessMatrix(// Nodes:
		      // node id ['d', <displacement>] ['f', <force>]
		      {{1, 'd', 0},
		       {2},
		       {3},
		       {4},
		       {5, 'd', 0.02}
		      },
		      // Springs:
		      // spring id  node1 node2  spring constant
		      {{1,  1, 2,  200},
		       {2,  2, 3,  200},
		       {3,  3, 4,  200},
		       {4,  4, 5,  200}
		      },
		      // Stiffness matrix:
		      {{ 200, -200,    0,    0,    0},
		       {-200,  400, -200,    0,    0},
		       {   0, -200,  400, -200,    0},
		       {   0,    0, -200,  400, -200},
		       {   0,    0,    0, -200,  200},
		      }
		      );

  // Test case 3
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.3, page 46.
  //
  testStiffnessMatrix(// Nodes:
		      // node id ['d', <displacement>] ['f', <force>]
		      {{1, 'd', 0},
           	       {2},
           	       {3, 'd', 0},
           	       {4, 'd', 0}
           	      },
		      // Springs:
		      // spring id  node1 node2  spring constant
           	      {{1,  1, 2,  1},
           	       {2,  2, 3,  2},
           	       {3,  2, 4,  3}
           	      },
		      // Stiffness matrix:
		      {{ 1,    -1,  0,  0},
		       {-1, 1+2+3, -2, -3},
		       { 0,    -2,  2,  0},
		       { 0,    -3,  0,  3}
		      }
		      );

  // Test case 4
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.4, page 54.
  //
  testStiffnessMatrix(// Nodes:
		      // node id ['d', <displacement>] ['f', <force>]
		      {{1, 'd', 0},
		       {2, 'f', 1000}
           	      },
		      // Springs:
		      // spring id  node1 node2  spring constant
           	      {{1,  1, 2,  500}
           	      },
		      // Stiffness matrix:
		      {{ 500, -500},
		       {-500,  500}
		      }
		      );
}

/**
   Test case.
 */
void testCase(std::vector<std::vector<double> > nodes,
	      std::vector<std::vector<double> > springs,
	      std::vector<std::vector<double> > displacements,
	      std::vector<std::vector<double> > forces,
	      std::vector<std::vector<double> > localForces)
{
  FEM fem;

  // Assemble model
  for (auto node : nodes) addNode(fem, node);
  for (auto spring : springs) addSpring(fem, spring);

  // Solve model
  fem.solve();

  // Check displacements
  for (auto displacementData : displacements)
    {
      int node = displacementData[0];
      double expectedDisplacement = displacementData[1];
      double displacement = fem.getGlobalDisplacement(node);

      bool testResult = (expectedDisplacement == displacement);

      BOOST_CHECK( testResult );

      if (!testResult)
	std::cout
	  << ">>> Displacement at node " << node << ": "
	  << "expected: " << expectedDisplacement << ", "
	  << "calculated: " << displacement << std::endl
	  ;
    }

  // Check forces
  for (auto forceData : forces)
    {
      int node = forceData[0];
      double expectedForce = forceData[1];
      double force = fem.getGlobalForce(node);

      bool testResult = abs(expectedForce - force) < 1e-12;

      BOOST_CHECK( testResult );

      if (!testResult)
	std::cout
	  << ">>> Force at node " << node << ": "
	  << "expected: " << expectedForce << ", "
	  << "calculated: " << force << std::endl
	  ;
    }

  // Check localForces
  for (auto localForceData : localForces)
    {
      int node = localForceData[0];
      DVector expectedLocalForces({localForceData[1], localForceData[2]});
      DVector localForces = fem.getLocalForces(node);

      double distance = euclideanDistance(expectedLocalForces, localForces);
      bool testResult = distance < 1e-12;

      BOOST_CHECK( testResult );

      if (!testResult)
      	std::cout
      	  << ">>> Local forces at node " << node << ": "
      	  << "expected: " << expectedLocalForces << ", "
      	  << "calculated: " << localForces << "  "
      	  << "(euclidian distance: " << distance << ")" << std::endl
      	  ;
    }
}

BOOST_AUTO_TEST_CASE(Test_testCases)
{
  // Test case 1
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.1, page 40.
  //
  testCase(// Nodes:
  	   // node id ['d', <displacement>] ['f', <force>]
	   {{1, 'd', 0},
	    {2, 'd', 0},
	    {3},
	    {4, 'f', 5000}
  	   },
  	   // Springs:
  	   // spring id  node1 node2  spring constant
  	   {{1,  1, 3,  1000},
            {2,  3, 4,  2000},
  	    {3,  4, 2,  3000}
  	   },
	   // Displacements:
	   // node id  displacement
	   {{1, 0},
	    {2, 0},
	    {3, 10.0 / 11.0},
	    {4, 15.0 / 11.0}
	   },
	   // Forces:
	   // node id  force
	   {{1, -10000.0 / 11},
	    {2, -45000.0 / 11},
	    {3,      0.0},
	    {4,  55000.0 / 11}
	   },
	   // Local forces:
	   // spring id  local force node1, node2
	   {{1, -10000.0 / 11,  10000.0 / 11},
	    {2, -10000.0 / 11,  10000.0 / 11},
	    {3,  45000.0 / 11, -45000.0 / 11}
	   }
  	   );

  // Test case 2
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.2, page 43.
  //
  testCase(// Nodes:
	   // node id ['d', <displacement>] ['f', <force>]
	   {{1, 'd', 0},
	    {2},
	    {3},
	    {4},
	    {5, 'd', 0.02}
	   },
	   // Springs:
	   // spring id  node1 node2  spring constant
	   {{1,  1, 2,  200},
	    {2,  2, 3,  200},
	    {3,  3, 4,  200},
	    {4,  4, 5,  200}
	   },
	   // Displacements:
	   // node id  displacement
	   {{1, 0},
	    {2, 0.005},
	    {3, 0.01},
	    {4, 0.015},
	    {5, 0.02}
	   },
	   // Forces:
	   // node id  force
	   {{1, -1},
	    {2,  0},
	    {3,  0},
	    {4,  0},
	    {5,  1}
	   },
	   // Local forces:
	   // spring id  local force node1, node2
	   {{1, -1, 1},
	    {2, -1, 1},
	    {3, -1, 1},
	    {4, -1 ,1}
	   }
  	   );

  // Test case 3
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.3, page 46.
  //
  testCase(// Nodes:
	   // node id ['d', <displacement>] ['f', <force>]
	   {{1, 'd', 0},
	    {2, 'f', 6}, // Force: P = 6
	    {3, 'd', 0},
	    {4, 'd', 0}
	   },
	   // Springs:
	   // spring id  node1 node2  spring constant
	   {{1,  1, 2,  1}, // Spring constant:  k1 = 1
	    {2,  2, 3,  2}, // Spring constant:  k2 = 2
	    {3,  2, 4,  3}  // Spring constant:  k3 = 3
	   },
	   // Displacements:
	   // node id  displacement
	   {{1, 0},
	    {2, 1}, // P / (k1 + k2 + k3) = 6 / (1 + 2 + 3) = 1
	    {3, 0},
	    {4, 0}
	   },
	   // Forces:
	   // node id  force
	   // d2 = P / (k1 + k2 + k3) = 6 / (1 + 2 + 3) = 1
	   {{1, -1}, // F1 = -k1 * d2 = -k1 = -1
	    {2,  6}, // F2 = P = 6
	    {3, -2}, // F3 = -k2 * d2 = -k2 = -2
	    {4, -3}  // F4 = -k3 * d2 = -k3 = -3
	   },
	   // Local forces:
	   // spring id  local force node1, node2
	   // d2 = P / (k1 + k2 + k3) = 6 / (1 + 2 + 3) = 1
	   {{1, -1,  1}, // k1 * d1 + -k1 * d2 = 1 * 0 + -1 * 1 = -1
	    {2,  2, -2}, // k2 * d2 + -k2 * d3 = 2 * 1 + -2 * 0 =  2
	    {3,  3, -3}  // k3 * d2 + -k3 * d4 = 3 * 1 + -3 * 0 =  3
	   }
  	   );

  // Test case 4
  //
  // Taken from the book
  // "A First Course in the Finite element Method, third edition"
  // by Daryl L. Logan,
  // Example 2.4, page 54.
  //
  testCase(// Nodes:
	   // node id ['d', <displacement>] ['f', <force>]
	   {{1, 'd', 0},
	    {2, 'f', 1000}
	   },
	   // Springs:
	   // spring id  node1 node2  spring constant
	   {{1,  1, 2,  500}
	   },
	   // Displacements:
	   // node id  displacement
	   {{1, 0},
	    {2, 2}
	   },
	   // Forces:
	   // node id  force
	   {{1, -1000},
	    {2,  1000}
	   },
	   // Local forces:
	   // spring id  local force node1, node2
	   {{1, -1000, 1000}
	   }
  	   );
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace nsl

/* fin */


/*
  boost_random.hpp : Random number class implimentaion header using BOOST functions

  Copyright (C) 2015 Anup Gopalakrishna Pillai, Suhita Nadkarni Lab, IISER, Pune <anupgpillai@gmail.com>
  Copyright (C) 2015 Pranav Kulkarni, Collins Assisi Lab, IISER, Pune <pranavcode@gmail.com>

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef BOOST_RANDOM_HPP_INCLUDED
#define BOOST_RANDOM_HPP_INCLUDED

#include <iostream>
#include <ctime>
#include <sys/types.h>
#include <unistd.h>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>

typedef boost::random::mt19937 base_generator_type;
typedef boost::random::uniform_int_distribution<> uni_int_dist_type;
typedef boost::random::uniform_real_distribution<> uni_real_dist_type;
typedef boost::random::normal_distribution<double> norm_dist_type;

typedef boost::random::variate_generator<base_generator_type&, uni_int_dist_type> uni_int_var_gen_type;
typedef boost::random::variate_generator<base_generator_type&, uni_real_dist_type> uni_real_var_gen_type;
typedef boost::random::variate_generator<base_generator_type&, norm_dist_type> norm_var_gen_type;

class boost_rng
{
 private:
  long long seed;
  base_generator_type gen;
  uni_int_dist_type uni_int_dist;
  uni_real_dist_type uni_real_dist;
  norm_dist_type norm_dist;

  uni_int_var_gen_type rand_uni_int;
  norm_var_gen_type rand_norm;
 public:
  boost_rng(long long seed_ = std::time(0) + (long long) getpid()):
      seed(seed_), gen(seed), uni_int_dist(0,1), rand_uni_int(gen,uni_int_dist),
      norm_dist(0.0,1.0),rand_norm(gen,norm_dist) {};
  int uni_int(void);
  double norm_double(void);
};

// generates uniform distribution (int)
int boost_rng::uni_int(void)
{
  return (rand_uni_int());
}

// generates normal distribution ( double )
double boost_rng::norm_double(void)
{
  return (rand_norm());
}

#endif	// BOOST_RANDOM_HPP_INCLUDED


// test_inverse_chi_squared.cpp

// Copyright Paul A. Bristow 2010.
// Copyright John Maddock 2010.

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

#ifdef _MSC_VER
#  pragma warning (disable : 4310) // cast truncates constant value.
#endif

// http://www.wolframalpha.com/input/?i=inverse+chisquare+distribution

#include <boost/math/tools/test.hpp>
#include <boost/math/concepts/real_concept.hpp> // for real_concept
using ::boost::math::concepts::real_concept;

//#include <boost/math/tools/test.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // for test_main
#include <boost/test/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE_FRACTION
#include "test_out_of_range.hpp"

#include <boost/math/distributions/inverse_chi_squared.hpp> // for inverse_chisquared_distribution
using boost::math::inverse_chi_squared_distribution;
using boost::math::cdf;
using boost::math::pdf;

// Use Inverse Gamma distribution to check their relationship:
// inverse_chi_squared<>(v) == inverse_gamma<>(v / 2., 0.5)
#include <boost/math/distributions/inverse_gamma.hpp> // for inverse_gamma_distribution
using boost::math::inverse_gamma_distribution;
using boost::math::inverse_gamma;
//  using  ::boost::math::cdf;
//  using  ::boost::math::pdf;

#include <boost/math/special_functions/gamma.hpp>
using boost::math::tgamma; // for naive pdf.

#include <iostream>
using std::cout;
using std::endl;
#include <limits>
using std::numeric_limits; // for epsilon.

template <class RealType>
RealType naive_pdf(RealType df, RealType scale, RealType x)
{ // Formula from Wikipedia
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType result = pow(scale * df/2, df/2) * exp(-df * scale/(2 * x));
   result /= tgamma(df/2) * pow(x, 1 + df/2);
   return result;
}

// Test using a spot value from some other reference source,
// in this case test values from output from R provided by Thomas Mang,
// and Wolfram Mathematica by Mark Coleman.

template <class RealType>
void test_spot(
     RealType degrees_of_freedom, // degrees_of_freedom,
     RealType scale, // scale,
     RealType x, // random variate x,
     RealType pd, // expected pdf,
     RealType P, // expected CDF,
     RealType Q, // expected complement of CDF,
     RealType tol) // test tolerance.
{
   boost::math::inverse_chi_squared_distribution<RealType> dist(degrees_of_freedom, scale);

   BOOST_CHECK_CLOSE_FRACTION
      ( // Compare to expected PDF.
      pdf(dist, x), // calculated.
      pd, // expected
      tol);

   BOOST_CHECK_CLOSE_FRACTION( // Compare to naive pdf formula (probably less accurate).
      pdf(dist, x), naive_pdf(dist.degrees_of_freedom(), dist.scale(), x), tol);

   BOOST_CHECK_CLOSE_FRACTION( // Compare to expected CDF.
      cdf(dist, x), P, tol);

   if((P < 0.999) && (Q < 0.999))
   {  // We can only check this if P is not too close to 1,
      // so that we can guarantee Q is accurate:
      BOOST_CHECK_CLOSE_FRACTION(
        cdf(complement(dist, x)), Q, tol); // 1 - cdf
      BOOST_CHECK_CLOSE_FRACTION(
        quantile(dist, P), x, tol); // quantile(cdf) = x
      BOOST_CHECK_CLOSE_FRACTION(
        quantile(complement(dist, Q)), x, tol); // quantile(complement(1 - cdf)) = x
   }
} // test_spot

template <class RealType> // Any floating-point type RealType.
void test_spots(RealType)
{
  // Basic sanity checks, some test data is to six decimal places only,
  // so set tolerance to 0.000001 (expressed as a percentage = 0.0001%).

  RealType tolerance = 0.000001f;
  cout << "Tolerance = " << tolerance * 100 << "%." << endl;

// This test values from output from geoR (17 decimal digits) guided by Thomas Mang.
  test_spot(static_cast<RealType>(2), static_cast<RealType>(1./2.),
    // degrees_of_freedom, default scale = 1/df.
  static_cast<RealType>(1.L), // x.
  static_cast<RealType>(0.30326532985631671L), // pdf.
  static_cast<RealType>(0.60653065971263365L), // cdf.
  static_cast<RealType>(1 - 0.606530659712633657L), // cdf complement.
  tolerance  // tol
  );

// Tests from Mark Coleman & Georgi Boshnakov using Wolfram Mathematica.
  test_spot(static_cast<RealType>(10), static_cast<RealType>(0.1L), // degrees_of_freedom, scale
  static_cast<RealType>(0.2), // x
  static_cast<RealType>(1.6700235722635659824529759616528281217001163943570L), // pdf
  static_cast<RealType>(0.89117801891415124234834646836872197623907651175353L), // cdf
  static_cast<RealType>(1 - 0.89117801891415127L), // cdf complement
  tolerance  // tol
  );

  test_spot(static_cast<RealType>(10), static_cast<RealType>(0.1L), // degrees_of_freedom, scale
  static_cast<RealType>(0.5), // x
  static_cast<RealType>(0.03065662009762021L), // pdf
  static_cast<RealType>(0.99634015317265628765454354418728984933240514654437L), // cdf
  static_cast<RealType>(1 - 0.99634015317265628765454354418728984933240514654437L), // cdf complement
  tolerance  // tol
  );

  test_spot(static_cast<RealType>(10), static_cast<RealType>(2), // degrees_of_freedom, scale
  static_cast<RealType>(0.5), // x
  static_cast<RealType>(0.00054964096598361569L), // pdf
  static_cast<RealType>(0.000016944743930067383903707995865261004246785511612700L), // cdf
  static_cast<RealType>(1 - 0.000016944743930067383903707995865261004246785511612700L), // cdf complement
  tolerance  // tol
  );

  // Check some bad parameters to the distribution cause expected exception to be thrown.
#ifndef BOOST_NO_EXCEPTIONS
  BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType> ichsqbad1(-1), std::domain_error); // negative degrees_of_freedom.
  BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType> ichsqbad2(1, -1), std::domain_error); // negative scale.
  BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType> ichsqbad3(-1, -1), std::domain_error); // negative scale and degrees_of_freedom.
#else
  BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType>(-1), std::domain_error); // negative degrees_of_freedom.
  BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType>(1, -1), std::domain_error); // negative scale.
  BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType>(-1, -1), std::domain_error); // negative scale and degrees_of_freedom.
#endif
  check_out_of_range<boost::math::inverse_chi_squared_distribution<RealType> >(1, 1);

  inverse_chi_squared_distribution<RealType> ichsq;

  if(std::numeric_limits<RealType>::has_infinity)
  {
    BOOST_MATH_CHECK_THROW(pdf(ichsq, +std::numeric_limits<RealType>::infinity()), std::domain_error); // x = + infinity, pdf = 0
    BOOST_MATH_CHECK_THROW(pdf(ichsq, -std::numeric_limits<RealType>::infinity()),  std::domain_error); // x = - infinity, pdf = 0
    BOOST_MATH_CHECK_THROW(cdf(ichsq, +std::numeric_limits<RealType>::infinity()),std::domain_error ); // x = + infinity, cdf = 1
    BOOST_MATH_CHECK_THROW(cdf(ichsq, -std::numeric_limits<RealType>::infinity()), std::domain_error); // x = - infinity, cdf = 0
    BOOST_MATH_CHECK_THROW(cdf(complement(ichsq, +std::numeric_limits<RealType>::infinity())), std::domain_error); // x = + infinity, c cdf = 0
    BOOST_MATH_CHECK_THROW(cdf(complement(ichsq, -std::numeric_limits<RealType>::infinity())), std::domain_error); // x = - infinity, c cdf = 1
#ifndef BOOST_NO_EXCEPTIONS
    BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType> nbad1(std::numeric_limits<RealType>::infinity(), static_cast<RealType>(1)), std::domain_error); // +infinite mean
    BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType> nbad1(-std::numeric_limits<RealType>::infinity(),  static_cast<RealType>(1)), std::domain_error); // -infinite mean
    BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType> nbad1(static_cast<RealType>(0), std::numeric_limits<RealType>::infinity()), std::domain_error); // infinite sd
#else
    BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType>(std::numeric_limits<RealType>::infinity(), static_cast<RealType>(1)), std::domain_error); // +infinite mean
    BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType>(-std::numeric_limits<RealType>::infinity(),  static_cast<RealType>(1)), std::domain_error); // -infinite mean
    BOOST_MATH_CHECK_THROW(boost::math::inverse_chi_squared_distribution<RealType>(static_cast<RealType>(0), std::numeric_limits<RealType>::infinity()), std::domain_error); // infinite sd
#endif
  }

  if (std::numeric_limits<RealType>::has_quiet_NaN)
  { // If no longer allow x or p to be NaN, then these tests should throw.
    BOOST_MATH_CHECK_THROW(pdf(ichsq, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // x = NaN
    BOOST_MATH_CHECK_THROW(cdf(ichsq, +std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // x = NaN
    BOOST_MATH_CHECK_THROW(cdf(complement(ichsq, +std::numeric_limits<RealType>::quiet_NaN())), std::domain_error); // x = + infinity
    BOOST_MATH_CHECK_THROW(quantile(ichsq, std::numeric_limits<RealType>::quiet_NaN()), std::domain_error); // p = + quiet_NaN
    BOOST_MATH_CHECK_THROW(quantile(complement(ichsq, std::numeric_limits<RealType>::quiet_NaN())), std::domain_error); // p = + quiet_NaN
  }
    // Spot check for pdf using 'naive pdf' function
  for(RealType x = 0.5; x < 5; x += 0.5)
  {
    BOOST_CHECK_CLOSE_FRACTION(
      pdf(inverse_chi_squared_distribution<RealType>(5, 6), x),
      naive_pdf(RealType(5), RealType(6), x),
      tolerance);
  }   // Spot checks for parameters:

  RealType tol_2eps = boost::math::tools::epsilon<RealType>() * 2; // 2 eps as a fraction.
  inverse_chi_squared_distribution<RealType> dist51(5, 1);
  inverse_chi_squared_distribution<RealType> dist52(5, 2);
  inverse_chi_squared_distribution<RealType> dist31(3, 1);
  inverse_chi_squared_distribution<RealType> dist111(11, 1);
  // 11 mean 0.10000000000000001, variance  0.0011111111111111111, sd 0.033333333333333333

  using namespace std; // ADL of std names.
  using namespace boost::math;

  inverse_chi_squared_distribution<RealType> dist10(10);
  //  mean, variance etc
  BOOST_CHECK_CLOSE_FRACTION(mean(dist10), static_cast<RealType>(0.125), tol_2eps);
  BOOST_CHECK_CLOSE_FRACTION(variance(dist10), static_cast<RealType>(0.0052083333333333333333333333333333333333333333333333L), tol_2eps);
  BOOST_CHECK_CLOSE_FRACTION(mode(dist10), static_cast<RealType>(0.08333333333333333333333333333333333333333333333L), tol_2eps);
  BOOST_CHECK_CLOSE_FRACTION(median(dist10), static_cast<RealType>(0.10704554778227709530244586234274024205738435512468L), tol_2eps);
  BOOST_CHECK_CLOSE_FRACTION(cdf(dist10, median(dist10)), static_cast<RealType>(0.5L), 4 * tol_2eps);
  BOOST_CHECK_CLOSE_FRACTION(skewness(dist10), static_cast<RealType>(3.4641016151377545870548926830117447338856105076208L), tol_2eps);
  BOOST_CHECK_CLOSE_FRACTION(kurtosis(dist10), static_cast<RealType>(45), tol_2eps);
  BOOST_CHECK_CLOSE_FRACTION(kurtosis_excess(dist10), static_cast<RealType>(45-3), tol_2eps);

  tol_2eps = boost::math::tools::epsilon<RealType>() * 2; // 2 eps as a percentage.

  // Special and limit cases:

  RealType mx = (std::numeric_limits<RealType>::max)();
  RealType mi = (std::numeric_limits<RealType>::min)();

  BOOST_CHECK_EQUAL(
  pdf(inverse_chi_squared_distribution<RealType>(1),
    static_cast<RealType>(mx)), // max()
    static_cast<RealType>(0)
    );

  BOOST_CHECK_EQUAL(
  pdf(inverse_chi_squared_distribution<RealType>(1),
    static_cast<RealType>(mi)), // min()
    static_cast<RealType>(0)
    );

  BOOST_CHECK_EQUAL(
    pdf(inverse_chi_squared_distribution<RealType>(1), static_cast<RealType>(0)), static_cast<RealType>(0));
  BOOST_CHECK_EQUAL(
    pdf(inverse_chi_squared_distribution<RealType>(3), static_cast<RealType>(0))
    , static_cast<RealType>(0.0f));
  BOOST_CHECK_EQUAL(
    cdf(inverse_chi_squared_distribution<RealType>(1), static_cast<RealType>(0))
    , static_cast<RealType>(0.0f));
  BOOST_CHECK_EQUAL(
    cdf(inverse_chi_squared_distribution<RealType>(2), static_cast<RealType>(0))
    , static_cast<RealType>(0.0f));
  BOOST_CHECK_EQUAL(
    cdf(inverse_chi_squared_distribution<RealType>(3L), static_cast<RealType>(0L))
    , static_cast<RealType>(0));
  BOOST_CHECK_EQUAL(
    cdf(complement(inverse_chi_squared_distribution<RealType>(1), static_cast<RealType>(0)))
    , static_cast<RealType>(1));
  BOOST_CHECK_EQUAL(
    cdf(complement(inverse_chi_squared_distribution<RealType>(2), static_cast<RealType>(0)))
    , static_cast<RealType>(1));
  BOOST_CHECK_EQUAL(
    cdf(complement(inverse_chi_squared_distribution<RealType>(3), static_cast<RealType>(0)))
    , static_cast<RealType>(1));

  BOOST_MATH_CHECK_THROW(
    pdf(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(-1)), // degrees_of_freedom negative.
    static_cast<RealType>(1)), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    pdf(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1)), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    cdf(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(1)), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    cdf(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1)), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    cdf(complement(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(1))), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    cdf(complement(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1))), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    quantile(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(0.5)), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    quantile(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1)), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    quantile(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(1.1)), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    quantile(complement(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(-1)),
    static_cast<RealType>(0.5))), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    quantile(complement(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(-1))), std::domain_error
    );
  BOOST_MATH_CHECK_THROW(
    quantile(complement(
    inverse_chi_squared_distribution<RealType>(static_cast<RealType>(8)),
    static_cast<RealType>(1.1))), std::domain_error
    );
} // template <class RealType>void test_spots(RealType)

BOOST_AUTO_TEST_CASE( test_main )
{
  BOOST_MATH_CONTROL_FP;

  double tol_few_eps = numeric_limits<double>::epsilon() * 4;

  // Check that can generate inverse_chi_squared distribution using the two convenience methods:
  // inverse_chi_squared_distribution; // with default parameters, degrees_of_freedom = 1, scale - 1
  using boost::math::inverse_chi_squared;

  // Some constructor tests using default double.
  double tol4eps = boost::math::tools::epsilon<double>() * 4; // 4 eps as a fraction.

  inverse_chi_squared ichsqdef; // Using typedef and both default parameters.

  BOOST_CHECK_EQUAL(ichsqdef.degrees_of_freedom(), 1.); // df == 1
  BOOST_CHECK_EQUAL(ichsqdef.scale(), 1); // scale == 1./df
  BOOST_CHECK_CLOSE_FRACTION(pdf(ichsqdef, 1), 0.24197072451914330, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(pdf(ichsqdef, 9), 0.013977156581221969, tol4eps);

  inverse_chi_squared_distribution<double> ichisq102(10., 2); // Both parameters specified.
  BOOST_CHECK_EQUAL(ichisq102.degrees_of_freedom(), 10.); // Check both parameters stored OK.
  BOOST_CHECK_EQUAL(ichisq102.scale(), 2.); // Check both parameters stored OK.

  inverse_chi_squared_distribution<double> ichisq10(10.); // Only df parameter specified (unscaled).
  BOOST_CHECK_EQUAL(ichisq10.degrees_of_freedom(), 10.); // Check  parameter stored.
  BOOST_CHECK_EQUAL(ichisq10.scale(), 0.1); // Check default scale = 1/df = 1/10 = 0.1
  BOOST_CHECK_CLOSE_FRACTION(pdf(ichisq10, 1),  0.00078975346316749169, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(pdf(ichisq10, 10), 0.0000000012385799798186384, tol4eps);

  BOOST_CHECK_CLOSE_FRACTION(mode(ichisq10), 0.0833333333333333333333333333333333333333, tol4eps);
  // nu * xi / nu + 2 = 10 * 0.1 / (10 + 2) = 1/12 =  0.0833333...
  // mode is not defined in Mathematica.
  // See Discussion section http://en.wikipedia.org/wiki/Talk:Scaled-inverse-chi-square_distribution
  // for origin of this formula.

  inverse_chi_squared_distribution<double> ichisq5(5.); // // Only df parameter specified.
  BOOST_CHECK_EQUAL(ichisq5.degrees_of_freedom(), 5.); // check  parameter stored.
  BOOST_CHECK_EQUAL(ichisq5.scale(), 1./5.); // check default is 1/df
  BOOST_CHECK_CLOSE_FRACTION(pdf(ichisq5, 0.2), 3.0510380337346841, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(cdf(ichisq5, 0.5), 0.84914503608460956, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(cdf(complement(ichisq5, 0.5)), 1 - 0.84914503608460956, tol4eps);

  BOOST_CHECK_CLOSE_FRACTION(quantile(ichisq5, 0.84914503608460956), 0.5, tol4eps*100);
  BOOST_CHECK_CLOSE_FRACTION(quantile(complement(ichisq5, 1. - 0.84914503608460956)), 0.5, tol4eps*100);

  // Check mean, etc spot values.
  inverse_chi_squared_distribution<double> ichisq81(8., 1.); // degrees_of_freedom = 5, scale = 1
  BOOST_CHECK_CLOSE_FRACTION(mean(ichisq81),1.33333333333333333333333333333333333333333, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(variance(ichisq81), 0.888888888888888888888888888888888888888888888, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(skewness(ichisq81), 2 * std::sqrt(8.), tol4eps);
  inverse_chi_squared_distribution<double> ichisq21(2., 1.);
  BOOST_CHECK_CLOSE_FRACTION(mode(ichisq21), 0.5, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(median(ichisq21), 1.4426950408889634, tol4eps);

  inverse_chi_squared ichsq4(4.); // Using typedef and degrees_of_freedom parameter (and default scale = 1/df).
  BOOST_CHECK_EQUAL(ichsq4.degrees_of_freedom(), 4.); // df == 4.
  BOOST_CHECK_EQUAL(ichsq4.scale(), 0.25); // scale  == 1 /df == 1/4.

  inverse_chi_squared ichsq32(3, 2);
  BOOST_CHECK_EQUAL(ichsq32.degrees_of_freedom(), 3.); // df == 3.
  BOOST_CHECK_EQUAL(ichsq32.scale(), 2); // scale  == 2

  inverse_chi_squared ichsq11(1, 1); // Using explicit degrees_of_freedom parameter, and default scale = 1).
  BOOST_CHECK_CLOSE_FRACTION(mode(ichsq11), 0.3333333333333333333333333333333333333333, tol4eps);
  // (1 * 1)/ (1 + 2) = 1/3 using Wikipedia nu * xi /(nu + 2)
  BOOST_CHECK_EQUAL(ichsq11.degrees_of_freedom(), 1.); // df == 1 (default).
  BOOST_CHECK_EQUAL(ichsq11.scale(), 1.); // scale == 1.
  /*
  // Used to find some 'exact' values for testing mean, variance ...
  // First with scale fixed at unity (Wikipedia definition 1)
  cout << "df      scale            mean            variance              sd              median" << endl;
  for (int degrees_of_freedom = 8; degrees_of_freedom < 30; degrees_of_freedom++)
  {
    inverse_chi_squared ichisq(degrees_of_freedom, 1);
    cout.precision(17);
    cout << degrees_of_freedom << "    "  << 1 << "  " << mean(ichisq) << ' '
      << variance(ichisq) << ' ' << standard_deviation(ichisq)
      << ' ' << median(ichisq) << endl;
  }

  // Default scale = 1 / df
  cout << "|\n" << "df           scale          mean            variance              sd              median" << endl;
  for (int degrees_of_freedom = 8; degrees_of_freedom < 30; degrees_of_freedom++)
  {
    inverse_chi_squared ichisq(degrees_of_freedom);
    cout.precision(17);
    cout << degrees_of_freedom << "    "  << 1./degrees_of_freedom << "  " << mean(ichisq) << ' '
      << variance(ichisq) << ' ' << standard_deviation(ichisq)
      << ' ' << median(ichisq) << endl;
  }
  */
  inverse_chi_squared_distribution<> ichisq14(14, 1); // Using default RealType double.
  BOOST_CHECK_CLOSE_FRACTION(mean(ichisq14), 1.166666666666666666666666666666666666666666666, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(variance(ichisq14), 0.272222222222222222222222222222222222222222222, tol4eps);

  inverse_chi_squared_distribution<> ichisq121(12); // Using default RealType double.
  BOOST_CHECK_CLOSE_FRACTION(mean(ichisq121),  0.1, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(variance(ichisq121), 0.0025, tol4eps);
  BOOST_CHECK_CLOSE_FRACTION(standard_deviation(ichisq121), 0.05, tol4eps);

  // and "using boost::math::inverse_chi_squared_distribution;".
  inverse_chi_squared_distribution<> ichsq23(2., 3.); // Using default RealType double.
  BOOST_CHECK_EQUAL(ichsq23.degrees_of_freedom(), 2.); //
  BOOST_CHECK_EQUAL(ichsq23.scale(), 3.); //
  BOOST_MATH_CHECK_THROW(mean(ichsq23), std::domain_error); // Degrees of freedom (nu) must be > 2
  BOOST_MATH_CHECK_THROW(variance(ichsq23), std::domain_error); // Degrees of freedom (nu) must be > 4
  BOOST_MATH_CHECK_THROW(skewness(ichsq23), std::domain_error); // Degrees of freedom (nu) must be > 6
  BOOST_MATH_CHECK_THROW(kurtosis_excess(ichsq23), std::domain_error); // Degrees of freedom (nu) must be > 8

  { // Check relationship between inverse gamma and inverse chi_squared distributions.
  using boost::math::inverse_gamma_distribution;

  double df = 2.;
  double scale = 1.;
  double alpha = df/2; // aka inv_gamma shape
  double beta = scale /2; // inv_gamma scale.

  inverse_gamma_distribution<> ig(alpha, beta);

  inverse_chi_squared_distribution<> ichsq(df, 1./df); // == default scale.
  BOOST_CHECK_EQUAL(pdf(ichsq, 0), 0); // Special case of zero x.

  double x = 0.5;
  BOOST_CHECK_EQUAL(pdf(ig, x), pdf(ichsq, x)); // inv_gamma compared to inv_chisq
  BOOST_CHECK_EQUAL(cdf(ichsq, 0), 0); // Special case of zero.
  BOOST_CHECK_EQUAL(cdf(ig, x), cdf(ichsq, x)); // invgamma == invchisq

  // Test pdf by comparing using naive_pdf with relation to inverse gamma distribution
  // wikipedia http://en.wikipedia.org/wiki/Scaled-inverse-chi-square_distribution related distributions.
  // So if naive_pdf is correct, inverse_chi_squared_distribution should agree.
  df = 1.; scale = 1.;
  BOOST_CHECK_CLOSE_FRACTION(naive_pdf(df, scale, x), pdf(ichsq11, x), tol_few_eps);

  //inverse_gamma_distribution<> igd(df/2, (df * scale)/2);
  inverse_gamma_distribution<> igd11(df/2, df * scale/2);
  BOOST_CHECK_CLOSE_FRACTION(naive_pdf(df, scale, x), pdf(igd11, x), tol_few_eps);
  BOOST_CHECK_CLOSE_FRACTION(naive_pdf(df, scale, x), pdf(ichsq11, x), tol_few_eps);

  df = 2; scale = 1;
  inverse_gamma_distribution<> igd21(df/2, df * scale/2);
  inverse_chi_squared_distribution<> ichsq21(df, scale);
  BOOST_CHECK_CLOSE_FRACTION(naive_pdf(df, scale, x), pdf(igd21, x), tol_few_eps); // 0.54134113294645081 OK
  BOOST_CHECK_CLOSE_FRACTION(naive_pdf(df, scale, x), pdf(ichsq21, x), tol_few_eps);

  df = 2; scale = 2;
  inverse_gamma_distribution<> igd22(df/2, df * scale/2);
  inverse_chi_squared_distribution<> ichsq22(df, scale);
  BOOST_CHECK_CLOSE_FRACTION(naive_pdf(df, scale, x), pdf(igd22, x), tol_few_eps);
  BOOST_CHECK_CLOSE_FRACTION(naive_pdf(df, scale, x), pdf(ichsq22, x), tol_few_eps);
  }

  // Check using float.
  inverse_chi_squared_distribution<float> igf23(1.f, 2.f); // Using explicit RealType float.
  BOOST_CHECK_EQUAL(igf23.degrees_of_freedom(), 1.f); //
  BOOST_CHECK_EQUAL(igf23.scale(), 2.f); //

  // Check throws from bad parameters.
  inverse_chi_squared ig051(0.5, 1.); // degrees_of_freedom < 1, so wrong for mean.
  BOOST_MATH_CHECK_THROW(mean(ig051), std::domain_error);
  inverse_chi_squared ig191(1.9999, 1.); // degrees_of_freedom < 2, so wrong for variance.
  BOOST_MATH_CHECK_THROW(variance(ig191), std::domain_error);
  inverse_chi_squared ig291(2.9999, 1.); // degrees_of_freedom < 3, so wrong for skewness.
  BOOST_MATH_CHECK_THROW(skewness(ig291), std::domain_error);
  inverse_chi_squared ig391(3.9999, 1.); // degrees_of_freedom < 1, so wrong for kurtosis and kurtosis_excess.
  BOOST_MATH_CHECK_THROW(kurtosis(ig391), std::domain_error);
  BOOST_MATH_CHECK_THROW(kurtosis_excess(ig391), std::domain_error);

  inverse_chi_squared ig102(10, 2); // Wolfram.com/ page 2, quantile = 2.96859.
  //http://reference.wolfram.com/mathematica/ref/InverseChiSquareDistribution.html
  BOOST_CHECK_CLOSE_FRACTION(quantile(ig102, 0.75), 2.96859, 0.000001);
  BOOST_CHECK_CLOSE_FRACTION(cdf(ig102, 2.96859), 0.75 , 0.000001);
  BOOST_CHECK_CLOSE_FRACTION(cdf(complement(ig102, 2.96859)), 1 - 0.75 , 0.00001);
  BOOST_CHECK_CLOSE_FRACTION(quantile(complement(ig102, 1 - 0.75)), 2.96859, 0.000001);

  // Basic sanity-check spot values.
  // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x0582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
  std::cout << "<note>The long double tests have been disabled on this platform "
    "either because the long double overloads of the usual math functions are "
    "not available at all, or because they are too inaccurate for these tests "
    "to pass.</note>" << std::endl;
#endif

 /*    */

} // BOOST_AUTO_TEST_CASE( test_main )

/*

Output:

*/


/*
 * Simulation of an ensemble of Roessler attractors
 *
 * Copyright 2014 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#include <iostream>
#include <vector>
#include <random>

#include <boost/timer.hpp>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

namespace odeint = boost::numeric::odeint;

typedef boost::timer timer_type;

typedef double fp_type;
//typedef float fp_type;

typedef boost::array<fp_type, 3> state_type;
typedef std::vector<state_type> state_vec;

//---------------------------------------------------------------------------
struct roessler_system {
    const fp_type m_a, m_b, m_c;

    roessler_system(const fp_type a, const fp_type b, const fp_type c)
        : m_a(a), m_b(b), m_c(c)
    {}

    void operator()(const state_type &x, state_type &dxdt, const fp_type t) const
    {
        dxdt[0] = -x[1] - x[2];
        dxdt[1] = x[0] + m_a * x[1];
        dxdt[2] = m_b + x[2] * (x[0] - m_c);
    }
};

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
if(argc<3)
{
    std::cerr << "Expected size and steps as parameter" << std::endl;
    exit(1);
}
const size_t n = atoi(argv[1]);
const size_t steps = atoi(argv[2]);
//const size_t steps = 50;

const fp_type dt = 0.01;

const fp_type a = 0.2;
const fp_type b = 1.0;
const fp_type c = 9.0;

// random initial conditions on the device
std::vector<fp_type> x(n), y(n), z(n);
std::default_random_engine generator;
std::uniform_real_distribution<fp_type> distribution_xy(-8.0, 8.0);
std::uniform_real_distribution<fp_type> distribution_z(0.0, 20.0);
auto rand_xy = std::bind(distribution_xy, std::ref(generator));
auto rand_z = std::bind(distribution_z, std::ref(generator));
std::generate(x.begin(), x.end(), rand_xy);
std::generate(y.begin(), y.end(), rand_xy);
std::generate(z.begin(), z.end(), rand_z);

state_vec state(n);
for(size_t i=0; i<n; ++i)
{
    state[i][0] = x[i];
    state[i][1] = y[i];
    state[i][2] = z[i];
}

std::cout.precision(16);

std::cout << "# n: " << n << std::endl;

std::cout << x[0] << std::endl;

// Stepper type - use never_resizer for slight performance improvement
odeint::runge_kutta4_classic<state_type, fp_type, state_type, fp_type,
                             odeint::array_algebra,
                             odeint::default_operations,
                             odeint::never_resizer> stepper;

roessler_system sys(a, b, c);

timer_type timer;

fp_type t = 0.0;

for (int step = 0; step < steps; step++)
{
    for(size_t i=0; i<n; ++i)
    {
        stepper.do_step(sys, state[i], t, dt);
    }
    t += dt;
}

std::cout << "Integration finished, runtime for " << steps << " steps: ";
std::cout << timer.elapsed() << " s" << std::endl;

// compute some accumulation to make sure all results have been computed
fp_type s = 0.0;
for(size_t i = 0; i < n; ++i)
{
    s += state[i][0];
}

std::cout << state[0][0] << std::endl;
std::cout << s/n << std::endl;

}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
#define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

/**
 * @brief Box QP solution
 *
 * It contains the Box QP solution data which consists of
 *  - the inverse of the free space Hessian
 *  - the optimal decision vector
 *  - the indexes for the free space
 *  - the indexes for the clamped (constrained) space
 */
struct BoxQPSolution {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the QP solution structure
   */
  BoxQPSolution() {}

  /**
   * @brief Initialize the QP solution structure
   *
   * @param[in] Hff_inv      Inverse of the free space Hessian
   * @param[in] x            Decision vector
   * @param[in] free_idx     Free space indexes
   * @param[in] clamped_idx  Clamped space indexes
   */
  BoxQPSolution(const Eigen::MatrixXd& Hff_inv, const Eigen::VectorXd& x, const std::vector<size_t>& free_idx,
                const std::vector<size_t>& clamped_idx)
      : Hff_inv(Hff_inv), x(x), free_idx(free_idx), clamped_idx(clamped_idx) {}

  Eigen::MatrixXd Hff_inv;          //!< Inverse of the free space Hessian
  Eigen::VectorXd x;                //!< Decision vector
  std::vector<size_t> free_idx;     //!< Free space indexes
  std::vector<size_t> clamped_idx;  //!< Clamped space indexes
};

/**
 * @brief This class implements a Box QP solver based on a Projected Newton method.
 *
 * We consider a box QP problem of the form:
 * \f{eqnarray*}{
 *   \min_{\mathbf{x}} &= \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{q}^T\mathbf{x} \\
 *   \textrm{subject to} & \hspace{1em} \mathbf{\underline{b}} \leq \mathbf{x} \leq \mathbf{\bar{b}} \\
 * \f}
 * where \f$\mathbf{H}\f$, \f$\mathbf{q}\f$ are the Hessian and gradient of the problem,
 * respectively, \f$\mathbf{\underline{b}}\f$, \f$\mathbf{\bar{b}}\f$ are lower and upper
 * bounds of the decision variable \f$\mathbf{x}\f$.
 *
 * The algorithm procees by iteratively identifying the active bounds, and then
 * performing a projected Newton step in the free sub-space.
 * The projection uses the Hessian of the free sub-space and is computed
 * efficiently using a Cholesky decomposition.
 * It uses a line search procedure with polynomial step length values in a
 * backtracking fashion.
 * The steps are checked using an Armijo condition together L2-norm gradient.
 *
 * For more details about this solver, we encourage you to read the following
 * article:
 * \include bertsekas-siam82.bib
 */
class BoxQP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the Projected-Newton QP for bound constraints
   *
   * @param[in] nx             Dimension of the decision vector
   * @param[in] maxiter        Maximum number of allowed iterations (default 100)
   * @param[in] th_acceptstep  Acceptance step threshold (default 0.1)
   * @param[in] th_grad        Gradient tolerance threshold (default 1e-9)
   * @param[in] reg            Regularization value (default 1e-9)
   */
  BoxQP(const std::size_t nx, const std::size_t maxiter = 100, const double th_acceptstep = 0.1,
        const double th_grad = 1e-9, const double reg = 1e-9);
  /**
   * @brief Destroy the Projected-Newton QP solver
   */
  ~BoxQP();

  /**
   * @brief Compute the solution of bound-constrained QP based on Newton projection
   *
   * @param[in] H      Hessian (dimension nx * nx)
   * @param[in] q      Gradient (dimension nx)
   * @param[in] lb     Lower bound (dimension nx)
   * @param[in] ub     Upper bound (dimension nx)
   * @param[in] xinit  Initial guess (dimension nx)
   * @return The solution of the problem
   */
  const BoxQPSolution& solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& lb,
                             const Eigen::VectorXd& ub, const Eigen::VectorXd& xinit);

  /**
   * @brief Return the stored solution
   */
  const BoxQPSolution& get_solution() const;

  /**
   * @brief Return the decision vector dimension
   */
  std::size_t get_nx() const;

  /**
   * @brief Return the maximum allowed number of iterations
   */
  std::size_t get_maxiter() const;

  /**
   * @brief Return the acceptance step threshold
   */
  double get_th_acceptstep() const;

  /**
   * @brief Return the gradient tolerance threshold
   */
  double get_th_grad() const;

  /**
   * @brief Return the regularization value
   */
  double get_reg() const;

  /**
   * @brief Return the stack of step lengths using by the line-search procedure
   */
  const std::vector<double>& get_alphas() const;

  /**
   * @brief Modify the decision vector dimension
   */
  void set_nx(const std::size_t nx);

  /**
   * @brief Modify the maximum allowed number of iterations
   */
  void set_maxiter(const std::size_t maxiter);

  /**
   * @brief Modify the acceptance step threshold
   */
  void set_th_acceptstep(const double th_acceptstep);

  /**
   * @brief Modify the gradient tolerance threshold
   */
  void set_th_grad(const double th_grad);

  /**
   * @brief Modify the regularization value
   */
  void set_reg(const double reg);

  /**
   * @brief Modify the stack of step lengths using by the line-search procedure
   */
  void set_alphas(const std::vector<double>& alphas);

 private:
  std::size_t nx_;          //!< Decision variable dimension
  BoxQPSolution solution_;  //!< Solution of the Box QP
  std::size_t maxiter_;     //!< Allowed maximum number of iterations
  double th_acceptstep_;    //!< Threshold used for accepting step
  double th_grad_;          //!< Tolerance for stopping the algorithm (gradient threshold)
  double reg_;              //!< Current regularization value

  double fold_;                 //!< Cost of previous iteration
  double fnew_;                 //!< Cost of current iteration
  std::size_t nf_;              //!< Free space dimension
  std::size_t nc_;              //!< Constrained space dimension
  std::vector<double> alphas_;  //!< Set of step lengths using by the line-search procedure
  Eigen::VectorXd x_;           //!< Guess of the decision variable
  Eigen::VectorXd xnew_;        //!< New decision variable guess
  Eigen::VectorXd g_;           //!< Current gradient
  Eigen::VectorXd dx_;          //!< Current search direction

  Eigen::VectorXd qf_;                       //!< Current problem gradient in the free subspace
  Eigen::VectorXd xf_;                       //!< Current decision variable in the free subspace
  Eigen::VectorXd xc_;                       //!< Current decision variable in the constrained subspace
  Eigen::VectorXd dxf_;                      //!< Search direction in the free subspace
  Eigen::MatrixXd Hff_;                      //!< Hessian in the free subspace
  Eigen::MatrixXd Hfc_;                      //!< Hessian in the constrained subspace
  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt_;  //!< Cholesky solver
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_