#ifdef USE_EIGEN
#define EIGEN_USE_MKL_ALL
#endif
// do this before include Eigen.

#include "optim.hpp"

#include "alap.h"
#include "matrix.h" // ""

#include "timer.h"

#ifdef USE_EIGEN
#include <Eigen/Core>
#endif

#include <iostream>

#ifdef USE_LBFGSPP
#include <LBFGS.h>
#endif

#include <iostream>

#include "helper.h"
#include "mesh.h"

static void show_usage()
{
    std::cerr << "Usage: "
              << "Options:\n"
              << "\t-h,--help\t\tShow this help message\n"
              << "\t-d,--destination DESTINATION\tSpecify the destination path"
              << std::endl;
}

/*
Differentiable Projection of Weights
*/

arma::mat W2F_n(const arma::mat& W)
{
    using namespace arma;

    arma::mat SW = sum(W, 1); // dim=1 so it sums all cols for each row.

    arma::mat FW = W.each_col() / SW;

    return FW;
}

const double DEFAULT_W_EPSILON = 1e-2;

arma::mat W2F_m1(const arma::mat& W, const double epsilon=DEFAULT_W_EPSILON)
{
    // In matlab grammar: W2F_m = @(W) (W>epsilon) .* W + (W<=epsilon) .* (- W.^3 / epsilon^2  + 2 * W.^2 / epsilon );

    using namespace arma;

    arma::mat FW = (W > epsilon) % W + (W <= epsilon) % (- W % W % W / (epsilon*epsilon) + 2 * W % W / epsilon);

    return FW;
}

arma::mat dFW2dW_m1(const arma::mat& D, const arma::mat& W, const double epsilon=DEFAULT_W_EPSILON)
{
    // In matlab grammar: dFW2dW_m = @(D,W) ( (W>epsilon) + (W<=epsilon) .* (- 3 * W.^2 / epsilon^2  + 4 * W / epsilon ) ) .* D;

    using namespace arma;

    arma::mat FD = ( (W > epsilon) + (W <= epsilon) % (-3 * W % W / (epsilon*epsilon) + 4 * W / epsilon) ) % D;

    return FD;
}

const double DEFAULT_W_EPSILON1 = 0e-2;
const double DEFAULT_W_EPSILON2 = 1e-4;

arma::mat W2F_m2(const arma::mat& W, const double e1 = DEFAULT_W_EPSILON1, const double e2 = DEFAULT_W_EPSILON2)
{

    using namespace arma;

    double c1 = (e1 + e2) / ((e2 - e1) * (e2 - e1) * (e2 - e1));
    double c2 = (e1 + 2 * e2) / ((e2 - e1) * (e2 - e1));

    arma::mat FW = (W <= -1000) % W -(W > e1) % (W < e2) % pow(W - e1, 3) * c1 + (W > e1) % (W < e2) % pow(W - e1, 2) * c2 + (W >= e2) % W;

    return FW;
}

arma::mat dFW2dW_m2(const arma::mat& D, const arma::mat& W, const double e1 = DEFAULT_W_EPSILON1, const double e2 = DEFAULT_W_EPSILON2)
{

    using namespace arma;

    double c1 = (e1 + e2) / ((e2 - e1) * (e2 - e1) * (e2 - e1));
    double c2 = (e1 + 2 * e2) / ((e2 - e1) * (e2 - e1));

    arma::mat FD = ((W <= -1000) - (W > e1) % (W < e2) % pow(W - e1, 2) * 3 * c1 + (W > e1) % (W < e2) % pow(W - e1, 2) * c2 + (W >= e2) ) % D;

    return FD;
}

arma::mat dFW2dW_n(const arma::mat& D, const arma::mat& W)
{
    // In matlab grammar: dFW2dW_n = @(D,W) bsxfun(@rdivide, D, sum(W,2)) - bsxfun(@times, bsxfun(@rdivide, W, sum(W,2).^2), sum(D,2));

    using namespace arma;

    arma::mat FD;

    arma::mat SW = sum(W, 1);
    arma::mat SD = sum(D, 1);

    arma::mat MM = W.each_col() / (SW % SW);

    FD = D.each_col() / SW - MM.each_col() % SD; // % is the element-wise multiplication, NOT '*'!!!

    return FD;
}

Dense W2F_n(const Dense& W)
{
    // const arma::mat W_arma(W.head(), W.nrow(), W.ncol());
    const arma::mat W_arma = dense_array_to_arma_mat(W);

    arma::mat FW_arma = W2F_n(W_arma);

    Dense FW = arma_mat_to_dense_array(FW_arma);

    return FW;
}

Dense dFW2dW_n(const Dense& D, const Dense& W)
{

    const arma::mat W_arma = dense_array_to_arma_mat(W);
    const arma::mat D_arma = dense_array_to_arma_mat(D);

    arma::mat FD = dFW2dW_n(D_arma, W_arma);

    return arma_mat_to_dense_array(FD);
}

Dense W2F_m(const Dense& W)
{
    const arma::mat W_arma = dense_array_to_arma_mat(W);

    arma::mat FW_arma = W2F_m2(W_arma);

    Dense FW = arma_mat_to_dense_array(FW_arma);

    return FW;
}

