#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#define pi 3.14159

using namespace std;

static boost::random::mt19937 gen;

int test(float bounds[8]);
float rand_gen(float lim[2]);

int test(float bounds[8]){

  USING_NAMESPACE_ACADO

  DifferentialState        q1,q2,qd1,qd2;   // the differential states
  Control                  tau1,tau2;       // the control input u
  Parameter                T;
  DifferentialEquation     f( 0.0, 0.5 );

  //  -------------------------------------
  OCP ocp( 0.0, 0.5 );

  ocp.minimizeLagrangeTerm(tau1*tau1 + tau2*tau2);
  // ocp.minimizeMayerTerm(T*T + tau1*tau1 + tau2*tau2);

  f << dot(q1) == qd1;
  f << dot(q2) == qd2;
  f << dot(qd1) == -(48*tau1 - 48*tau2 + 24*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 18*qd1*qd1*sin(2*q2) - 72*tau2*cos(q2) + 48*qd1*qd2*sin(q2))/(36*cos(q2)*cos(q2) - 64);
  f << dot(qd2) == (48*tau1 - 240*tau2 + 120*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 36*qd1*qd1*sin(2*q2) + 18*qd2*qd2*sin(2*q2) + 72*tau1*cos(q2) - 144*tau2*cos(q2) + 48*qd1*qd2*sin(q2) + 36*qd1*qd2*sin(2*q2))/(18*cos(2*q2) - 46);

  ocp.subjectTo(f);
  ocp.subjectTo(AT_START, q1 ==  bounds[0] );

  ocp.subjectTo(AT_START, q2 ==  bounds[1] );

  ocp.subjectTo(AT_START, qd1 ==  bounds[2] );
  ocp.subjectTo(AT_START, qd2 ==  bounds[3] );

  ocp.subjectTo(AT_END, q1 == bounds[4]);
  ocp.subjectTo(AT_END, q2 == bounds[5]);
  ocp.subjectTo(AT_END, qd1 == bounds[6]);
  ocp.subjectTo(AT_END, qd2 == bounds[7]);

  ocp.subjectTo(-400 <= tau1 <= 400);  // bounds on the control input u,
  ocp.subjectTo(-400 <= tau2 <= 400);
  //  -------------------------------------

  // ---------PLOT FOR TESTING
  // GnuplotWindow window;
  // window.addSubplot(q1, "q1");
  // window.addSubplot(q2, "q2");
  // window.addSubplot(qd1, "qd1");
  // window.addSubplot(qd2, "qd2");
  // window.addSubplot(tau1, "tau1");
  // window.addSubplot(tau2, "tau2");

  OptimizationAlgorithm algorithm(ocp);     // the optimization algorithm
  algorithm.set( DISCRETIZATION_TYPE , MULTIPLE_SHOOTING);
  algorithm.set( INTEGRATOR_TYPE , INT_RK45);
  algorithm.set( HESSIAN_APPROXIMATION   , BLOCK_BFGS_UPDATE);
  algorithm.set( KKT_TOLERANCE   , 1e-4);
  algorithm.set( ABSOLUTE_TOLERANCE, 1e-4);
  algorithm.set( INTEGRATOR_TOLERANCE, 1e-4);
  algorithm.set( MAX_NUM_ITERATIONS, 1000);
  algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 10000);

  algorithm.set(PRINT_COPYRIGHT,BT_FALSE);
  algorithm.set(PRINTLEVEL,LOW);
  algorithm.set(PRINT_INTEGRATOR_PROFILE,BT_FALSE);
  algorithm.set(PRINT_SCP_METHOD_PROFILE,BT_FALSE);
  // algorithm << window;
  algorithm.solve();                        // solves the problem.

  algorithm.getDifferentialStates("states.txt");
  algorithm.getObjectiveValue("parameters.txt");
  algorithm.getControls("control.txt");

  clearAllStaticCounters();
  return 0;
}

float rand_gen(float lim[2]) {
    static boost::random::uniform_real_distribution<> dist(0, 1);
    return lim[0] + dist(gen)*(lim[1] - lim[0]);
}

int main(int argc, char const *argv[])
{
  float q_lims[2] = {0.0,2*pi};
  float qd_lims[2] = {-30.0,30.0};
  float bounds[8] = {5.11906, 0.851226, 24.3475, 20.1005, 0.797881, 6.08757, 24.8026, -16.738};
  // float bounds[8] = {rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims),rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims)};
  cout << bounds[0] << " " << bounds[1] << " " << bounds[2] << " " << bounds[3] << " " << bounds[4] << " " << bounds[5] << " " << bounds[6] << " " << bounds[7] << endl;
  bool success = test(bounds);
  return 0;
}


#ifndef LSC_PLANNER_BERNSTEIN_TRAJECTORY_HPP
#define LSC_PLANNER_BERNSTEIN_TRAJECTORY_HPP

#include <queue>
#include <sp_const.hpp>
#include <Eigen/Dense>

namespace DynamicPlanning{
    static int nChoosek(int n, int k){
        if(k > n) return 0;
        if(k * 2 > n) k = n-k;
        if(k == 0) return 1;

        int result = n;
        for(int i = 2; i <= k; i++){
            result *= (n-i+1);
            result /= i;
        }
        return result;
    }

    static double getBernsteinBasis(int n, int i, double t_normalized){
        return nChoosek(n, i) * pow(t_normalized, i) * pow(1-t_normalized, n - i);
    }

    static octomap::point3d getPointFromControlPoints(std::vector<octomap::point3d> control_points,
                                                      double t_normalized){
        octomap::point3d point;
        double t = t_normalized;
        if(t < 0 - SP_EPSILON || t > 1 + SP_EPSILON){
            throw std::invalid_argument("[Polynomial] Input of getPointFromControlPoints is out of bound");
        }

        int n_ctrl = (int)control_points.size() - 1;
        double x = 0, y = 0, z = 0;
        for(int i = 0; i < n_ctrl + 1; i++){
            double b_i_n = getBernsteinBasis(n_ctrl, i, t_normalized);
            x += control_points[i].x() * b_i_n;
            y += control_points[i].y() * b_i_n;
            z += control_points[i].z() * b_i_n;
        }

        point = octomap::point3d((float)x, (float)y, (float)z);
        return point;
    }

    static double getPointFromControlPoints(std::vector<double> control_points, double t_normalized){
        double t = t_normalized;
        if(t < 0 - SP_EPSILON || t > 1 + SP_EPSILON){
            throw std::invalid_argument("[Polynomial] Input of getPointFromControlPoints is out of bound");
        }

        int n_ctrl = (int)control_points.size() - 1;
        double x = 0;
        for(int i = 0; i < n_ctrl + 1; i++){
            double b_i_n = getBernsteinBasis(n_ctrl, i, t_normalized);
            x += control_points[i] * b_i_n;
        }

        return x;
    }

    static dynamic_msgs::State getStateFromControlPoints(std::vector<std::vector<octomap::point3d>> control_points,
                                                         double current_time, int M, int n, double dt){
        dynamic_msgs::State state;
        int m = static_cast<int>(current_time / dt);
        if(m == M and current_time < M * dt + SP_EPSILON){
            m = M - 1;
        }
        else if(m >= M){
            throw std::invalid_argument("[Polynomial] Input of getOdom is out of bound");
        }

        octomap::point3d position = getPointFromControlPoints(control_points[m], current_time/dt - m);
        state.pose.position.x = position.x();
        state.pose.position.y = position.y();
        state.pose.position.z = position.z();

        std::vector<octomap::point3d> velocity_points;
        velocity_points.resize(n);
        for(int i = 0; i < n; i++){
            velocity_points[i] = (control_points[m][i+1] - control_points[m][i]) * n * pow(dt, -1);
        }
        octomap::point3d velocity = getPointFromControlPoints(velocity_points, current_time/dt - m);
        state.velocity.linear.x = velocity.x();
        state.velocity.linear.y = velocity.y();
        state.velocity.linear.z = velocity.z();

        std::vector<octomap::point3d> acceleration_points;
        acceleration_points.resize(n-1);
        for(int i = 0; i < n-1; i++){
            acceleration_points[i] = (velocity_points[i+1] - velocity_points[i]) * (n-1) * pow(dt, -1);
        }
        octomap::point3d acceleration = getPointFromControlPoints(acceleration_points, current_time/dt - m);
        state.acceleration.linear.x = acceleration.x();
        state.acceleration.linear.y = acceleration.y();
        state.acceleration.linear.z = acceleration.z();

        std::vector<octomap::point3d> jerk_points;
        jerk_points.resize(n-2);
        for(int i = 0; i < n-2; i++){
            jerk_points[i] = (acceleration_points[i+1] - acceleration_points[i]) * (n-2) * pow(dt, -1);
        }
        octomap::point3d jerk = getPointFromControlPoints(jerk_points, current_time/dt - m);

        octomap::point3d thrust = acceleration + octomap::point3d(0, 0, 9.81); // add gravity

        octomap::point3d z_body = thrust.normalized();
        octomap::point3d x_world(1, 0, 0);
        octomap::point3d y_body = (z_body.cross(x_world)).normalized();
        octomap::point3d x_body = y_body.cross(z_body);

        octomap::point3d jerk_orth_zbody = jerk - z_body * jerk.dot(z_body);
        octomap::point3d h_w = jerk_orth_zbody * (1 / thrust.norm());
        octomap::point3d omega(-h_w.dot(y_body), h_w.dot(x_body), 0); //TODO: yaw is 0
        state.velocity.angular.x = omega.x();
        state.velocity.angular.y = omega.y();
        state.velocity.angular.z = omega.z();

        return state;
    }

