#include <vector>
#include <boost/math/distributions/normal.hpp>
#include "normal_dist.h"

stochastic::NormalDistribution::NormalDistribution(double mean, double std_dev)
  : Distribution(),
    mean_{mean},
    std_dev_{std_dev},
    distribution_{mean, std_dev_}
{}

std::vector<double> stochastic::NormalDistribution::cumulative_dist_func(
    const std::vector<double>& locations) const {
  std::vector<double> evaluations(locations.size());

  for (unsigned int i = 0; i < locations.size(); ++i) {
    evaluations[i] = cdf(distribution_, locations[i]);
  }

  return evaluations;
}

std::vector<double> stochastic::NormalDistribution::inv_cumulative_dist_func(
    const std::vector<double>& probabilities) const {
  std::vector<double> evaluations(probabilities.size());

  for (unsigned int i = 0; i < probabilities.size(); ++i) {
    evaluations[i] = quantile(distribution_, probabilities[i]);
  }

  return evaluations;
}


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


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


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


#include <bspline_fiting.hh>
#include <Geo/iterate.hh>

#pragma warning( push )
#pragma warning( disable : 4714 )
#include <Eigen/Dense>

#include <fstream>
#include <vector>

namespace Geo {

template<size_t dimT>
struct BsplineFitting : public IBsplineFitting<dimT>
{
  bool init(const size_t _deg,
    const std::vector<double>& _knots, const IFunction& _f)
  {
    if (_knots.empty())
      return false;
    auto extn = (_knots.back() - _knots.front()) / 2;
    knots_.push_back(_knots.front() - extn);
    knots_.insert(knots_.end(), _knots.begin(), _knots.end());
    knots_.push_back(_knots.back() + extn);
    deg_ = _deg;
    f_ = &_f;
    X_.clear();
    A_.clear();
    B_.clear();
    return true;
  }

  void set_parameter_correction_iterations(size_t _itr_nmbr) { itr_nmbr_ = _itr_nmbr; }
  void set_favour_boundaries(const bool _fvr_bndr) { fvr_bndr_ = _fvr_bndr; }
  void set_samples_per_interval(const size_t _smpl_per_intrvl)
  {
    smpl_per_intrvl_ = _smpl_per_intrvl;
  }

  void compute();
  const std::vector<VectorD<dimT>>& X() const { return X_; }
  VectorD<dimT> eval(const double _t);
private:
  void find_equations();
  double N(size_t _i, const size_t _k, const double _t);
  void add_equation(const double _t, const double _wi);

  std::vector<std::vector<double>> A_;
  std::vector<double> knots_;
  std::vector<VectorD<dimT>> B_;
  std::vector<VectorD<dimT>> X_;
  size_t deg_ = 0;
  const IFunction* f_ = nullptr;
  size_t itr_nmbr_ = 0;
  bool fvr_bndr_ = true;
  size_t smpl_per_intrvl_ = 4;
};

template<size_t dimT>
double BsplineFitting<dimT>::N(size_t _i, const size_t _p, const double _t)
{
  if (_p == 0)
  {
    if (_t < knots_[_i] || _t >= knots_[_i + 1])
      return 0;
    return 1;
  }
  double res = 0;

  auto b = N(_i, _p - 1, _t);
  if (b != 0)
    res += b * (_t - knots_[_i]) / (knots_[_i + _p] - knots_[_i]);

  b = N(_i + 1, _p - 1, _t);
  if (b != 0)
  {
    auto end_kn = knots_[_i + _p + 1];
    res += b * (end_kn - _t) / (end_kn - knots_[_i + 1]);
  }
  return res;
}

template<size_t dimT>
void BsplineFitting<dimT>::add_equation(const double _t, const double _wi)
{
  A_.emplace_back();
  const auto wi_sqr = sqrt(_wi);
  for (int j = 0; j < knots_.size() - deg_ - 1; ++j)
    A_.back().push_back(N(j, deg_, _t) * wi_sqr);

  VectorD<dimT> pt_crv;
  if (X_.empty() || (_t == knots_[1]) || (_t == knots_[knots_.size() - 2]))
    pt_crv = f_->evaluate(_t);
  else
    pt_crv = f_->closest_point(eval(_t), _t);
  B_.emplace_back(pt_crv * wi_sqr);
};

template<size_t dimT>
void BsplineFitting<dimT>::find_equations()
{
  A_.clear();
  B_.clear();
  if (fvr_bndr_)
  {
    double w_prev = 0;
    const auto last_idx = knots_.size() - 2;
    for (size_t i = 2; i <= last_idx; ++i)
    {
      auto dw = knots_[i] - knots_[i - 1];
      if (dw <= 0)
        continue;
      const double step = 1. / smpl_per_intrvl_;
      const double w_step = dw * step;
      auto wi = w_prev + w_step / 2;
      for (double x = 0; x < 1; x += step)
      {
        auto t = knots_[i - 1] * (1 - x) + knots_[i] * x;
        add_equation(t, wi);
        wi = w_step;
      }
      w_prev = w_step / 2;
    }
    add_equation(knots_[last_idx], w_prev);
  }
  else
  {
    const auto last_idx = knots_.size() - 2;
    for (size_t i = 2; i <= last_idx; ++i)
    {
      auto dw = knots_[i] - knots_[i - 1];
      if (dw <= 0)
        continue;
      const double step = 1. / smpl_per_intrvl_;
      const double wi = dw * step;
      for (double x = step / 2; x < 1; x += step)
        add_equation(knots_[i - 1] * (1 - x) + knots_[i] * x, wi);
    }
  }
}

template<size_t dimT>
void BsplineFitting<dimT>::compute()
{
  for (size_t iter = 0; iter <= itr_nmbr_; ++iter)
  {
    find_equations();
    const auto row_nmbr = A_.size();
    const auto col_nmbr = A_.front().size();
    Eigen::MatrixXd A(row_nmbr, col_nmbr);
    for (int i = 0; i < row_nmbr; ++i)
    {
      for (int j = 0; j < col_nmbr; ++j)
        A(i, j) = A_[i][j];
    }
    const auto& jsvd =
      A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
    for (int j = 0; j < B_[0].size(); ++j)
    {
      Eigen::VectorXd B(B_.size());
      for (int i = 0; i < row_nmbr; ++i)
        B(i) = B_[i][j];
      Eigen::VectorXd res = jsvd.solve(B);
      X_.resize(col_nmbr);
      for (int i = 0; i < col_nmbr; ++i)
        X_[i][j] = res(i);
    }
  }
}

template<size_t dimT>
VectorD<dimT> BsplineFitting<dimT>::eval(const double _t)
{
  VectorD<dimT> res = { 0 };
  for (int i = 0; i < X_.size(); ++i)
    res += N(i, deg_, _t) * X_[i];
  return res;
}

template<size_t dimT>
std::shared_ptr<IBsplineFitting<dimT>> IBsplineFitting<dimT>::make()
{
  return std::make_shared<BsplineFitting<dimT>>();
}

template struct IBsplineFitting<2>;
template struct IBsplineFitting<3>;

}//namespace Geo