Dense dFW2dW_m(const Dense& D, const Dense& W)
{

    const arma::mat W_arma = dense_array_to_arma_mat(W);
    const arma::mat D_arma = dense_array_to_arma_mat(D);

    arma::mat FD = dFW2dW_m2(D_arma, W_arma);

    return arma_mat_to_dense_array(FD);
}

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

    const std::string SNAPSHOT_NONE = std::string("none");

    std::vector <std::string> sources;
    std::string EXAMPLE = std::string("/qhw/qhw/data/tibiman-H");
    std::string SOLVER = std::string("adamd");
    std::string SNAPSHOT = SNAPSHOT_NONE;
    std::string OUTPUT = std::string("");
    int NUM_ITER = 50;
    double STEP_SIZE = 0.1;
    bool PROJECT_SIMPLEX = false;
    bool TIMING = false;
    int REPEAT = 5;
    bool LOG_HISTORY = false;
    int LBFGS_M = 10;
    double COND_BOUND = 0.2;
    double DELTA = 0;
    int verbose = 3;

    for (int i = 1; i < argc; i++) {
        std::string arg = argv[i];
        if ((arg == "-h") || (arg == "--help")) {
            show_usage();
            return 0;
        }
        else if ((arg == "--project")) {
            PROJECT_SIMPLEX = true;
        }
        else if ((arg == "--timing")) {
            TIMING = true;
        }
        else if ((arg == "-e") || (arg == "--example")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                EXAMPLE = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--example option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--snapshot")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                SNAPSHOT = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--snapshot option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--output")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                OUTPUT = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--output option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--solver")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                SOLVER = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--solver option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--lbfgs_m")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                LBFGS_M = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--lbfgs_m option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "-n") || (arg == "--num_iter")) {
            if (i + 1 < argc) {
                NUM_ITER = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--num_iter option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--repeat")) {
            if (i + 1 < argc) {
                REPEAT = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--repeat option requires one argument." << std::endl;
                return 1;
            }
        }
        else if (arg == "--verbose") {
            if (i + 1 < argc) {
                verbose = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--verbose option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--log_history")) {
            LOG_HISTORY = true;
        }
        else if ( (arg == "--step_size")) {
            if (i + 1 < argc) {
                STEP_SIZE = atof(argv[++i]);
                printf("step_size=%f\n", STEP_SIZE);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--step_size option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--cond_bound")) {
            if (i + 1 < argc) {
                COND_BOUND = atof(argv[++i]);
                printf("cond_bound=%f\n", COND_BOUND);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--cond_bound option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--delta")) {
            if (i + 1 < argc) {
                DELTA = atof(argv[++i]);
                printf("delta=%f\n", DELTA);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--delta option requires one argument." << std::endl;
                return 1;
            }
        }
        else {
            sources.push_back(argv[i]);
        }
    }

    double t = 0;

    std::string folder;
    int dim = 2;

    folder = EXAMPLE + std::string("/");

    printf("Loading files from %s.\n", folder.c_str());

    if (OUTPUT.length() > 0) {
        OUTPUT = OUTPUT;
    }

    int length_his = NUM_ITER * (REPEAT + 1) * 2 + 1; // This *2 should not be need, just for redundancy.
    double* energy_his = new double[length_his];
    double* time_his = new double[length_his];

    cholmod_common* cm = Begin();

    Dense V;
    DenseInt F;

    Dense FA;

//#ifdef USE_LARGER_G
    Sparse Gk, Gu, G;
//#endif

    Sparse Gx, Gy, Gz;

    Sparse Gxk, Gxu, Gyk, Gyu;
    Sparse Gzk, Gzu; // for 3D

    DenseInt known;
    DenseInt unknown;
    int nk, nu;

    Sparse L; // Note L is not p.d., just p.s.d.
    Sparse Mass;
    Sparse invMass;

    Dense mass_vertex;
    mass_vertex.read(folder + "mv.mtx");

    int f, mcdim;

    Dense BC(0, 1);
    BC.read(folder + "BC.mtx");
    printf("BC size (%d,%d)\n", BC.nrow(), BC.ncol());

    int nr;
    known.read(folder + "B.mtx", true); // setting 'true' will delete 1 from the matrix for 0-index.

    nk = known.nrow();
    assert(known.ncol() == 1);

    {

        V.read(folder + "V.mtx");
        F.read(folder + "F.mtx", true); // from 1-index matrix.

        printf("Files loaded successfully!\n");

        f = F.nrow();
        dim = F.ncol() - 1;

        complementary_list(known, V.nrow(), unknown);

        FA = (dim == 3) ? volume(V, F) : area(V, F);

        sparse_grads(V, F,
            known,
            Gx, Gy, Gz,
            Gxk, Gxu, Gyk, Gyu,
            Gzk, Gzu);

        Mass = Sparse::Diag(mass_vertex);
        invMass = Sparse::Diag(Dense::div(1.0, mass_vertex));

    }

    mcdim = (dim == 2) ? 3 : 6; // so au is of dim (f*mcdim)

    Sparse GxkT = Gxk.transposed();
    Sparse GxuT = Gxu.transposed();
    Sparse GykT = Gyk.transposed();
    Sparse GyuT = Gyu.transposed();
    Sparse GzkT;
    Sparse GzuT;
    if (dim == 3) {
        GzkT = Gzk.transposed();
        GzuT = Gzu.transposed();
    }

    Sparse GxuT_A00 = GxuT;
    Sparse GxuT_A01 = GxuT;

    Sparse GyuT_A01 = GyuT;
    Sparse GyuT_A11 = GyuT;

    // Used for 3D only:
    Sparse GxuT_A02 = GxuT;
    Sparse GzuT_A02 = GzuT;
    Sparse GyuT_A12 = GyuT;
    Sparse GzuT_A12 = GzuT;
    Sparse GzuT_A22 = GzuT;

    Sparse Mf = Sparse::Diag(FA);

    Dense Zf = Dense::Zeros(f, 1);

    Dense RFA = Dense::concatenate(FA,
        Dense::concatenate(Zf, FA));

    int stype = 0; // 0; // 0; unsymmetric // 1: symmetric and use triu

    stype = 1; // 1: symmetric and use triu
    Sparse A = GxuT_A00.mul(Gxu, stype)
        + GyuT_A01.mul(Gxu, stype)
        + GxuT_A01.mul(Gyu, stype)
        + GyuT_A11.mul(Gyu, stype);
    if (dim == 3) {
        A = A
            + GzuT_A22.mul(Gzu, stype)
            + GxuT_A02.mul(Gzu, stype)
            + GzuT_A02.mul(Gxu, stype)
            + GyuT_A12.mul(Gzu, stype)
            + GzuT_A12.mul(Gyu, stype);
    }

    L = (Mf * Gx).transposed().mul(Gx, stype)
        + (Mf * Gy).transposed().mul(Gy, stype);
    if (dim == 3) {
        L = L + (Mf * Gz).transposed().mul(Gz, stype);
    }

    int* I;
    int* J;
    symmetric_tensor_assemble_indices(I, J, f, dim);

    // Setup the objective function:
    Sparse Q;
    stype = 1; // 1: symmetric and use triu
    (L * invMass).mul(L, Q, stype);
    // So Q is a symmetric matrix such that
    // Q==L*M^-1*L.

    Sparse diagm = Sparse::Diag(
        Dense::concatenate(FA, dim==2? FA : Dense::concatenate(FA, FA))
    );

    Sparse Quu, Quk;
    Sparse Qku, Qkk;

    Sparse Lua = (GxuT * Mf * Gx + GyuT * Mf * Gy);
    Sparse Lka = (GxkT * Mf * Gx + GykT * Mf * Gy);
    if (dim == 3) {
        Lua = Lua + GzuT * Mf * Gz;
        Lka = Lka + GzkT * Mf * Gz;
    }
    Sparse Lau = Lua.transposed();
    Sparse Lak = Lka.transposed();
    (Lua* invMass).mul(Lau, Quu, 0);
    (Lka* invMass).mul(Lak, Qkk, 0);
    (Lua* invMass).mul(Lak, Quk, 0);

    Qku = Quk.transposed();

    Sparse SA = GxuT * Mf * Gxk + GyuT * Mf * Gyk;
    if (dim == 3)
        SA = SA + GzuT * Mf * Gzk;
    Dense B = (SA * BC) * (-1);

    Dense X(B.nrow(), B.ncol());

    if (verbose>2)
    {
        t = GetTime();
        for (int i = 0; i < 1; i++)
        {
            X = A.mul(B);
        }
        printf("Time for linear matrix-vector mul :%f.\n", GetTime() - t);

        Dense X3(B.nrow(), B.ncol());
        t = GetTime();
        for (int i = 0; i < 1000; i++)
        {
            Dense::saxy(X, X3, -0.4);
        }
        printf("Time for saxy x1000:%f.\n", GetTime() - t);

        t = GetTime();
        configure_solve(A.cm);
        A.solve(B, X); // X = A \ B;
        printf("Time for one linear solve :%f.\n", GetTime() - t);
    }

    Dense Y = A.mul(X);

    t = GetTime();
    A.symbolic_factor();
    printf("symbolic_factor: %f seconds.\n", GetTime() - t);

    t = GetTime();
    A.numerical_factor();
    printf("numerical_factor: %f seconds.\n", GetTime() - t);

    Dense W_u;
    Dense GW;
    Dense Res_u;
    Dense BR;
    Dense PS;

    Dense st0 = Dense::Ones(f, 1);
    // Dense st0 = FA;

    Dense at0 = Dense::concatenate(st0,
        Dense::concatenate(st0 * 1e-6, st0));

    if (dim == 3)
    {
        at0 = Dense::concatenate(at0,
            Dense::concatenate(Dense::concatenate(st0,st0) * 1e-6, st0));
    }

    if (SNAPSHOT != SNAPSHOT_NONE)
    {
        Dense at0_snapshot;
        at0_snapshot.read(SNAPSHOT);
        if (at0.nrow() == at0_snapshot.nrow() && at0.ncol() == at0_snapshot.ncol() ) {
            printf("Snapshot file %s is loaded.\n", SNAPSHOT.c_str());
            at0 = at0_snapshot;
        }
        else {
            printf("Snapshot file is of wrong size! File ignored. \n");
        }
    }

    Dense au = at0;

    Sparse diagAU = diagm;

    Dense W(L.nrow(), BC.ncol());
    W.slice_assign_value(known, BC, 0); // W(known, :) = BC;

    Dense FW(L.nrow(), BC.ncol());
    FW.slice_assign_value(known, BC, 0);

    Dense FW_u, FW_k;

    Dense gau = Dense::Zeros(f * mcdim, 1);
    Dense gau_j = Dense::Zeros(f * mcdim, 1);

    t = GetTime();

    std::function<Dense(const Dense&)> W2F_pn = [&](const Dense& W)
    {
        return W2F_n(W2F_m(W));
        //return W2F_n(W);
    };

    std::function<Dense(const Dense&, const Dense&)> dFW2dW_pn = [&](const Dense& D, const Dense& W)
    {
        return dFW2dW_n(dFW2dW_m(D, W), W2F_m(W));
        //return dFW2dW_n(D, W);
    };

    std::function<Dense(const Dense&)> W2F;
    std::function<Dense(const Dense&, const Dense&)> dFW2dW;

    if (PROJECT_SIMPLEX)
    {
        W2F = W2F_pn;
        dFW2dW = dFW2dW_pn;
    }
    else
    {
        W2F = [&](const Dense& W)
        {
            return W;
        };
        dFW2dW = [&](const Dense& D, const Dense& W)
        {
            return D;
        };
    }

    Grad grad = compute_grads_pre(V, F);

    // GRADtf64 grad_tf = GRADtf64(V, F, W.ncol());
    GRADtf grad_tf = GRADtf(V, F, W.ncol());

    int i = 0;
    double start_time = GetTime();
    // Dense gat = Dense::Zeros(mcdim * f, 1);
    // Dense at = Dense::concatenate(Dense::Ones(f, 1),
    //     Dense::concatenate(Dense::Ones(f, 1) * 1e-6, Dense::Ones(f, 1))); // do not use Dense::Zeros(f, 1)

    const Para para;

    auto fun = [&](const arma::vec& at, arma::vec& gat)
    {

        t = GetTime();

        if (TIMING)
        {
            printf("Entered loop for timing.\n");
        }

        /* update au from at */

        // arma::vec at_tmp = at;
        // s_at2au_fast(at_tmp, au, FA, dim, para);
        s_at2au(at, au, FA, dim, para);

        if (TIMING)
        {
            printf("Check point 1: %f.\t Apply Parameterization.\n", GetTime() - t); t = GetTime();
        }

        int stype = 1; // 1: symmetric and use triu

        Sparse A2 = Sparse::assemble_lap(GxuT, GyuT, GzuT, au, dim);
        Sparse::assign_value_same_pattern(A2, A);
        // A.symbolic_factor(); // no need since sparsity pattern remain unchanged.

        B = Sparse::assemble_lap_off_diag(GxuT, GyuT, GzuT, GxkT, GykT, GzkT, au, dim) * (BC * (-1));

        if (TIMING)
        {
            printf("Check point 2: %f.\t Assemble Lap.\n", GetTime() - t); t = GetTime();
        }

        configure_solve(A.cm);

        A.numerical_factor();

        if (TIMING)
        {
            printf("Check point 3: %f.\t Numerical factor.\n", GetTime() - t); t = GetTime();
        }

        W_u = A.solve_with_factor(B); // W_u = A\B; but more efficiently.

        if (TIMING)
        {
            printf("Check point 4: %f.\t Back Sub 1.\n", GetTime() - t); t = GetTime();
        }

        // project the weights to the probability simplex.
        FW_u = W2F(W_u);
        FW_k = BC;

        W.slice_assign_value(unknown, W_u, 0); // W(unknown, :) = W_u;

        double e = 1 / (i + 1);
        // many optimizers do not rely on the energy value e, only its gradient.
        // in this case e can be an arbitary value to save time.

        bool last_iter = i == (NUM_ITER * (REPEAT + 1)); // it's not (i+1) here.

        if (verbose > 2 || last_iter)
        {

            Dense H_u = Quu.mul(FW_u) + Quk.mul(FW_k);
            Dense H_k = Qku.mul(FW_u) + Qkk.mul(FW_k);
            e = ((Dense::times(FW_u, H_u).reduce_sum(0) + Dense::times(FW_k, H_k).reduce_sum(0)).reduce_sum(1))(0, 0);

            printf("\nIter %04d: energy=%f \t", i, e);

            if (verbose > 3)
            {
                printf("W_u.min()=");
                W_u.reduce_min().print();
                printf("FW_u.min()=");
                FW_u.reduce_min().print();
            }

            if (i < length_his)
            {
                energy_his[i] = e;
                time_his[i] = GetTime() - start_time;
            }
            if (LOG_HISTORY)
            {
                FW.slice_assign_value(unknown, FW_u, 0);
                char fname[64];
                sprintf(fname, "W%04d.mtx", i);
                FW.write(OUTPUT + fname); // projected weights
                sprintf(fname, "UW%04d.mtx", i);
                W.write(OUTPUT + fname); // unprojected weights
            }
        }
        else
        {
            printf("Iter %04d...\t", i);
        }

        if (verbose>4)
        {
            Dense NW = W2F_n(W);
            double pou = (W - NW).norm() / W.norm();
            printf("Checking partition of unity: %f, expecting ~0.\n", pou);
        }

        if (TIMING)
        {
            printf("Check point 5: %f.\n", GetTime() - t); t = GetTime();
        }

        // Calculate gradients.

        switch (4) { // 4 is fastest

        case 0:

            GW = G * W; // old code

            break;

        case 1:

            // [Gxu, Gxk]
            // [Gyu, Gyk] [W_u] =
            // [Gzu, Gzk] [W_k]
            GW = Dense::concatenate(Gxu * W_u + Gxk * BC, Gyu * W_u + Gyk * BC);
            if (dim == 3)
                GW = Dense::concatenate(GW, Gzu * W_u + Gzk * BC);

            break;

        case 2:

            compute_grads(V, F, grad, W, GW); // somehow slower than G * W...

            break;

        case 3:

            tf_compute_grads(V, F, W, GW);

            break;

        case 4:

            grad_tf.run(W, GW);

            break;
        default:
            ;
        }

        if (TIMING)
        {
            printf("Check point 6: %f. \t Gradient computation. \n", GetTime() - t); t = GetTime();
        }

        Res_u = (Quu.mul(FW_u) + Quk.mul(FW_k))* 2.0;

        Dense dW = dFW2dW(Res_u, W_u); // since dFW2dW is a row-wise operation

        if (TIMING)
        {
            printf("Check point 7: %f.\n", GetTime() - t); t = GetTime();
        }

        BR = A.solve_with_factor(dW);

        if (TIMING)
        {
            printf("Check point 8: %f. \t Back Sub 2\n", GetTime() - t); t = GetTime();
        }

        // Calculate gradients for the second time

        switch (2) { // 2 is fastest

        case 0:

            PS = Gu.mul(BR); // old code

            break;

        case 1:

            PS = Dense::concatenate(Gxu.mul(BR), Gyu.mul(BR));
            if (dim == 3)
                PS = Dense::concatenate(PS, Gzu.mul(BR));

            break;

        case 2:

            static Dense BRa;
            if (i==0)
                BRa = Dense::Zeros(V.nrow(), BR.ncol());

            BRa.slice_assign_value(unknown, BR, 0);

            grad_tf.run(BRa, PS);

            break;

        default:
            ;
        }

        if (TIMING)
        {
            printf("Check point 9: %f. \t Mat Multiplication 2.\n", GetTime() - t); t = GetTime();
        }

        gau = Dense::Zeros(f * mcdim, 1);

        for (int j = 0; j < W.ncol(); j++)
        {
            symmetric_tensor_span_dot(&GW(0, j), &PS(0, j), gau_j.head(), f, dim);
            // gau = gau - gau_j;
            Dense::saxy(gau_j, gau, -1.0);
        }

        if (TIMING)
        {
            printf("Check point 10: %f.\n", GetTime() - t); t = GetTime();
        }

        /* gat <- gau: back-prop grad */

        // s_pdapdt_lmul_fast(at_tmp, gau, gat, FA, dim, para);
        s_pdapdt_lmul(at, gau, gat, FA, dim, para);

        if (TIMING)
        {
            printf("Check point 11: %f.\n", GetTime() - t); t = GetTime();
        }

        i++;
        return e;
        // e may not be the function value, depends on parameters.
    };

    Dense at_out;
    {
        using arma::vec;
        using arma::mat;
        vec at = dense_array_to_arma_vec(at0);

        printf("Solver: %s\n", SOLVER.c_str());

        if (SOLVER==std::string("adamd"))
        {
            /* Adam - based optim */

            optim::algo_settings_t settings;
            settings.gd_method = 6;
            settings.gd_settings.step_size = STEP_SIZE;
            settings.iter_max = NUM_ITER;

            std::function<double(const arma::vec&, arma::vec*, void*)> fn
                = [&](const arma::vec& at, arma::vec* pgat, void* opt_data) {
                double v = fun(at, *pgat);
                return v;
            };

            for (int j = 0; j < REPEAT; j++)
            {
                optim::gd2(at, fn, NULL, settings); // gd2 gets rid of an extra call of fn at the end.

                settings.gd_settings.step_size /= 2.0;
                printf("\nlr=%g \n", settings.gd_settings.step_size);
            }

            optim::gd(at, fn, NULL, settings);
            // NUM_ITER * REPEAT + NUM_ITER: in total

        }
        else if (SOLVER == std::string("adam"))
        {

            optim::algo_settings_t settings;
            settings.gd_method = 6;
            settings.gd_settings.step_size = STEP_SIZE;
            settings.iter_max = NUM_ITER * (REPEAT+1);

            std::function<double(const arma::vec&, arma::vec*, void*)> fn
                = [&](const arma::vec& at, arma::vec* pgat, void* opt_data) {
                double v = fun(at, *pgat);
                return v;
            };

            optim::gd(at, fn, NULL, settings);

        }
        else if (SOLVER == std::string("lbfgs"))
        {

            optim::algo_settings_t settings;
            settings.iter_max = NUM_ITER * (REPEAT + 1);
            settings.lbfgs_par_M = LBFGS_M;

            std::function<double(const arma::vec&, arma::vec*, void*)> fn
                = [&](const arma::vec& at, arma::vec* pgat, void* opt_data) {
                double v = fun(at, *pgat);
                return v;
            };

            optim::lbfgs(at, fn, NULL, settings);

        }
        else
        {
            std::cerr << "--solver not supported." << std::endl;
        }

        at_out = arma_vec_to_dense_array(at);
    }

    printf("numerical_factor + solving: %f seconds.\n", GetTime() - start_time);

    if (LOG_HISTORY)
    {
        printf("Timing is not meaningful due to logging cost. \n");
    }

    FW.slice_assign_value(unknown, FW_u, 0); // FW(unknown, :) = FW_u;
    FW.write(OUTPUT+"W.mtx"); // projected weights
    W.write(OUTPUT+"UW.mtx"); // unprojected weights
    at_out.write(OUTPUT+"at.mtx");
    au.write(OUTPUT + "au.mtx");

    End(cm);

    free(I);
    free(J);

    FILE* filename = fopen((OUTPUT+"log.txt").c_str(), "wb");
    if (filename != NULL)
    {
        for (int i = 0; i < length_his; i++)
        {
            fprintf(filename, "%04d\t%g\t%g\n", i, energy_his[i], time_his[i]);
        }
        fclose(filename);
    }

    free(energy_his);

    return 0;
}

// #include "iter_lap_timing.cpp"


/* test_triangle.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/triangle_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/triangular.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::triangle_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME lognormal
#define BOOST_MATH_DISTRIBUTION boost::math::triangular
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME b
#define BOOST_RANDOM_ARG1_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.0001, 0.9999)
#define BOOST_RANDOM_DISTRIBUTION_INIT (0.0, b, 1.0)
#define BOOST_MATH_DISTRIBUTION_INIT (0.0, b, 1.0)

#include "test_real_distribution.ipp"


	#include "savageFunctions.h"
#include <iostream>
#include <cmath>
#include <cstdlib>
#include <string>
#include <Eigen/Dense>

// a little documentation up front:
// an eigen vector is what you'd expect--a vector that only scales in a linear transformation
// an "Eigen" vector is a vector using the Eigen api

// also, it's pretty convenient that eigen objects work as regular objects for pass by value and return by value
// so that's nice to keep in mind

Eigen::Matrix3d savageFunctions::skew(Eigen::Vector3d v)
{//pass in an "Eigen" vector

	//initialize a "skewwed" matrix
	Eigen::Matrix3d skewwed;

	//populate the "skewwed" matrix
	skewwed <<    0, -v(2),  v(1),
			   v(2),     0, -v(0),
			  -v(1),  v(0),     0;

  //return the skewwed matrix
  return skewwed;
}

Eigen::VectorXd savageFunctions::stateIntegrate(Eigen::Vector3d omega, Eigen::Vector3d accel,
												  Eigen::VectorXd prevState, Eigen::Matrix3d prevDCM, double d_t)
{
	//state of form,
	// 0 x
	// 1 y
	// 2 z
	// 3 v_x
	// 4 v_y
	// 5 v_z
	// 6 a_x
	// 7 a_y
	// 8 a_z

	//and of course the previous dcm is needed

	//EVERY PARAMETER must be of the "Eigen" type

	//Okay getting started here
	//First things first,
	//use the previous time step DCM and the current angular rates to
	//find the current derivative of the current DCM
	//the d_ will be used to denore a time rate of change for something that doesn't have a
	//standard usage (eg. theta->omega)
	//taken from 3-53 of savage (eq. 3.3.2-6)
	Eigen::Matrix3d d_DCM = prevDCM * skew(omega);

	//calculate the current DCM
	Eigen::Matrix3d DCM = d_DCM * d_t + prevDCM;

	// use the current DCM to convert acceleration to the inertial frame
	Eigen::Vector3d accelInertial = DCM * accel;

	//okay now working down through the state vector and integrating as I go

	Eigen::VectorXd state(9);  // x               v*t                 .5at^2
	               state << prevState[0] + prevState[3] * d_t + accelInertial[0] * pow(d_t,2) * .5, //x
							prevState[1] + prevState[4] * d_t + accelInertial[1] * pow(d_t,2) * .5, //y
							prevState[2] + prevState[5] * d_t + accelInertial[2] * pow(d_t,2) * .5, //z
							 // v                 a*t
							prevState[3] + accelInertial[0] * d_t, //v_x
							prevState[4] + accelInertial[1] * d_t, //v_y
							prevState[5] + accelInertial[2] * d_t, //v_z
							 // a
							accelInertial[0], //a_x
							accelInertial[1], //a_y
							accelInertial[2]; //a_z

	std::cout << DCM << std::endl;

	return state;
} s


#include <string>
#include <vector>
#include <map>
#include <set>
#include <cmath>
#include <utility>
#include <algorithm>

#include "main.h"
#include "option.h"
#include "likelihood.h"
#include <boost/math/distributions/chi_squared.hpp>

// needed by alglib
#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "optimization.h"
#include "ap.h"

struct fn_data {
    std::string         base;
    std::vector<double> errRateV;
};

// composite log likelihood: l_c(theta)
// mat theta is a column vector which has 4 elements for A, C, G, T, respectively.
//
double composite_LogLikelihood (
        const string         &base,
        const vector<double> &errRateV,
        const alglib::real_1d_array  &theta )
{
    double l_c(0.0);

    for (size_t i(0); i != base.size(); i++ ) {
        const double &e = errRateV[i]/3;
        switch( base[i] ) {
            case 'A': l_c += log( (1-4*e) * theta[0] + e ); break;
            case 'C': l_c += log( (1-4*e) * theta[1] + e ); break;
            case 'G': l_c += log( (1-4*e) * theta[2] + e ); break;
            case 'T': l_c += log( (1-4*e) * theta[3] + e ); break;
            default: cerr << "unknown base in " << base << endl,exit(1);
        }
    }

    return l_c;
}

// -composite score function: -U_c(theta)
// return a column vector
//
alglib::real_1d_array composite_score (
        const string         &base,
        const vector<double> &errRateV,
        const alglib::real_1d_array  &theta )
{
    alglib::real_1d_array U_c = "[0,0,0,0]";

    for (size_t i(0); i != base.size(); i++ ) {
        const double &e = errRateV[i]/3;

        switch( base[i] ) {
            case 'A': U_c[0] -= (1-4*e) / ( (1-4*e)*theta[0] + e );  break;
            case 'C': U_c[1] -= (1-4*e) / ( (1-4*e)*theta[1] + e );  break;
            case 'G': U_c[2] -= (1-4*e) / ( (1-4*e)*theta[2] + e );  break;
            case 'T': U_c[3] -= (1-4*e) / ( (1-4*e)*theta[3] + e );  break;
            default: cerr << "unknown base in " << base << endl, exit(1);
        }
    }

    return U_c;
}

// gradient optimization
void function1_grad (
        const alglib::real_1d_array  &x,
        double                       &func,
        alglib::real_1d_array        &grad,
        void                         *opt_data )
{
    fn_data* objfn_data = reinterpret_cast<fn_data*>(opt_data);
    const std::string &base = objfn_data->base;
    const std::vector<double> &errRateV = objfn_data->errRateV;

    func = -composite_LogLikelihood( base, errRateV, x);
    grad = composite_score( base, errRateV, x );
}

string initAlleleFreq (
        mCharUlong       &fr,
        const double     &depth,
        const char       &except_b )
{
    ostringstream s;
    s << '[';

    if ( depth == fr[except_b] ) {
        for ( auto b : "ACGT" ) {
            b == except_b ? s << 0.0 : s << 0.333333333;
            b == 'T' ? s << ']' : s << ',';
            if ( b == 'T' ) break;
        }
    }
    else {
        for ( auto b : "ACGT" ) {
            b == except_b ? s << 0.0 : s << fr[b] / (depth - fr[except_b]);
            b == 'T' ? s << ']' : s << ',';
            if ( b== 'T' ) break;
        }
    }
    return s.str();
}

string initAlleleFreq (
        mCharUlong       &fr,
        double           depth,
        const set<char>  &except_bs )
{
    ostringstream s;

    for ( auto b : "ACGT" ) {
        set<char>::const_iterator it = except_bs.find(b);
        if ( it != except_bs.end() ) {
            fr[b] = 0;
            depth -= fr[b];
        }

        if ( b== 'T' ) break;
    }

    if ( depth == 0 ) {
        s  << "[0.25,0.25,0.25,0.25]";
        return s.str();
    }

    s << '[';
    for ( auto b : "ACGT" ) {
        set<char>::const_iterator it = except_bs.find(b);
        it != except_bs.end() ? s << 0.0 : s << fr[b] / depth;  // depth here don't contain num of except_bs
        b == 'T' ? s << ']' : s << ',';
        if ( b== 'T' ) break;
    }

    return s.str();
}

string _upBoundary(const char except_b)
{
    ostringstream s;
    s << '[';

    for ( auto b : "ACGT" ) {
        b == except_b ? s << 0 : s << 1;
        b == 'T' ? s << ']' : s << ',';
        if ( b == 'T' ) break;
    }
    return s.str();
}

string _upBoundary(const set<char> except_bs)
{
    ostringstream s;
    s << '[';

    for ( auto b : "ACGT" ) {
        set<char>::const_iterator it = except_bs.find(b);
        it != except_bs.end() ? s << 0 : s << 1;
        b == 'T' ? s << ']' : s << ',';
        if ( b == 'T' ) break;
    }
    return s.str();
}

map<char, vector<double> > llh_genotype(const string &s, const string &q, const Option &opt)
{
//    mCharDouble ntP; // nt => pvalue
    map<char, vector<double> > ntPF; // nt => pvalue, fraction

    boost::math::chi_squared X2_dist(1);

    mCharUlong fr;
    for ( auto b : "ACGTN" ) {
        fr[b] = 0;
        if ( b == 'N' ) break;
    }

    string new_s(""), new_q("");
    for ( size_t i(0); i != s.size(); i++ ) {
        if ( lowQuality(q[i], opt) || s[i] == 'N' || s[i] == '*' )  continue; // fr['N'] == 0
        fr[ s[i] ]++;
        new_s += s[i];
        new_q += q[i];
    }

    double depth(new_s.size());
    vector<double> errV = quaToErrorRate(new_q, opt);

    if ( depth == 0 ) return ntPF;

    fn_data data;
    data.base = new_s;
    data.errRateV = errV;

    // var for alglib
    alglib::minbleicstate state;
    alglib::minbleicreport rep;
    double epsg(0.000001);
    double epsf(0.0);
    double epsx(0.0);
    alglib::ae_int_t maxits(0);

    // constraint: sum of frequency of 4 alleles == 1
    alglib::real_2d_array c = "[[1,1,1,1,1]]";  // sum of four allele == 1
    alglib::integer_1d_array ct = "[0]";    // equal
    alglib::real_1d_array bndl = "[0,0,0,0]";  // lower boundary

    // four allele maximize
    double cl_4(0.0);
    try {
        string AFstr = initAlleleFreq(fr, depth, 'N');
        alglib::real_1d_array alg_x = AFstr.c_str();
        alglib::real_1d_array bndu = "[1,1,1,1]";

        alglib::minbleiccreate(alg_x, state);
        alglib::minbleicsetlc(state, c, ct);
        alglib::minbleicsetbc(state, bndl, bndu);
        alglib::minbleicsetcond(state, epsg, epsf, epsx, maxits);
        alglib::minbleicoptimize(state, function1_grad, NULL, &data );
        alglib::minbleicresults(state, alg_x, rep);

        if ( opt.debug ) {
            printf("%d\n", int(rep.terminationtype)); // EXPECTED: 4
            printf("%s\n", alg_x.tostring(20).c_str());
        }

        cl_4 = composite_LogLikelihood( data.base, data.errRateV, alg_x );
        if ( opt.debug ) cout << "cl_4: " << setprecision(20) << cl_4 << endl;
    }
    catch ( alglib::ap_error &e ) {
        cerr << "catch error: " << e.msg << " at seq[" << new_s << "] qua[" << new_q << "]" << endl;
    }

    map<char, string> init_V;
    map<char, string> bndu_V;

    for ( auto b : "ACGT" ) {
        init_V[b] = initAlleleFreq(fr, depth, b);
        bndu_V[b] = _upBoundary(b);
        if ( b == 'T' ) break;
    }

    for ( mCharUlong::const_iterator it = fr.begin(); it != fr.end(); it++ )
    {
        if ( it->second < opt.minSupOnEachStrand || it->second/depth < opt.minFractionInFam ) continue;

        double cl_3(0.0);
        try {
            alglib::real_1d_array alg_x = init_V[ it->first ].c_str();
            alglib::real_1d_array bndu = bndu_V[ it->first ].c_str();

            alglib::minbleiccreate(alg_x, state);
            alglib::minbleicsetlc(state, c, ct);
            alglib::minbleicsetbc(state, bndl, bndu);
            alglib::minbleicsetcond(state, epsg, epsf, epsx, maxits);
            alglib::minbleicoptimize(state, function1_grad, NULL, &data );
            alglib::minbleicresults(state, alg_x, rep);

            if ( opt.debug ) {
                printf("%d\n", int(rep.terminationtype)); // EXPECTED: 4
                printf("%s\n", alg_x.tostring(20).c_str());
            }

            cl_3 = composite_LogLikelihood( data.base, data.errRateV, alg_x );
            if ( opt.debug ) cout << "cl_3: " << cl_3 << endl;
        }
        catch ( alglib::ap_error &e ) {
            cerr << "catch error: " << e.msg << " at seq[" << new_s << "] qua[" << new_q
                << "] for base[" << it->first << "]" << endl;
        }

        if ( cl_4 - cl_3 > opt.lhrGapCutoff ) {
//            ntP[ it->first ] = 1 - boost::math::cdf(X2_dist, 2*(cl_4 - cl_3) );
            ntPF[ it->first ].push_back( 1 - boost::math::cdf(X2_dist, 2*(cl_4 - cl_3)) );
        }
    }

    if ( ntPF.size() == 1 ) {
        ntPF[ ntPF.begin()->first ].push_back(1.0);
        return ntPF;
    }
    else if ( ntPF.size() > 1 ) {
        set<char> except_bs;
        for ( auto b : "ACGT" ) {
            map<char, vector<double> >::const_iterator it = ntPF.find(b);
            if ( it == ntPF.end() ) {  // not in ntPF
                except_bs.insert(b);
            }

            if ( b == 'T' ) break;
        }

        string AFstr = initAlleleFreq(fr, depth, except_bs);
        alglib::real_1d_array alg_x = AFstr.c_str();

        try {
            string upBnd = _upBoundary(except_bs);
            alglib::real_1d_array bndu = upBnd.c_str();

            alglib::minbleiccreate(alg_x, state);
            alglib::minbleicsetlc(state, c, ct);
            alglib::minbleicsetbc(state, bndl, bndu);
            alglib::minbleicsetcond(state, epsg, epsf, epsx, maxits);
            alglib::minbleicoptimize(state, function1_grad, NULL, &data );
            alglib::minbleicresults(state, alg_x, rep);

            if ( opt.debug ) {
                printf("%d\n", int(rep.terminationtype)); // EXPECTED: 4
                printf("%s\n", alg_x.tostring(20).c_str());
            }

            cl_4 = composite_LogLikelihood( data.base, data.errRateV, alg_x );
            if ( opt.debug ) cout << "cl_4: " << setprecision(20) << cl_4 << endl;
        }
        catch ( alglib::ap_error &e ) {
            cerr << "catch error: " << e.msg << " at seq[" << new_s << "] qua[" << new_q << "]" << endl;
        }

        string st = "ACGT";
        map<char, double> mBaseFrac;
        for ( int i(0); i != 4; i++ ) {
            mBaseFrac[ st[i] ] = alg_x[i];
        }

        for ( auto &p : ntPF ) {
            ntPF[ p.first ].push_back( mBaseFrac[p.first] );
        }

        return ntPF;
    }
    else if ( ntPF.size() > 4 ) {
        cerr << "ntPF contain unknown base" << endl, exit(1);
    }
    else {
        return ntPF;
    }
}


//#define BOOST_NUMERIC_BINDINGS_LAPACK_2

#include <cstddef>
#include <iostream>
#include <algorithm>
#include <complex>
#include <boost/numeric/bindings/lapack/gesdd.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> m_t;
typedef ublas::vector<cmplx_t> v_t;
typedef ublas::matrix<real_t, ublas::column_major> rm_t;
typedef ublas::vector<real_t> rv_t;

int main() {

  cout << endl;

  size_t m = 1, n = 3;
  size_t minmn = m < n ? m : n;
  m_t a (m, n);
  a(0,0) = cmplx_t (2, -1);
  a(0,1) = cmplx_t (1, 1);
  a(0,2) = cmplx_t (-2, 0);

  m_t a2 (a); // for part 2
  m_t a3 (a); // for part 3

  print_m (a, "A");
  cout << endl;

  rv_t s (minmn); // singular values are real
  m_t u (m, m);
  m_t vt (n, n);

  size_t lw;

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
  lw = lapack::gesdd_work ('O', 'N', a);
  cout << "opt N lw: " << lw << endl;
  lw = lapack::gesdd_work ('O', 'A', a);
  cout << "opt A lw: " << lw << endl;
  lw = lapack::gesdd_work ('O', 'S', a);
  cout << "opt S lw: " << lw << endl;
  lw = lapack::gesdd_work ('O', 'O', a);
  cout << "opt O lw: " << lw << endl;
#endif
  lw = lapack::gesdd_work ('M', 'A', a);
  cout << "min lw: " << lw << endl << endl;

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
  lw = lapack::gesdd_work ('O', 'A', a);
#endif

  std::vector<cmplx_t> w (lw);

  size_t lrw = lapack::gesdd_rwork ('A', a);
  cout << "lrw: " << lrw << endl << endl;
  std::vector<real_t> rw (lrw);

  size_t liw = lapack::gesdd_iwork (a);
  cout << "liw: " << liw << endl << endl;
  std::vector<int> iw (liw);

  lapack::gesdd ('A', a, s, u, vt, w, rw, iw);

  print_v (s, "s");
  cout << endl;
  print_m (u, "U");
  cout << endl;
  print_m (vt, "V^T");
  cout << endl;

  rm_t sm (m, n);
  for (size_t i = 0; i < s.size(); ++i)
    sm (i,i) = s (i);
  print_m (sm, "S");
  cout << endl;

  a = ublas::prod (u, m_t (ublas::prod (sm, vt)));
  print_m (a, "A == U S V^T");
  cout << endl;

  // part 2

  cout << endl << "part 2" << endl << endl;

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
  lapack::gesdd ('A', a2, s, u, vt);
#else
  lapack::gesdd ('M', 'A', a2, s, u, vt);
#endif

  print_v (s, "s");
  cout << endl;
  print_m (u, "U");
  cout << endl;
  print_m (vt, "V^T");
  cout << endl;

  for (size_t i = 0; i < s.size(); ++i)
    sm (i,i) = s (i);
  print_m (sm, "S");
  cout << endl;

  a2 = ublas::prod (u, m_t (ublas::prod (sm, vt)));
  print_m (a2, "A == U S V^T");
  cout << endl;

  // part 3

  cout << endl << "part 3" << endl << endl;

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
  cout << "opt lw: " << lapack::gesdd_work ('O', 'N', a3) << endl << endl;
  lapack::gesdd (a3, s);
#else
  cout << "min lw: " << lapack::gesdd_work ('M', 'N', a3) << endl << endl;
  lapack::gesdd ('M', 'N', a3, s, u, vt);
#endif

  print_v (s, "singular value only");
  cout << endl;

  cout << endl;
}


#ifndef FEA_MATERIAL
#define FEA_MATERIAL

#include <Eigen/Dense>
#include "../euclid/Geometry"

namespace FEA {
class Material {
	double _young;
	double _poisson;
	public:
	Material(double E=1.,double nu=0.3) : _young(E), _poisson(nu) {};
	const Eigen::Matrix<double,6,6> ConstitutiveLinearIsotropicElastic ();
};

inline const Eigen::Matrix<double,6,6>
Material::ConstitutiveLinearIsotropicElastic () {
	double l = ( _young * _poisson ) / ((1. + _poisson ) * (1. - 2. * _poisson ));
	double g =   _young / (2. * (1. + _poisson ));
	double G = 2. * g;
	Eigen::Matrix<double,6,6> C;
	C << 	l+G, 	l, 		l, 		0., 	0., 	0.,
			l, 		l+G, 	l, 		0., 	0., 	0.,
			l, 		l, 		l+G, 	0., 	0., 	0.,
			0., 	0., 	0., 	g, 		0., 	0.,
			0., 	0., 	0., 	0., 	g, 		0.,
			0., 	0., 	0., 	0., 	0., 	g;
	return C;
};

}

#endif


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                          *** w90.hpp ***                                  //
//                                                                           //
// class to manage methods relating to wannier90 hamiltonian                 //
// templatized over dimension of the mesh                                    //
//                                                                           //
// created November 27, 2017                                                 //
// copyright Christopher N. Singh Binghamton University Physics              //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#ifndef W90_hpp
#define W90_hpp

#include <cassert>
#include <Eigen/Dense>
#include <fstream>
#include <string>
#include <sstream>
#include <vector>

class W90
{
public:
	W90(std::string hr_file) {

		std::ifstream file(hr_file);
		assert(file.is_open());

		std::string line;
		double data;
		std::vector<double> v;

		while (std::getline(file, line)) {
			std::stringstream input(line);
			for (int i = 0; i < 7; ++i) {
				input >> data;
				v.push_back(data);
			}
		}

		hr = Eigen::Map<Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic,
		   Eigen::RowMajor> >(&v[0], v.size()/7, 7);
	}

	Eigen::MatrixXcd fft(const Eigen::Vector3d& k) {

		int dimension = hr.col(3).maxCoeff();
		Eigen::MatrixXcd h = Eigen::MatrixXcd::Zero(dimension, dimension);

		for (int nn = 0; nn < hr.rows(); ++nn) {
			h((int)hr(nn, 3)-1, (int)hr(nn, 4)-1) += tij(nn, k);
		}

		return h;
	}

private:
	Eigen::ArrayXXd hr;

	inline std::complex<double> tij(int nn, const Eigen::Vector3d& k) {

		std::complex<double> i(0, 1);
		std::complex<double> hopping(hr(nn, 5), hr(nn, 6));
		std::complex<double> phase;

		phase = exp(i * (k(0)*hr(nn, 0) + k(1)*hr(nn,1) + k(2)*hr(nn, 2)));

		return hopping * phase;
	}
};

#endif /* W90_hpp */


/* Boost libs/numeric/odeint/examples/multiprecision/cmp_precision.cpp

 Copyright 2009-2013 Karsten Ahnert
 Copyright 2009-2013 Mario Mulansky

 example comparing double to multiprecision using Boost.Multiprecision

 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 <boost/numeric/odeint.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace std;
using namespace boost::numeric::odeint;

typedef boost::multiprecision::cpp_dec_float_50 mp_50;

/* we solve the simple ODE x' = 3/(2t^2) + x/(2t)
 * with initial condition x(1) = 0.
 * Analytic solution is x(t) = sqrt(t) - 1/t
 */

void rhs_m( const mp_50 x , mp_50 &dxdt , const mp_50 t )
{   // version for multiprecision
    dxdt = mp_50(3)/(mp_50(2)*t*t) + x/(mp_50(2)*t);
}

void rhs_d( const double x , double &dxdt , const double t )
{   // version for double precision
    dxdt = 3.0/(2.0*t*t) + x/(2.0*t);
}

// state_type = mp_50 = deriv_type = time_type = mp_50
typedef runge_kutta4< mp_50 , mp_50 , mp_50 , mp_50 , vector_space_algebra , default_operations , never_resizer > stepper_type_m;

typedef runge_kutta4< double , double , double , double , vector_space_algebra , default_operations , never_resizer > stepper_type_d;

int main()
{

    stepper_type_m stepper_m;
    stepper_type_d stepper_d;

    mp_50 dt_m( 0.5 );
    double dt_d( 0.5 );

    cout << "dt" << '\t' << "mp" << '\t' << "double" << endl;

    while( dt_m > 1E-20 )
    {

        mp_50 x_m = 0; //initial value x(1) = 0
        stepper_m.do_step( rhs_m , x_m , mp_50( 1 ) , dt_m );
        double x_d = 0;
        stepper_d.do_step( rhs_d , x_d , 1.0 , dt_d );

        cout << dt_m << '\t';
        cout << abs((x_m - (sqrt(1+dt_m)-mp_50(1)/(1+dt_m)))/x_m) << '\t' ;
        cout << abs((x_d - (sqrt(1+dt_d)-mp_50(1)/(1+dt_d)))/x_d) << endl ;
        dt_m /= 2;
        dt_d /= 2;
    }
}


/*
 * Copyright 2019 Denis Yaroshevskiy
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "algo/factoriadic_representation.h"

#include <algorithm>
#include <array>
#include <numeric>

#include <boost/multiprecision/cpp_int.hpp>

#include "test/catch.h"

namespace algo {
namespace {

using big_int = boost::multiprecision::cpp_int;

struct FailCompilation {};

template <size_t N>
constexpr bool test_factoriadic(int value, std::array<int, N> expected) {
  if (compute_factoriadic_representation_length(value) != N) {
    throw FailCompilation{};
  }
  std::array<int, N> actual = {};
  to_factoriadic_representation(value, actual.begin());

  // non-constexpr array still
  for (size_t i = 0; i < N; ++i) {
    if (actual[i] != expected[i]) return false;
  }

  return value ==
         from_factoriadic_representation<int>(expected.begin(), expected.end());
}

template <typename N>
void test_factoriadic_runtime(N n) {
  std::vector<int> representation(compute_factoriadic_representation_length(n));
  to_factoriadic_representation(n, representation.begin());
  N actual = from_factoriadic_representation<N>(representation.begin(),
                                                representation.end());
  REQUIRE(n == actual);
}

TEST_CASE("algorithm.to_from_factoriadic_representation.builtin",
          "[algorithm]") {
  // Specific examples
  {
    static_assert(test_factoriadic(0, std::array{0}));
    static_assert(test_factoriadic(1, std::array{0, 1}));

    static_assert(test_factoriadic(2, std::array{0, 0, 1}));
    static_assert(test_factoriadic(3, std::array{0, 1, 1}));
    static_assert(test_factoriadic(4, std::array{0, 0, 2}));
    static_assert(test_factoriadic(5, std::array{0, 1, 2}));

    static_assert(test_factoriadic(6, std::array{0, 0, 0, 1}));

    static_assert(test_factoriadic(349, std::array{0, 1, 0, 2, 4, 2}));
  }

  {
    for (int i = 0; i < 1000; ++i) {
      test_factoriadic_runtime(i);
    }
  }
}

TEST_CASE("algorithm.to_factoriadic_representation.cpp_int", "[algorithm]") {
  {
    big_int x{6};
    std::ptrdiff_t lenght_default =
        compute_factoriadic_representation_length(x);
    int length_int = compute_factoriadic_representation_length<int>(x);
    short length_short =
        compute_factoriadic_representation_length<short, int>(x);
    REQUIRE(lenght_default == 4);
    REQUIRE(length_int == 4);
    REQUIRE(length_short == 4);
  }
  {
    auto factorial = [](int x) {
      big_int res(1);
      for (int i = 1; i < x; ++i) {
        res *= i;
      }
      return res;
    };

    for (int i = 100; i < 200; ++i) test_factoriadic_runtime(factorial(i));
  }
}

}  // namespace
}  // namespace algo


/*
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Tests for the Statistics kernels of the utility operations
 */

#define BOOST_TEST_MODULE StatisticsKernels
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>

#include <stdexcept>

#include "StatisticsKernels.h"

using namespace cupcfd::utility::kernels;

// === sum ===

// Test 1: Test sum of arbitrary values, integer
BOOST_AUTO_TEST_CASE(sum_test1)
{
	int data[10] = {5, 6, 1, 7, 2, 61, 51, 32, 13, 1041};
	int sumCmp = 1219;
	int result;

	result = sum(data, 10);

	BOOST_CHECK_EQUAL(result, sumCmp);
}

// Test 2: Test sum of arbitrary values, doubles
BOOST_AUTO_TEST_CASE(sum_test2)
{
	double data[10] = {434.2, 213.1, 52.2, 14.1, 0.000434, 2315.2, 413.2, 754.52, 5267.2, 173543.2};
	double sumCmp = 183006.920434;
	double result;

	result = sum(data, 10);

	BOOST_CHECK_EQUAL(result, sumCmp);
}

// === mean ===
// Test 1:
BOOST_AUTO_TEST_CASE(mean_test1)
{

}

// === median ===
// Test 1:
BOOST_AUTO_TEST_CASE(median_test1)
{

}

// === mode ===
// Test 1:
BOOST_AUTO_TEST_CASE(mode_test1)
{

}

// === stDev ===
// Test 1:
BOOST_AUTO_TEST_CASE(stDev_test1)
{

}

// === count ===
// Test 1:
BOOST_AUTO_TEST_CASE(count_test1)
{

}


#include "multi_cg/multi_cg.hpp"

#include <Eigen/Core>

#include <iostream>

using namespace Eigen;

template<typename T>
struct BlockVector {
    Matrix<T, Dynamic, Dynamic> vec;

    typedef T value_type;

    typedef Matrix<T, Dynamic, 1> VectorT;

    BlockVector(Matrix<T, Dynamic, Dynamic> && X) : vec(std::move(X)) {}

    // Make it easy to switch between f32 and f64.
    template <typename U>
    BlockVector(BlockVector<U> const &X) : vec(X.vec.template cast<T>()) {}

    template <typename U>
    void block_add(BlockVector<U> const &X, size_t num) {
        vec.leftCols(num) += X.vec.leftCols(num).template cast<T>();
    }

    void block_axpy(std::vector<T> alphas, BlockVector const &X, size_t num) {
        DiagonalMatrix<T,Dynamic,Dynamic> D = Map<VectorT>(alphas.data(), num).asDiagonal();
        vec.leftCols(num) += X.vec.leftCols(num) * D;
    }

    void block_axpy_scatter(std::vector<T> alphas, BlockVector const &X, std::vector<size_t> ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            vec.col(ids[i]) += alphas[i] * X.vec.col(i);
        }
    }

    // rhos[i] = dot(X[i], Y[i])
    void block_dot(BlockVector const &Y, std::vector<T> &rhos, size_t num) {
        VectorT result = (vec.leftCols(num).transpose() * Y.vec.leftCols(num)).diagonal();
        VectorT::Map(rhos.data(), result.size()) = result;
    }

    // X[:, i] = Z[:, i] + alpha[i] * X[:, i] for i < num_unconverged
    void block_xpby(BlockVector const &Z, std::vector<T> alphas, size_t num) {
        DiagonalMatrix<T,Dynamic,Dynamic> D = Map<VectorT>(alphas.data(), num).asDiagonal();
        vec.leftCols(num) = Z.vec.leftCols(num) + vec.leftCols(num) * D;
    }

    void copy(BlockVector const &X, size_t num) {
        vec.leftCols(num) = X.vec.leftCols(num);
    }

    void fill(T val) {
        vec.fill(val);
    }

    auto cols() {
        return vec.cols();
    }

    void repack(std::vector<size_t> const &ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            auto j = ids[i];
            if (j != i) {
                vec.col(i) = vec.col(j);
            }
        }
    }
};