    static dynamic_msgs::State getStateFromControlPoints(std::vector<std::vector<octomap::point3d>> control_points,
                                                         double current_time, int M, int n,
                                                         std::vector<double> time_segments){
        dynamic_msgs::State state;
        int m = 0;
        double t_normalized, segment_length, segment_start_time = 0;
        bool is_time_end = false;
        while(m < M and current_time > time_segments[m+1]){
            segment_start_time = time_segments[m];
            m++;
        }
        if(m == M){
            m = M - 1;
            segment_length = time_segments[m+1] - time_segments[m];
            t_normalized = 1.0;
            is_time_end = true;
        }
        else{
            segment_length = time_segments[m+1] - time_segments[m];
            t_normalized = (current_time - time_segments[m]) / segment_length;
        }

        octomap::point3d position = getPointFromControlPoints(control_points[m], t_normalized);
        state.pose.position.x = position.x();
        state.pose.position.y = position.y();
        state.pose.position.z = position.z();

        if(is_time_end){
            return state;
        }

        std::vector<octomap::point3d> velocity_points;
        velocity_points.resize(n);
        for(int i = 0; i < n; i++){
            velocity_points[i] = (control_points[m][i+1] - control_points[m][i]) * n * pow(segment_length, -1);
        }
        octomap::point3d velocity = getPointFromControlPoints(velocity_points, t_normalized);
        state.velocity.linear.x = velocity.x();
        state.velocity.linear.y = velocity.y();
        state.velocity.linear.z = velocity.z();

        std::vector<octomap::point3d> acceleration_points;
        acceleration_points.resize(n-1);
        for(int i = 0; i < n-1; i++){
            acceleration_points[i] = (velocity_points[i+1] - velocity_points[i]) * (n-1) * pow(segment_length, -1);
        }
        octomap::point3d acceleration = getPointFromControlPoints(acceleration_points, t_normalized);
        state.acceleration.linear.x = acceleration.x();
        state.acceleration.linear.y = acceleration.y();
        state.acceleration.linear.z = acceleration.z();

        std::vector<octomap::point3d> jerk_points;
        jerk_points.resize(n-1);
        for(int i = 0; i < n-1; i++){
            jerk_points[i] = (acceleration_points[i+1] - acceleration_points[i]) * (n-2) * pow(segment_length, -1);
        }
        octomap::point3d jerk = getPointFromControlPoints(jerk_points, t_normalized);

        octomap::point3d thrust = acceleration + octomap::point3d(0, 0, 9.81); // add gravity

        octomap::point3d z_body = thrust.normalized();
        octomap::point3d x_world(1, 0, 0);
        octomap::point3d y_body = (z_body.cross(x_world)).normalized();
        octomap::point3d x_body = y_body.cross(z_body);

        octomap::point3d jerk_orth_zbody = jerk - z_body * jerk.dot(z_body);
        octomap::point3d h_w = jerk_orth_zbody * (1 / thrust.norm());
        octomap::point3d omega(-h_w.dot(y_body), h_w.dot(x_body), 0); //TODO: yaw is 0
        state.velocity.angular.x = omega.x();
        state.velocity.angular.y = omega.y();
        state.velocity.angular.z = omega.z();

        return state;
    }

    static std::vector<octomap::point3d> bernsteinFitting(std::vector<octomap::point3d> target_points,
                                                          std::vector<double> ts_normalized){
        int n = (int)target_points.size() - 1;
        Eigen::MatrixXd B = Eigen::MatrixXd(n+1, n+1);
        for(int i = 0; i < n + 1; i++){
            for(int j = 0; j < n + 1; j++){
                B(i, j) = getBernsteinBasis(n, j, ts_normalized[i]);
            }
        }

        Eigen::MatrixXd X = Eigen::MatrixXd(n+1, 3);
        for(int i = 0; i < n + 1; i++){
            X(i, 0) = target_points[i].x();
            X(i, 1) = target_points[i].y();
            X(i, 2) = target_points[i].z();
        }

        Eigen::MatrixXd C = B.inverse() * X;
        std::vector<octomap::point3d> control_points;
        control_points.resize(n+1);
        for(int i = 0; i < n + 1; i++){
            control_points[i] = octomap::point3d(C(i, 0), C(i, 1), C(i, 2));
        }
        return control_points;
    }

    static int coef_derivative(int n, int phi){
        if(n < phi){
            return 0;
        }

        int coef = 1;
        for(int i = 0; i < phi; i++){
            coef *= n - i;
        }
        return coef;
    }

    struct BisectionElement{
        double c;
        int k;
        Eigen::VectorXd coef;
    };

    // return list of [a,b] includes one real root of polynomial which coefficient is coef
    static std::vector<std::pair<double, double>> realRootIsolation(const Eigen::VectorXd& coef, int n) {
        std::queue<BisectionElement> L;
        L.push(BisectionElement{0, 0, coef});
        std::vector<std::pair<double, double>> Isol;
        int n_poly = 2 * n - 1;

        while (not L.empty()) {
            BisectionElement current_element = L.front();
            L.pop();
//                std::cout << "current_coef: " << std::endl << current_element.coef << std::endl;
            if (current_element.coef(0) == 0) {
                for (int i = 0; i < n_poly; i++) {
                    current_element.coef(i) = current_element.coef(0, i + 1);
                }
                current_element.coef(n_poly) = 0;
                n_poly--;
                Isol.emplace_back(std::make_pair(current_element.c / pow(2, current_element.k),
                                                 current_element.c / pow(2, current_element.k)));
            }

            Eigen::VectorXd coef_test = Eigen::VectorXd::Zero(n_poly + 1);
            for (int i = 0; i < n_poly + 1; i++) {
                for (int j = 0; j < n_poly + 1 - i; j++) {
                    coef_test(j) += current_element.coef(i) * nChoosek(n_poly - i, j);
                }
            }
//                std::cout << "coef_test: " << std::endl << coef_test << std::endl;
            int var = 0;
            for (int i = 0; i < n_poly; i++) {
                if (coef_test(i) * coef_test(i + 1) < 0) {
                    var++;
                }
            }
            if (var == 1) {
                Isol.emplace_back(std::make_pair(current_element.c / pow(2, current_element.k),
                                                 (current_element.c + 1) / pow(2, current_element.k)));
            }
            if (var > 1) {
                Eigen::VectorXd coef1 = current_element.coef;
                for (int i = 0; i < n_poly + 1; i++) {
                    coef1(i) *= pow(2, n_poly - i);
                }
//                    std::cout << "coef1: " << std::endl << coef1 << std::endl;
                L.push(BisectionElement{2 * current_element.c, current_element.k + 1, coef1});

                Eigen::VectorXd coef2 = Eigen::VectorXd::Zero(n_poly + 1);
                for (int i = 0; i < n_poly + 1; i++) {
                    for (int j = 0; j < i + 1; j++) {
                        coef2(j) += current_element.coef(i) * pow(2, n_poly - i) * nChoosek(i, j);
                    }
                }
//                    std::cout << "coef2: " << std::endl << coef2 << std::endl;
                L.push(BisectionElement{2 * current_element.c + 1, current_element.k + 1, coef2});
            }
        }
        return Isol;
    }

    static double evalPoly(const Eigen::VectorXd& coef, double t){
        double ret = 0;
        for(int i = 0; i < coef.size(); i++){
            ret += coef(i) * pow(t, i);
        }
        return ret;
    }

    // pi_i = obstacle position, pi_j = agent_position
    static double distanceBetweenPolys(const std::vector<octomap::point3d>& control_points_agent,
                                       const std::vector<octomap::point3d>& control_points_obs,
                                       const Eigen::MatrixXd& B,
                                       double poly_root_tolerance,
                                       octomap::point3d& closest_point) {
        if(control_points_agent.size() != control_points_obs.size()){
            throw std::invalid_argument("[Polynomial] degree of two polynomials are not same.");
        }

        int n = (int)control_points_agent.size() - 1;
        int dim = 3;

        //get control points of relative trajectory
        std::vector<octomap::point3d> control_points_rel;
        control_points_rel.resize(n + 1);
        for(int i = 0; i < n + 1; i++){
            control_points_rel[i].x() = control_points_agent[i].x() - control_points_obs[i].x();
            control_points_rel[i].y() = control_points_agent[i].y() - control_points_obs[i].y();
            control_points_rel[i].z() = control_points_agent[i].z() - control_points_obs[i].z();
        }

        Eigen::MatrixXd control_points_rel_mtx = Eigen::MatrixXd::Zero(dim, n + 1);
        for(int j = 0; j < n + 1; j++){
            control_points_rel_mtx(0, j) = control_points_rel[j].x();
            control_points_rel_mtx(1, j) = control_points_rel[j].y();
            control_points_rel_mtx(2, j) = control_points_rel[j].z();
        }

        // convert to coef of standard basis
        Eigen::MatrixXd coef = control_points_rel_mtx * B;

        // get derivative of coef
        Eigen::MatrixXd coef_derivative = Eigen::MatrixXd::Zero(dim, n + 1);
        for(int j = 0; j < n; j++){
            coef_derivative(0, j) = (j + 1) * coef(0, j + 1);
            coef_derivative(1, j) = (j + 1) * coef(1, j + 1);
            coef_derivative(2, j) = (j + 1) * coef(2, j + 1);
        }

        // coef dot coef_derivative
        Eigen::VectorXd coef_g = Eigen::VectorXd::Zero(2 * n);
        for(int j0 = 0; j0 < n + 1; j0++){
            for(int j1 = 0; j1 < n; j1++){
                for(int k = 0; k < dim; k++) {
                    coef_g(j0 + j1) += coef(k, j0) * coef_derivative(k, j1);
                }
            }
        }

        // real root isolation
        std::vector<std::pair<double, double>> Isol = realRootIsolation(coef_g, n);

        // compute closest point by Newton method
        bool isClosestPointInSection = false;
        double a, b, m, g_m, t_cand, dist_cand;
        double dist_closest = SP_INFINITY;
        octomap::point3d p_cand;
        for(auto section : Isol){
            a = section.first;
            b = section.second;
            if(evalPoly(coef_g, a) < 0 and evalPoly(coef_g, b) > 0) {
                while (true) {
                    if (b - a < poly_root_tolerance) {
                        t_cand = (a + b) / 2;
                        break;
                    }
                    m = (a + b) / 2;
                    g_m = evalPoly(coef_g, m);
                    if (g_m == 0) {
                        t_cand = m;
                        break;
                    } else if (g_m < 0) {
                        a = m;
                    } else {
                        b = m;
                    }
                }
                p_cand = getPointFromControlPoints(control_points_rel, t_cand);
                dist_cand = p_cand.norm();
                if(dist_cand < dist_closest){
                    closest_point = p_cand;
                    dist_closest = dist_cand;
//                        t_normalized_closest = t_cand;
                    isClosestPointInSection = true;
                }
            }
        }
        if(not isClosestPointInSection){
            if(control_points_rel[0].norm() < control_points_rel[n].norm()){
                closest_point = control_points_rel[0];
//                    t_normalized_closest = 0;
            }
            else{
                closest_point = control_points_rel[n];
//                    t_normalized_closest = 1;
            }
        }

        return closest_point.norm();
//            // Exception handle for first segment
//            if(m == 0 && p_closest.norm() < d[0] - SP_EPSILON_FLOAT){
//                p_closest = control_points_rel[0];
//            }
    }