// --------------------------------------------------------------
// SL 2019-02-25
// A simple, easily hackable topopt code, directly inspired
// from "A 99 line topology optimization code written in MATLAB"
// MIT-license
// (c) Sylvain Lefebvre, https://github.com/sylefeb
// --------------------------------------------------------------
/*

This code is directly inspired from the fantastic paper

A 99 line topology optimization code written in MATLAB
Structural and Multidisciplinary Optimization 21(2), 2001, pp. 120-127
by Ole Sigmund.

http://www.topopt.mek.dtu.dk/Apps-and-software/A-99-line-topology-optimization-code-written-in-MATLAB

I simply re-implemented it in C/C++ using LibSL-small and Eigen.
And yes, it takes more than 99 lines of C/C++ ;-)

Notations
---------

- dens : optimized densities
- nelx : number of elements along X axis (nelx+1 corners)
- nely : number of elements along Y axis (nely+1 corners)
- dc   : compliance gradient
- KE   : stiffness matrix for a single element

*/
// --------------------------------------------------------------

#include <LibSL/LibSL.h>
#include <Eigen/Sparse>

#include <iostream>
#include <ctime>
#include <cmath>
#include <set>
#include <limits>

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

using namespace std;

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

LIBSL_WIN32_FIX;

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

// stiffness matrix for a unit square element
Array2D<double> KE;

// Computes the stiffness matrix of a single, unit square element
// (forward declaration, code at the end)
void lk(Array2D<double>& _KE);

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

// matrix vector multiply
template <typename T>
void mv_mul(const Array2D<T>& A, const Array<T>& v, Array<T>& _res)
{
  _res.allocate(v.size());
  ForIndex(l, A.ysize()) {
    T al = 0;
    ForIndex(c, A.xsize()) {
      al = al + A.at(c, l) * v[c];
    }
    _res[l] = al;
  }
}

// vector-vector multiply
template <typename T>
T vv_mul(const Array<T>& a, const Array<T>& b)
{
  T res = 0;
  ForIndex(i, a.size()) {
    res = res + a[i] * b[i];
  }
  return res;
}

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

// Applies optimality criterion (OC) to update densities (x) from compliance gradient (dc).
// Maintains the volume fraction
void OC(int nelx, int nely, Array2D<double>& _dens, double volfrac, const Array2D<double>& dc)
{
  double move = 0.2;
  Array2D<double> dens_new(_dens.xsize(), _dens.ysize());
  double vtot = 0.0;
  double l1 = 0, l2 = 100000;
  while (l2 - l1 > 1e-3) { // bisection search for volume preservation
    double lmid = 0.5*(l2 + l1);
    vtot = 0.0;
    ForArray2D(dens_new, c, l) {
      // OC term
      double Be = (-dc.at(c, l)) / lmid;
      dens_new.at(c, l) = max(0.02,
        max(_dens.at(c, l) - move,
          min(1.0,
            min(_dens.at(c, l) + move, _dens.at(c, l) * sqrt(Be))
          )
        )
      );
      vtot += dens_new.at(c, l);
    }
    if (vtot - volfrac * nelx * nely > 0.0) {
      l1 = lmid;
    } else {
      l2 = lmid;
    }
  }
  cerr << "Vol frac in result  " << vtot / (nelx* nely) << endl;
  _dens = dens_new;
}

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

// Filters the result to prevent the 'checkerboard effect' due to the solver
// attempting to produce infinitely small 'bubbles' (composite).
void filter(int nelx, int nely, double rmin, const Array2D<double>& dens, Array2D<double>& _dc)
{
  Array2D<double> dcn(nelx, nely);
  dcn.fill(0);
  ForArray2D(dcn, i, j) {
    double sum = 0.0;
    ForRange(l, max(j - round(rmin), 0), min(j + round(rmin), nely - 1)) {
      ForRange(k, max(i - round(rmin), 0), min(i + round(rmin), nelx - 1)) {
        double fac = rmin - sqrt((double)(i - k)*(i - k) + (double)(j - l)*(j - l));
        sum += max(0.0f, fac);
        dcn.at(i, j) = dcn.at(i, j) + max(0.0f, fac) * dens.at(k, l) * _dc.at(k, l);
      }
    }
    dcn.at(i, j) = dcn.at(i, j) / (dens.at(i, j)*sum);
  }
  _dc = dcn;
}

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