// This is a linear but special operator A(X)
// producing AX + XD where D_ii = shifts[i] is a diagonal matrix.
// So column-wise it performs (A + shift[i])X[:, i]
// the multiply function basically does a gemv on every column with a different shift
// so alpha * A(X) + beta * Y.
template <typename T>
struct PosDefMatrixShifted {
    DiagonalMatrix<T, Dynamic, Dynamic> A;
    Matrix<T, Dynamic, 1> shifts;

    PosDefMatrixShifted(DiagonalMatrix<T, Dynamic, Dynamic> && A, Matrix<T, Dynamic, 1> && shifts)
        : A(std::move(A)),
          shifts(std::move(shifts))
    {}

    // Make it easy to switch between f32 and f64.
    template <typename U>
    PosDefMatrixShifted(PosDefMatrixShifted<U> const &mat)
        : A(mat.A.diagonal().template cast<T>().asDiagonal()),
          shifts(mat.shifts.template cast<T>())
    {}

    void multiply(T alpha, BlockVector<T> const &u, T beta, BlockVector<T> &v, size_t num) {
        v.vec.leftCols(num) = alpha * A * u.vec.leftCols(num)
                              + alpha * u.vec.leftCols(num) * shifts.head(num).asDiagonal()
                              + beta * v.vec.leftCols(num);
    }

