#include "MPC_functions.h"
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>

extern "C"
{
#include <gurobi_c.h>
}

using namespace Eigen;
using namespace std;


void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, MatrixAtype &A, MatrixBtype &B)
{
    double m = params.m;
    A = MatrixAtype::Zero();
    A.topRightCorner(3,3) = Matrix3d::Identity();

    double gamma = u_vec(0);
    double beta = u_vec(1);
    double alpha = u_vec(2);
    double ft = u_vec(3);

    double c_alpha = cos(alpha);
    double s_alpha = sin(alpha);

    double s_beta = sin(beta);
    double c_beta = cos(beta);
    double c_gamma = cos(gamma);
    double s_gamma = sin(gamma);

    B = MatrixBtype::Zero();
    B.row(3) << (ft*(c_gamma*s_alpha - c_alpha*s_beta*s_gamma))/m, (ft*c_alpha*c_beta*c_gamma)/m, (ft*(c_alpha*s_gamma - c_gamma*s_alpha*s_beta))/m,  (s_alpha*s_gamma + c_alpha*c_gamma*s_beta)/m;
    B.row(4) << -(ft*(c_alpha*c_gamma + s_alpha*s_beta*s_gamma))/m, (ft*c_beta*c_gamma*s_alpha)/m, (ft*(s_alpha*s_gamma + c_alpha*c_gamma*s_beta))/m, -(c_alpha*s_gamma - c_gamma*s_alpha*s_beta)/m;
    B.row(5) << -(ft*c_beta*s_gamma)/m,           -(ft*c_gamma*s_beta)/m,                                                                0,                                     (c_beta*c_gamma)/m;

}

void F_crazyflie_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec)
{

    double m = params.m;
    double g = params.g;

    double c_alpha = cos(u_vec(2));
    double s_alpha = sin(u_vec(2));
    double s_beta = sin(u_vec(1));
    double c_beta = cos(u_vec(1));
    double c_gamma = cos(u_vec(0));
    double s_gamma = sin(u_vec(0));

    double c = u_vec(3)/m;

    dx_vec.head(3) = x_vec.tail(3);
    dx_vec.tail(3) << c*(c_alpha*s_beta*c_gamma + s_alpha*s_gamma),
                      c*(s_alpha*s_beta*c_gamma - c_alpha*s_gamma),
                      c*(c_beta*c_gamma) - g;

}


void discretization_affine(const MatrixAtype A, const MatrixBtype B, const VectorXtype x_tray, const VectorUtype u_tray, params_t params, MatrixAtype &A_d, MatrixBtype &B_d, VectorXtype &g_d)
{
    VectorXtype g;
    VectorXtype temp;

    F_crazyflie_6_states(x_tray,u_tray, params, temp);

    g = temp - A*x_tray - B*u_tray;

    int c_A = A.cols();
    int c_B = B.cols();

    MatrixXd M(c_A + c_B + 1, c_A + c_B + 1);


    M << A, B, g,
        MatrixXd::Zero(c_B+1, c_A), MatrixXd::Zero(c_B+1, c_B), MatrixXd::Zero(c_B+1,1);

    // std::cout << M << std::endl;

    // % discretization
    MatrixXd M_d;

    M = M*params.Ts;
    M_d = M.exp();


    A_d = M_d.topLeftCorner(c_A, c_A);
    B_d = M_d.block(0,c_A,c_A,c_B);
    g_d = M_d.topRightCorner(c_A, 1);


    // Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
    // std::cout << A_d.format(OctaveFmt) << std::endl << std::endl;
    // std::cout << B_d.format(OctaveFmt) << std::endl << std::endl;
    // std::cout << g_d.format(OctaveFmt) << std::endl << std::endl;
}