    static void buildBernsteinBasis(int n, Eigen::MatrixXd& B, Eigen::MatrixXd& B_inv) {
        B = Eigen::MatrixXd::Zero(n + 1, n + 1);
        for(int i = 0; i < n + 1; i++){
            for(int j = 0; j < n + 1; j++){
                if(j >= i){
                    B(i,j) = nChoosek(n, i) * nChoosek(n-i, n-j) * pow(-1, j-i);
                }
                else{
                    B(i,j) = 0;
                }
            }
        }
        B_inv = B.inverse();
    }

    static std::vector<octomap::point3d> subdivisionBernsteinCurve(
            const std::vector<octomap::point3d>& control_points, double a, double b,
            const Eigen::MatrixXd& B,  const Eigen::MatrixXd& B_inv){
        size_t n = control_points.size() - 1;
        Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
        for(int i = 0; i < n + 1; i++){
            for(int j = 0; j < i + 1; j++){
                A(i, j) = nChoosek(i, j) * pow(a, j) * pow(b, i - j);
            }
        }

        Eigen::MatrixXd c_tr = Eigen::MatrixXd::Zero(3, n + 1);
        for(int k = 0; k < 3; k++){
            for(int i = 0; i < n + 1; i++){
                c_tr(k, i) = control_points[i](k);
            }
        }
        c_tr = c_tr * B * A * B_inv;

        std::vector<octomap::point3d> control_points_tr;
        control_points_tr.resize(n + 1);
        for(int i = 0; i < n + 1; i++){
            control_points_tr[i] = octomap::point3d(c_tr(0, i), c_tr(1, i), c_tr(2, i));
        }
        return control_points_tr;
    }
}
#endif //LSC_PLANNER_BERNSTEIN_TRAJECTORY_HPP