    void repack(std::vector<size_t> const &ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            auto j = ids[i];

            if (j != i)
                shifts[i] = shifts[j];
        }
    }
};

struct IdentityPreconditioner {
    template<typename T>
    void apply(BlockVector<T> &C, BlockVector<T> const &B) {
        C = B;
    }
    void repack(std::vector<size_t> const &ids) {
        // nothing to do;
    }
};

int main(int argc, char ** argv) {
    // The general idea is to solve Ax = b in mixed precision
    // Define the residual r_k = b - Ax_k
    // and the error e_k := x - x_k
    // which satisfy Ae_k = Ax - Ax_k = b - Ax_k = r_k.
    // So we are gonna solve Ae_k = r_k approximately for e_k, and by definition
    // x = e_k + x_k.
    // So we compute r_k = b - Ax_k in f64, and then solve Ae_k = r_k
    // approximately in f32.
    auto m = argc >= 2 ? std::stoul(argv[1]) : 1000;
    auto n = argc >= 3 ? std::stoul(argv[2]) : 20;
    auto outer_iter = argc >= 4 ? std::stoul(argv[3]) : 100;
    auto inner_iter = argc >= 5 ? std::stoul(argv[4]) : 40;

    if (n > m)
        throw std::runtime_error("matrix order should be >= block size");

    // Let's stick to this no-op preconditioner.
    auto P = IdentityPreconditioner{};

    // Setup f64 matrices and vecs
    auto A_hi = PosDefMatrixShifted<double>{
        VectorXd::LinSpaced(m, 1, m).asDiagonal(),
        VectorXd::LinSpaced(n, 1, n)
    };
    auto X_hi = BlockVector<double>{MatrixXd::Zero(m, n)};
    auto B_hi = BlockVector<double>{MatrixXd::Random(m, n)};
    auto R_hi = BlockVector<double>{MatrixXd::Zero(m, n)};
    auto U_hi = BlockVector<double>{MatrixXd::Zero(m, n)};
    auto C_hi = BlockVector<double>{MatrixXd::Zero(m, n)};

    // Setup f32 stuff.
    auto U_lo = BlockVector<float>{MatrixXf::Zero(m, n)};
    auto C_lo = BlockVector<float>{MatrixXf::Zero(m, n)};
    auto E_lo = BlockVector<float>(MatrixXf::Zero(m, n));
    auto R_lo = BlockVector<float>(MatrixXf::Zero(m, n));

    auto tol = 1e-10;

    std::vector<std::vector<float>> all_resnorms(n);

    for (size_t outer = 0; outer < outer_iter; ++outer) {
        // A_lo is mutated during multi_cg, so let's just reinitialize.
        auto A_lo = PosDefMatrixShifted<float>(A_hi);
        R_hi = B_hi;
        A_hi.multiply(-1.0, X_hi, 1.0, R_hi, n);

        E_lo.fill(0);
        R_lo = R_hi;
        auto iter_resnorms = sirius::cg::multi_cg(
            A_lo, P,
            E_lo, R_lo, U_lo, C_lo,
            inner_iter, tol, true
        );
        X_hi.block_add(E_lo, n);

        // Save all the resnorms
        bool done = true;
        for (size_t i = 0; i < n; ++i) {
            done &= iter_resnorms[i].back() <= tol;
            all_resnorms[i].insert(all_resnorms[i].end(), iter_resnorms[i].begin(), iter_resnorms[i].end());
        }
        if (done) break;
    }

    for (auto r : all_resnorms[0])
        std::cout << r << '\n';
    std::cout << '\n';

    // Compare to a f64-only run.
    X_hi.fill(0);
    R_hi = B_hi;
    auto resnorms_64 = sirius::cg::multi_cg(
        A_hi, P,
        X_hi, R_hi, U_hi, C_hi,
        m * n, tol, true
    );

    for (auto r : resnorms_64[0])
        std::cout << r << '\n';
    std::cout << '\n';
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE fft_evaluation_domain_test

#include <boost/test/unit_test.hpp>

#include <memory>
#include <vector>
#include <cstdint>

#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>

#include <nil/crypto3/algebra/fields/mnt4/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/mnt4/base_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt4.hpp>

#include <nil/crypto3/algebra/fields/mnt6/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/mnt6/base_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt6.hpp>

#include <nil/crypto3/math/coset.hpp>
#include <nil/crypto3/math/domains/arithmetic_sequence_domain.hpp>
#include <nil/crypto3/math/domains/basic_radix2_domain.hpp>
#include <nil/crypto3/math/domains/extended_radix2_domain.hpp>
#include <nil/crypto3/math/domains/geometric_sequence_domain.hpp>
#include <nil/crypto3/math/domains/step_radix2_domain.hpp>

#include <nil/crypto3/math/algorithms/make_evaluation_domain.hpp>

#include <nil/crypto3/math/polynomial/evaluate.hpp>

#include <typeinfo>

using namespace nil::crypto3::algebra;
using namespace nil::crypto3::math;

/**
 * Note: Templatized type referenced with FieldType (instead of canonical FieldType)
 * https://github.com/google/googletest/blob/master/googletest/docs/AdvancedGuide.md#typed-tests
 */

template<typename FieldType>
void test_fft() {
    typedef typename FieldType::value_type value_type;

    const std::size_t m = 4;
    std::vector<value_type> f = {2, 5, 3, 8};

    std::shared_ptr<evaluation_domain<FieldType>> domain;

    domain = make_evaluation_domain<FieldType>(m);

    std::vector<value_type> a(f);

    domain->fft(a);

    std::vector<value_type> idx(m);

    for (std::size_t i = 0; i < m; i++) {
        idx[i] = domain->get_domain_element(i);
    }

    std::cout << "FFT: key = " << typeid(*domain).name() << std::endl;
    for (std::size_t i = 0; i < m; i++) {
        value_type e = evaluate_polynomial(f, idx[i], m);
        std::cout << "idx[" << i << "] = " << idx[i].data << std::endl;
        std::cout << "e = " << e.data << std::endl;
        BOOST_CHECK_EQUAL(e.data, a[i].data);
    }
    std::cout << "is_basic_radix2_domain = " << detail::is_basic_radix2_domain<FieldType>(m) << std::endl;
    std::cout << "is_extended_radix2_domain = " << detail::is_extended_radix2_domain<FieldType>(m) << std::endl;
    std::cout << "is_step_radix2_domain = " << detail::is_step_radix2_domain<FieldType>(m) << std::endl;
    std::cout << "is_geometric_sequence_domain = " << detail::is_geometric_sequence_domain<FieldType>(m) << std::endl;
    std::cout << "is_arithmetic_sequence_domain = " << detail::is_arithmetic_sequence_domain<FieldType>(m) << std::endl;
}

template<typename FieldType>
void test_inverse_fft_of_fft() {
    typedef typename FieldType::value_type value_type;
    const std::size_t m = 4;
    std::vector<value_type> f = {2, 5, 3, 8};

    std::shared_ptr<evaluation_domain<FieldType>> domain;

    domain = make_evaluation_domain<FieldType>(m);

    std::vector<value_type> a(f);
    domain->fft(a);
    domain->inverse_fft(a);

    std::cout << "inverse FFT of FFT: key = " << typeid(*domain).name() << std::endl;
    for (std::size_t i = 0; i < m; i++) {
        std::cout << "a[" << i << "] = " << a[i].data << std::endl;
        BOOST_CHECK_EQUAL(f[i].data, a[i].data);
    }
}

template<typename FieldType>
void test_inverse_coset_ftt_of_coset_fft() {
    typedef typename FieldType::value_type value_type;
    const std::size_t m = 4;
    std::vector<value_type> f = {2, 5, 3, 8};

    value_type coset = value_type(fields::arithmetic_params<FieldType>::multiplicative_generator);

    std::shared_ptr<evaluation_domain<FieldType>> domain;

    domain = make_evaluation_domain<FieldType>(m);

    std::vector<value_type> a(f);
    multiply_by_coset(a, coset);
    domain->fft(a);
    domain->inverse_fft(a);
    multiply_by_coset(a, coset.inversed());

    for (std::size_t i = 0; i < m; i++) {
        BOOST_CHECK_EQUAL(f[i].data, a[i].data);
    }
}

template<typename FieldType>
void test_lagrange_coefficients() {
    typedef typename FieldType::value_type value_type;

    const std::size_t m = 8;
    value_type t = value_type(10);

    std::shared_ptr<evaluation_domain<FieldType>> domain;

    domain = make_evaluation_domain<FieldType>(m);

    std::vector<value_type> a;
    a = domain->evaluate_all_lagrange_polynomials(t);

    std::cout << "LagrangeCoefficients: key = " << typeid(*domain).name() << std::endl;
    std::vector<value_type> d(m);
    for (std::size_t i = 0; i < m; i++) {
        d[i] = domain->get_domain_element(i);
        std::cout << "d[" << i << "] = " << d[i].data << std::endl;
    }

    for (std::size_t i = 0; i < m; i++) {
        value_type e = evaluate_lagrange_polynomial(d, t, m, i);
        BOOST_CHECK_EQUAL(e.data, a[i].data);
        std::cout << "e = " << e.data << std::endl;
    }
}

template<typename FieldType>
void test_compute_z() {
    typedef typename FieldType::value_type value_type;

    const std::size_t m = 8;
    value_type t = value_type(10);

    std::shared_ptr<evaluation_domain<FieldType>> domain;
    domain = make_evaluation_domain<FieldType>(m);

    value_type a;
    a = domain->compute_vanishing_polynomial(t);

    value_type Z = value_type::one();
    std::cout << "ComputeZ: key = " << typeid(*domain).name() << std::endl;
    for (std::size_t i = 0; i < m; i++) {
        Z *= (t - domain->get_domain_element(i));
        std::cout << "Z = " << Z.data << std::endl;
    }

    BOOST_CHECK_EQUAL(Z.data, a.data);
}

BOOST_AUTO_TEST_SUITE(fft_evaluation_domain_test_suite)

BOOST_AUTO_TEST_CASE(fft) {
    test_fft<fields::bls12<381>>();
    test_fft<fields::mnt4<298>>();
}

BOOST_AUTO_TEST_CASE(inverse_fft_to_fft) {
    test_inverse_fft_of_fft<fields::bls12<381>>();
    test_inverse_fft_of_fft<fields::mnt4<298>>();
}

BOOST_AUTO_TEST_CASE(inverse_coset_ftt_to_coset_fft) {
    test_inverse_coset_ftt_of_coset_fft<fields::bls12<381>>();
    test_inverse_coset_ftt_of_coset_fft<fields::mnt4<298>>();
}

BOOST_AUTO_TEST_CASE(lagrange_coefficients) {
    test_lagrange_coefficients<fields::bls12<381>>();
    test_lagrange_coefficients<fields::mnt4<298>>();
}

BOOST_AUTO_TEST_CASE(compute_z) {
    test_compute_z<fields::bls12<381>>();
    test_compute_z<fields::mnt4<298>>();
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2018 CNRS
//

#include "pinocchio/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"

#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
{
  using CppAD::AD;
  using CppAD::NearEqual;

  typedef double Scalar;
  typedef AD<Scalar> ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  // Sample random configuration
  typedef Model::ConfigVectorType CongigVectorType;
  typedef Model::TangentVectorType TangentVectorType;
  CongigVectorType q(model.nq);
  q = pinocchio::randomConfiguration(model);

  TangentVectorType v(TangentVectorType::Random(model.nv));
  TangentVectorType a(TangentVectorType::Random(model.nv));

  Eigen::MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
  Eigen::MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
  Eigen::MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();

  pinocchio::computeRNEADerivatives(model,data,q,v,a,
                              rnea_partial_dq,
                              rnea_partial_dv,
                              rnea_partial_da);

  rnea_partial_da.triangularView<Eigen::StrictlyLower>()
  = rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();

  typedef ADModel::ConfigVectorType ADCongigVectorType;
  typedef ADModel::TangentVectorType ADTangentVectorType;

  ADCongigVectorType ad_q = q.cast<ADScalar>();
  ADTangentVectorType ad_dq = ADTangentVectorType::Zero(model.nv);
  ADTangentVectorType ad_v = v.cast<ADScalar>();
  ADTangentVectorType ad_a = a.cast<ADScalar>();

  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
  pinocchio::crba(model,data,q);
  data.M.triangularView<Eigen::StrictlyLower>()
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  Data::TangentVectorType tau = pinocchio::rnea(model,data,q,v,a);

  // dtau_dq
  {
    CppAD::Independent(ad_dq);
    ADCongigVectorType ad_q_plus = pinocchio::integrate(ad_model,ad_q,ad_dq);
    pinocchio::rnea(ad_model,ad_data,ad_q_plus,ad_v,ad_a);

    VectorXAD Y(model.nv);
    Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.tau;

    CppAD::ADFun<Scalar> ad_fun(ad_dq,Y);

    CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
    Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1).setZero();

    CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0,x);
    BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(),model.nv,1).isApprox(data.tau));

    CPPAD_TESTVECTOR(Scalar) dtau_dq = ad_fun.Jacobian(x);
    Data::MatrixXs dtau_dq_mat = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dtau_dq.data(),model.nv,model.nv);
    BOOST_CHECK(dtau_dq_mat.isApprox(rnea_partial_dq));
  }

  // dtau_dv
  {
    CppAD::Independent(ad_v);
    pinocchio::rnea(ad_model,ad_data,ad_q,ad_v,ad_a);

    VectorXAD Y(model.nv);
    Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.tau;

    CppAD::ADFun<Scalar> ad_fun(ad_v,Y);

    CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
    Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = v;

    CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0,x);
    BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(),model.nv,1).isApprox(data.tau));

    CPPAD_TESTVECTOR(Scalar) dtau_dv = ad_fun.Jacobian(x);
    Data::MatrixXs dtau_dv_mat = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dtau_dv.data(),model.nv,model.nv);
    BOOST_CHECK(dtau_dv_mat.isApprox(rnea_partial_dv));
  }

  // dtau_da
  {
    CppAD::Independent(ad_a);
    pinocchio::rnea(ad_model,ad_data,ad_q,ad_v,ad_a);

    VectorXAD Y(model.nv);
    Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.tau;

    CppAD::ADFun<Scalar> ad_fun(ad_a,Y);

    CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
    Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = a;

    CPPAD_TESTVECTOR(Scalar) tau = ad_fun.Forward(0,x);
    BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(tau.data(),model.nv,1).isApprox(data.tau));

    CPPAD_TESTVECTOR(Scalar) dtau_da = ad_fun.Jacobian(x);
    Data::MatrixXs dtau_da_mat = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(dtau_da.data(),model.nv,model.nv);
    BOOST_CHECK(dtau_da_mat.isApprox(rnea_partial_da));
    BOOST_CHECK(dtau_da_mat.isApprox(data.M));
  }

}