// Straightforward linear finite element solver
// The degrees of freedom (variables) are the corners of the elements, times two (x and y coordinates)
// Each corner at (cx,cy) is associated to two ids:
//   - grid id computed cx + (nelx + 1) * cy
//   - variable id which identifies it in the sparse system of equations
// The x/y coordinates are at respectively id*2+0 and id*2+1
void FE(int nelx, int nely, const Array2D<double>& dens, double penal, Array2D<v2f>& _U)
{
  // variables to lock: there are attachement points of the structure
  set<int> locked;
  // -> here we attach the left third of the bottom row (x in  [0,(nelx + 1) / 3 - 1] , y = nely
  ForIndex(x, (nelx + 1) / 3) {
    locked.insert((x + (nelx + 1) * nely) * 2 + 0); // x
    locked.insert((x + (nelx + 1) * nely) * 2 + 1); // y
  }
  // mapping between variable and ids
  Array<int>   grid2var(2 * (nelx + 1)*(nely + 1));
  Array<int>   var2grid(2 * (nelx + 1)*(nely + 1) - (int)locked.size());
  grid2var.fill(-1);
  var2grid.fill(-1);
  int varid = 0;
  ForIndex(gridid, 2 * (nelx + 1)*(nely + 1)) {
    if (locked.find(gridid) == locked.end()) {
      grid2var[gridid] = varid;
      var2grid[varid]  = gridid;
      varid++;
    }
  }
  sl_assert(varid == 2 * (nelx + 1)*(nely + 1) - (int)locked.size());
  // matrix A (FE equations)
  std::vector<Eigen::Triplet<double> > coefficients;
  ForIndex(x, nelx) {
    ForIndex(y, nely) {
      int p00 = (x + (y)* (nelx + 1));
      int p01 = (x + (y + 1) * (nelx + 1));
      int p11 = ((x + 1) + (y + 1) * (nelx + 1));
      int p10 = ((x + 1) + (y)* (nelx + 1));
      Array<int> corners(8);
      corners[0] = p00 * 2 + 0;
      corners[1] = p00 * 2 + 1;
      corners[2] = p10 * 2 + 0;
      corners[3] = p10 * 2 + 1;
      corners[4] = p11 * 2 + 0;
      corners[5] = p11 * 2 + 1;
      corners[6] = p01 * 2 + 0;
      corners[7] = p01 * 2 + 1;
      double pow_x = pow(dens.at(x, y), penal);
      // add coefficients only for non-locked variables
      ForIndex(k, 8) {
        if (grid2var[corners[k]] > -1) {
          ForIndex(l, 8) {
            if (grid2var[corners[l]] > -1) {
              coefficients.push_back(Eigen::Triplet<double>(grid2var[corners[k]], grid2var[corners[l]],
                KE.at(l, k) * pow_x
                ));
            }
          }
        }
      }
    }
  }
  Eigen::SparseMatrix<double> A(
    2 * (nelx + 1)*(nely + 1) - locked.size(),
    2 * (nelx + 1)*(nely + 1) - locked.size());
  A.setFromTriplets(coefficients.begin(), coefficients.end());
  // vector b, contains external forces
  Eigen::VectorXd b = Eigen::VectorXd(2 * (nelx + 1)*(nely + 1) - locked.size());
  ForIndex(i, b.size()) {
    b[i] = 0.0;
  }
  // -> here we apply external forces at two specific points (near the image top)
  b[grid2var[(nelx / 2     + 24 * (nelx + 1)) * 2 + 1]] = -1;
  b[grid2var[(nelx * 5 / 6 + 16 * (nelx + 1)) * 2 + 1]] = -1;
  //                                         ^^^^^^^ force in y direction
  // solver
  cerr << "solving ...";
  Eigen::SparseLU<Eigen::SparseMatrix<double> > solver(A);
  Eigen::VectorXd result = solver.solve(b);
  cerr << " done.\n";
  // store computed displacement
  _U.allocate(nelx + 1, nely + 1);
  ForIndex(varid, result.size()) {
    int gridid = var2grid[varid];
    int x = (gridid / 2) % (nelx + 1);
    int y = (gridid / 2) / (nelx + 1);
    int c = (gridid & 1);
    _U.at(x, y)[c] = (float)result[varid];
  }
}

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

// Executes the global optimization loop until 'convergence' (change below threshold)
// volfrac is the target volume fraction in [0-1]
// penal   is the density penality (typically 3)
// rmin    controls the filter size
void topopt(int nelx, int nely, double volfrac, double penal, double rmin)
{
  ImageFloat1     img(nelx, nely);  // for image output
  Array2D<double> dens(nelx, nely); // optimized density grid
  Array2D<double> dens_old;         // result from previous iteration
  Array2D<double> dc(nelx, nely);   // compliance gradient

  // initialization with a random field of selected volume fraction (volfrac)
  double tot = 0;
  ForArray2D(dens, i, j) {
    dens.at(i, j) = rnd();
    tot += dens.at(i, j);
  }
  tot = tot / (nelx*nely);
  ForArray2D(dens, i, j) {
    dens.at(i, j) = volfrac * dens.at(i, j) / tot;
  }
  int loop = 0;
  double change = 1;
  while (change > 0.01) {

    loop++;
    dens_old = dens;

    // solve for displacement (finite element solution)
    Array2D<v2f> U;
    FE(nelx, nely, dens, penal, /*out*/ U);

    // option to output displacement magnitudes
    if (0) {
      ImageFloat1 img;
      img.pixels().allocate(nelx + 1, nely + 1);
      ForImage((&img), i, j) {
        img.pixel(i, j) = length(U.at(i, j));
      }
      img.remap(0.0f, 255.0f);
      saveImage(sprint("%03d_FE.tga", loop), img.cast<ImageL8>());
    }

    // compute compliance and gradient
    // -> for each element
    double c_comp = 0.0;
    ForIndex(ely, nely) {
      ForIndex(elx, nelx) {
        // get displacement for the element
        Array<double> corners(8);
        corners[0] = U.at(elx, ely)[0];
        corners[1] = U.at(elx, ely)[1];
        corners[2] = U.at(elx + 1, ely)[0];
        corners[3] = U.at(elx + 1, ely)[1];
        corners[4] = U.at(elx + 1, ely + 1)[0];
        corners[5] = U.at(elx + 1, ely + 1)[1];
        corners[6] = U.at(elx, ely + 1)[0];
        corners[7] = U.at(elx, ely + 1)[1];
        // compute compliance
        Array<double> KE_Ue;
        mv_mul(KE, corners, /*out*/KE_Ue);
        v2d Ue_KE_Ue = vv_mul(KE_Ue, corners);
        double compliance = Ue_KE_Ue[0] + Ue_KE_Ue[1];
        double comp       = pow(dens.at(elx, ely), penal) * compliance;
        c_comp += comp;
        // gradient
        double dcomp    = -penal * pow(dens.at(elx, ely), penal - 1.0f) * compliance;
        dc.at(elx, ely) = dcomp;
      }
    }

    // filtering
    filter(nelx, nely, rmin, dens, dc/*out*/);

    // OC method
    OC(nelx, nely, dens/*out*/, volfrac, dc);

#if 0
    // For fun: kills a circle of density (obstacle)
    ForArray2D(dens, i, j) {
      if ( length(v2f((float)i, (float)j) - v2f((float)nelx/2, (float)nely/2)) < nelx/6.0f) {
        dens.at(i, j) = 0.01;
      }
    }
#endif

    // compute max change
    change = 0.0;
    ForArray2D(dens, i, j) {
      change = max(change, abs(dens.at(i, j) - dens_old.at(i, j)));
    }
    // output iteration stats
    cerr << sprint(" Loop %d, compliance = %f, change = %f\n", loop, c_comp, change);
    // output image
    ForImage((&img), i, j) {
      img.pixel(i,j) = 255.0f * (1.0f - (float)pow(dens.at(i,j),penal));
    }
    saveImage(sprint("%03d_struct.tga", loop), img.cast<ImageL8>());

  }

}

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