void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vector<VectorUtype> U_tray, params_t params, std::vector<MatrixAtype> &A_tray, std::vector<MatrixBtype> &B_tray, std::vector<VectorXtype> &g_tray)
{
    int N = X_tray.size();

    A_tray.clear();
    B_tray.clear();
    g_tray.clear();

    for(int i = 0; i < N; i++)
    {
        MatrixAtype A_temp;
        MatrixBtype B_temp;
        VectorXtype g_temp;

        linearization_fast_euler_6_states(X_tray[i], U_tray[i], params, A_temp, B_temp);
        discretization_affine(A_temp, B_temp, X_tray[i], U_tray[i], params, A_temp, B_temp, g_temp);

        A_tray.push_back(A_temp);
        B_tray.push_back(B_temp);
        g_tray.push_back(g_temp);
    }
}



void euler_method_forward(F_callback_type pF, double t0, double h, double tfinal, VectorXtype y0, VectorUtype u, params_t params, std::vector<VectorXtype> &yout)
{
    VectorXtype y = y0;
    VectorXtype y_next;
    std::vector<VectorXtype> yout_temp;
    yout_temp.push_back(y);

    for(double t = t0; t < tfinal; t = t + h)
    {
        pF(y, u, params, y_next);

        y = y + h*y_next;
        yout_temp.push_back(y);
    }
    yout = yout_temp;
}

void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, int N, MatrixXd &Sx, MatrixXd &Su, MatrixXd &Sg)
{
    int N_A = A_tray.size();
    int N_B = B_tray.size();

    if(N_A != N)
        std::cout << "Error: number of A matrices hsould be equal to N, however it is equal to: "<< N_A <<endl << endl;
    if(N_B != N)
        std::cout << "Error: number of B matrices hsould be equal to N, however it is equal to: "<< N_B <<endl << endl;
    // int r_A = A_tray[0].rows();
    // int c_A = A_tray[0].cols();
    // int r_A = B_tray[0].rows();
    // int c_A = B_tray[0].cols();

    int r_A = N_x;
    int c_A = N_x;
    int r_B = N_x;
    int c_B = N_u;

    // S_x is the temporal, local variable that will in the end be the output, Sx

    MatrixXd S_x = MatrixXd::Zero(r_A*(N+1), c_A);
    MatrixAtype A_mult = MatrixXd::Identity(r_A, r_A);

    // construct S_x:
    for(int i = 0; i < N+1; i++)
    {

        S_x.middleRows(i*r_A, r_A) << A_mult;
        if (i < N)              // we cannot access index N in A_tray, protect it
        {
            A_mult = A_tray[i]*A_mult;
        }
    }


    // construct Su

    MatrixXd S_u = MatrixXd::Zero(r_B*(N+1), c_B*N);

    // temporal variables:

    MatrixXd column_Su;


    for(int i = 0; i < N; i++)
    {
        column_Su = MatrixXd::Ones(r_B * (N+1), c_B);
        A_mult = MatrixXd::Identity(r_A, c_A);

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

                column_Su.middleRows(r_B*j, r_B) << B_tray[i];
            }
            else if (j > i+1)
            {
                A_mult = A_tray[j-1]  * A_mult;
                column_Su.middleRows(r_B*j, r_B) << A_mult * B_tray[i];
            }
            else
            {
                // can possibly omit this else, just for clarity. Of course first we need to initizlize to zero instead of ones columns Su
                column_Su.middleRows(r_B*j, r_B).setZero();
            }
        }

        S_u.middleCols(c_B*i, c_B) << column_Su;
    }

    // build Sg matrix

    MatrixXd S_g = MatrixXd::Zero(r_A*(N+1), c_A*N);

    // temporal variables:

    MatrixXd column_Sg;


    for(int i = 0; i < N; i++)
    {
        column_Sg = MatrixXd::Ones(r_A * (N+1), c_A);
        A_mult = MatrixXd::Identity(r_A, c_A);

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

                column_Sg.middleRows(r_A*j, r_A).setIdentity();
            }
            else if (j > i+1)
            {
                A_mult = A_tray[j-1]  * A_mult;
                column_Sg.middleRows(r_A*j, r_A) << A_mult;
            }
            else
            {
                // can possibly omit this else, just for clarity. Of course first we need to initizlize to zero instead of ones columns Su
                column_Sg.middleRows(r_A*j, r_A).setZero();
            }
        }

        S_g.middleCols(c_A*i, c_A) << column_Sg;
    }

    Sx = S_x;
    Su = S_u;
    Sg = S_g;
}