BOOST_AUTO_TEST_CASE(test_aba_derivatives)
{
  using CppAD::AD;
  using CppAD::NearEqual;

  typedef double Scalar;
  typedef AD<Scalar> ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  // Sample random configuration
  typedef Model::ConfigVectorType CongigVectorType;
  typedef Model::TangentVectorType TangentVectorType;
  CongigVectorType q(model.nq);
  q = pinocchio::randomConfiguration(model);

  TangentVectorType v(TangentVectorType::Random(model.nv));
  TangentVectorType tau(TangentVectorType::Random(model.nv));

  Eigen::MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
  Eigen::MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
  Eigen::MatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();

  pinocchio::computeABADerivatives(model,data,q,v,tau,
                             aba_partial_dq,
                             aba_partial_dv,
                             aba_partial_dtau);

  aba_partial_dtau.triangularView<Eigen::StrictlyLower>()
  = aba_partial_dtau.transpose().triangularView<Eigen::StrictlyLower>();

  typedef ADModel::ConfigVectorType ADCongigVectorType;
  typedef ADModel::TangentVectorType ADTangentVectorType;

  ADCongigVectorType ad_q = q.cast<ADScalar>();
  ADTangentVectorType ad_dq = ADTangentVectorType::Zero(model.nv);
  ADTangentVectorType ad_v = v.cast<ADScalar>();
  ADTangentVectorType ad_tau = tau.cast<ADScalar>();

  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
  pinocchio::computeMinverse(model,data,q);
  data.Minv.triangularView<Eigen::StrictlyLower>()
  = data.Minv.transpose().triangularView<Eigen::StrictlyLower>();

  Data::TangentVectorType ddq = pinocchio::aba(model,data,q,v,tau);

  // dddq_dq
  {
    CppAD::Independent(ad_dq);
    ADCongigVectorType ad_q_plus = pinocchio::integrate(ad_model,ad_q,ad_dq);
    pinocchio::aba(ad_model,ad_data,ad_q_plus,ad_v,ad_tau);

    VectorXAD Y(model.nv);
    Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.ddq;

    CppAD::ADFun<Scalar> ad_fun(ad_dq,Y);

    CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
    Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1).setZero();

    CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0,x);
    BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(),model.nv,1).isApprox(data.ddq));

    CPPAD_TESTVECTOR(Scalar) ddq_dq = ad_fun.Jacobian(x);
    Data::MatrixXs ddq_dq_mat = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(ddq_dq.data(),model.nv,model.nv);
    BOOST_CHECK(ddq_dq_mat.isApprox(aba_partial_dq));
  }

  // dddq_dv
  {
    CppAD::Independent(ad_v);
    pinocchio::aba(ad_model,ad_data,ad_q,ad_v,ad_tau);

    VectorXAD Y(model.nv);
    Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.ddq;

    CppAD::ADFun<Scalar> ad_fun(ad_v,Y);

    CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
    Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = v;

    CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0,x);
    BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(),model.nv,1).isApprox(data.ddq));

    CPPAD_TESTVECTOR(Scalar) ddq_dv = ad_fun.Jacobian(x);
    Data::MatrixXs ddq_dv_mat = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(ddq_dv.data(),model.nv,model.nv);
    BOOST_CHECK(ddq_dv_mat.isApprox(aba_partial_dv));
  }

  // dddq_da
  {
    CppAD::Independent(ad_tau);
    pinocchio::aba(ad_model,ad_data,ad_q,ad_v,ad_tau);

    VectorXAD Y(model.nv);
    Eigen::Map<ADData::TangentVectorType>(Y.data(),model.nv,1) = ad_data.ddq;

    CppAD::ADFun<Scalar> ad_fun(ad_tau,Y);

    CPPAD_TESTVECTOR(Scalar) x((size_t)model.nv);
    Eigen::Map<Data::TangentVectorType>(x.data(),model.nv,1) = tau;

    CPPAD_TESTVECTOR(Scalar) ddq = ad_fun.Forward(0,x);
    BOOST_CHECK(Eigen::Map<Data::TangentVectorType>(ddq.data(),model.nv,1).isApprox(data.ddq));

    CPPAD_TESTVECTOR(Scalar) ddq_dtau = ad_fun.Jacobian(x);
    Data::MatrixXs ddq_dtau_mat = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Data::MatrixXs)>(ddq_dtau.data(),model.nv,model.nv);
    BOOST_CHECK(ddq_dtau_mat.isApprox(aba_partial_dtau));
    BOOST_CHECK(ddq_dtau_mat.isApprox(data.Minv));
  }

}