// Program entry point.
int main(int argc, char **argv)
{
  try {

    // generate stiffness matrix for unit element
    lk(KE);

    // go ahead!
    topopt(256, 96, 0.3f, 3.0f, 1.2f);

  } catch (Fatal& e) {
    cerr << Console::red << e.message() << Console::gray << endl;
    return (-1);
  }

  return (0);
}

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

// Computes the stiffness matrix of a single, unit square element
void lk(Array2D<double>& _KE)
{
  double E = 1.0f;
  double nu = 0.3f;
  double k[8];
  k[0] = 1.0f / 2.0 - nu / 6.0;
  k[1] = 1.0f / 8.0 + nu / 8.0;
  k[2] = -1.0f / 4.0 - nu / 12.0;
  k[3] = -1.0f / 8.0 + 3.0*nu / 8.0;
  k[4] = -1.0f / 4.0 + nu / 12.0;
  k[5] = -1.0f / 8.0 - nu / 8.0;
  k[6] = nu / 6.0;
  k[7] = 1.0f / 8.0 - 3.0*nu / 8.0;
  _KE.allocate(8, 8);

  _KE.at(0, 0) = E / (1.0 - nu * nu) * k[0];
  _KE.at(1, 0) = E / (1.0 - nu * nu) * k[1];
  _KE.at(2, 0) = E / (1.0 - nu * nu) * k[2];
  _KE.at(3, 0) = E / (1.0 - nu * nu) * k[3];
  _KE.at(4, 0) = E / (1.0 - nu * nu) * k[4];
  _KE.at(5, 0) = E / (1.0 - nu * nu) * k[5];
  _KE.at(6, 0) = E / (1.0 - nu * nu) * k[6];
  _KE.at(7, 0) = E / (1.0 - nu * nu) * k[7];

  _KE.at(0, 1) = E / (1.0 - nu * nu) * k[1];
  _KE.at(1, 1) = E / (1.0 - nu * nu) * k[0];
  _KE.at(2, 1) = E / (1.0 - nu * nu) * k[7];
  _KE.at(3, 1) = E / (1.0 - nu * nu) * k[6];
  _KE.at(4, 1) = E / (1.0 - nu * nu) * k[5];
  _KE.at(5, 1) = E / (1.0 - nu * nu) * k[4];
  _KE.at(6, 1) = E / (1.0 - nu * nu) * k[3];
  _KE.at(7, 1) = E / (1.0 - nu * nu) * k[2];

  _KE.at(0, 2) = E / (1.0 - nu * nu) * k[2];
  _KE.at(1, 2) = E / (1.0 - nu * nu) * k[7];
  _KE.at(2, 2) = E / (1.0 - nu * nu) * k[0];
  _KE.at(3, 2) = E / (1.0 - nu * nu) * k[5];
  _KE.at(4, 2) = E / (1.0 - nu * nu) * k[6];
  _KE.at(5, 2) = E / (1.0 - nu * nu) * k[3];
  _KE.at(6, 2) = E / (1.0 - nu * nu) * k[4];
  _KE.at(7, 2) = E / (1.0 - nu * nu) * k[1];

  _KE.at(0, 3) = E / (1.0 - nu * nu) * k[3];
  _KE.at(1, 3) = E / (1.0 - nu * nu) * k[6];
  _KE.at(2, 3) = E / (1.0 - nu * nu) * k[5];
  _KE.at(3, 3) = E / (1.0 - nu * nu) * k[0];
  _KE.at(4, 3) = E / (1.0 - nu * nu) * k[7];
  _KE.at(5, 3) = E / (1.0 - nu * nu) * k[2];
  _KE.at(6, 3) = E / (1.0 - nu * nu) * k[1];
  _KE.at(7, 3) = E / (1.0 - nu * nu) * k[4];

  _KE.at(0, 4) = E / (1.0 - nu * nu) * k[4];
  _KE.at(1, 4) = E / (1.0 - nu * nu) * k[5];
  _KE.at(2, 4) = E / (1.0 - nu * nu) * k[6];
  _KE.at(3, 4) = E / (1.0 - nu * nu) * k[7];
  _KE.at(4, 4) = E / (1.0 - nu * nu) * k[0];
  _KE.at(5, 4) = E / (1.0 - nu * nu) * k[1];
  _KE.at(6, 4) = E / (1.0 - nu * nu) * k[2];
  _KE.at(7, 4) = E / (1.0 - nu * nu) * k[3];

  _KE.at(0, 5) = E / (1.0 - nu * nu) * k[5];
  _KE.at(1, 5) = E / (1.0 - nu * nu) * k[4];
  _KE.at(2, 5) = E / (1.0 - nu * nu) * k[3];
  _KE.at(3, 5) = E / (1.0 - nu * nu) * k[2];
  _KE.at(4, 5) = E / (1.0 - nu * nu) * k[1];
  _KE.at(5, 5) = E / (1.0 - nu * nu) * k[0];
  _KE.at(6, 5) = E / (1.0 - nu * nu) * k[7];
  _KE.at(7, 5) = E / (1.0 - nu * nu) * k[6];

  _KE.at(0, 6) = E / (1.0 - nu * nu) * k[6];
  _KE.at(1, 6) = E / (1.0 - nu * nu) * k[3];
  _KE.at(2, 6) = E / (1.0 - nu * nu) * k[4];
  _KE.at(3, 6) = E / (1.0 - nu * nu) * k[1];
  _KE.at(4, 6) = E / (1.0 - nu * nu) * k[2];
  _KE.at(5, 6) = E / (1.0 - nu * nu) * k[7];
  _KE.at(6, 6) = E / (1.0 - nu * nu) * k[0];
  _KE.at(7, 6) = E / (1.0 - nu * nu) * k[5];

  _KE.at(0, 7) = E / (1.0 - nu * nu) * k[7];
  _KE.at(1, 7) = E / (1.0 - nu * nu) * k[2];
  _KE.at(2, 7) = E / (1.0 - nu * nu) * k[1];
  _KE.at(3, 7) = E / (1.0 - nu * nu) * k[4];
  _KE.at(4, 7) = E / (1.0 - nu * nu) * k[3];
  _KE.at(5, 7) = E / (1.0 - nu * nu) * k[6];
  _KE.at(6, 7) = E / (1.0 - nu * nu) * k[5];
  _KE.at(7, 7) = E / (1.0 - nu * nu) * k[0];
}

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


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