void funQbar(MatrixXd Q, MatrixXd P, int N, MatrixXd &Qbar)
{
    MatrixXd Q_bar;
    MatrixXd diag_mask = VectorXd::Ones(N+1).asDiagonal();
    Q_bar = kroneckerProduct(diag_mask, Q);
    Q_bar.bottomRightCorner(P.rows(), P.cols()) = P;

    Qbar = Q_bar;
}

void funRbar(MatrixXd R, int N, MatrixXd &Rbar)
{
    MatrixXd R_bar;
    MatrixXd diag_mask = VectorXd::Ones(N).asDiagonal();
    R_bar = kroneckerProduct(diag_mask, R);
    Rbar = R_bar;
}

VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXd Q, MatrixXd R, MatrixXd P, int N, VectorXtype x0, VectorXd X_ref, VectorXd U_ref, double ft_min, double ft_max, double af)
{
    Eigen::MatrixXd Sx;
    Eigen::MatrixXd Su;
    Eigen::MatrixXd Sg;

    VectorUtype u_optimal;

    funSxSuSg_varying_affine(A_tray, B_tray, N, Sx, Su, Sg);

    VectorXd g_tray_col(N*N_x);

    for(int i = 0; i < g_tray.size(); i++)
    {
        g_tray_col.segment(i*N_x, N_x) = g_tray[i];
    }

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

    Eigen::MatrixXd Qbar;
    Eigen::MatrixXd Rbar;

    funQbar(Q, P, N, Qbar);
    funRbar(R,N, Rbar);

    Eigen::MatrixXd H;
    Eigen::MatrixXd F;

    H = Su.transpose()*Qbar*Su + Rbar;

    F = 2*(x0.transpose()*Sx.transpose()*Qbar*Su + g_tray_col.transpose()*Sg.transpose()*Qbar*Su - X_ref.transpose()*Qbar*Su - U_ref.transpose()*Rbar);

    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");

    // std::cout << "H test:" << std::endl << std::endl;
    // std::cout << H.format(CleanFmt) << std::endl;

    // ft_min and ft_max constraints:
    RowVectorXd pattern(4);
    pattern << 0, 0, 0, 1;

    // std::cout << "pattern test:" << std::endl << std::endl;
    // std::cout << pattern.format(CleanFmt) << std::endl;

    MatrixXd Au_kernel(2*N, N);
    Au_kernel.topRows(N) = MatrixXd::Identity(N,N);
    Au_kernel.bottomRows(N) = (-1)*MatrixXd::Identity(N,N);

    // std::cout << "Au_kernel test:" << std::endl << std::endl;
    // std::cout << Au_kernel.format(CleanFmt) << std::endl;




    MatrixXd Au = kroneckerProduct(Au_kernel, pattern);
    VectorXd bu(2*N);
    bu.topRows(N) = VectorXd::Ones(N)*ft_max;
    bu.bottomRows(N) = VectorXd::Ones(N)*(-ft_min);

    // std::cout << "Au test:" << std::endl << std::endl;
    // std::cout << Au.format(CleanFmt) << std::endl;

    // std::cout << "bu test:" << std::endl << std::endl;
    // std::cout << bu.format(CleanFmt) << std::endl;

    // Eigen::MatrixXd W = MatrixXd::Zero(4,4);

    // W(1,1) = 1/(af^2);
    // W(2,2) = 1/(af^2);

    Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu, af);
    // Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
    // Eigen::VectorXd U_0 = solve_QP(H, F);

    return U_0;
}


VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXd Q, MatrixXd R, MatrixXd P, int N, VectorXtype x0, VectorXd X_ref, VectorXd U_ref, double ft_min, double ft_max, double af)
{
    Eigen::MatrixXd Sx;
    Eigen::MatrixXd Su;
    Eigen::MatrixXd Sg;

    funSxSuSg_varying_affine(A_tray, B_tray, N, Sx, Su, Sg);

    VectorXd g_tray_col(N*N_x);

    for(int i = 0; i < g_tray.size(); i++)
    {
        g_tray_col.segment(i*N_x, N_x) = g_tray[i];
    }

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

    Eigen::MatrixXd Qbar_1;
    Eigen::MatrixXd Rbar;

    funQbar(Q, Q, N-1, Qbar_1);
    funRbar(R,N, Rbar);

    Eigen::MatrixXd H;
    Eigen::MatrixXd F;

    int size_1 = N_x*(N-1);

    Eigen::MatrixXd Su_1 = Su.topRows(size_1);
    Eigen::MatrixXd Sx_1 = Sx.topRows(size_1);
    Eigen::MatrixXd Sg_1 = Sg.topRows(size_1);

    Eigen::VectorXd X_ref_1 = X_ref.head(size_1);

    Eigen::MatrixXd Su_N = Su.bottomRows(N_x);
    Eigen::MatrixXd Sx_N = Sx.bottomRows(N_x);
    Eigen::MatrixXd Sg_N = Sg.bottomRows(N_x);

    VectorXtype X_ref_N = X_ref.tail(N_x);

    H = Su_1.transpose()*Qbar_1*Su_1 + Rbar;

    // in this case, F is a row vector
    F = 2*(x0.transpose()*Sx_1.transpose()*Qbar_1*Su_1 + g_tray_col.transpose()*Sg_1.transpose()*Qbar_1*Su_1 - X_ref_1.transpose()*Qbar_1*Su_1 - U_ref.transpose()*Rbar);

    // include epigraph in the matrices:
    Eigen::MatrixXd H_final = Eigen::MatrixXd::Zero(H.rows() + 1, H.cols() + 1);
    Eigen::MatrixXd F_final = Eigen::MatrixXd::Zero(F.rows(), F.cols() + 1);

    H_final.topLeftCorner(H.rows(),H.cols()) << H;

    F_final.leftCols(N*N_u) << F;
    F_final.rightCols(1) << 1.0;

    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");

    // std::cout << "H test:" << std::endl << std::endl;
    // std::cout << H.format(CleanFmt) << std::endl;

    // ft_min and ft_max constraints:

    RowVectorXd pattern(4);
    pattern << 0, 0, 0, 1;

    // std::cout << "pattern test:" << std::endl << std::endl;
    // std::cout << pattern.format(CleanFmt) << std::endl;

    MatrixXd Au_kernel(2*N, N);
    Au_kernel.topRows(N) = MatrixXd::Identity(N,N);
    Au_kernel.bottomRows(N) = (-1)*MatrixXd::Identity(N,N);

    // std::cout << "Au_kernel test:" << std::endl << std::endl;
    // std::cout << Au_kernel.format(CleanFmt) << std::endl;




    MatrixXd Au = kroneckerProduct(Au_kernel, pattern);
    VectorXd bu(2*N);
    bu.topRows(N) = VectorXd::Ones(N)*ft_max;
    bu.bottomRows(N) = VectorXd::Ones(N)*(-ft_min);

    // std::cout << "Au test:" << std::endl << std::endl;
    // std::cout << Au.format(CleanFmt) << std::endl;

    // std::cout << "bu test:" << std::endl << std::endl;
    // std::cout << bu.format(CleanFmt) << std::endl;

    // Eigen::MatrixXd W = MatrixXd::Zero(4,4);

    // W(1,1) = 1/(af^2);
    // W(2,2) = 1/(af^2);

    Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu, af);
    // Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
    // Eigen::VectorXd U_0 = solve_QP(H, F);

    return U_0;
}

VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b, double af)
{
    // H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
    // min(x'Hx + F'x)

    int size_H = H.cols();
    int cols_A = A.cols();
    int rows_A = A.rows();

    std::vector<int> q_row;
    std::vector<int> q_col;
    std::vector<double> q_val;
    std::vector<double> l_obj_coeff;

    for(int i = 0; i < size_H; i++)
    {
        for(int j = i; j < size_H; j++)
        {
            // std::cout << "i,j -->" << i << "," << j << std::endl;
            if(H(i,j) != 0)
            {
                q_row.push_back(i);
                q_col.push_back(j);

                if(i == j)
                {
                    q_val.push_back(H(i,j));
                    // std::cout << "H(i,j)" << H(i,j) << endl;
                }
                else
                {
                    q_val.push_back(2*H(i,j));
                    // std::cout << "2H(i,j)" << 2*H(i,j) << endl;
                }
            }
        }
        l_obj_coeff.push_back(F(i));
    }



    int N_vars = size_H;
    int N_q_coeffs = q_val.size();

    // BEGINNING C INTERFACE:

    GRBenv   *env   = NULL;
    GRBmodel *model = NULL;
    int       error = 0;
    double    sol[N_vars];
    int       ind[N_vars];
    double    val[N_vars];
    int*       qrow = q_row.data();
    int*       qcol = q_col.data();
    double*    qval = q_val.data();
    int       optimstatus;
    double    objval;

    std::vector<double> ub(N_vars, GRB_INFINITY);
    std::vector<double> lb(N_vars, -GRB_INFINITY);

    std::vector<int> l_ind;
    std::vector<double> l_coeff;

    int quad_row[2];
    int quad_col[2];
    double quad_coeff[2];

    int N_horizon = size_H/N_u;




    /* Create environment */
    error = GRBloadenv(&env, "qp.log");
    if (error) goto QUIT;

    /* Create a model */
    error = GRBnewmodel(env, &model, "qp", N_vars, l_obj_coeff.data(),lb.data(), ub.data(), NULL, NULL);
    if (error) goto QUIT;

    /* Quadratic objective terms */

    error = GRBaddqpterms(model, N_q_coeffs, qrow, qcol, qval);
    if (error) goto QUIT;

    error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0);
    if (error) goto QUIT;

    for(int i = 0; i < rows_A; i++)
    {
        l_ind.clear();
        l_coeff.clear();
        for(int j = 0; j < cols_A; j++)
        {
            if(A(i,j) != 0)
            {
                l_ind.push_back(j);
                l_coeff.push_back(A(i,j));
            }
        }
        error = GRBaddconstr(model, l_ind.size(), l_ind.data(), l_coeff.data(), GRB_LESS_EQUAL, b(i), NULL);
        if (error) goto QUIT;
    }



    // Quadratic constraints.

    for(int i = 0; i < N_horizon; i++)
    {
        quad_row[0] = N_u*i;
        quad_col[0] = N_u*i;
        quad_row[1] = N_u*i + 1;
        quad_col[1] = N_u*i + 1;
        quad_coeff[0] = 1/(af*af);
        quad_coeff[1] = 1/(af*af);

        error = GRBaddqconstr(model, 0, NULL, NULL, 2, quad_row, quad_col, quad_coeff,
                        GRB_LESS_EQUAL, 1.0, NULL);
        if (error) goto QUIT;
    }


    // Optimize model

    error = GRBoptimize(model);
    if (error) goto QUIT;

    /* Write model to 'qp.lp' */
    error = GRBwrite(model, "qp.lp");
    if (error) goto QUIT;

    /* Capture solution information */
    error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
    if (error) goto QUIT;

    error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
    if (error) goto QUIT;

    error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, N_vars, sol);
    if (error) goto QUIT;

    // printf("\nOptimization complete\n");
    if (optimstatus == GRB_OPTIMAL) {
        // printf("Optimal objective: %.4e\n", objval);
        // for(int i = 0; i < N_vars; i++)
        // {
        //     printf("sol[i] = %.4f\n", sol[i]);
        // }
    } else if (optimstatus == GRB_INF_OR_UNBD) {
        printf("Model is infeasible or unbounded\n");
    } else {
        printf("Optimization was stopped early\n");
    }