BOOST_AUTO_TEST_SUITE_END()


#ifndef __DYSKTRA__
#define __DYSKTRA__

#include <Eigen/Dense>
#include "proximals.hpp"

namespace proxopp {

//  Dykstra's algorithm to compute projections on intersection of convex sets
//  P. L. Combettes and J.-C. Pesquet, "Proximal splitting methods in signal processing"

class DykstraProjection
{
	public:
		DykstraProjection(proximalOperator *pF, proximalOperator *pG,
				int n_iterations=10) :
			pF(pF), pG(pG), n_iterations(n_iterations) {}
		// Provided the convex F and G are "simple", convergence
		// should be fast.

		~DykstraProjection() {}

		Eigen::VectorXf project(Eigen::VectorXf& x0)
		{
			Eigen::VectorXf x,y,p,q;

			x = x0;
			p = q = Eigen::VectorXf::Zeros(x0.rows());

			for(int k=0; k < n_iterations;k++)
			{
				y = (*pF)(x+p);
				p = x + p - y;
				x = (*pG)(y+q);
				q = y + q - x;
			}

			return x;
		}
	private:
		proximalOperator *pF, *pG;
		int n_itierations;
};
} // namespace proxopp

#endif


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

#ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_
#define CROCODDYL_CORE_SOLVERS_DDP_HPP_