// Continuous version, and with polytope constraints.

#include "gurobi_c++.h"
#include <sstream>
#include <Eigen/Dense>
#include <type_traits>
using namespace std;

template <typename T>
GRBQuadExpr GetNorm2(const std::vector<T>& x)  // Return the squared norm of a vector
{
  GRBQuadExpr result = 0;
  for (int i = 0; i < x.size(); i++)
  {
    result = result + x[i] * x[i];
  }
  return result;
}

std::vector<GRBLinExpr> MatrixMultiply(const std::vector<std::vector<double>>& A, const std::vector<GRBVar>& x)
{
  std::vector<GRBLinExpr> result;

  for (int i = 0; i < A.size(); i++)
  {
    GRBLinExpr lin_exp = 0;
    for (int m = 0; m < x.size(); m++)
    {
      lin_exp = lin_exp + A[i][m] * x[m];
    }
    result.push_back(lin_exp);
  }
  return result;
}

std::vector<GRBLinExpr> MatrixMultiply(const std::vector<std::vector<double>>& A, const std::vector<GRBLinExpr>& x)
{
  std::vector<GRBLinExpr> result;

  for (int i = 0; i < A.size(); i++)
  {
    GRBLinExpr lin_exp = 0;
    for (int m = 0; m < x.size(); m++)
    {
      lin_exp = lin_exp + A[i][m] * x[m];
    }
    result.push_back(lin_exp);
  }
  return result;
}

template <typename T>  // Overload + to sum Elementwise std::vectors
std::vector<T> operator+(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::plus<T>());
  return result;
}

template <typename T>  // Overload - to substract Elementwise std::vectors
std::vector<T> operator-(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::minus<T>());
  return result;
}

std::vector<GRBLinExpr> operator-(const std::vector<GRBVar>& x, const std::vector<double>& b)
{
  std::vector<GRBLinExpr> result;
  for (int i = 0; i < x.size(); i++)
  {
    GRBLinExpr tmp = x[i] - b[i];
    result.push_back(tmp);
  }
  return result;
}

template <typename T>
std::vector<T> eigenVector2std(const Eigen::Matrix<T, -1, 1>& x)
{
  std::vector<T> result = 0;
  for (int i = 0; i < x.rows(); i++)
  {
    result.push_back(x(i, 1));
  }
  return result;
}

template <typename T>
std::vector<std::vector<T>> eigenMatrix2std(const Eigen::Matrix<T, -1, -1>& x)
{
  std::vector<std::vector<T>> result;

  for (int i = 0; i < x.rows(); i++)
  {
    std::vector<T> row;
    for (int j = 0; j < x.cols(); j++)
    {
      row.push_back(x(i, j));
    }
    result.push_back(row);
  }
  return result;
}

template <typename T>
std::vector<T> GetColumn(std::vector<std::vector<T>> x, int column)
{
  std::vector<T> result;

  for (int i = 0; i < x.size(); i++)
  {
    result.push_back(x[i][column]);
  }
  return result;
}