QUIT:

    /* Error reporting */

    if (error) {
        printf("ERROR: %s\n", GRBgeterrormsg(env));
        exit(1);
    }

    /* Free model */

    GRBfreemodel(model);

    /* Free environment */

    GRBfreeenv(env);

    // END C INTERFACE

    Map<VectorXd> U_0(sol,N_vars);
    // Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    // cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
    return U_0;
}

VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b)
{
    // H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
    // min(x'Hx + F'x)

    int size_H = H.cols();
    int cols_A = A.cols();
    int rows_A = A.rows();

    std::vector<int> q_row;
    std::vector<int> q_col;
    std::vector<double> q_val;
    std::vector<double> l_obj_coeff;

    for(int i = 0; i < size_H; i++)
    {
        for(int j = i; j < size_H; j++)
        {
            // std::cout << "i,j -->" << i << "," << j << std::endl;
            if(H(i,j) != 0)
            {
                q_row.push_back(i);
                q_col.push_back(j);

                if(i == j)
                {
                    q_val.push_back(H(i,j));
                    // std::cout << "H(i,j)" << H(i,j) << endl;
                }
                else
                {
                    q_val.push_back(2*H(i,j));
                    // std::cout << "2H(i,j)" << 2*H(i,j) << endl;
                }
            }
        }
        l_obj_coeff.push_back(F(i));
    }



    int N_vars = size_H;
    int N_q_coeffs = q_val.size();

    // BEGINNING C INTERFACE:

    GRBenv   *env   = NULL;
    GRBmodel *model = NULL;
    int       error = 0;
    double    sol[N_vars];
    int       ind[N_vars];
    double    val[N_vars];
    int*       qrow = q_row.data();
    int*       qcol = q_col.data();
    double*    qval = q_val.data();
    int       optimstatus;
    double    objval;

    std::vector<double> ub(N_vars, GRB_INFINITY);
    std::vector<double> lb(N_vars, -GRB_INFINITY);

    std::vector<int> l_ind;
    std::vector<double> l_coeff;


    /* Create environment */
    error = GRBloadenv(&env, "qp.log");
    if (error) goto QUIT;

    /* Create a model */
    error = GRBnewmodel(env, &model, "qp", N_vars, l_obj_coeff.data(),lb.data(), ub.data(), NULL, NULL);
    if (error) goto QUIT;

    /* Quadratic objective terms */

    error = GRBaddqpterms(model, N_q_coeffs, qrow, qcol, qval);
    if (error) goto QUIT;

    error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0);
    if (error) goto QUIT;

    for(int i = 0; i < rows_A; i++)
    {
        l_ind.clear();
        l_coeff.clear();
        for(int j = 0; j < cols_A; j++)
        {
            if(A(i,j) != 0)
            {
                l_ind.push_back(j);
                l_coeff.push_back(A(i,j));
            }
        }
        error = GRBaddconstr(model, l_ind.size(), l_ind.data(), l_coeff.data(), GRB_LESS_EQUAL, b(i), NULL);
        if (error) goto QUIT;
    }

    // Optimize model

    error = GRBoptimize(model);
    if (error) goto QUIT;

    /* Write model to 'qp.lp' */
    error = GRBwrite(model, "qp.lp");
    if (error) goto QUIT;

    /* Capture solution information */
    error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
    if (error) goto QUIT;

    error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
    if (error) goto QUIT;

    error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, N_vars, sol);
    if (error) goto QUIT;

    // printf("\nOptimization complete\n");
    if (optimstatus == GRB_OPTIMAL) {
        // printf("Optimal objective: %.4e\n", objval);
        // for(int i = 0; i < N_vars; i++)
        // {
        //     printf("sol[i] = %.4f\n", sol[i]);
        // }
    } else if (optimstatus == GRB_INF_OR_UNBD) {
        printf("Model is infeasible or unbounded\n");
    } else {
        printf("Optimization was stopped early\n");
    }