#include <Eigen/Cholesky>
#include <vector>

#include "crocoddyl/core/solver-base.hpp"

namespace crocoddyl {

/**
 * @brief Differential Dynamic Programming (DDP) solver
 *
 * The DDP solver computes an optimal trajectory and control commands by iterates running `backwardPass()` and
 * `forwardPass()`. The backward-pass updates locally the quadratic approximation of the problem and computes descent
 * direction. If the warm-start is feasible, then it computes the gaps \f$\mathbf{\bar{f}}_s\f$ and run a modified
 * Riccati sweep:
 * \f{eqnarray*}
 *   \mathbf{Q}_{\mathbf{x}_k} &=& \mathbf{l}_{\mathbf{x}_k} + \mathbf{f}^\top_{\mathbf{x}_k} (V_{\mathbf{x}_{k+1}} +
 * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
 *   \mathbf{Q}_{\mathbf{u}_k} &=& \mathbf{l}_{\mathbf{u}_k} + \mathbf{f}^\top_{\mathbf{u}_k} (V_{\mathbf{x}_{k+1}} +
 * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
 *   \mathbf{Q}_{\mathbf{xx}_k} &=& \mathbf{l}_{\mathbf{xx}_k} + \mathbf{f}^\top_{\mathbf{x}_k} V_{\mathbf{xx}_{k+1}}
 * \mathbf{f}_{\mathbf{x}_k},\\
 *   \mathbf{Q}_{\mathbf{xu}_k} &=& \mathbf{l}_{\mathbf{xu}_k} + \mathbf{f}^\top_{\mathbf{x}_k} V_{\mathbf{xx}_{k+1}}
 * \mathbf{f}_{\mathbf{u}_k},\\
 *   \mathbf{Q}_{\mathbf{uu}_k} &=& \mathbf{l}_{\mathbf{uu}_k} + \mathbf{f}^\top_{\mathbf{u}_k} V_{\mathbf{xx}_{k+1}}
 * \mathbf{f}_{\mathbf{u}_k}.
 * \f}
 * Then, the forward-pass rollouts this new policy by integrating the system dynamics along a tuple of optimized
 * control commands \f$\mathbf{u}^*_s\f$, i.e.
 * \f{eqnarray}
 *   \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0,\\
 *   \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\
 *   \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k).
 * \f}
 *
 * \sa `backwardPass()` and `forwardPass()`
 */
class SolverDDP : public SolverAbstract {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the DDP solver
   *
   * @param[in] problem  Shooting problem
   */
  explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
  virtual ~SolverDDP();

  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
                     const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t& maxiter = 100,
                     const bool& is_feasible = false, const double& regInit = 1e-9);
  virtual void computeDirection(const bool& recalc = true);
  virtual double tryStep(const double& steplength = 1);
  virtual double stoppingCriteria();
  virtual const Eigen::Vector2d& expectedImprovement();