GRBLinExpr getPos(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{
  if (solved == true)
  {
    GRBLinExpr pos = (x[t][0 + ii].get(GRB_DoubleAttr_X)) * tau * tau * tau +
                     (x[t][3 + ii].get(GRB_DoubleAttr_X)) * tau * tau + (x[t][6 + ii].get(GRB_DoubleAttr_X)) * tau +
                     (x[t][9 + ii].get(GRB_DoubleAttr_X));
    return pos;
  }
  else
  {
    GRBLinExpr pos = x[t][0 + ii] * tau * tau * tau + x[t][3 + ii] * tau * tau + x[t][6 + ii] * tau + x[t][9 + ii];
    return pos;
  }
}

GRBLinExpr getVel(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{  // t is the segment, tau is the time inside a specific segment (\in[0,dt], i is the axis)
  if (solved == true)
  {
    GRBLinExpr vel = (3 * x[t][0 + ii].get(GRB_DoubleAttr_X)) * tau * tau +
                     (2 * x[t][3 + ii].get(GRB_DoubleAttr_X)) * tau + (x[t][6 + ii].get(GRB_DoubleAttr_X));
    return vel;
  }
  else
  {
    GRBLinExpr vel = 3 * x[t][0 + ii] * tau * tau + 2 * x[t][3 + ii] * tau + x[t][6 + ii];
    return vel;
  }
}

GRBLinExpr getAccel(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{  // t is the segment, tau is the time inside a specific segment(\in[0, dt], i is the axis)
  if (solved == true)
  {
    GRBLinExpr accel = (6 * x[t][0 + ii].get(GRB_DoubleAttr_X)) * tau + 2 * x[t][3 + ii].get(GRB_DoubleAttr_X);
    return accel;
  }
  else
  {
    GRBLinExpr accel = 6 * x[t][0 + ii] * tau + 2 * x[t][3 + ii];
    return accel;
  }
}

GRBLinExpr getJerk(int t, double tau, int ii, bool solved, std::vector<std::vector<GRBVar>> x)
{  // t is the segment, tau is the time inside a specific segment (\in[0,dt], i is the axis)
  if (solved == true)
  {
    GRBLinExpr jerk = 6 * x[t][0 + ii].get(GRB_DoubleAttr_X);  // Note that here tau doesn't appear (makes sense)
    return jerk;
  }
  else
  {
    GRBLinExpr jerk = 6 * x[t][0 + ii];  // Note that here tau doesn't appear (makes sense)
    return jerk;
  }
}

int main(int argc, char* argv[])
{
  GRBEnv* env = 0;
  GRBVar* open = 0;
  GRBVar** transport = 0;
  int transportCt = 0;
  try
  {
    // Model
    env = new GRBEnv();
    GRBModel m = GRBModel(*env);
    m.set(GRB_StringAttr_ModelName, "planning");

    int N = 10;
    double umax = 5;
    double amax = 3;
    double vmax = 5;
    /*    double q = 20000000000;*/
    double dt = 5.0 / N;
    /*    double dt2 = dt * dt / 2.0;
        double dt3 = dt * dt * dt / 6.0;
        std::vector<std::string> states = { "x", "y", "z", "vx", "vy", "vz", "ax", "ay", "az" };
        std::vector<std::string> inputs = { "jx", "jy", "jz" };*/

    std::vector<std::string> coeff = { "ax", "ay", "az", "bx", "by", "bz", "cx", "cy", "cz", "dx", "dy", "dz" };

    std::vector<double> x0 = { 5, 11.5, 0.5, 0, 0, 0, 0, 0, 0 };

    std::vector<double> xf = { 14, 5, 2.5, 0, 0, 0, 0, 0, 0 };

    // std::cout << "here1" << std::endl;

    std::vector<std::vector<GRBVar>> x;
    std::vector<std::vector<GRBVar>> u;

    for (int t = 0; t < N + 1; t++)
    {
      std::vector<GRBVar> row_t;
      for (int i = 0; i < 12; i++)
      {
        row_t.push_back(m.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_CONTINUOUS, coeff[i] + std::to_string(t)));
      }
      x.push_back(row_t);
    }

    // std::cout << "here2" << std::endl;

    /*    for (int i = 0; i < 3; i++)
        {
          std::vector<GRBVar> row_i;
          for (int t = 0; t < N; t++)
          {
            row_i.push_back(m.addVar(-umax, umax, 0, GRB_CONTINUOUS, inputs[i] + std::to_string(t)));
          }
          u.push_back(row_i);
        }*/

    // Constraints x_t+1=Ax_t+Bu_t
    for (int t = 0; t < N - 1; t++)
    {
      for (int i = 0; i < 3; i++)
      {
        m.addConstr(getPos(t, dt, i, false, x) == getPos(t + 1, 0, i, false, x));      // Continuity in position
        m.addConstr(getVel(t, dt, i, false, x) == getVel(t + 1, 0, i, false, x));      // Continuity in velocity
        m.addConstr(getAccel(t, dt, i, false, x) == getAccel(t + 1, 0, i, false, x));  // Continuity in acceleration
      }
    }

    // std::cout << "here3" << std::endl;
    // Constraint x0==x_initial

    for (int i = 0; i < 3; i++)
    {
      m.addConstr(getPos(0, 0, i, false, x) == x0[i]);        // Initial position
      m.addConstr(getVel(0, 0, i, false, x) == x0[i + 3]);    // Initial velocity
      m.addConstr(getAccel(0, 0, i, false, x) == x0[i + 6]);  // Initial acceleration}
    }

    //  std::cout << "here4" << std::endl;

    // Constraint xT==x_final
    for (int i = 0; i < 3; i++)
    {
      m.addConstr(getPos(N - 1, dt, i, false, x) - xf[i] <= 0.2);   // Final position
      m.addConstr(getPos(N - 1, dt, i, false, x) - xf[i] >= -0.2);  // Final position

      m.addConstr(getVel(N - 1, dt, i, false, x) - xf[i + 3] <= 0.2);   // Final velocity
      m.addConstr(getVel(N - 1, dt, i, false, x) - xf[i + 3] >= -0.2);  // Final velocity

      m.addConstr(getAccel(N - 1, dt, i, false, x) - xf[i + 6] <= 0.2);   // Final acceleration
      m.addConstr(getAccel(N - 1, dt, i, false, x) - xf[i + 6] >= -0.2);  // Final acceleration
    }

    // std::cout << "here5" << std::endl;
    // Constraint v<=vmax, a<=amax, u<=umax
    for (int t = 0; t < N - 1; t++)
    {
      for (int i = 0; i < 3; i++)
      {
        m.addConstr(getVel(t, dt, i, false, x) <= vmax);
        m.addConstr(getVel(t, dt, i, false, x) >= -vmax);

        m.addConstr(getAccel(t, dt, i, false, x) <= amax);
        m.addConstr(getAccel(t, dt, i, false, x) >= -amax);

        m.addConstr(getJerk(t, dt, i, false, x) <= umax);
        m.addConstr(getJerk(t, dt, i, false, x) >= -umax);
      }
    }

    //  std::cout << "here6" << std::endl;

    /*    Eigen::Matrix<double, -1, 3> A1;
        Eigen::Matrix<double, -1, 3> A2;
        Eigen::Matrix<double, -1, 3> A3;*/

    Eigen::MatrixXd A1(12, 3);
    Eigen::MatrixXd A2(10, 3);
    Eigen::MatrixXd A3(10, 3);

    Eigen::VectorXd b1(12);
    Eigen::VectorXd b2(10);
    Eigen::VectorXd b3(10);

    A1 << -0.0990887, 0.994031, -0.0456529,  ////////////////////////////////////
        -0.11874, 0.992494, 0.0292636,       ////////////////////////////////////
        0.315838, 0.947835, -0.0430724,      ////////////////////////////////////
        0.279625, 0.956348, 0.0849006,       ////////////////////////////////////
        0.0379941, -0.999235, -0.00925698,   ////////////////////////////////////
        0.031154, -0.999406, 0.0147274,      ////////////////////////////////////
        0, -1, 0,                            ////////////////////////////////////
        -0, 1, -0,                           ////////////////////////////////////
        0.95448, 0, 0.298275,                ////////////////////////////////////
        -0.95448, -0, -0.298275,             ////////////////////////////////////
        0.298275, 0, -0.95448,               ////////////////////////////////////
        -0.298275, -0, 0.95448;              ////////////////////////////////////

    std::cout << "here6.5" << std::endl;

    b1 << 11.2113,
        11.1351,   ////////////////////////////////////
        15.1733,   ////////////////////////////////////
        15.1708,   ////////////////////////////////////
        -9.26336,  ////////////////////////////////////
        -9.27607,  ////////////////////////////////////
        -9.5,      ////////////////////////////////////
        13.5,      ////////////////////////////////////
        14.3031,   ////////////////////////////////////
        -3.92154,  ////////////////////////////////////
        2.01413,   ////////////////////////////////////
        -0.014135;

    A1 << -0.0990887, 0.994031, -0.0456529,  ////////////////////////////////////
        -0.11874, 0.992494, 0.0292636,       ////////////////////////////////////
        0.315838, 0.947835, -0.0430724,      ////////////////////////////////////
        0.279625, 0.956348, 0.0849006,       ////////////////////////////////////
        0.0379941, -0.999235, -0.00925698,   ////////////////////////////////////
        0.031154, -0.999406, 0.0147274,      ////////////////////////////////////
        0, -1, 0,                            ////////////////////////////////////
        -0, 1, -0,                           ////////////////////////////////////
        0.95448, 0, 0.298275,                ////////////////////////////////////
        -0.95448, -0, -0.298275,             ////////////////////////////////////
        0.298275, 0, -0.95448,               ////////////////////////////////////
        -0.298275, -0, 0.95448;              ////////////////////////////////////

    A2 << -0.199658, 0.976518, 0.0809297,  ////////////////////////////////////
        -0.166117, 0.983608, -0.0701482,   ////////////////////////////////////
        -0.358298, -0.933434, 0.0179951,   ////////////////////////////////////
        -0.365568, -0.928824, -0.0603848,  ////////////////////////////////////
        -0.707107, -0.707107, 0,           ////////////////////////////////////
        0.707107, 0.707107, -0,            ////////////////////////////////////
        0.485071, -0.485071, -0.727607,    ////////////////////////////////////
        -0.485071, 0.485071, 0.727607,     ////////////////////////////////////
        -0.514496, 0.514496, -0.685994,    ////////////////////////////////////
        0.514496, -0.514496, 0.685994;     ////////////////////////////////////

    b2 << 9.06362,  ////////////////////////////////////
        9.21024,    ////////////////////////////////////
        -13.5612,   ////////////////////////////////////
        -13.7295,   ////////////////////////////////////
        -15.3241,   ////////////////////////////////////
        19.3241,    ////////////////////////////////////
        1.60634,    ////////////////////////////////////
        2.45521,    ////////////////////////////////////
        -1.82973,   ////////////////////////////////////
        3.82973;    ////////////////////////////////////

    A3 << -0.999958, 0.00342454, 0.00852636,  ////////////////////////////////////
        -0.999832, 0.00363672, -0.0179664,    ////////////////////////////////////
        -0.999778, -0.0204566, 0.00504416,    ////////////////////////////////////
        -0.999564, -0.0227306, -0.0188383,    ////////////////////////////////////
        -1, -0, 0,                            ////////////////////////////////////
        1, 0, -0,                             ////////////////////////////////////
        0, -0.98387, 0.178885,                ////////////////////////////////////
        -0, 0.98387, -0.178885,               ////////////////////////////////////
        0, -0.178885, -0.98387,               ////////////////////////////////////
        -0, 0.178885, 0.98387;

    b3 << -12.7365,  ////////////////////////////////////
        -12.7834,    ////////////////////////////////////
        -12.9236,    ////////////////////////////////////
        -12.9824,    ////////////////////////////////////
        -12,         ////////////////////////////////////
        16,          ////////////////////////////////////
        -3.47214,    ////////////////////////////////////
        11.0623,     ////////////////////////////////////
        -2.3541,     ////////////////////////////////////
        4.3541;
    std::cout << "here6.7" << std::endl;

    std::vector<std::vector<double>> A1std = eigenMatrix2std(A1);
    std::vector<std::vector<double>> A2std = eigenMatrix2std(A2);
    std::vector<std::vector<double>> A3std = eigenMatrix2std(A3);

    /*    std::vector<std::vector<double>> b1std = eigenMatrix2std(b1);
        std::vector<std::vector<double>> b2std = eigenMatrix2std(b2);
        std::vector<std::vector<double>> b3std = eigenMatrix2std(b3);*/

    // m.update();

    std::cout << "here7" << std::endl;
    std::vector<std::vector<GRBVar>> b;
    for (int t = 0; t < N + 1; t++)
    {
      std::vector<GRBVar> row;
      for (int i = 0; i < 3; i++)  // For the three polytopes
      {
        GRBVar variable =
            m.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_BINARY, "s" + std::to_string(i) + "_" + std::to_string(t));
        row.push_back(variable);
      }
      b.push_back(row);
    }

    std::cout << "here8" << std::endl;

    // If is 1 --> in that polytope

    for (int t = 0; t < N; t++)
    {
      GRBLinExpr sum = 0;
      for (int col = 0; col < b[0].size(); col++)
      {
        sum = sum + b[t][col];
      }
      m.addConstr(sum == 1);

      std::vector<GRBLinExpr> pos = { getPos(t, 0, 0, false, x), getPos(t, 0, 1, false, x), getPos(t, 0, 2, false, x) };

      for (int i = 0; i < b1.rows(); i++)
      {
        m.addGenConstrIndicator(b[t][0], 1, MatrixMultiply(A1std, pos)[i], '<',
                                b1[i]);  // If b[t,0]==1, then...
      }
      for (int i = 0; i < b2.rows(); i++)
      {
        m.addGenConstrIndicator(b[t][1], 1, MatrixMultiply(A2std, pos)[i], '<',
                                b2[i]);  // If b[t,1]==1, then...
      }
      for (int i = 0; i < b3.rows(); i++)
      {
        m.addGenConstrIndicator(b[t][2], 1, MatrixMultiply(A3std, pos)[i], '<',
                                b3[i]);  // If b[t,2]==1, then...
      }
    }
    std::cout << "here9" << std::endl;

    GRBQuadExpr control_cost = 0;
    for (int t = 0; t < N; t++)
    {
      std::vector<GRBLinExpr> ut = { getJerk(t, 0, 0, false, x), getJerk(t, 0, 1, false, x),
                                     getJerk(t, 0, 2, false, x) };
      control_cost = control_cost + GetNorm2(ut);
    }

    std::cout << "here10" << std::endl;

    /*    GRBQuadExpr final_state_cost = 0;
        std::vector<GRBVar> xFinal = GetColumn(x, N);
        // std::vector<GRBLinExpr> prueba = xFinal - xf;
        final_state_cost = GetNorm2(xFinal - xf);
        final_state_cost = q * final_state_cost;*/

    m.setObjective(control_cost, GRB_MINIMIZE);

    // Solve*/
    m.update();
    std::cout << "here11" << std::endl;
    m.write("debug.lp");
    m.optimize();
    std::cout << "here12" << std::endl;

    std::cout << "\nOBJECTIVE: " << m.get(GRB_DoubleAttr_ObjVal) << std::endl;
    std::cout << "Positions X:" << std::endl;
    for (int t = 0; t < N + 1; t++)
    {
      std::cout << getPos(t, 0, 0, true, x) << std::endl;
    }
  }

  catch (GRBException e)
  {
    cout << "Error code = " << e.getErrorCode() << endl;
    cout << e.getMessage() << endl;
  }
  /*catch (...)
  {
    cout << "Exception during optimization" << endl;
  }*/

  delete env;
  return 0;
}


#include <iostream>
#include <vector>
#include <tuple>
#include <algorithm>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                boost::property <boost::edge_weight_t, long> > > > > graph;
typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it;

// Custom edge adder class
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    //std::cout << from << " -> " << to << ": capacity = " << capacity << ", cost = " << cost << std::endl;
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

using namespace std;

struct elephant {
  int x, y, cost, capacity;
};

// Compute max flow min cost given a limit for the flow
tuple<int, int> flowAndCost(int n, int start, int end, vector<elephant> &elephants, int flowLimit, int budget) {
  graph G(n);
  edge_adder adder(G);
  auto c_map = boost::get(boost::edge_capacity, G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);

  for (auto e : elephants) {
    adder.add_edge(e.x, e.y, e.capacity, e.cost);
  }

  int source = boost::add_vertex(G);
  int target = boost::add_vertex(G);
  adder.add_edge(source, start, flowLimit, 0);
  adder.add_edge(end, target, flowLimit, 0);

  boost::successive_shortest_path_nonnegative_weights(G, source, target);
  int flow = 0;
  out_edge_it e, eend;
  for(boost::tie(e, eend) = boost::out_edges(boost::vertex(source,G), G); e != eend; ++e) {
      flow += c_map[*e] - rc_map[*e];
  }
  int cost = boost::find_flow_cost(G);
  return {flow, cost};
}

// Check if it is feasible to transport numSuitcases suitcases
bool isFeasible(int n, int start, int end, vector<elephant> &elephants, int numSuitcases, int budget) {
  int flow, cost;
  tie(flow, cost) = flowAndCost(n, start, end, elephants, numSuitcases, budget);

  if (flow < numSuitcases) {
    return false;
  }
  return cost <= budget;
}

// Strategy:
// - Binary search to find largest feasible number
void solve() {
  int n, m, budget, start, end;
  cin >> n >> m >> budget >> start >> end;

  int x, y, cost, capacity;
  vector<elephant> elephants;
  elephants.reserve(m);
  for (int i = 0; i < m; ++i) {
    cin >> x >> y >> cost >> capacity;
    elephants.push_back({x, y, cost, capacity});
  }

  // Find an upper bound (what is the max flow?)
  int maxFlow = get<0>(flowAndCost(n, start, end, elephants, numeric_limits<int>::max(), budget));

  // Binary search for highest feasible number of elephants
  int a = 0;
  int b = maxFlow;
  while (a != b) {
    int m = a + (b - a + 1) / 2;

    if (isFeasible(n, start, end, elephants, m, budget)) {
      a = m;
    }
    else {
      b = m - 1;
    }
  }

  cout << a << endl;
}

int main() {
    ios_base::sync_with_stdio(false);
    int t;
    cin >> t;
    while (t--) {
      solve();
    }
    return 0;
}