/* test_student_t.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: test_student_t.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/student_t_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/students_t.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::student_t_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME student_t
#define BOOST_MATH_DISTRIBUTION boost::math::students_t
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME n
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


// Copyright (c) 2017 Evan S Weinberg
// Test code for a real operator.

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <sstream>
#include <complex>
#include <random>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>
using namespace Eigen;

#include "blas/generic_vector.h"
#include "inverters/generic_cg.h"
#include "inverters/generic_ca_cg.h"

#include "square_laplace.h"

using namespace std;

// Prepare vectors for various tests.
void reset_vectors(complex<double>* rhs, complex<double>* lhs, complex<double>* check, int length)
{
  zero_vector(lhs, length*length);
  zero_vector(rhs, length*length);
  rhs[length/2+(length/2)*length] = 1.0; // set a point on rhs.
  zero_vector(check, length*length);
}

int main(int argc, char** argv)
{
    //double *lattice; // Holds the gauge field.
  complex<double> *lhs, *rhs, *check; // For some Kinetic terms.
  complex<double> *gauge_links;
  double explicit_resid = 0.0;
  double bnorm = 0.0;
  inversion_info invif;
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.

  // Basic information about the lattice.
  int length = 32;
  double m_sq = 0.01;
  double inv_variance = 6.0; // inverse of variance for gaussian non-compact U(1) links.

  // Create a random compact U(1) link.
  gauge_links = allocate_vector<complex<double>>(2*length*length);

  // Remark: fills real and imaginary with gaussian numbers.
  gaussian(gauge_links, 2*length*length, generator, 1.0/inv_variance);
  polar(gauge_links, 2*length*length); // ignores imag part to make compact link.

  // Structure which gets passed to the function.
  laplace_gauged_struct lapstr;
  lapstr.length = length;
  lapstr.m_sq = m_sq;
  lapstr.gauge_links = gauge_links;

  // Parameters related to solve.
  double tol = 1e-8;
  int max_iter = 200;
  //double minres_relaxation_param = 0.8;
  //double richardson_relaxation_param = 0.2;
  //int restart_freq = 64; // for restarted solves.
  //int bicgstabl = 8; // L for, well, BiCGstab-L.

  // Some start-up.
  int volume = length*length;

  //lattice = allocate_vector<double>(2*volume);  // Gauge field eventually.

  // Vectors.
  lhs = allocate_vector<complex<double>>(volume);
  rhs = allocate_vector<complex<double>>(volume);
  check = allocate_vector<complex<double>>(volume);

  // Zero out vectors, set rhs point.
  // zero_vector(lattice, 2*volume);
  reset_vectors(rhs, lhs, check, length);

  // Get norm for rhs.
  bnorm = sqrt(norm2sq(rhs, volume));

  printf("Solving A [lhs] = [rhs] for lhs, using a point source. Single mass test: m^2 = %f.\n\n", m_sq);

  /****************
  * NORMAL SOLVES *
  ****************/

  // lhs = A^(-1) rhs
  // Arguments:
  // 1: lhs
  // 2: rhs
  // 3: size of vector
  // 4: maximum iterations
  // 5: residual
  // 5a for gcr_restart: how often to restart.
  // 5b for richardson, minres: overrelaxation parameter (can leave this out, assumes 1)
  // 5c for richardson: how often to check the residual
  // 6: function pointer
  // 7: "extra data":
  // 8: optional, verbosity information.

  inversion_verbose_struct verb(VERB_DETAIL, "Details: ");

  /* CG */
  reset_vectors(rhs, lhs, check, length);
  invif = minv_vector_cg(lhs, rhs, volume, max_iter, tol, square_laplacian_gauged, &lapstr, &verb);
  if (invif.success == true)
  {
    printf("Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
  }
  else // failed, maybe.
  {
    printf("Potential error! Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
  }
  printf("Computing [check] = A [lhs] as a confirmation.\n");
  // Check and make sure we get the right answer.
  square_laplacian_gauged(check, lhs, &lapstr);
  explicit_resid = sqrt(diffnorm2sq(rhs, check, volume))/bnorm; // sqrt(|rhs - check|^2)/bnorm
  printf("[check] should equal [rhs]. The residual is %15.20e.\n\n", explicit_resid);

  /* CA-CG */
  for (int ca_s = 2; ca_s <= 10; ca_s++)
  {
    reset_vectors(rhs, lhs, check, length);
    invif = minv_vector_ca_cg(lhs, rhs, volume, max_iter, tol, ca_s, square_laplacian_gauged, &lapstr, &verb);
    if (invif.success == true)
    {
      printf("Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }
    else // failed, maybe.
    {
      printf("Potential error! Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }
    printf("Computing [check] = A [lhs] as a confirmation.\n");
    // Check and make sure we get the right answer.
    square_laplacian_gauged(check, lhs, &lapstr);
    explicit_resid = sqrt(diffnorm2sq(rhs, check, volume))/bnorm; // sqrt(|rhs - check|^2)/bnorm
    printf("[check] should equal [rhs]. The residual is %15.20e.\n\n", explicit_resid);
  }

  //////////////
  // CLEAN UP //
  //////////////

  // Free the lattice.
  //delete[] lattice;
  deallocate_vector(&lhs);
  deallocate_vector(&rhs);
  deallocate_vector(&check);
  deallocate_vector(&gauge_links);
  return 0;
}


#include <algorithm>
#include <chrono>
#include <cstddef>
#include <functional>
#include <iostream>
#include <map>
#include <random>
#include <string>
#include <vector>

#include <boost/algorithm/string/join.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include <mkl_cblas.h>

/// helper functions
template <typename ForwardIt> void fill_random(ForwardIt begin, ForwardIt end) {
	std::random_device rndDev;
	std::mt19937 rndEng{rndDev()};
	using T = typename std::iterator_traits<ForwardIt>::value_type;
	std::uniform_real_distribution<T> dist{-1.0, 1.0};

	std::generate(begin, end, [&] { return dist(rndEng); });
}

template <typename T> auto timed(T&& f) {
	const auto starttime = std::chrono::high_resolution_clock::now();
	f();
	return std::chrono::duration<double>{
	    std::chrono::high_resolution_clock::now() - starttime}
	    .count();
}

template <typename Map>[[noreturn]] void failInvalidArgs(const Map& modes) {
	std::cerr << "invalid args!\nusage: prog "
	          << boost::algorithm::join(
	                 modes | boost::adaptors::transformed(
	                             [](const auto& p) { return p.first; }),
	                 "|")
	          << "\n";
	std::exit(EXIT_FAILURE);
}

/// gemm implementations
template <typename T>
void serialIkj(const T* a, const T* b, T* c, std::size_t aRows,
               std::size_t aCols, std::size_t bCols) {
	for (std::size_t i{0}; i < aRows; ++i) {
		for (std::size_t k{0}; k < aCols; ++k) {
			for (std::size_t j{0}; j < bCols; ++j) {
				c[i * bCols + j] += a[i * aCols + k] * b[k * bCols + j];
			}
		}
	}
}

template <typename T>
void serialIjk(const T* a, const T* b, T* c, std::size_t aRows,
               std::size_t aCols, std::size_t bCols) {
	for (std::size_t i{0}; i < aRows; ++i) {
		for (std::size_t j{0}; j < bCols; ++j) {
			for (std::size_t k{0}; k < aCols; ++k) {
				c[i * bCols + j] += a[i * aCols + k] * b[k * bCols + j];
			}
		}
	}
}

template <typename T>
void parallelGemm(const T* a, const T* b, T* c, std::size_t aRows,
                  std::size_t aCols, std::size_t bCols) {
// use std::for_each(std::execution::par, ... ) in
// C++17 instead
#pragma omp parallel for
	for (std::size_t i = 0; i < aRows; ++i) {
		for (std::size_t k{0}; k < aCols; ++k) {
			for (std::size_t j{0}; j < bCols; ++j) {
				c[i * bCols + j] += a[i * aCols + k] * b[k * bCols + j];
			}
		}
	}
}

// float overload
void blasGemm(const float* a, const float* b, float* c, std::size_t aRows,
              std::size_t aCols, std::size_t bCols) {
	const auto m = aRows;
	const auto k = aCols;
	const auto n = bCols;
	const float alf = 1;
	const float bet = 0;

	cblas_sgemm(CblasRowMajor, // CBLAS_LAYOUT layout,
	            CblasNoTrans,  // CBLAS_TRANSPOSE TransA,
	            CblasNoTrans,  // CBLAS_TRANSPOSE TransB,
	            m,             // const int M,
	            n,             // const int N,
	            k,             // const int K,
	            alf,           // const float alpha,
	            a,             // const float *A,
	            k,             // const int lda,
	            b,             // const float *B,
	            n,             // const int ldb,
	            bet,           // const float beta,
	            c,             // float *C,
	            n              // const int ldc
	            );
}

// double overload
void blasGemm(const double* a, const double* b, double* c, std::size_t aRows,
              std::size_t aCols, std::size_t bCols) {
	const auto m = aRows;
	const auto k = aCols;
	const auto n = bCols;
	const double alf = 1;
	const double bet = 0;

	cblas_dgemm(CblasRowMajor, // CBLAS_LAYOUT layout,
	            CblasNoTrans,  // CBLAS_TRANSPOSE TransA,
	            CblasNoTrans,  // CBLAS_TRANSPOSE TransB,
	            m,             // const int M,
	            n,             // const int N,
	            k,             // const int K,
	            alf,           // const double alpha,
	            a,             // const double *A,
	            k,             // const int lda,
	            b,             // const double *B,
	            n,             // const int ldb,
	            bet,           // const double beta,
	            c,             // double *C,
	            n              // const int ldc
	            );
}

int main(int argc, char* argv[]) {
	using Real = float;
	using gemmFuncType = void(const Real*, const Real*, Real*, std::size_t,
	                          std::size_t, std::size_t);
	using gemmFuncPrtType = void (*)(const Real*, const Real*, Real*,
	                                 std::size_t, std::size_t, std::size_t);

	// this map manages all implementations
	std::map<std::string, std::function<gemmFuncType>> modes{
	    {"serialikj", &serialIkj<Real>},
	    {"serialijk", &serialIjk<Real>},
	    {"parallel", &parallelGemm<Real>},
	    {"blas", static_cast<gemmFuncPrtType>(&blasGemm)}};

	// validate cmd args and select gemm implementation
	if (argc != 2) { failInvalidArgs(modes); }
	const std::string modeArg = argv[1];
	if (modes.count(modeArg) != 1) { failInvalidArgs(modes); }
	const auto gemm = modes[modeArg];

	// benchmark sizes
	const auto minSize = 2u;
	const auto maxSize = 2048u;
	const auto maxRepetitions = 2048u;

	// do measurements with increasing matrix dimensions and decreasing
	// repetitions to keep wall clock time short
	std::size_t repetitions{};
	std::size_t size{};
	for (size = minSize, repetitions = maxRepetitions; size <= maxSize;
	     size *= 2, repetitions /= 2) {

		/// set up data
		const auto aRows = size;
		const auto aCols = size;
		const auto bRows = size;
		const auto bCols = size;

		std::vector<Real> a(aRows * aCols); // m * k
		std::vector<Real> b(bRows * bCols); // k * n
		std::vector<Real> c(aRows * bCols); // m * n

		fill_random(std::begin(a), std::end(a));
		fill_random(std::begin(b), std::end(b));

		// create reference solution matrix using serialIkj
		std::vector<Real> reference(c.size());
		serialIkj(a.data(), b.data(), reference.data(), aRows, aCols, bCols);

		/// warm up the caches
		gemm(a.data(), b.data(), c.data(), aRows, aCols, bCols);

		/// timed computations
		auto time = 0.0;
		for (std::size_t r{0}; r < repetitions; ++r) {
			std::fill(std::begin(c), std::end(c), 0);
			time += timed([&]() {
				gemm(a.data(), b.data(), c.data(), aRows, aCols, bCols);
			});

			// validate the result c
			if (!std::equal(std::begin(c), std::end(c), std::begin(reference),
			                std::end(reference),
			                [](const Real& l, const Real& r) {
				                return std::abs(l - r) < 1.0e-3;
				            })) {
				std::cerr << "validation error!\n";
				return EXIT_FAILURE;
			}
		}
		time /= repetitions; // get avg time per call

		std::cout << size << ";" << time << '\n';
	}
}


#include <libv/core/miscmath.hpp>
#include <libv/lma/lma.hpp>
#include <libv/graphic/viewer_context.hpp>
#include <libv/geometry/rotation.hpp>

#include <GL/gl.h>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <boost/bind.hpp>

  template<class T> using vector = std::vector<T,Eigen::aligned_allocator<T>>;
  using namespace Eigen;
  using namespace lma;

  Vector3d on_sphere(double theta, double phi, double r)
  {
    return {r*std::cos(theta)*std::cos(phi),
           r*std::cos(theta)*std::sin(phi),
           r*std::sin(theta)};
  }

  auto draw_points = [](auto& points, auto& rotate)
  {
    v::viewer(0).layer(2).clear().pen_color(v::red).pen_width(3).point_style(v::Round).add_opengl([&]()
    {
      glBegin(GL_POINTS);
      for(auto& p : points)
        glVertex3dv((rotate*p).eval().data());
      glEnd();
    }).update();
  };

  auto draw_sphere = [](auto& sphere, auto& rotate)
  {
    v::viewer(0).layer(1).clear().pen_color(v::blue).pen_width(1).point_style(v::Round).add_opengl([&]()
    {
      for(double theta = -M_PI/2.0 ; theta < M_PI/2.0 ; theta+=0.1)
      {
        glBegin(GL_LINE_LOOP);
        for(double phi = -M_PI ; phi < M_PI ; phi+=0.1)
          glVertex3dv((rotate*on_sphere(theta,phi,sphere[3])).eval().data());
        glEnd();
      }

      for(double phi = -M_PI ; phi < M_PI ; phi+=0.1)
      {
        glBegin(GL_LINE_STRIP);
          for(double theta = -M_PI/2.0 ; theta < M_PI/2.0 ; theta+=0.1)
         glVertex3dv((rotate*on_sphere(theta,phi,sphere[3])).eval().data());
        glEnd();
      }
    }).update();
  };

  enum State { before, after0, after1, after2 };

  auto draw_text = [](State state)
  {
    std::string str = "";
    if (state == after0)
      str = "  Optimise Sphere";
    if (state == after1)
      str = "  Optimise Sphere + Points 3D";
    else if(state == after2)
     str ="  Optimise Sphere + Points 3D + Contrainte";
    v::viewer(0).layer(0).clear().pen_color(v::red).font_size(30).font_style(v::Bold).add_text(40,50,str).update();
  };

  void draw(Vector4d& sphere, vector<Vector3d>& points, State& state)
  {
    v::start_viewers();
    v::set_interval(5);
    Matrix3d rotate = Matrix3d::Identity();

    for(;;)
    {
      v::apply_rotation(rotate,{0.0,0.0,0.001});
      draw_text(state);
      draw_sphere(sphere,rotate);
      draw_points(points,rotate);

      v::tempo(5);
    }
  };

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

  auto optimise = [](Vector4d& sphere, vector<Vector3d>& points, const vector<Vector3d>& refs, int mode)
  {

    struct DistanceToSphere1
    {
      const Vector3d& point;
      DistanceToSphere1(const Vector3d& point_):point(point_){}

      bool operator()(const Vector4d& sphere, double& error) const
      {
        error = (sphere.head<3>()-point).norm() - sphere[3];
        return true;
      }
    };

    struct DistanceToSphere
    {
      bool operator()(const Vector4d& sphere, const Vector3d& point, double& error) const
      {
        error = (sphere.head<3>()-point).norm() - sphere[3];
        return true;
      }
    };

    struct DistanceTo3DPoint
    {
      const Vector3d& ref;
      DistanceTo3DPoint(const Vector3d& ref_):ref(ref_){}

      bool operator()(const Vector3d& point, double& error) const
      {
        error = (ref - point).norm();
        return true;
      }
    };

    Solver<DistanceToSphere1,DistanceToSphere,DistanceTo3DPoint> solver(1,50);

    if (mode==0)
    for(Vector3d& p : points)
      solver.add(DistanceToSphere1(p),&sphere);

    if (mode>0)
    for(Vector3d& p : points)
      solver.add(DistanceToSphere(),&sphere,&p);

    if (mode==2)
    for(size_t i = 0 ; i < refs.size() ; ++i)
      solver.add(DistanceTo3DPoint(refs[i]),&points[i]);

    solver.solve(DENSE_SCHUR,enable_verbose_output());
  };

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

int main()
{
  v::viewer(0).size(800,660);
  Vector4d sphere;
  sphere << 0,0,0,100;

  vector<Vector3d> points,refs;

  for(double theta = -M_PI/2.0 ; theta < M_PI/2.0 ; theta+=0.1)
    for(double phi = -M_PI ; phi < M_PI ; phi+=0.1)
      points.emplace_back(
        on_sphere(theta,phi,sphere[3])
        );

  refs = points;

  for(auto& p : points) p += Vector3d::Random()*50.0;

  sphere << 0,0,0,50;

  State state = before;
  boost::thread th(boost::bind(draw,boost::ref(sphere),boost::ref(points),boost::ref(state)));

  getchar();
  optimise(sphere,points,refs,0);

  state = after0;
  getchar();
  optimise(sphere,points,refs,1);
  state = after1;
  getchar();
  optimise(sphere,points,refs,2);
  state = after2;

  v::wait_viewers();
  th.interrupt();
  th.join();

  return 0;
}


#pragma once

#include "constants.hpp"
#include "EcosWrapper.hpp"
#include <Eigen/Dense>

class model_simple_4th_order {
public:

    static constexpr size_t n_states = 4;
    static constexpr size_t n_inputs = 1;
    static string get_name(){ return "model_simple_4th_order"; }

    using StateVector   = Eigen::Matrix<double, n_states,        1>;
    using ControlVector = Eigen::Matrix<double, n_inputs,        1>;
    using StateMatrix   = Eigen::Matrix<double, n_states, n_states>;
    using ControlMatrix = Eigen::Matrix<double, n_states, n_inputs>;

    double total_time_guess() { return 3; }

    void initialize(Eigen::Matrix<double, n_states, K> &X, Eigen::Matrix<double, n_inputs, K> &U);

    StateVector                ode(const StateVector &x, const ControlVector &u);
    StateMatrix     state_jacobian(const StateVector &x, const ControlVector &u);
    ControlMatrix control_jacobian(const StateVector &x, const ControlVector &u);

    void add_application_constraints(optimization_problem::SecondOrderConeProgram &socp);

    StateVector get_random_state();
    ControlVector get_random_input();

};


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/expr.h"

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include <iostream>
#include <fstream>

using namespace std;
using namespace dynet;
using namespace dynet::expr;

int main(int argc, char** argv) {
  dynet::initialize(argc, argv);

  // parameters
  const unsigned HIDDEN_SIZE = 8;
  Model m;
  SimpleSGDTrainer sgd(&m);
  //MomentumSGDTrainer sgd(&m);

  ComputationGraph cg;

  Parameter p_W, p_b, p_V, p_a;
  if (argc == 2) {
    ifstream in(argv[1]);
    boost::archive::text_iarchive ia(in);
    ia >>  m >> p_W >> p_b >> p_V >> p_a;
  }
  else {
    p_W = m.add_parameters({HIDDEN_SIZE, 2});
    p_b = m.add_parameters({HIDDEN_SIZE});
    p_V = m.add_parameters({1, HIDDEN_SIZE});
    p_a = m.add_parameters({1});
  }

  Expression W = parameter(cg, p_W);
  Expression b = parameter(cg, p_b);
  Expression V = parameter(cg, p_V);
  Expression a = parameter(cg, p_a);

  vector<float> x_values(2);  // set x_values to change the inputs to the network
  Expression x = input(cg, {2}, &x_values);
  dynet::real y_value;  // set y_value to change the target output
  Expression y = input(cg, &y_value);

  Expression h = tanh(W*x + b);
  Expression y_pred = logistic(V*h + a);
  Expression loss_expr = binary_log_loss(y_pred, y);

  cg.print_graphviz();

  // train the parameters
  for (unsigned iter = 0; iter < 2000; ++iter) {
    double loss = 0;
    for (unsigned mi = 0; mi < 4; ++mi) {
      bool x1 = mi % 2;
      bool x2 = (mi / 2) % 2;
      x_values[0] = x1 ? 1 : 0;
      x_values[1] = x2 ? 1 : 0;
      y_value = (x1 != x2) ? 1 : 0;
      loss += as_scalar(cg.forward(loss_expr));
      cg.backward(loss_expr);
      sgd.update(1.0);
    }
    sgd.update_epoch();
    loss /= 4;
    cerr << "E = " << loss << endl;
  }

  // Output the model and parameter objects
  // to a cout.
  boost::archive::text_oarchive oa(cout);
  oa << m << p_W << p_b << p_V << p_a;
}


#include <iostream>

#include <NTL/ZZ_p.h>

#include "polynomial.hpp"

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

    NTL::ZZ p((long)17);
    NTL::ZZ_p::init(p);
    NTL::ZZ_p k(6);

    std::cout << k << std::endl;
    std::cout << k * 2 << std::endl;
    std::cout << k * 3 << std::endl;
    std::cout << k * 4 << std::endl;

    unsigned int arity = 3;
    unsigned int degree = 5;
    unsigned int order = 7;

    MultiVariatePolynomial poly(arity, degree, order);

    NTL::ZZ_p value(5);
    std::vector<unsigned int> monomial = { 0, 3, 1 };
    poly.setElement(monomial, value);

    poly.print();

    std::cout << poly.to_string() << std::endl;
    std::cout << poly.add(poly).to_string() << std::endl;
    std::cout << (poly + poly).to_string() << std::endl;
    std::cout << (poly - poly).to_string() << std::endl;

    std::cout << poly.getMaxNbElements() << std::endl;

    std::vector<NTL::ZZ_p> point = { NTL::ZZ_p(1), NTL::ZZ_p(1), NTL::ZZ_p(4) };
    std::cout << poly.evaluate(point) << std::endl;

    return 0;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip8

#include <boost/math/distributions/negative_binomial.hpp>
using boost::math::negative_binomial_distribution;

using namespace boost::math::policies;

typedef negative_binomial_distribution<
      double,
      policy<discrete_quantile<integer_round_nearest> >
   > dist_type;

// Lower quantile rounded (down) to nearest:
double x = quantile(dist_type(20, 0.3), 0.05); // 27
// Upper quantile rounded (down) to nearest:
double y = quantile(complement(dist_type(20, 0.3), 0.05)); // 68

//] //[/policy_ref_snip8]

#include <iostream>
using std::cout; using std::endl;

int main()
{
   cout << "using policy<discrete_quantile<integer_round_nearest> " << endl
     << "quantile(dist_type(20, 0.3), 0.05) = " << x << endl
     << "quantile(complement(dist_type(20, 0.3), 0.05)) " << y << endl;
}

/*

Output:

   using policy<discrete_quantile<integer_round_nearest>
  quantile(dist_type(20, 0.3), 0.05) = 27
  quantile(complement(dist_type(20, 0.3), 0.05)) 68

*/


//
//  utils.hpp
//  Jacobian
//
//  Created by David Freifeld
//

#ifndef UTILS_H
#define UTILS_H

#include <functional>
#include <Eigen/Dense>
#include "bpnn.hpp"

namespace Jacobian {
namespace activations {
float sigmoid(float x);
float sigmoid_deriv(float x);
float linear(float x);
float linear_deriv(float x);
float step(float x);
float step_deriv(float x);
float bipolar(float x);
float bipolar_deriv(float x);
float mytanh(float x);
float tanh_deriv(float x);
float lecun_tanh(float x);
float lecun_tanh_deriv(float x);
float cloglog(float x);
float cloglog_deriv(float x);
float softplus(float x);
float softplus_deriv(float x);
float inverse_logit(float x);
float inverse_logit_deriv(float x);
float hard_tanh(float x);
float hard_tanh_deriv(float x);
float bipolar_sigmoid(float x);
float bipolar_sigmoid_deriv(float x);
float leaky_relu(float x);
float leaky_relu_deriv(float x);
std::function<float(float)> rectifier(float (*activation)(float));
}

namespace optimizers {
std::function<void(Layer&, Eigen::MatrixXf, float)> momentum(float beta);
std::function<void(Layer&, Eigen::MatrixXf, float)> demon(float beta_init, int max_ep);
std::function<void(Layer&, Eigen::MatrixXf, float)> adam(float beta1, float beta2, float epsilon);
std::function<void(Layer&, Eigen::MatrixXf, float)> adamax(float beta1, float beta2, float epsilon);
}

namespace decays {
std::function<void(float &)> step(float a_0, float k);
std::function<void(float &)> exponential(float a_0, float k);
std::function<void(float &)> fractional(float a_0, float k);
std::function<void(float &)> linear(int max_ep);
}

Eigen::MatrixXf strassen_mul(Eigen::MatrixXf a, Eigen::MatrixXf b);

}

#endif /* MODULE_H */


#include <cmath>
#include <iostream>
#include <vector>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "bateman.hpp"

using Real_t = boost::multiprecision::cpp_dec_float_50;
using vec_t = std::vector<Real_t>;
Real_t exp_cb(Real_t arg){
    return boost::multiprecision::exp(arg);
}

int main(){
   Real_t one = 1;
   Real_t d = one/365;
   Real_t h = d/24;
   Real_t ln2 = boost::multiprecision::log(2*one);
   vec_t lmbd {{ ln2/1.405e10, ln2/5.75, ln2/(6.25*h),
       ln2/1.9116, ln2/(3.6319*d), ln2/(10.64*h), ln2/(60.55/60*h) }};
   auto p = bateman::bateman_parent(lmbd, static_cast<Real_t>(100), exp_cb);
   std::cout << std::setprecision(30);  // show 30 of our 50 digits
   for (auto v : p)
       std::cout << v << " ";
   std::cout << std::endl;
   return 0;
}


#include <iostream>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <boost/multiprecision/cpp_int.hpp>

#define KEY_BYTES_SIZE 32

using BigInt = boost::multiprecision::int1024_t;

struct Point {
    int x;
    BigInt y;

    Point(int x, const BigInt& y) : x(x), y(y) {}

    friend std::ostream& operator<<(std::ostream& stream, const Point& rhs);
};

std::string format_to_hex(const BigInt& num) {
    std::stringstream ss;
    ss << std::hex << std::setfill('0') << std::setw(KEY_BYTES_SIZE * 2) << num;

    return ss.str();
}

std::ostream& operator<<(std::ostream& stream, const Point& rhs) {
    stream << rhs.x << " " << format_to_hex(rhs.y);

    return stream;
}

template <typename T>
T convert_from_hex(const std::string& hex_str) {
    std::stringstream ss;
    ss << std::hex << hex_str;

    T v;
    ss >> v;

    return v;
}

template <size_t bytes_count>
std::string generate_random_hex() {
    std::stringstream ss;
    ss << std::hex << std::setfill('0');

    for (size_t i = 0; i < bytes_count; i++) {
        ss << std::setw(2) << rand() % 256;
    }

    return ss.str();
}

BigInt PRIME = convert_from_hex<BigInt>("FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF43");

std::vector<BigInt> generate_random_coefficients(uint32_t size) {
    std::vector<BigInt> coefficients;
    for (uint32_t i = 0; i < size; i++) {
        BigInt random = convert_from_hex<BigInt>(generate_random_hex<KEY_BYTES_SIZE>());
        coefficients.push_back(random % PRIME);
    }

    return coefficients;
}

BigInt calculate_polynomial(
    std::vector<BigInt>& coefficients,
    uint8_t argument
) {
    BigInt result = 0;
    size_t coefficients_count = coefficients.size();
    for (size_t i = 1; i <= coefficients_count; i++) {
        result += coefficients[i - 1] * pow(BigInt(argument), coefficients_count - i);
    }

    return result % PRIME;
}

std::vector<Point> split(
    uint8_t shares_count,
    uint8_t entry_threshold,
    const BigInt& secret
) {
    std::vector<BigInt> coefficients = generate_random_coefficients(entry_threshold - 1);
    coefficients.push_back(secret);

    std::vector<Point> points;
    for (uint8_t i = 1; i <= shares_count; i++) {
        points.push_back(Point(i, calculate_polynomial(coefficients, i)));
    }

    return points;
}

BigInt recover(std::vector<Point>& shares) {
    auto l = [shares](int x, size_t i) -> BigInt {
        double result = 1;

        for (size_t j = 0; j < shares.size(); j++) {
            if (j != i) {
                result *= (
                    static_cast<double>(x) -
                    static_cast<double>(shares[j].x)) /
                    (static_cast<double>(shares[i].x) -
                    static_cast<double>(shares[j].x)
                );
            }
        }

        return BigInt(result);
    };

    BigInt result = 0;
    for (size_t i = 0; i < shares.size(); i++) {
        result += shares[i].y * l(0, i);
    }

    return result % PRIME;
}

int main(int args_count, char** args_value) {
    srand(static_cast<unsigned int>(time(0)));

    if (args_count < 2) {
        return 1;
    }

    std::string first_argument(args_value[1]);

    if (first_argument == "recover") {
        int shares_count = 0;
        std::cin >> shares_count;

        std::vector<Point> shares;
        for (int i = 0; i < shares_count; i++) {
            int x = 0;
            std::string y;

            std::cin >> x >> y;
            shares.push_back(Point(x, convert_from_hex<BigInt>(y)));
        }

        BigInt result = recover(shares);

        std::cout << format_to_hex(result) << std::endl;
    } else if (first_argument == "split") {
        int shares_count = 0;
        int entry_threshold = 0;
        std::string string_secret;

        std::cin >> shares_count >> entry_threshold >> string_secret;

        if (shares_count < entry_threshold) {
            std::cout << "Shares count must be higher than entry threshold" << std::endl;
            return 1;
        }

        BigInt secret = convert_from_hex<BigInt>(string_secret);

        if (secret > PRIME) {
            std::cout << "Your secret must be less than prime number" << std::endl;
            return 1;
        }

        auto shares = split(shares_count, entry_threshold, secret);

        for (const auto& point : shares) {
            std::cout << point << std::endl;
        }
    } else {
        std::cout << "Unknow arguments" << std::endl;
    }
}


/**
 * linalg.hpp
 *
 * 2021 Gabriel Moreira
 *
 * https://github.com/gabmoreira/maks
 *
 * This software and the related documents  are provided as  is,  with no express
 * or implied  warranties,  other  than those  that are  expressly stated  in the
 * License.
 *
 * Copyright © 2021 Gabriel Moreira. All rights reserved.
 */

#ifndef LINALG_HPP
#define LINALG_HPP

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

/* Linear algebra namespace */
namespace alg {

/**
 * Converts two vectors i and j to matrix indices (in-place).
 *
 * @param ei (input) Eigen::VectorXi - row indices.
 * @param ej (input) Eigen::VectorXi - column indices.
 * @param num_nodes (output) int - number of nodes.
 */
void convertToIdx(Eigen::Ref<Eigen::VectorXi> ei,
                  Eigen::Ref<Eigen::VectorXi> ej,
                  int& num_nodes);

/**
 * Builds sparse graph adjacency matrix.
 *
 * @param ei (input) Eigen::VectorXi - row indices.
 * @param ej (input) Eigen::VectorXi - column indices.
 * @param num_edges (input) int - number of edges.
 * @param sym (input) bool - whether to make the matrix symmetric.
 * @param dst (output) Eigen::SparseMatrix<double> - output adjacency matrix.
 */
void adjacency(const Eigen::Ref<Eigen::VectorXi> ei,
               const Eigen::Ref<Eigen::VectorXi> ej,
               int   num_edges,
               bool  sym,
               Eigen::SparseMatrix<double>& dst);

/**
 * Builds sparse block matrix.
 *
 * @param ei (input) Eigen::VectorXi - row indices.
 * @param ej (input) Eigen::VectorXi - column indices.
 * @param blocks (input) Eigen::MatrixXd - row block matrix containing contiguous blocks.
 * @param block_height (input) int - block height.
 * @param block_width (input) int - block width.
 * @param num_blocks (input) int - number of contiguous blocks.
 * @param sym (input) bool - whether to make the matrix symmetric.
 * @param dst (output) Eigen::SparseMatrix<double> - output adjacency matrix.
 */
void sparseBlocks(const Eigen::Ref<Eigen::VectorXi> ei,
                  const Eigen::Ref<Eigen::VectorXi> ej,
                  const Eigen::Ref<Eigen::MatrixXd> blocks,
                  int   block_height,
                  int   block_width,
                  int   num_blocks,
                  bool  sym,
                  Eigen::SparseMatrix<double>& dst);

/**
 * Solves the orthogonal Procrustes problem in SO(3) (in-place).
 *
 * @param mat (input/output) Eigen::MatrixXd - 3x3 matrix
 */
void orthoProcrustesSO3(Eigen::Ref<Eigen::Matrix3d> mat);

/**
 * Solves the orthogonal Procrustes problem in O(3) (in-place).
 *
 * @param mat (input/output) Eigen::MatrixXd - 3x3 matrix.
 */
void orthoProcrustesO3(Eigen::Ref<Eigen::Matrix3d> mat);

/**
 * Finds the projection of a vector on a subpace spanned by the columns of a matrix.
 *
 * @param subspace_basis (input) Eigen::MatrixXd - Dense matrix whose columns span a subspace
 *        where we want to project the vector v.
 * @param idx (input) unsigned int - Sub-selects of the first idx columns of A only.
 * @param v (input) Eigen::VectorXd - Vector to project.
 * @return The component of the vector in the subspace spanned by the first
 *         idx columns of the input matrix A.
 */
Eigen::VectorXd projectionOnto(const Eigen::Ref<Eigen::MatrixXd> subspace_basis,
                               unsigned int idx,
                               const Eigen::Ref<Eigen::VectorXd> v);

/**
 * Projects a vector onto a subpace spanned by the columns of a matrix (in-place)
 *
 * @param subspace_basis (input) Eigen::MatrixXd - Dense matrix whose columns span a subspace where we
 *        want to project the vector v.
 * @param idx (input) unsigned int - Sub-selects of the first idx columns of A only.
 * @param v (input/output) Eigen::VectorXd - Vector to project.
 */
void projectOnto(const Eigen::Ref<Eigen::MatrixXd> subspace_basis,
                 unsigned int idx,
                 Eigen::Ref<Eigen::VectorXd> v);

/**
 * Symmetric sparse solver via LDLT decomposition.
 *
 * Solves sparse system Ax = b via LDLt decomposition.
 *
 * @param solver (input) Eigen::PardisoLDLT<Sparse> - LDLt Intel PARDISO solver
 * @param mat (input) Eigen::MatrixXd - matrix to factorize.
 * @param rhs (input) Eigen::VectorXd - right hand side.
 * @param x (output) Eigen::VectorXd - solution of the system.
 */
void sparseSolverLDL(Eigen::PardisoLDLT<Eigen::SparseMatrix<double>>* solver,
                     const Eigen::SparseMatrix<double>& mat,
                     const Eigen::Ref<Eigen::MatrixXd> rhs,
                     Eigen::Ref<Eigen::MatrixXd> x);

/**
 * Robust reorthogonalization. For an input matrix V and an input
 * vector r, tries to iteratively orthogonalize r against the j first columns
 * of V. This prevents numerical errors and ensures orthogonality. If, by any
 * chance, and after multiple iterations of subtracting projections, the vector
 * has a norm smaller than 1/sqrt(2) of its starting norm, it cannot be
 * reorthogonalized. This method then attempts to find another orthogonal vector
 * through random restarts. The flag stop is activated if everything fails.
 * (See G. W. Stewart 2001)
 *
 * @param V (input) Dense matrix of doubles whose columns contain orthogonal basis
 *        vectors
 * @param r (input/output) Vector to orthogonalize
 * @param residual_norm (output) Residual norm or r after it has been orthogonalized against the first j columns of V.
 * @param idx (input) Index specifying the number of columns of V used in the orthogonalization.
 * @param stop (output) Stop the algorithm flag. Indicates that it is impossible to find, to machine precision, a vector r orthogonal to the first j cols of V.
 */
void reorthogonalize(const Eigen::Ref<Eigen::MatrixXd> V,
                     Eigen::Ref<Eigen::VectorXd> r,
                     double& residual_norm,
                     unsigned int idx,
                     int& stop);

/**
 * Symmetric Krylov-Schur LDLt eigensolver.
 *
 * For a real sparse symmetric matrix A, this function computes k real eigenvalues
 * near a real target sigma using the Krylov-Schur method (G. W. Stewart 2001).
 *
 * @param solver (input) Eigen::PardisoLDLT<Sparse> - LDLt Intel PARDISO solver.
 * @param mat (input) Eigen::SparseMatrix<double> - sparse matrix to compute eigenvalues and eigenvectors.
 * @param eigenvalues (output) Eigen::VectorXd - k x 1 matrix to store the computed eigenvalues.
 * @param eigenvectors (output) Eigen::MatrixXd - n x k matrix to store normalized eigenvectors.
 * @param k (input) int - number of eigenvalues and eigenvectors to be computed.
 * @param sigma (input) double - spectral shift / eigenvalue target.
*/
void symKrylovSchurLDL(Eigen::PardisoLDLT<Eigen::SparseMatrix<double>>* solver,
                       const Eigen::SparseMatrix<double>& mat,
                       Eigen::Ref<Eigen::VectorXd> eigenvalues,
                       Eigen::Ref<Eigen::MatrixXd> eigenvectors,
                       unsigned int k,
                       double sigma);

};

#endif /* LINALG_HPP */


#ifndef CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHOD_HPP_
#define CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHOD_HPP_

#include <cstddef> // size_t

#include <Eigen/Dense>

namespace cppmath
{
    /**
     * Implementation of Downhill Simplex or Nelder-Mead method for nonlinear optimization from 1998.
     * J. Lagarias, J. Reeds, M. Wright, P. Wright,
     * "Convergence Properties of the Nelder-Mead Simplex Method in Low Dimensions,"
     * SIAM Journal of Optimization, 1998, 9, 112-147
     *
     * \author cpieloth
     * \copyright Copyright 2014 Christof Pieloth, Licensed under the Apache License, Version 2.0
     */
    template< size_t DIM >
    class DownhillSimplexMethod
    {
    public:
        typedef Eigen::Matrix< double, DIM, 1 > ParamsT; /**< Abbreviation for a vector of parameters. */

        /**
         * Enum to indicate how the optimization was converged.
         */
        enum Converged
        {
            CONVERGED_NO, /**< Optimization was not started. */
            CONVERGED_EPSILON, /**< Optimization is smaller than epsilon/threshold. */
            CONVERGED_ITERATIONS, /**< Maximum iterations was reached. */
            CONVERGED_YES /**< Optimization is converged, but not specified how. */
        };

        DownhillSimplexMethod();
        virtual ~DownhillSimplexMethod();

        /**
         * Implementation of the function to minimize.
         *
         * \param x  n-dimensional parameter vector.
         * \return function value for vector x.
         */
        virtual double func( const ParamsT& x ) const = 0;

        double getReflectionCoeff() const;

        void setReflectionCoeff( double coeff );

        double getContractionCoeff() const;

        void setContractionCoeff( double coeff );

        double getExpansionCoeff() const;

        void setExpansionCoeff( double coeff );

        double getShrinkageCoeff() const;

        void setShrinkageCoeff( double coeff );

        size_t getMaximumIterations() const;

        void setMaximumIterations( size_t iterations );

        double getEpsilon() const;

        void setEpsilon( double eps );

        double getInitialFactor() const;

        void setInitialFactor( double factor );

        size_t getResultIterations() const;

        ParamsT getResultParams() const;

        double getResultError() const;

        /**
         * Starts the optimization.
         *
         * \param initial Initial start point.
         * \return Enum::Converged
         */
        Converged optimize( const ParamsT& initial );

    protected:
        /**
         * Orders the parameters x and f(x) from min to max.
         */
        virtual void order();

        /**
         * Checks if the optimization has been converged.
         *
         * \return Enum::Converged
         */
        virtual Converged converged() const;

        /**
         * Creates the initial parameter set and their function values.
         *
         * \param initial Start parameter used to calculate initials.
         */
        virtual void createInitials( const ParamsT& initial );

        double m_initFactor; /**< Factor to create the initial parameter set. */

        ParamsT m_x[DIM + 1]; /**< Vector of all n+1 points. */
        double m_f[DIM + 1]; /**< Stores the function values to reduce re-calculation. */

        double m_epsilon; /**< Threshold or deviation for convergence. */
        size_t m_maxIterations; /**< Maximum iterations until the algorithm is canceled. */
        size_t m_iterations; /**< Iteration counter used for break condition. */

        const size_t DIMENSION; /**< Constant for dimension. */
        const size_t VALUES; /**< Constant for DIM+1. */

    private:
        enum Step
        {
            STEP_START, STEP_EXIT, STEP_REFLECTION, STEP_EXPANSION, STEP_CONTRACTION, STEP_SHRINKAGE
        };

        const size_t N; /**< Index n=DIM-1 */
        const size_t N1; /**< Index n+1=DIM */

        double m_refl; /**< Reflection coefficient. */
        double m_contr; /**< Contraction coefficient. */
        double m_exp; /**< Expansion coefficient. */
        double m_shri; /**< Shrinkage coefficient. */

        void centroid();
        void accept( const ParamsT& x, double f );

        Step reflection();
        Step expansion();
        Step contraction();
        Step shrinkage();

        ParamsT m_xo;
        ParamsT m_xr;
        double m_fr;
    };
} /* namespace cppmath */

// Load the implementation
#include "DownhillSimplexMethod-impl.hpp"

#endif  // CPPMATH_OPTIMIZATION_DOWNHILLSIMPLEXMETHOD_HPP_


#ifndef SM_MATH_HOMOGENEOUS_COORDINATES_HPP
#define SM_MATH_HOMOGENEOUS_COORDINATES_HPP

#include <Eigen/Core>

namespace sm {
namespace kinematics {

Eigen::Matrix<double, 4, 3> toHomogeneousJacobian(const Eigen::Vector3d& v);
Eigen::Vector4d toHomogeneous(const Eigen::Vector3d& v, Eigen::Matrix<double, 4, 3>* jacobian = NULL);

Eigen::Matrix<double, 3, 4> fromHomogeneousJacobian(const Eigen::Vector4d& v);
Eigen::Vector3d fromHomogeneous(const Eigen::Vector4d& v, Eigen::Matrix<double, 3, 4>* jacobian = NULL);

Eigen::MatrixXd toHomogeneousColumns(const Eigen::MatrixXd& M);
Eigen::MatrixXd fromHomogeneousColumns(const Eigen::MatrixXd& M);

template <int N>
Eigen::Matrix<double, N, 1> normalize(const Eigen::Matrix<double, N, 1>& v) {
    return v / v.norm();
}

template <int N>
Eigen::Matrix<double, N, 1> normalizeAndJacobian(const Eigen::Matrix<double, N, 1>& v,
                                                 Eigen::Matrix<double, N, N>& outJacobian) {
    double recip_s_vtv = 1.0 / v.norm();

    outJacobian = (Eigen::Matrix<double, N, N>::Identity() * recip_s_vtv) -
                  (recip_s_vtv * recip_s_vtv * recip_s_vtv) * v * v.transpose();

    return v * recip_s_vtv;
}

/// \brief to the matrix that implements "plus" for homogeneous coordinates.
///        this operation has the same result as adding the corresponding Euclidean points.
Eigen::Matrix4d toHomogeneousPlus(const Eigen::Vector4d& ph);

/// \brief to the matrix that implements "minus" for homogeneous coordinates.
///        this operation has the same result as adding the corresponding Euclidean points.
Eigen::Matrix4d toHomogeneousMinus(const Eigen::Vector4d& ph);

}  // namespace kinematics
}  // namespace sm

#endif /* SM_MATH_HOMOGENEOUS_COORDINATES_HPP */


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <boost/format.hpp>
#include "logistic_regression.h"
#include "utils.h"
#include <omp.h>
#include <ctime>

using namespace std;
using namespace Eigen;

LogisticRegression::LogisticRegression() : max_iter(1000), alpha(0.01), lambda(0.0), tolerance(1e-7), seed(2018), use_batch(false) {}

LogisticRegression::LogisticRegression(const int& max_iter, const double& alpha, const double& lambda, const double& tolerance, const int& seed, bool use_batch): max_iter(max_iter), alpha(alpha), lambda(lambda), tolerance(tolerance), seed(seed), use_batch(use_batch){}

LogisticRegression::~LogisticRegression(){}

void LogisticRegression::fit(const MatrixXd& X, const VectorXd& y, const int& batch_size, const int& early_stopping_round, double (*metric)(double* y, double* pred, int size)){
	// learn VectorXd W, consider reg, max_iter, tol
	srand(seed);
	W = VectorXd::Random(X.cols()+1);   // the last column of weight represent bias
	MatrixXd X_new(X.rows(), X.cols()+1);
	X_new<<X, MatrixXd::Ones(X.rows(),1);
	// perform early stopping
	double best_acc = -1.0;
	double best_loss = -1.0;
	MatrixXd bestW = W;
	int become_worse_round = 0;

	if(use_batch){
		MatrixXd X_batch;
		VectorXd y_batch;
		MatrixXd X_new_batch;
		for(int iter = 0; iter < max_iter; iter++){
			// index of this batch samples
			int start_idx = (batch_size*iter)%(static_cast<int>(X.rows()));
			int end_idx = min(start_idx+batch_size, static_cast<int>(X.rows()));

			X_batch = Utils::slice(X, start_idx, end_idx-1);
			y_batch = Utils::slice(y, start_idx, end_idx-1);
			X_new_batch = Utils::slice(X_new, start_idx, end_idx-1);

			VectorXd y_pred = predict_prob(X_batch);
			VectorXd E = y_pred - y_batch;

			// update W
			W -= (alpha * X_new_batch.transpose() * E + lambda * W);
			// calcalate the logloss and accuracy after this step
			y_pred = predict_prob(X_batch);

			double loss = Utils::crossEntropyLoss(y_batch, y_pred);
			//double acc = metric(y_batch, y_pred);
			double acc = metric(Utils::VectorXd_to_double_array(y_batch), Utils::VectorXd_to_double_array(y_pred), end_idx-start_idx);
			cout << boost::format("Iteration: %d, logloss:%.5f, accuracy:%.5f") %iter %loss %acc << endl;

			// when loss < tolerance, break
			if(loss <= tolerance) break;

			// perform early stopping
			if(acc < best_acc){
				become_worse_round += 1;
			}else{
				become_worse_round = 0;
				best_acc = acc;
				bestW = W;
				best_loss = loss;
			}
			if(become_worse_round >= early_stopping_round){
				W = bestW;
				cout << boost::format("Early stopping in %d, the best iteration is: %d, logloss: %.5f, accuracy:%.5f") %iter %(iter-early_stopping_round) %best_loss %best_acc << endl;
				cout << "Early stopping." << endl;
				break;
			}
		}
	}
	else{
		for(int iter = 0; iter < max_iter; iter++){
			// index of this batch samples

			VectorXd y_pred = predict_prob(X);
			VectorXd E = y_pred - y;

			// update W
			W -= (alpha * X_new.transpose() * E + lambda * W);
			// calcalate the logloss and accuracy after this step
			y_pred = predict_prob(X);

			double loss = Utils::crossEntropyLoss(y, y_pred);
			//double acc = metric(y, y_pred);
			double acc = metric(Utils::VectorXd_to_double_array(y), Utils::VectorXd_to_double_array(y_pred), X.rows());
			cout << boost::format("Iteration: %d, logloss:%.5f, accuracy:%.5f") %iter %loss %acc << endl;

			// when loss < tolerance, break
			if(loss <= tolerance) break;

			// perform early stopping
			if(acc < best_acc){
				become_worse_round += 1;
			}else{
				become_worse_round = 0;
				best_acc = acc;
				bestW = W;
				best_loss = loss;
			}
			if(become_worse_round >= early_stopping_round){
				W = bestW;
				cout << boost::format("Early stopping in %d, the best iteration is: %d, logloss: %.5f, accuracy:%.5f") %iter %(iter-early_stopping_round) %best_loss %best_acc << endl;
				cout << "Early stopping." << endl;
				break;
			}
		}
	}
}

VectorXd LogisticRegression::predict_prob(const MatrixXd& X){
	// predict the probability (of label 1) for given data X
	MatrixXd X_new(X.rows(), X.cols()+1);
	X_new << X, MatrixXd::Ones(X.rows(),1);
	int num_samples = X_new.rows();
	VectorXd y_pred_prob = VectorXd::Zero(num_samples);
	#pragma omp parallel for
	for(int num = 0; num < num_samples; num++){
		y_pred_prob(num) = Utils::sigmoid(X_new.row(num).dot(W));
	}
	return y_pred_prob;
}

VectorXi LogisticRegression::predict(const MatrixXd& X){
	// predict the label for given data X
	VectorXd y_pred_prob = predict_prob(X);
	VectorXi y_pred(y_pred_prob.size());
	#pragma omp parallel for
	for(int num = 0; num < y_pred_prob.size(); num++){
		y_pred(num) = y_pred_prob(num)>0.5?1:0;
	}
	return y_pred;
}

VectorXd LogisticRegression::getW(){
	return W;
}

void LogisticRegression::saveWeights(const std::string& fpath){
	// save the model (save the weight)
	std::ofstream ofile;
	ofile.open(fpath.c_str());
	if (!ofile.is_open()){
		std::cerr << "Can not open the file when call LogisticRegression::saveWeights" << std::endl;
		return;
	}
	// W wirte into the file
	for(int i = 0; i < W.size() - 1; i++){
		ofile << W(i) << " ";
	}
	ofile << W(W.size()-1);
	ofile.close();
}

void LogisticRegression::loadWeights(const std::string& fpath){
	// load the model (load the weight ) from filename
	std::ifstream ifile;
	ifile.open(fpath.c_str());
	if (!ifile.is_open()){
		std::cerr << "Can not open the file when call LogisticRegression::loadWeights" << std::endl;
		return;
	}
	// read the weights into vector<double>
	std::string line;
	std::vector<double> weights;
	getline(ifile, line);   // only one line
	std::stringstream ss(line);
	double tmp;
	while(!ss.eof()){
		ss >> tmp;
		weights.push_back(tmp);
	}
	// initialize VectorXd with std::vector
	W = VectorXd::Map(weights.data(), weights.size());
	ifile.close();
}


#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

class Solver
{
    public:
    Solver();
    double power_iteration(const MatrixXd &A, const double epsilon,const int maxStep);
    VectorXd iterative_refinement(const Ref<const MatrixXd> &A,const VectorXd &b, const double mu_ir, const double mu_rel, const double epsilon, const int max_iter);
    VectorXd solveQP( MatrixXd P, const VectorXd &q, const VectorXd &warm_start , const double epsilon, const double mu_prox, const int max_iter, const bool adaptative_rho);
    VectorXd dualFromPrimalQP(const MatrixXd &P,const VectorXd &q,const VectorXd &l, const double &epsilon);
    VectorXd solveDerivativesQP(const MatrixXd &P, const VectorXd &q, const VectorXd &l, const VectorXd &gamma, const VectorXd &grad_l, const double &epsilon);
    //VectorXd prox_circle(VectorXd l, const VectorXd &l_n);
    void prox_circle(VectorXd &l, const VectorXd &l_n);
    VectorXd solveQCQP( MatrixXd P, const VectorXd &q, const VectorXd &l_n, const VectorXd &warm_start, const double epsilon, const double mu_prox, const int max_iter,const bool adaptative_rho);
    VectorXd dualFromPrimalQCQP(const MatrixXd &P, const VectorXd &q, const VectorXd &l_n, const VectorXd &l, const double &epsilon);
    VectorXd solveDerivativesQCQP(const MatrixXd &P, const VectorXd &q, const VectorXd &l_n, const VectorXd &l, const VectorXd &gamma, const VectorXd &grad_l, const double &epsilon);
    std::tuple<MatrixXd,MatrixXd> getE12QCQP(const VectorXd &l_n, const VectorXd &mu, const VectorXd &gamma);

    void prox_lorentz(VectorXd &l);
    VectorXd solveLCQP( MatrixXd P, const VectorXd &q, const VectorXd &warm_start, const double epsilon, const double mu_prox, const int max_iter,const bool adaptative_rho);
    VectorXd solveDerivativesLCQP(const MatrixXd &P, const VectorXd &q, const VectorXd &l, const VectorXd &grad_l, const double &epsilon);

    int test();

    private:
    int prob_id;
};