  /**
   * @brief Update the Jacobian and Hessian of the optimal control problem
   *
   * These derivatives are computed around the guess state and control trajectory. These trajectory can be set by using
   * `setCandidate()`.
   *
   * @return  The total cost around the guess trajectory
   */
  virtual double calcDiff();

  /**
   * @brief Run the backward pass (Riccati sweep)
   *
   * It assumes that the Jacobian and Hessians of the optimal control problem have been compute (i.e. `calcDiff()`).
   * The backward pass handles infeasible guess through a modified Riccati sweep:
   * \f{eqnarray*}
   *   \mathbf{Q}_{\mathbf{x}_k} &=& \mathbf{l}_{\mathbf{x}_k} + \mathbf{f}^\top_{\mathbf{x}_k} (V_{\mathbf{x}_{k+1}}
   * +
   * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
   *   \mathbf{Q}_{\mathbf{u}_k} &=& \mathbf{l}_{\mathbf{u}_k} + \mathbf{f}^\top_{\mathbf{u}_k} (V_{\mathbf{x}_{k+1}}
   * +
   * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
   *   \mathbf{Q}_{\mathbf{xx}_k} &=& \mathbf{l}_{\mathbf{xx}_k} + \mathbf{f}^\top_{\mathbf{x}_k}
   * V_{\mathbf{xx}_{k+1}}
   * \mathbf{f}_{\mathbf{x}_k},\\
   *   \mathbf{Q}_{\mathbf{xu}_k} &=& \mathbf{l}_{\mathbf{xu}_k} + \mathbf{f}^\top_{\mathbf{x}_k}
   * V_{\mathbf{xx}_{k+1}}
   * \mathbf{f}_{\mathbf{u}_k},\\
   *   \mathbf{Q}_{\mathbf{uu}_k} &=& \mathbf{l}_{\mathbf{uu}_k} + \mathbf{f}^\top_{\mathbf{u}_k}
   * V_{\mathbf{xx}_{k+1}} \mathbf{f}_{\mathbf{u}_k}, \f} where
   * \f$\mathbf{l}_{\mathbf{x}_k}\f$,\f$\mathbf{l}_{\mathbf{u}_k}\f$,\f$\mathbf{f}_{\mathbf{x}_k}\f$ and
   * \f$\mathbf{f}_{\mathbf{u}_k}\f$ are the Jacobians of the cost function and dynamics,
   * \f$\mathbf{l}_{\mathbf{xx}_k}\f$,\f$\mathbf{l}_{\mathbf{xu}_k}\f$ and \f$\mathbf{l}_{\mathbf{uu}_k}\f$ are the
   * Hessians of the cost function, \f$V_{\mathbf{x}_{k+1}}\f$ and \f$V_{\mathbf{xx}_{k+1}}\f$ defines the
   * linear-quadratic approximation of the Value function, and \f$\mathbf{\bar{f}}_{k+1}\f$ describes the gaps of the
   * dynamics.
   */
  virtual void backwardPass();

  /**
   * @brief Run the forward pass or rollout
   *
   * It rollouts the action model given the computed policy (feedforward terns and feedback gains) by the
   * `backwardPass()`:
   * \f{eqnarray}
   *   \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0,\\
   *   \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\
   *   \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k).
   * \f}
   * We can define different step lengths \f$\alpha\f$.
   *
   * @param  stepLength  applied step length (\f$0\leq\alpha\leq1\f$)
   */
  virtual void forwardPass(const double& stepLength);

  /**
   * @brief Compute the feedforward and feedback terms using a Cholesky decomposition
   *
   * To compute the feedforward \f$\mathbf{k}_k\f$ and feedback \f$\mathbf{K}_k\f$ terms, we use a Cholesky
   * decomposition to solve \f$\mathbf{Q}_{\mathbf{uu}_k}^{-1}\f$ term:
   * \f{eqnarray}
   * \mathbf{k}_k &=& \mathbf{Q}_{\mathbf{uu}_k}^{-1}\mathbf{Q}_{\mathbf{u}},\\
   * \mathbf{K}_k &=& \mathbf{Q}_{\mathbf{uu}_k}^{-1}\mathbf{Q}_{\mathbf{ux}}.
   * \f}
   *
   * Note that if the Cholesky decomposition fails, then we re-start the backward pass and increase the
   * state and control regularization values.
   */
  virtual void computeGains(const std::size_t& t);

  /**
   * @brief Increase the state and control regularization values by a `regfactor_` factor
   */
  void increaseRegularization();

  /**
   * @brief Decrease the state and control regularization values by a `regfactor_` factor
   */
  void decreaseRegularization();

  /**
   * @brief Allocate all the internal data needed for the solver
   */
  virtual void allocateData();

  /**
   * @brief Return the regularization factor used to decrease / increase it
   */
  const double& get_regfactor() const;

  /**
   * @brief Return the minimum regularization value
   */
  const double& get_regmin() const;

  /**
   * @brief Return the maximum regularization value
   */
  const double& get_regmax() const;

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

  /**
   * @brief Return the step-length threshold used to decrease regularization
   */
  const double& get_th_stepdec() const;

  /**
   * @brief Return the step-length threshold used to increase regularization
   */
  const double& get_th_stepinc() const;

  /**
   * @brief Return the tolerance of the expected gradient used for testing the step
   */
  const double& get_th_grad() const;

  /**
   * @brief Return the threshold for accepting a gap as non-zero
   */
  const double& get_th_gaptol() const;

  /**
   * @brief Return the Hessian of the Value function \f$V_{\mathbf{xx}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Vxx() const;

  /**
   * @brief Return the Hessian of the Value function \f$V_{\mathbf{x}_s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_Vx() const;

  /**
   * @brief Return the Hessian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{xx}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Qxx() const;

  /**
   * @brief Return the Hessian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{xu}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Qxu() const;

  /**
   * @brief Return the Hessian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{uu}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Quu() const;

  /**
   * @brief Return the Jacobian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{x}_s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_Qx() const;

  /**
   * @brief Return the Jacobian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{u}_s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_Qu() const;

  /**
   * @brief Return the feedback gains \f$\mathbf{K}_{s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_K() const;

  /**
   * @brief Return the feedforward gains \f$\mathbf{k}_{s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_k() const;

  /**
   * @brief Return the gaps \f$\mathbf{\bar{f}}_{s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_fs() const;

  /**
   * @brief Modify the regularization factor used to decrease / increase it
   */
  void set_regfactor(const double& reg_factor);

  /**
   * @brief Modify the minimum regularization value
   */
  void set_regmin(const double& regmin);

  /**
   * @brief Modify the maximum regularization value
   */
  void set_regmax(const double& regmax);

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

  /**
   * @brief Modify the step-length threshold used to decrease regularization
   */
  void set_th_stepdec(const double& th_step);

  /**
   * @brief Modify the step-length threshold used to increase regularization
   */
  void set_th_stepinc(const double& th_step);

  /**
   * @brief Modify the tolerance of the expected gradient used for testing the step
   */
  void set_th_grad(const double& th_grad);

  /**
   * @brief Modify the threshold for accepting a gap as non-zero
   */
  void set_th_gaptol(const double& th_gaptol);

 protected:
  double regfactor_;  //!< Regularization factor used to decrease / increase it
  double regmin_;     //!< Minimum allowed regularization value
  double regmax_;     //!< Maximum allowed regularization value

  double cost_try_;                      //!< Total cost computed by line-search procedure
  std::vector<Eigen::VectorXd> xs_try_;  //!< State trajectory computed by line-search procedure
  std::vector<Eigen::VectorXd> us_try_;  //!< Control trajectory computed by line-search procedure
  std::vector<Eigen::VectorXd> dx_;

  // allocate data
  std::vector<Eigen::MatrixXd> Vxx_;  //!< Hessian of the Value function
  std::vector<Eigen::VectorXd> Vx_;   //!< Gradient of the Value function
  std::vector<Eigen::MatrixXd> Qxx_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::MatrixXd> Qxu_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::MatrixXd> Quu_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::VectorXd> Qx_;   //!< Gradient of the Hamiltonian
  std::vector<Eigen::VectorXd> Qu_;   //!< Gradient of the Hamiltonian
  std::vector<Eigen::MatrixXd> K_;    //!< Feedback gains
  std::vector<Eigen::VectorXd> k_;    //!< Feed-forward terms
  std::vector<Eigen::VectorXd> fs_;   //!< Gaps/defects between shooting nodes

  Eigen::VectorXd xnext_;                              //!< Next state
  Eigen::MatrixXd FxTVxx_p_;                           //!< fxTVxx_p_
  std::vector<Eigen::MatrixXd> FuTVxx_p_;              //!< fuTVxx_p_
  Eigen::VectorXd fTVxx_p_;                            //!< fTVxx_p term
  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;  //!< Cholesky LLT solver
  std::vector<Eigen::VectorXd> Quuk_;                  //!< Quuk term
  std::vector<double> alphas_;                         //!< Set of step lengths using by the line-search procedure
  double th_grad_;     //!< Tolerance of the expected gradient used for testing the step
  double th_gaptol_;   //!< Threshold limit to check non-zero gaps
  double th_stepdec_;  //!< Step-length threshold used to decrease regularization
  double th_stepinc_;  //!< Step-length threshold used to increase regularization
  bool was_feasible_;  //!< Label that indicates in the previous iterate was feasible
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_DDP_HPP_