QUIT:

    /* Error reporting */

    if (error) {
        printf("ERROR: %s\n", GRBgeterrormsg(env));
        exit(1);
    }

    /* Free model */

    GRBfreemodel(model);

    /* Free environment */

    GRBfreeenv(env);

    // END C INTERFACE

    // Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    Map<VectorXd> U_0(sol,N_vars);
    // cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
    return U_0;
}



VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
{
    // H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
    // min(x'Hx + F'x)

    int size_H = H.cols();

    std::vector<int> q_row;
    std::vector<int> q_col;
    std::vector<double> q_val;
    std::vector<double> l_obj_coeff;

    for(int i = 0; i < size_H; i++)
    {
        for(int j = i; j < size_H; j++)
        {
            // std::cout << "i,j -->" << i << "," << j << std::endl;
            if(H(i,j) != 0)
            {
                q_row.push_back(i);
                q_col.push_back(j);

                if(i == j)
                {
                    q_val.push_back(H(i,j));
                    // std::cout << "H(i,j)" << H(i,j) << endl;
                }
                else
                {
                    q_val.push_back(2*H(i,j));
                    // std::cout << "2H(i,j)" << 2*H(i,j) << endl;
                }
            }
        }
        l_obj_coeff.push_back(F(i));
    }



    int N_vars = size_H;
    int N_q_coeffs = q_val.size();

    // BEGINNING C INTERFACE:

    GRBenv   *env   = NULL;
    GRBmodel *model = NULL;
    int       error = 0;
    double    sol[N_vars];
    int       ind[N_vars];
    double    val[N_vars];
    int*       qrow = q_row.data();
    int*       qcol = q_col.data();
    double*    qval = q_val.data();
    int       optimstatus;
    double    objval;

    std::vector<double> ub(N_vars, GRB_INFINITY);
    std::vector<double> lb(N_vars, -GRB_INFINITY);

    /* Create environment */
    error = GRBloadenv(&env, "qp.log");
    if (error) goto QUIT;

    /* Create a model */
    error = GRBnewmodel(env, &model, "qp", N_vars, l_obj_coeff.data(),lb.data(), ub.data(), NULL, NULL);
    if (error) goto QUIT;

    /* Quadratic objective terms */

    error = GRBaddqpterms(model, N_q_coeffs, qrow, qcol, qval);
    if (error) goto QUIT;

    error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0);
    if (error) goto QUIT;

    // Optimize model

    error = GRBoptimize(model);
    if (error) goto QUIT;

    /* Write model to 'qp.lp' */
    error = GRBwrite(model, "qp.lp");
    if (error) goto QUIT;

    /* Capture solution information */
    error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
    if (error) goto QUIT;

    error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
    if (error) goto QUIT;

    error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, N_vars, sol);
    if (error) goto QUIT;

    // printf("\nOptimization complete\n");
    if (optimstatus == GRB_OPTIMAL) {
        // printf("Optimal objective: %.4e\n", objval);
        // for(int i = 0; i < N_vars; i++)
        // {
        //     printf("sol[i] = %.4f\n", sol[i]);
        // }
    } else if (optimstatus == GRB_INF_OR_UNBD) {
        printf("Model is infeasible or unbounded\n");
    } else {
        printf("Optimization was stopped early\n");
    }


QUIT:

    /* Error reporting */

    if (error) {
        printf("ERROR: %s\n", GRBgeterrormsg(env));
        exit(1);
    }

    /* Free model */

    GRBfreemodel(model);

    /* Free environment */

    GRBfreeenv(env);

    // END C INTERFACE

    // Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    Map<VectorXd> U_0(sol,N_vars);
    // cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
    return U_0;
}