/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

#include <cstdlib>
#include <cstdio>
#include <random>
#include <iostream>
#include <armadillo>
#include <ctime>
#include <chrono>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

std::default_random_engine generator;

std::normal_distribution<double> distribution1(0,1);

std::chi_squared_distribution<double> distribution2(1.0);

std::student_t_distribution<double> distribution3(1.0);

std::uniform_real_distribution<double> distribution4(-1.0,1.0);

std::exponential_distribution<double> distribution5(1.0);

std::geometric_distribution<int> distribution6(0.5);

//double laplace_random_number = distribution5(generator) ;
//
//if(distribution4(generator) < 0.0 )
//
//   laplace_random_number = - laplace_random_number;

double get_random_number(int distribution)
{
    switch(distribution)
    {
        case 1: // Normal distribution
            return distribution1(generator);
        case 2: // chi-square
            return distribution2(generator);
        case 3: // student-t
            return distribution3(generator);
        case 4: // uniform
            return distribution4(generator);
        case 5: // exponential
            return distribution5(generator);
        case 6: // geometric
            return distribution6(generator);
        case 7:
            return distribution5(generator)*(distribution4(generator) < 0.0 ? -1 : 1);
        default:
            return 0.0;
            ;
    }
}

int get_distribution_code(const char* var_dist)
{
    if(strcmp(var_dist,"normal") == 0)
        return 1;
    else if(strcmp(var_dist,"chi-square") == 0)
        return 2;
    else if(strcmp(var_dist,"student-t") == 0)
        return 3;
    else if(strcmp(var_dist,"uniform") == 0)
        return 4;
    else if(strcmp(var_dist,"exponential") == 0)
        return 5;
    else if(strcmp(var_dist,"geometric") == 0)
        return 6;
    else if(strcmp(var_dist, "laplace")== 0)
        return 7;
    return 0;
}

int main(int argc, char** argv)

{
    //int64_t seed = time(NULL);
    //generator.seed(seed);
    generator.seed(std::chrono::system_clock::now().time_since_epoch().count());
    double signal_noise_ratio = 6.0;
    int num_vars = 2;
    int dependent_var_idx = 0;
    int distribution = 1; // 1 for normal distribution
    int noise_distribution = 1;
    std::string outputFile;
    int N = 1000;//sample size, number of rows
    std::string description = std::string("Generate a csv file.  Example usage: ") + argv[0] + " ";
//    po::options_description desc(description);
//    desc.add_options()
//            ("signal,s", po::value<double > (&signal_noise_ratio), "signal noise ratio")
//            ("output,o", po::value<std::string > (&outputFile), "The output file. Second positional argument.")
//            ("num_independents,i", po::value<int>(&num_vars)->default_value(1), "The number of independent variables")
//            ("distribution,d", po::value<int>(&distribution)->default_value(1)
//              , (std::string("The probability distributions\n\t 1 for normal distribution\n2 for chi-square")
//             // + "\n\t 3 for student-t distribution"
//             // + "\n\t 4 for uniform distribution"
//             // + "\n\t 5 for exponential distribution"
//              + "\n\t 6 for geometric distribution").c_str()
//            )
//            ("noise_distribution,z", po::value<int>(&noise_distribution)->default_value(1), "noise distribution")
//            ("sample size, n", po::value<int>(&N)->default_value(1000), "number of rows")
//            ("help,h", "Show this help message.")
//            ;

//    po::positional_options_description positionalOptions;
//    positionalOptions.add("input", 1);
//    positionalOptions.add("output", 1);

//    po::variables_map vm;
//    po::store(po::parse_command_line(argc, argv, desc), vm);
//    po::notify(vm);
//    if (vm.count("help") || argc == 1) {
//        std::cout << desc;
//        return 0;
//    }

    // temporary solution due to program options not working
    if(argc < 4)
    {
        printf("Usage: %s num_vars distribution noise_distribution noise_strength\n", argv[0]);
        exit(1);
    }
    num_vars = atoi(argv[1]);
    distribution = get_distribution_code ( argv[2] );
    noise_distribution = get_distribution_code( argv[3] );

    double noise_strength = 1.0;
    if(argc > 4)
        noise_strength = atof(argv[4]);

       const int cols =  num_vars + 1;

       double data[N][cols];

       //arma::vec error(N);

       double sumX=0.0;

       double sumX2=0.0;

       //dependent_var_idx = int((get_random_number(4)+1) * cols + 1) % cols;
       arma::vec beta(num_vars+1);
       //beta(dependent_var_idx) = 0.0;
       std::vector<int> indices(cols);
       for(int j=0;j<cols;j++)
           indices[j] = j;

       //random_shuffle(indices.begin(), indices.end());

       for(int i=0; i < N; i++)
       {
           data[i][ indices[0] ] = get_random_number(distribution);
       }
       for(int j=1; j < cols; j++)
       {
           const int curr = indices[j];
           const int prev = indices[j-1];
           beta[curr] = get_random_number(4) + 8.0;// range (6, 11) + signal_noise_ratio;
           if(abs(beta[curr]) < signal_noise_ratio)
               beta[curr] = abs(beta[curr])/beta[curr]*signal_noise_ratio;
           //double strength = int((get_random_number(4)) * 2000) % 50;

           for(int i=0; i<N; i++)
           {
                data[i][curr] = beta[curr] * data[i][prev] + noise_strength*get_random_number(noise_distribution);
           }
       }

       for(int j=1; j < cols; j++)
           printf("# %d -> %d, %12.4f\n", indices[j-1], indices[j], beta(indices[j]));
       printf("\n");
       for(int i=0; i<N; i++)
       {
           for(int j=0; j < cols; j++)
              printf("%12.4f%s ", data[i][j], j == cols-1 ? "" : ",");
           printf("\n");
       }

       return 0;
}


// test_newton.cpp
// (c) Tivole

#include <boost/test/unit_test.hpp>
#include "../src/numerary.hpp"

namespace numerary
{

    BOOST_AUTO_TEST_SUITE(TestNewton)

    void test_newton_function_1(double *x, double *fv, int n) {
        fv[0] = x[0]*x[0] + x[1]*x[1] - 5;
        fv[1] = x[1] - 3*x[0] + 5;
    }

    void test_newton_function_2(double *x, double *fv, int n) {
        fv[0] = pow(x[0], 5) + pow(x[1], 3) * pow(x[2], 4) + 1;
        fv[1] = x[0]*x[0]*x[1]*x[2];
        fv[2] = pow(x[2], 4) - 1;
    }

    BOOST_AUTO_TEST_CASE(test_newton_1)
    {
        const int maxiter = 100;
        int result, expected_result;
        const double eps = 1.e-7;
        double *x = new double[2], *fv = new double[2];
        bool is_passed = false;
        expected_result = 1;
        x[0] = 1; x[1] = 2;

        result = Numerary::nonlinear_systems_of_equations(test_newton_function_1, x, fv, 2, "newton", eps, maxiter);
        BOOST_CHECK_EQUAL(result, expected_result);

        if ((fabs(x[0] - 1) < 1.e-5) && (fabs(x[1] + 2) < 1.e-5)) is_passed = true;
        if ((fabs(x[0] - 2) < 1.e-5) && (fabs(x[1] - 1) < 1.e-5)) is_passed = true;

        BOOST_CHECK(is_passed);

        delete[] x;
        delete[] fv;
    }

    BOOST_AUTO_TEST_CASE(test_newton_2)
    {
        const int maxiter = 100;
        int result, expected_result;
        const double eps = 1.e-7;
        double *x = new double[3], *fv = new double[3];
        bool is_passed = false;
        expected_result = 1;
        x[0] = 1; x[1] = 2, x[2] = -1;

        result = Numerary::nonlinear_systems_of_equations(test_newton_function_2, x, fv, 3, "newton", eps, maxiter);
        BOOST_CHECK_EQUAL(result, expected_result);

        if ((fabs(x[0] + 1) < 1.e-4) && (fabs(x[1]) < 1.e-4) && (fabs(x[2] + 1) < 1.e-4)) is_passed = true;
        if ((fabs(x[0] + 1) < 1.e-4) && (fabs(x[1]) < 1.e-4) && (fabs(x[2] - 1) < 1.e-4)) is_passed = true;
        if ((fabs(x[0]) < 1.e-4) && (fabs(x[1] + 1) < 1.e-4) && (fabs(x[2] + 1) < 1.e-4)) is_passed = true;
        if ((fabs(x[0]) < 1.e-4) && (fabs(x[1] + 1) < 1.e-4) && (fabs(x[2] - 1) < 1.e-4)) is_passed = true;

        BOOST_CHECK(is_passed);

        delete[] x;
        delete[] fv;
    }

    BOOST_AUTO_TEST_SUITE_END()
}


//
// Created by yuan on 27/07/2021.
//
#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <chrono>
#include <thread>
#include <array>
#include <cmath>
#include <functional>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <vector>

Eigen::MatrixXd pinv_Eigen_SVD(Eigen::MatrixXd &origin) {

  const float er = 0;
  // perform svd decomposition
  Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(origin, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Build SVD decomposition results
  Eigen::MatrixXd U = svd_holder.matrixU();
  Eigen::MatrixXd V = svd_holder.matrixV();
  Eigen::MatrixXd D = svd_holder.singularValues();

  // Build the S matrix
  Eigen::MatrixXd S(V.cols(), U.cols());
  S.setZero();

  for (unsigned int i = 0; i < D.size(); ++i) {

    if (D(i, 0) > er) {
      S(i, i) = 1 / D(i, 0);
    } else {
      S(i, i) = 0;
    }
  }

  // pinv_matrix = V * S * U^T
  return V * S * U.transpose();
}

int main()
{
  std::vector<int> values(10000);

  // Generate Random values
  auto f = []() -> int { return rand() % 10000; };

  // Fill up the vector
  generate(values.begin(), values.end(), f);

  // Get starting timepoint
  auto start = std::chrono::high_resolution_clock::now();

  // Call the function, here sort()
  Eigen::MatrixXd jacobian;

//  for ( int i(0); i<1; i++ ) {
    jacobian.setRandom(6,7);
//    Eigen::MatrixXd pinv_jacobian1 = pinv_Eigen_SVD(jacobian);
//    Eigen::MatrixXd pinv_jacobian2 = jacobian.completeOrthogonalDecomposition().pseudoInverse();
    Eigen::MatrixXd pinv_jacobian3 = jacobian.transpose() * (jacobian * jacobian.transpose()).inverse();
//  }

  // Get ending timepoint
  auto stop = std::chrono::high_resolution_clock::now();

//  std::cout << pinv_jacobian1 << std::endl << std::endl;
//  std::cout << pinv_jacobian2 << std::endl << std::endl;
//  std::cout << pinv_jacobian3 << std::endl << std::endl;

  // Get duration. Substart timepoints to
  // get durarion. To cast it to proper unit
  // use duration cast method
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);

  std::cout << "Time taken by function: "
       << duration.count() << " microseconds" << std::endl;

  return 0;
}


#include "wave.h"
#include <boost/math/special_functions/bessel.hpp>

/**
 * Hankel_n^{(1)} function
 *
 * @param n index
 * @param x argument
 * @return value
 */
std::complex<double>
hankel1(const int n, const double x) {
    double besselJ = boost::math::cyl_bessel_j<int, double>(n, x);
    double besselY = boost::math::cyl_neumann<int, double>(n, x);
    return besselJ + J1*besselY;
}

/**
 * Incident wave
 *
 * @param kvec incident wave vector
 * @param point target point
 * @return amplitude
 */
std::complex<double>
incident(const double kvec[], const double point[]) {
    return std::exp(J1*(kvec[0]*point[0] + kvec[1]*point[1]));
}

/**
 * Normal gradient of the incident wave, assumes incident wave is exp(1j * kvec.x)
 *
 * @param nvec normal vector pointing inwards
 * @param kvec incident wave vector
 * @param point (source) point
 * @return amplitude
 */
std::complex<double>
gradIncident(const double nvec[], const double kvec[],
             const double point[]) {
    return J1*(nvec[0]*kvec[0] + nvec[1]*kvec[1])*incident(kvec, point);
}

/**
 * Scattered wave contribution from a single segment
 *
 * @param kvec incident wave vector
 * @param p0 starting point of the segment
 * @param p1 end point of the segment
 * @param point observer point
 * @return wave contribution
 */
std::complex<double>
computeScatteredWaveElement(const double kvec[], const double p0[],
                            const double p1[], const double point[]) {

    // xdot is anticlockwise
    double xdot[] = {p1[0] - p0[0], p1[1] - p0[1]};

    // mid point of the segment
    double pmid[] = {0.5*(p0[0] + p1[0]), 0.5*(p0[1] + p1[1])};

    // segment length
    double dsdt = std::sqrt(xdot[0]*xdot[0] + xdot[1]*xdot[1]);

    // normal vector, pointintg inwards and normalised
    double nvec[] = {-xdot[1]/dsdt, xdot[0]/dsdt, 0.};

    // from segment mid-point to observer
    double rvec[] = {point[0] - pmid[0], point[1] - pmid[1]};
    double r = std::sqrt(rvec[0]*rvec[0] + rvec[1]*rvec[1]);

    double kmod = std::sqrt(kvec[0]*kvec[0] + kvec[1]*kvec[1]);
    double kr = kmod * r;

    // Green functions and normal derivatives
    std::complex<double> g = (J1/4.) * hankel1(0, kr);
    double nDotR = nvec[0]*rvec[0] + nvec[1]*rvec[1];
    std::complex<double> dgdn = (-J1/4.) * hankel1(1, kr) * kmod * nDotR / r;

    // contribution from the gradient of the incident wave on the surface
    // of the obstacle. The normal derivative of the scattered wave is
    // - normal derivative of the incident wave.
    std::complex<double> scattered_wave = - dsdt * g * gradIncident(nvec, kvec, pmid);

    // shadow side: total wave is nearly zero
    //              => scattered wave amplitude = -incident wave ampl.
    //
    // illuminated side:
    //              => scattered wave amplitude = +incident wave ampl.
    //
    double nDotK = nvec[0]*kvec[0] + nvec[1]*kvec[1];
    double shadow = (nDotK > 0 ? 1.0 : -1.0);

    scattered_wave += shadow * dsdt * dgdn * incident(kvec, pmid);

    return scattered_wave;
}

extern "C" void
cincident (const double kvec[], const double point[], double* real_part, double* imag_part) {
    std::complex<double> res = incident(kvec, point);
    *real_part = res.real();
    *imag_part = res.imag();
}

extern "C" void computeScatteredWave(const double kvec[], int nc, const double xc[], const double yc[],
                                     const double point[], double* real_part, double* imag_part) {
    double res_real = 0;
    double res_imag = 0;
    // ADD OPENMP PRAGMA HERE
    for (int i = 0; i < nc - 1; ++i) {
        double p0[] = {xc[i], yc[i]};
        double p1[] = {xc[i + 1], yc[i + 1]};
        std::complex<double> res = computeScatteredWaveElement(kvec, p0, p1, point);
        res_real += res.real();
        res_imag += res.imag();
    }
    *real_part = res_real;
    *imag_part = res_imag;
}


#include <Eigen/Dense>

#include "boundingbox.h"
#include "color.h"
#include "command.h"
#include "functions.h"
#include "numstring.h"
#include "part.h"
#include "renderer.h"
#include "hexgrid.h"

//using namespace BWCNC;

void BWCNC::HexGrid::nested_params( double spacing_ratio, int nth_from_outer, const Eigen::Vector3d & start, Eigen::Vector3d & pos, double & nsidelen )
{
    double hypot = spacing_ratio * nth_from_outer * m_sidelen;
    pos = start + hypot * Eigen::Vector3d( sin( M_PI/3.0 ), cos( M_PI/3.0 ), 0 );
    nsidelen = m_sidelen - hypot;
}

BWCNC::Part * BWCNC::HexGrid::make_inner_hexagon(
    BWCNC::PartContext & k,
    double inner_fraction,
    int nth_inner,
    const Eigen::Vector3d & start )
{
  Eigen::Vector3d pos(0,0,0);
  double nested_sidelen = 0;
  nested_params( inner_fraction, nth_inner, start, pos, nested_sidelen );
  return hexagon( k, nested_sidelen, pos, nullptr, true );
}

BWCNC::Part * BWCNC::HexGrid::hexagon( BWCNC::PartContext & k, const double sidelen, const Eigen::Vector3d & start, const uint8_t * sides2skip, bool closed )
{
  // orientation: left-right symmetry around the vertical center line through two vertices
  const double triangle_longside  = sidelen * cos(M_PI_2/3);
  const double triangle_shortside = sidelen * sin(M_PI_2/3);

  std::vector<Eigen::Vector3d> path;
  {
    path.push_back( Eigen::Vector3d( triangle_longside, -triangle_shortside, 0 ) );
    path.push_back( Eigen::Vector3d( triangle_longside,  triangle_shortside, 0 ) );
    path.push_back( Eigen::Vector3d(                 0,             sidelen, 0 ) );
    path.push_back( Eigen::Vector3d(-triangle_longside,  triangle_shortside, 0 ) );
    path.push_back( Eigen::Vector3d(-triangle_longside, -triangle_shortside, 0 ) );
    path.push_back( Eigen::Vector3d(                 0,            -sidelen, 0 ) );
  }

  BWCNC::Part * part = k.get_new_part();
  part->update_starting_position( start );

  Eigen::Vector3d vec = start;
  for( int i = 0; i < 6; i++ )
  {
    bool closedok = false;
    if( closed && i == 5 )
      part->lineto_close( closedok );

    if( ! closedok )
    {
      vec = vec + path[i];
      if( sides2skip && sides2skip[i] ) part->moveto( vec );
      else                              part->lineto( vec );
    }
  }

  return part;
}

void BWCNC::HexGrid::fill_partctx_with_grid( BWCNC::PartContext & k )
{
  const uint8_t skip0[]    = {1,0,0,0,0,0};
//const uint8_t skip01[]   = {1,1,0,0,0,0};
  const uint8_t skip05[]   = {1,0,0,0,0,1};
  const uint8_t skip015[]  = {1,1,0,0,0,1};
  const uint8_t skip1[]    = {0,1,0,0,0,0};
//const uint8_t skip2[]    = {0,0,1,0,0,0};  // debugging only
  const uint8_t skip5[]    = {0,0,0,0,0,1};

  //###############################################################################################################
  //#### Row #1
  //###############################################################################################################
  double x_ref = 0, y_ref = m_sidelen/2.0;
  Eigen::Vector3d start( x_ref, y_ref, 0);

  for( int i = m_nested; i > 0; i-- )
    k.add_part( make_inner_hexagon( k, m_nested_spacing, i, start ) );

  if( m_includegrid )
    k.add_part( hexagon( k, m_sidelen, start ) );

  for( int j = 1; j < m_cols; j++ )
  {
    start = Eigen::Vector3d( j * sqrt(3) * m_sidelen, y_ref, 0);

    for( int i =  m_nested; i > 0; i-- )
      k.add_part( make_inner_hexagon( k, m_nested_spacing, i, start ) );

    if( m_includegrid )
      k.add_part( hexagon( k, m_sidelen, start, skip5 ) );
  }
  //###############################################################################################################
  //#### End of Row #1
  //###############################################################################################################

  //###############################################################################################################
  //#### remaining rows
  //###############################################################################################################
  double xoffset;
  for( int row = 2; row <= m_rows; row++ )
  {
    bool row_iseven  = (row % 2 == 0);
    x_ref       = row_iseven ? (sqrt(3) * m_sidelen / 2.0) : 0.0;
    xoffset     = row_iseven ? 0.5 : 0.0;
    y_ref      += 1.5 * m_sidelen;

    start = Eigen::Vector3d( x_ref, y_ref, 0);

    for( int i = m_nested; i > 0; i-- )
      k.add_part( make_inner_hexagon( k, m_nested_spacing, i, start ) );

    if( m_includegrid )
    {
      if( row_iseven ) k.add_part( hexagon( k, m_sidelen, start, skip0 ) );
      else             k.add_part( hexagon( k, m_sidelen, start, skip1 ) );
    }

    for( int j = 1; j < m_cols; j++ )
    {
      x_ref = (j + xoffset) * sqrt(3) * m_sidelen;

      start = Eigen::Vector3d( x_ref, y_ref, 0);

      for( int i = m_nested; i > 0; i-- )
        k.add_part( make_inner_hexagon( k, m_nested_spacing, i, start ) );

      if( m_includegrid )
      {
        Part * p = nullptr;
        if( row_iseven )
        {
          if( j < m_cols - 1 ) p = hexagon( k, m_sidelen, start, skip015 );
          else                 p = hexagon( k, m_sidelen, start,  skip05 );
        }
        else
        {
          if( j == 1 )         p = hexagon( k, m_sidelen, start,   skip1 );
          else                 p = hexagon( k, m_sidelen, start, skip015 );
        }

        if( p != nullptr )
        {
          //std::cerr << "context: " << k.get_bbox();
          //std::cerr << " -- adding new: " << p->get_bbox();
          k.add_part( p );
          //std::cerr << " -- results in context: " << k.get_bbox() << "\n";
        }
        //else std::cerr << "no hexagon part was made\n";
      }
    }
  }
  //###############################################################################################################
  //#### end of remaining rows
  //###############################################################################################################
}

#ifdef HEXGRID_UNITTEST

struct cmdline_params {
    int cols;
    int rows;
    int nested;
    double nested_spacing;

    double sidelen;
    double scale;
    double yshift;
    double xshift;

    bool suppress_grid;
    const char * moveto_clr;
    const char * lineto_clr;
} parms = {
    //5, 5, 3, 2,
    40, 40, 1, .2,
    //4, 4, 1, .2,
    .2, 60, 0, 0,
    true,
    nullptr,     // "#0000FF",
    "#ff0000"
};

//static const double w = (2 * M_PI)/6.0;
static const double w = (2 * M_PI)/30;

static const Eigen::Vector3d shift_tform( const Eigen::Vector3d & v )
{
    return 1.8 * Eigen::Vector3d( cos(w*(v[1] + parms.yshift)), sin(w*(v[0] + parms.xshift)), 0 );
}

static const Eigen::Matrix3d rotation_tform( const Eigen::Vector3d & )
{
    Eigen::Matrix3d mat;
    double t = M_PI/2;
    mat <<  cos(t), -sin(t), 0,
            sin(t),  cos(t), 0,
            0, 0, 1 ;
    return mat;
}

static const Eigen::Matrix3d skew_tform( const Eigen::Vector3d & v )
{
    Eigen::Matrix3d mat;
    mat <<  cos(w*(v[1] + parms.yshift)), 0, 0,
            0, sin(w*(v[0] + parms.xshift)), 0,
            0, 0, 0 ;
    return 1.8 * mat;
}

static bool handle_params( int argc, char ** argv )
{
    for( int i = 1; i < argc; i++ )
    {
        if( 0 == strcmp( "--suppress_grid", argv[i] ) )
            parms.suppress_grid = true;
        else
            return false;
    }
    return true;
}

int main( int argc, char ** argv )
{
    Eigen::Vector3d shift_hist;
    BWCNC::PartContext k;

    if( handle_params( argc, argv ) )
    {
        BWCNC::HexGrid grid( parms.cols, parms.rows, parms.sidelen,
                             parms.nested, parms.nested_spacing, ! parms.suppress_grid );

        grid.fill_partctx_with_grid( k );
        //k.remake_boundingbox();

        shift2center( k, &shift_hist );

        k.position_dependent_transform( skew_tform, shift_tform );
        //k.remake_boundingbox();

        k.scale( parms.scale );

        k.position_dependent_transform( rotation_tform, nullptr );
        //k.remake_boundingbox();
        undo_shift2( k, shift_hist );

        BWCNC::SVG renderer;
        renderer.set_moveto_color( parms.moveto_clr );
        renderer.set_lineto_color( parms.lineto_clr );
        renderer.render_all( k );
    }

    return 0;
}
#endif

#if 0
    HexGrid( int c = 10, int r = 10, double len = 1,
             int nested_count = 1, double nested_pcnt = .5, bool includegrid = true,
             const char * clr_lineto = "#ff0000",
             const char * clr_moveto = nullptr,
             const char * clr_bckgrd = "#ffffff" )
#endif


// main.cpp
#include <stdio.h>
#include <iostream>
#include <Eigen/Dense>
#include <iostream>
#include "sophus/so3.hpp"
#include "sophus/common.hpp"

using Eigen::MatrixXd;
int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  m(1,1) = 0;
  std::cout << m << std::endl;

  // The following demonstrates the group multiplication of rotation matrices

  // Create rotation matrices from rotations around the x and y and z axes:
  const double kPi = Sophus::Constants<double>::pi();
  Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4);
  Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6);
  Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3);

  std::cout << "The rotation matrices are" << std::endl;
  std::cout << "R1:\n" << R1.matrix() << std::endl;
  std::cout << "R2:\n" << R2.matrix() << std::endl;
  std::cout << "R3:\n" << R3.matrix() << std::endl;
  std::cout << "Their product R1*R2*R3:\n"
            << (R1 * R2 * R3).matrix() << std::endl;
  std::cout << std::endl;

  // Rotation matrices can act on vectors
  Eigen::Vector3d x;
  x << 0.0, 0.0, 1.0;
  std::cout << "Rotation matrices can act on vectors" << std::endl;
  std::cout << "x\n" << x << std::endl;
  std::cout << "R2*x\n" << R2 * x << std::endl;
  std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl;
  std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl;
  std::cout << std::endl;

  // SO(3) are internally represented as unit quaternions.
  std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl;
  std::cout << "R1 in unit quaternion form:\n"
            << R1.unit_quaternion().coeffs() << std::endl;
  // Note that the order of coefficiences of Eigen's quaternion class is
  // (imag0, imag1, imag2, real)
  std::cout << std::endl;
  // The following demonstrates the group multiplication of rotation matrices

  // Create rotation matrices from rotations around the x and y and z axes:
  // const double kPi = Sophus::Constants<double>::pi();
  // Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4);
  // Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6);
  // Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3);

  // std::cout << "The rotation matrices are" << std::endl;
  // std::cout << "R1:\n" << R1.matrix() << std::endl;
  // std::cout << "R2:\n" << R2.matrix() << std::endl;
  // std::cout << "R3:\n" << R3.matrix() << std::endl;
  // std::cout << "Their product R1*R2*R3:\n"
  //           << (R1 * R2 * R3).matrix() << std::endl;
  // std::cout << std::endl;

  // // Rotation matrices can act on vectors
  // Eigen::Vector3d x;
  // x << 0.0, 0.0, 1.0;
  // std::cout << "Rotation matrices can act on vectors" << std::endl;
  // std::cout << "x\n" << x << std::endl;
  // std::cout << "R2*x\n" << R2 * x << std::endl;
  // std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl;
  // std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl;
  // std::cout << std::endl;

  // // SO(3) are internally represented as unit quaternions.
  // std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl;
  // std::cout << "R1 in unit quaternion form:\n"
  //           << R1.unit_quaternion().coeffs() << std::endl;
  // // Note that the order of coefficiences of Eigen's quaternion class is
  // (imag0, imag1, imag2, real)
  std::cout << std::endl;

}


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

#ifndef HEADER_EXT_MATRIX2_HPP_INCLUDED
#define HEADER_EXT_MATRIX2_HPP_INCLUDED

#include "vector2.hpp"

#include <cassert>

#include <algorithm>

#include <boost/operators.hpp>

namespace ext
{

	template <typename T>
	class Matrix2 :
		boost::equality_comparable<Matrix2<T>,
		boost::ring_operators<Matrix2<T>,
		boost::multipliable<Matrix2<T>, T
		>>>
	{
	public:
		using Type = T;

		Type x[4];
	};

	using Matrix2f = Matrix2<float>;
	using Matrix2d = Matrix2<double>;
	using Matrix2ld = Matrix2<long double>;

	template <typename T>
	bool operator==(Matrix2<T> const& m1, Matrix2<T> const& m2)
	{
		return std::equal(m1.x, m1.x + (sizeof m1.x / sizeof m1.x[0]), m2.x);
	}

	template <typename T>
	Matrix2<T>& operator+=(Matrix2<T>& m1, Matrix2<T> const& m2)
	{
		std::transform(m1.x, m1.x + (sizeof m1.x / sizeof m1.x[0]), m2.x, m1.x, [](T one, T two){ return one + two; });
		return m1;
	}

	template <typename T>
	Matrix2<T>& operator-=(Matrix2<T>& m1, Matrix2<T> const& m2)
	{
		std::transform(m1.x, m1.x + (sizeof m1.x / sizeof m1.x[0]), m2.x, m1.x, [](T one, T two){ return one - two; });
		return m1;
	}

	template <typename T>
	Matrix2<T>& operator*=(Matrix2<T>& m1, Matrix2<T> const& m2)
	{
		auto const m = m1;
		m1.x[0] = m.x[0] * m2.x[0] + m.x[1] * m2.x[2];
		m1.x[1] = m.x[0] * m2.x[1] + m.x[1] * m2.x[3];
		m1.x[2] = m.x[2] * m2.x[0] + m.x[3] * m2.x[2];
		m1.x[3] = m.x[2] * m2.x[1] + m.x[3] * m2.x[3];
		return m1;
	}

	template <typename T>
	Matrix2<T>& operator*=(Matrix2<T>& m, T const a)
	{
		std::transform(m.x, m.x + (sizeof m.x / sizeof m.x[0]), m.x, [a](T x){ return x * a; });
		return m;
	}

	template <typename T>
	Vector2<T> operator * (Matrix2<T> const& m, Vector2<T> const& v)
	{
		return {m.x[0] * v.x + m.x[1] * v.y, m.x[2] * v.x + m.x[3] * v.y};
	}

	template <typename T>
	Vector2<T> operator * (Vector2<T> const& v, Matrix2<T> const& m)
	{
		return {v.x * m.x[0] + v.y * m.x[2], v.x * m.x[1] + v.y * m.x[3]};
	}

	template <typename T>
	Matrix2<T> transpose(Matrix2<T> const& m)
	{
		auto t = m;
		t.x[1] = m.x[2];
		t.x[2] = m.x[1];
		return t;
	}

	template <typename T>
	T trace(Matrix2<T> const& m)
	{
		return m.x[0] + m.x[3];
	}

	template <typename T>
	T determinant(Matrix2<T> const& m)
	{
		return m.x[0] * m.x[3] - m.x[1] * m.x[2];
	}

	template <typename T>
	Matrix2<T> inverse(Matrix2<T> const& m)
	{
		assert(determinant(m) != 0);
		Matrix2<T> n;
		n.x[0] = m.x[3];
		n.x[1] = -m.x[1];
		n.x[2] = -m.x[2];
		n.x[3] = m.x[0];
		n *= 1/determinant(m);
		return n;
	}

} // namespace ext

#endif // !HEADER_EXT_MATRIX2_HPP_INCLUDED


#ifndef _LINEAR_ALGEBRA_HPP_
#define _LINEAR_ALGEBRA_HPP_

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/StdVector>
#include <dlib/optimization.h>
#include <mimkl/definitions.hpp>
#include <mimkl/utilities.hpp>
#include <numeric>
#include <spdlog/fmt/ostr.h>
#include <spdlog/spdlog.h>

using mimkl::definitions::Index;

namespace mimkl
{
namespace linear_algebra
{

//! \f$ K' = K / (tr(K) / n)\f$
/*!
devide matrix by a factor so that trace will equal the number of rows \f$ tr(K')
= n \f$
@param K square matrix to be trace_normalized by reference
@return trace_factor \f$ tr(K) / n\f$
*/
template <typename Derived>
typename Derived::Scalar trace_normalize(Derived &K)
{
    typename Derived::Scalar trace_factor = K.trace() / K.rows();
    K = K / trace_factor;
    return trace_factor;
}

//! \f$ k^{*}(x,y) = \fraq{k(x,y)}{\sqrt{k(x,x) k(y,y)}}\f$
template <typename Derived>
Derived normalize_kernel(const Derived &K)
{
    COLUMN(typename Derived::Scalar) diag = 1 / K.diagonal().array().sqrt();
    return K.array() * (diag * diag.transpose()).array(); // outer product of
                                                          // diagonal elements
}

//! \f$ k^{*}(x,y) = \fraq{k(x,y)}{\sqrt{k(x,x) k(y,y)}}\f$, where k(x,x) stems
//! from the training data and k(y,y) from the test data
template <typename LhsDerived, typename RhsDerived, typename Derived>
Derived normalize_kernel_prediction(const Derived &K,
                                    const LhsDerived &lhs,
                                    const RhsDerived &rhs)
{
    COLUMN(typename Derived::Scalar)
    diag_lhs = 1 / lhs.diagonal().array().sqrt();
    COLUMN(typename Derived::Scalar)
    diag_rhs = 1 / rhs.diagonal().array().sqrt();
    return K.array() *
           (diag_lhs * diag_rhs.transpose()).array(); // outer product of
                                                      // diagonal elements
}

//! centralizing the data in feature space \f$ \mathbf{K}' = ( \mathbf{I} -
//! \mathbf{1_N}) \mathbf{K} ( \mathbf{I} - \mathbf{1_N}) =  \mathbf{K} -
//! \mathbf{1_N} \mathbf{K} - \mathbf{K} \mathbf{1_N} + \mathbf{1_N} \mathbf{K}
//! \mathbf{1_N}\f$
/*!
where \f$\mathbf{1_N}\f$ denotes a N-by-N matrix for which each element takes
value 1/N
*/
template <typename Derived>
Derived centralize_kernel(const Derived &K)
{ // TODO how to do for test data?
    Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1> rowwise_mean =
    K.rowwise().mean();
    return ((K.colwise() - rowwise_mean).rowwise() - rowwise_mean.transpose())
           .array() +
           rowwise_mean.mean();
}

template <typename Scalar,
          typename StorageOrder> // TODO why does const & not compile?
Eigen::Map<const COLUMN(Scalar)> dlib_to_eigen(
const dlib::matrix<Scalar, 0, 1, dlib::default_memory_manager, StorageOrder> &vector)
{ // more general case
    // with template
    // specializations not
    // needed as of yet.
    spdlog::get("console")->debug("sum over passed vector:\n{}",
                                  dlib::sum(vector));
    typedef COLUMN(Scalar) Column;
    typedef Eigen::Map<const Column> MapColumn;
    // Map Dlib to Eigen
    const Scalar *dlib_ptr = vector.begin(); //&vector.data(0,0);
    if (dlib::is_row_major(vector) ^ (Column::Flags & Eigen::RowMajorBit))
    { // if XOR warn, for instance Y: Y.isRowMajor
        auto logger = spdlog::get("console");
        logger->critical("make sure LibMat is in col-major or that mapping "
                         "happens "
                         "in row-major:\n"
                         "dlib::is_row_major(vector): {}\n"
                         "Column::Options: {}\n"
                         "(Column::Flags&Eigen::RowMajorBit): {}",
                         dlib::is_row_major(vector), Column::Options,
                         (Column::Flags & Eigen::RowMajorBit));
    };
    return MapColumn(dlib_ptr, vector.nr(), 1);
}

//! squared euclidean distance of rows in X to itself: M_ij =  x_i^T*x_i +
//! x_j^T*x_j -
//! 2*x_i^T*x_j
template <typename LhsDerived, typename MDerived>
void squared_euclidean_distances(const Eigen::MatrixBase<LhsDerived> &lhs,
                                 const Eigen::MatrixBase<MDerived> &distance_matrix)
{
    MDerived lin_kernel = lhs * lhs.transpose();
    const_cast<Eigen::MatrixBase<MDerived> &>(distance_matrix) =
    ((-2 * lin_kernel.real()).colwise() + lin_kernel.diagonal()).rowwise() +
    lin_kernel.diagonal().adjoint();
}

template <typename Scalar>
MATRIX(Scalar)
rowwise_soft_max(MATRIX(Scalar) M)
{
    for (Index i = 0; i < M.rows(); i++)
    {
        Scalar max = M.row(i).maxCoeff(); // numerical stability
        Scalar sum = 0;
        for (Index j = 0; j < M.cols(); j++)
        {
            sum += std::exp(M(i, j) - max);
        }
        Scalar normalizer = std::log(sum); // numerical stability
        for (Index j = 0; j < M.cols(); j++)
        {
            M(i, j) = std::exp(M(i, j) - max - normalizer);
        }
    }
    return M;
}

// TODO documentation
template <typename Scalar>
MATRIX(Scalar)
sum_matrices(const std::vector<MATRIX(Scalar)> &Ks)
{
    // fold over Ks
    return std::accumulate(Ks.begin(), Ks.end(),
                           MATRIX(Scalar)::Zero((*Ks.begin()).rows(),
                                                (*Ks.begin()).cols())
                           .eval()); //, [](a,b){operator+(a,b)}
}

template <typename Scalar>
MATRIX(Scalar)
sum_trace_normalized_matrices(const std::vector<MATRIX(Scalar)> &Ks,
                              std::vector<Scalar> &trace_factors)
{
    // fold over Ks, trace_normalize K before summing and store its trace
    MATRIX(Scalar) acc; // not writing 0s
    MATRIX(Scalar) K;   // temporary holding the trace_normalized kernel
    if (Ks.begin() != Ks.end())
    {
        Index i = 0;
        acc = Ks[i]; // non const copy
        trace_factors[i] = trace_normalize(acc);
        ++i;
        for (; i < Ks.size(); ++i)
        {
            K = Ks[i];
            trace_factors[i] = trace_normalize(K);
            acc += K;
        }
    }
}

// not using std::accumulate, see commented version below
template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_kernels(const MATRIX(Scalar) & lhs,
                  const MATRIX(Scalar) & rhs,
                  const std::vector<Function> &Ks)
{
    MATRIX(Scalar) acc; // not writing 0s
    if (Ks.begin() != Ks.end())
    {
        typename std::vector<Function>::const_iterator iter = Ks.begin();
        acc = (*iter)(lhs, rhs);
        ++iter;
        for (; iter != Ks.end(); ++iter)
        {
            acc += (*iter)(lhs, rhs);
        }
    } // TODO else { throw | warn | ....}
    return acc;
}

template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_trace_normalized_kernels(const MATRIX(Scalar) & lhs,
                                   const MATRIX(Scalar) & rhs,
                                   const std::vector<Function> &Ks,
                                   std::vector<Scalar> &trace_factors,
                                   bool update_trace_factors)
{
    if (Ks.size() != trace_factors.size())
        throw std::length_error(
        " number of functions does not match number of trace_factors");
    MATRIX(Scalar) acc; // not writing 0s
    if (update_trace_factors)
    {
        MATRIX(Scalar) K; // temporary holding the trace_normalized kernel
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            K = Ks[i](lhs, rhs); // non const copy
            trace_factors[i] = trace_normalize(K);
            acc = K;
            ++i;
            for (; i < Ks.size(); ++i)
            {
                K = Ks[i](lhs, rhs);
                trace_factors[i] = trace_normalize(K);
                acc += K;
            }
        }
    }
    else
    {
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            acc = Ks[i](lhs, rhs) / trace_factors[i];
            ++i;
            for (; i < Ks.size(); ++i)
            {
                acc += Ks[i](lhs, rhs) / trace_factors[i];
            }
        }
    }
    return acc;
}

// TODO documentation
template <typename Scalar>
MATRIX(Scalar)
sum_weighted_matrices(const std::vector<MATRIX(Scalar)> &Ks,
                      const COLUMN(Scalar) & eta)
{
    return std::inner_product(
    Ks.begin(), Ks.end(),
    eta.data(), // Eigen has no .begin() and .end()
    MATRIX(Scalar)::Zero((*Ks.begin()).rows(), (*Ks.begin()).cols()).eval());
}

template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_weighted_kernels(const MATRIX(Scalar) & lhs,
                           const MATRIX(Scalar) & rhs,
                           const std::vector<Function> &fs,
                           const COLUMN(Scalar) & eta)
{
    MATRIX(Scalar) acc; // not writing 0s
    if (fs.begin() != fs.end())
    {
        Index i = 0;
        acc = fs[i](lhs, rhs) * eta(i);
        ++i;
        for (; i < fs.size(); ++i)
        {
            acc += fs[i](lhs, rhs) * eta(i);
        }
    } // TODO else { throw | warn | ....}
    return acc;
}

template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_weighted_trace_normalized_kernels(const MATRIX(Scalar) & lhs,
                                            const MATRIX(Scalar) & rhs,
                                            const std::vector<Function> &Ks,
                                            std::vector<Scalar> &trace_factors,
                                            bool update_trace_factors, // update_trace_factors
                                                                       // or
                                            // read trace_factors
                                            const COLUMN(Scalar) & eta)
{
    if (Ks.size() != trace_factors.size())
        throw std::length_error(
        " number of functions does not match number of trace_factors");
    MATRIX(Scalar) acc; // not writing 0s
    if (update_trace_factors)
    {
        MATRIX(Scalar) K; // temporary holding the trace_normalized kernel
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            K = Ks[i](lhs, rhs); // non const copy
            trace_factors[i] = trace_normalize(K);
            acc = K * eta(i);
            ++i;
            for (; i < Ks.size(); ++i)
            {
                K = Ks[i](lhs, rhs);
                trace_factors[i] = trace_normalize(K);
                acc += K * eta(i);
            }
        }
    }
    else
    {
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            acc = Ks[i](lhs, rhs) / trace_factors[i] * eta(i);
            ++i;
            for (; i < Ks.size(); ++i)
            {
                acc += Ks[i](lhs, rhs) / trace_factors[i] * eta(i);
            }
        }
    } // TODO else { throw | warn | ....}
    return acc;
}

//! fill a sparse diagonal matrix with a given value
/*! only if elements don't exist already, else use coeffRef() instead of
insert()
\param I a sparse matrix of type Eigen::SparseMatrixBase<Derived>.
\param value a value to fill the diagonal of type double.
\sa test/sparse_diagonal
*/
template <typename Derived>
void fill_sparse_diagonal(Eigen::SparseMatrixBase<Derived> const &I,
                          const double value)
{
    // size of diagonal
    const Index &m = I.rows();
    const Index &n = I.cols();
    Index d = (m < n ? m : n);
    Eigen::SparseMatrixBase<Derived> &I_ =
    const_cast<Eigen::SparseMatrixBase<Derived> &>(I);
    for (Index i = 0; i < d; ++i)
    {
        I_.derived().insert(i, i) = value;
    }
}

// helpers for indexing an Eigen::Matrix
// adapted from
// http://eigen.tuxfamily.org/dox-devel/TopicCustomizing_NullaryExpr.html#title1
template <class ArgumentType, class RowIndexType, class ColumnIndexType>
class IndexingFunctor
{
    private:
    const ArgumentType &argument_;
    const RowIndexType &row_indices_;
    const ColumnIndexType &column_indices_;

    public:
    typedef Eigen::Matrix<
    typename ArgumentType::Scalar,
    RowIndexType::SizeAtCompileTime,
    ColumnIndexType::SizeAtCompileTime,
    ArgumentType::Flags & Eigen::RowMajorBit ? Eigen::RowMajor : Eigen::ColMajor,
    RowIndexType::MaxSizeAtCompileTime,
    ColumnIndexType::MaxSizeAtCompileTime>
    MatrixType;
    IndexingFunctor(const ArgumentType &argument,
                    const RowIndexType &row_indices,
                    const ColumnIndexType &column_indices)
    : argument_(argument),
      row_indices_(row_indices),
      column_indices_(column_indices)
    {
    }
    const typename ArgumentType::Scalar &
    operator()(Eigen::Index row, Eigen::Index column) const
    {
        return argument_(row_indices_[row], column_indices_[column]);
    }
};

template <class ArgumentType, class RowIndexType, class ColumnIndexType>
Eigen::CwiseNullaryOp<
IndexingFunctor<ArgumentType, RowIndexType, ColumnIndexType>,
typename IndexingFunctor<ArgumentType, RowIndexType, ColumnIndexType>::MatrixType>
indexing(const Eigen::MatrixBase<ArgumentType> &argument,
         const RowIndexType &row_indices,
         const ColumnIndexType &column_indices)
{
    typedef IndexingFunctor<ArgumentType, RowIndexType, ColumnIndexType> Indexer;
    typedef typename Indexer::MatrixType MatrixType;
    return MatrixType::NullaryExpr(row_indices.size(), column_indices.size(),
                                   Indexer(argument.derived(), row_indices,
                                           column_indices));
};
} // namespace linear_algebra
} // namespace mimkl

#endif /*_LINEAR_ALGEBRA_HPP_*/


#include <iostream>
//
#include <polyvec/api.hpp>
#include <polyvec/core/log.hpp>
#include <polyvec/misc.hpp>
#include <polyvec/io/vtk_curve_writer.hpp>
#include <polyvec/curve-tracer/bezier_merging.hpp>
//
#include <Eigen/Core>
#include <Eigen/Geometry>

NAMESPACE_BEGIN()
NAMESPACE_END()

NAMESPACE_BEGIN(polyvectest)
NAMESPACE_BEGIN(BezierMerging)

int find_affine_transformation(int, char**) {

  // Creat the points and then test the mapping
  Eigen::Matrix<double, 2,3> points0;
  Eigen::Matrix<double, 2,3> points1;
  Eigen::Matrix2d A;
  Eigen::Vector2d b;
  Eigen::Matrix2d Amanu;
  Eigen::Vector2d bmanu;

  points0.col(0) << 2,10;
  points0.col(1) << 5,7;
  points0.col(2) << 4,12;

  //
  // Identity test
  //
  Amanu = Eigen::Matrix2d::Identity();
  bmanu = Eigen::Vector2d::Zero();
  points1 = (Amanu * points0).colwise() + bmanu;

  ::polyfit::BezierMerging::find_affine_transformation(points0, points1, A, b);
  assert_break( (A - Amanu).norm() < 1e-12 );
  assert_break( (b - bmanu).norm() < 1e-12 );

  //
  // Translation test
  //
  Amanu = Eigen::Matrix2d::Identity();
  bmanu = Eigen::Vector2d(2, -1.48);
  points1 = (Amanu * points0).colwise() + bmanu;

  ::polyfit::BezierMerging::find_affine_transformation(points0, points1, A, b);
  assert_break( (A - Amanu).norm() < 1e-12 );
  assert_break( (b - bmanu).norm() < 1e-12 );

  //
  // A test
  //
  Amanu << 5, 12, -1, 30;
  bmanu = Eigen::Vector2d::Zero();
  points1 = (Amanu * points0).colwise() + bmanu;

  ::polyfit::BezierMerging::find_affine_transformation(points0, points1, A, b);
  assert_break( (A - Amanu).norm() < 1e-12 );
  assert_break( (b - bmanu).norm() < 1e-12 );

  //
  // together
  //
  Amanu << 5, 12, -1, 30;
  bmanu << 9, -3;
  points1 = (Amanu * points0).colwise() + bmanu;

  ::polyfit::BezierMerging::find_affine_transformation(points0, points1, A, b);
  assert_break( (A - Amanu).norm() < 1e-12 );
  assert_break( (b - bmanu).norm() < 1e-12 );

  return EXIT_FAILURE;
}

NAMESPACE_END(BezierMerging)
NAMESPACE_END(polyvectest)


// Standard
#include <math.h>
#include <stdio.h>
#include <iostream>

// Timer
#include <chrono>

#include <ExternalSource/myOptimizer/Goldfarb/QuadProg++.hh>

#include <Eigen/Dense>
#include <eigen3/unsupported/Eigen/MatrixFunctions>
#include <vector>

// #define MPC_PRINT_ALL
// #define MPC_TIME_ALL

// We are following the MPC formulation from:
// Di Carlo, Jared, et al. "Dynamic locomotion in the mit cheetah 3 through
// convex model-predictive control." 2018 IEEE/RSJ International Conference on
// Intelligent Robots and Systems (IROS). IEEE, 2018.

inline Eigen::MatrixXd R_roll(const double& phi) {
    Eigen::MatrixXd Rx(3, 3);
    Rx.setZero();
    Rx << 1.0, 0.0, 0.0, 0.0, cos(phi), -sin(phi), 0.0, sin(phi), cos(phi);
    return Rx;
}

inline Eigen::MatrixXd R_pitch(const double& theta) {
    Eigen::MatrixXd Ry(3, 3);
    Ry.setZero();
    Ry << cos(theta), 0.0, sin(theta), 0.0, 1.0, 0.0, -sin(theta), 0.0,
        cos(theta);
    return Ry;
}

inline Eigen::MatrixXd R_yaw(const double& psi) {
    Eigen::MatrixXd Rz(3, 3);
    Rz.setZero();
    Rz << cos(psi), -sin(psi), 0.0, sin(psi), cos(psi), 0.0, 0.0, 0.0, 1.0;
    return Rz;
}

inline Eigen::MatrixXd skew_sym_mat(const Eigen::MatrixXd& r) {
    Eigen::MatrixXd cm(3, 3);
    cm << 0.f, -r(2), r(1), r(2), 0.f, -r(0), -r(1), r(0), 0.f;
    return cm;
}

// Transform input vector to nxm matrix. Vector must have dimension n*m.
void assemble_vec_to_matrix(const int& n, const int& m,
                            const Eigen::VectorXd& vec,
                            Eigen::MatrixXd& mat_out) {
    mat_out = Eigen::MatrixXd::Zero(n, m);
    for (int j = 0; j < m; j++) {
        mat_out.col(j) = vec.segment(j * n, n);
    }
    // std::cout << "mat_out:" << std::endl;
    // std::cout << mat_out << std::endl;
}

// Given dt, current x state, f_Mat, and r_feet. Find the next state, x1
void integrate_robot_dynamics(const double& dt,
                              const Eigen::VectorXd& x_current,
                              const Eigen::MatrixXd& f_Mat,
                              const Eigen::MatrixXd& r_feet, const double& mass,
                              const Eigen::MatrixXd& I_body,
                              Eigen::VectorXd& x_next) {
    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    double roll = x_current[0];
    double pitch = x_current[1];
    double yaw = x_current[2];
    // std::cout << "x_current:" << x_current.transpose() << std::endl;

    Eigen::VectorXd theta_prior = x_current.head(3);
    Eigen::VectorXd p_prior = x_current.segment(3, 3);
    Eigen::VectorXd omega_prior = x_current.segment(6, 3);
    Eigen::VectorXd pdot_prior = x_current.segment(9, 3);

    Eigen::MatrixXd Rx = R_roll(roll);
    Eigen::MatrixXd Ry = R_pitch(pitch);
    Eigen::MatrixXd Rz = R_yaw(yaw);

    Eigen::MatrixXd R_zyx = Rz * Ry * Rx;
    Eigen::MatrixXd I_world =
        R_zyx * I_body *
        R_zyx.transpose();  // Inertia matrix aligned to CoM frame.

    // CoM acceleration, velocity, and position
    Eigen::VectorXd pddot(3);  // pddot = 1/m sum(f_i) - g
    Eigen::VectorXd pdot(3);   // pdot = pddot*dt + pdot prior;
    Eigen::VectorXd p(3);      // p = 0.5*pddot*dt*dt + pdot_prior*dt + p_prior;

    Eigen::VectorXd grav_vec(3);
    grav_vec.setZero();
    grav_vec[2] = -9.81;
    pddot.setZero();
    pddot += grav_vec;
    for (int i = 0; i < f_Mat.cols(); i++) {
        pddot += ((1.0 / mass) * f_Mat.col(i));
    }
    pdot = pddot * dt + pdot_prior;                         // CoM velocity
    p = 0.5 * pddot * dt * dt + pdot_prior * dt + p_prior;  // CoM position

    // Angular Acceleration, velocity, and new orientation
    Eigen::VectorXd omega_dot(3);  // omega_dot = I_inv * (sum(skew(r_i)*f_i)
                                   // -skew(omega_prior)*I*omega_prior
    Eigen::VectorXd omega(3);      // omega = omega_dot*dt + omega_prior;
    Eigen::VectorXd theta;         // theta = Omega_Rate*(0.5*omega_dot*dt +
                                   // omega_prior)*dt + theta_prior

    // ZYX from ZYX rates integration
    Eigen::MatrixXd omega_to_zyx_rate(3, 3);
    omega_to_zyx_rate << cos(yaw) / cos(pitch), sin(yaw) / cos(pitch), 0,
        -sin(yaw), cos(yaw), 0, cos(yaw) * sin(pitch) / cos(pitch),
        sin(yaw) * sin(pitch) / cos(pitch), 1;

    Eigen::VectorXd moments(3);
    moments.setZero();
    for (int i = 0; i < f_Mat.cols(); i++) {
        moments += skew_sym_mat(r_feet.col(i)) * f_Mat.col(i);
    }
    omega_dot = I_world.inverse() *
                (moments - skew_sym_mat(omega_prior) * (I_world * omega_prior));
    omega = omega_dot * dt + omega_prior;
    theta = omega_to_zyx_rate * (0.5 * omega_dot * dt + omega_prior) * dt +
            theta_prior;

    // std::cout << "pddot:" << pddot.transpose() << std::endl;
    // std::cout << "pdot:" << pdot.transpose() << std::endl;
    // std::cout << "p_prior:" << p_prior.transpose() << std::endl;
    // std::cout << "p_post:" <<  p.transpose() << std::endl;

    // std::cout << std::endl;
    // std::cout << "omega_dot:" << omega_dot.transpose() << std::endl;
    // std::cout << "omega:" << omega.transpose() << std::endl;

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

    // std::cout << "theta_prior:" << theta_prior.transpose() << std::endl;
    // std::cout << "theta_post:" << theta.transpose() << std::endl;

    // Set new x_next;
    x_next.head(3) = theta;
    x_next.segment(3, 3) = p;
    x_next.segment(6, 3) = omega;
    x_next.segment(9, 3) = pdot;
    x_next[12] = -9.81;

    // std::cout << "x_next:" << x_next.transpose() << std::endl;
}

// Inputs: m, I, current yaw, and footstep locations.
// Output: A, B matrix to be used for zero-order hold MPC
void cont_time_state_space(const double& mass, const Eigen::MatrixXd& I_body,
                           const double& psi_in, const Eigen::MatrixXd& r_feet,
                           Eigen::MatrixXd& A, Eigen::MatrixXd& B) {
    // Get System State: Current Robot yaw
    Eigen::MatrixXd Rz = R_yaw(psi_in);

    Eigen::MatrixXd I_world =
        Rz * I_body * Rz.transpose();  // Inertia matrix aligned to CoM frame.
    Eigen::MatrixXd I_inv = I_world.inverse();  // Inverse

    // Number of reaction forces equal to number of feet contacts

    int n_Fr = r_feet.cols();

    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    // There is an additional gravity state.
    // Populate A matrix
    A = Eigen::MatrixXd::Zero(13, 13);
    A.block(0, 6, 3, 3) = Rz;
    A.block(3, 9, 3, 3) = Eigen::MatrixXd::Identity(3, 3);
    A(11, 12) = 1.0;  // Gravity vector

    // Populate B matrix
    B = Eigen::MatrixXd::Zero(13, 3 * n_Fr);
    Eigen::MatrixXd eye3 = Eigen::MatrixXd::Identity(3, 3);
    for (int i = 0; i < n_Fr; i++) {
        B.block(6, i * 3, 3, 3) = I_inv * skew_sym_mat(r_feet.col(i));
        B.block(9, i * 3, 3, 3) = eye3 * 1.0 / mass;
    }

#ifdef MPC_PRINT_ALL
    std::cout << "Rz:" << std::endl;
    std::cout << Rz << std::endl;

    std::cout << "I_body:" << std::endl;
    std::cout << I_body << std::endl;

    std::cout << "I_world:" << std::endl;
    std::cout << I_world << std::endl;

    std::cout << "I_inv:" << std::endl;
    std::cout << I_inv << std::endl;

    std::cout << "r_feet:" << std::endl;
    std::cout << r_feet << std::endl;

    std::cout << "A:" << std::endl;
    std::cout << A << std::endl;

    std::cout << "B:" << std::endl;
    std::cout << B << std::endl;

    std::cout << "A.exp()" << std::endl;
    std::cout << A.exp() << std::endl;
#endif
}

// Returns the discretized matrices for a given A and B matrix.
void discrete_time_state_space(const double& dt, const Eigen::MatrixXd& A,
                               const Eigen::MatrixXd& B, Eigen::MatrixXd& Adt,
                               Eigen::MatrixXd& Bdt) {
    // p.215 Raymond DeCarlo: Linear Systems: A State Variable Approach with
    // Numerical Implementation, Prentice Hall, NJ, 1989
    int n = A.cols() + B.cols();
    Eigen::MatrixXd AB(n, n);
    Eigen::MatrixXd expAB(n, n);
    AB.setZero();
    AB.block(0, 0, A.cols(), A.cols()) = A;
    AB.block(0, A.cols(), A.cols(), B.cols()) = B;

    expAB = (AB * dt).exp();

    Adt = expAB.block(0, 0, A.cols(), A.cols());
    Bdt = expAB.block(0, A.cols(), A.cols(), B.cols());

#ifdef MPC_PRINT_ALL
    std::cout << "Adt" << std::endl;
    std::cout << Adt << std::endl;

    std::cout << "Bdt" << std::endl;
    std::cout << Bdt * dt << std::endl;
#endif
}

// Zero order hold qp matrices
void qp_matrices(const int& horizon, const Eigen::MatrixXd& Adt,
                 const Eigen::MatrixXd& Bdt, Eigen::MatrixXd& Aqp,
                 Eigen::MatrixXd& Bqp) {
    if (horizon < 1) {
        std::cerr << "Error! MPC horizon must be at least 1 timestep long."
                  << std::endl;
    }

    int n = Adt.cols();
    int m = Bdt.cols();
    std::vector<Eigen::MatrixXd> powerMats;
    for (int i = 0; i < horizon + 1; i++) {
        powerMats.push_back(Eigen::MatrixXd::Identity(n, n));
    }

    // Construct power matrices foe Adt
    for (int i = 1; i < horizon + 1; i++) {
        powerMats[i] = powerMats[i - 1] * Adt;
    }

    // Construct Aqp and Bqp matrices
    Aqp.setZero();
    Bqp.setZero();

    for (int i = 0; i < horizon; i++) {
        Aqp.block(i * n, 0, n, n) = powerMats[i + 1];  // Adt.pow(i+1);
        for (int j = 0; j < horizon; j++) {
            if (i >= j) {
                int a_num = i - j;
                Bqp.block(i * n, j * m, n, m) =
                    powerMats[a_num] * Bdt;  // Adt.pow(a_num)*Bdt;
            }
        }
    }

    // Print power matrices for Adt
    // for(int i = 0; i < horizon; i++){
    // 	std::cout << "powerMats[" << i << "]" << std::endl;
    // 	std::cout << powerMats[i] << std::endl;
    // }

#ifdef MPC_PRINT_ALL
    std::cout << "Aqp" << std::endl;
    std::cout << Aqp << std::endl;

    std::cout << "Bqp" << std::endl;
    std::cout << Bqp << std::endl;
#endif
}

void get_desired_x(const int& horizon, Eigen::VectorXd& X_ref) {
    Eigen::VectorXd x_des(13);

    x_des.setZero();
    x_des[0] = 0.0;        // M_PI/8; //des roll orientation
    x_des[1] = 0.0;        //-M_PI/8; //des pitch orientation
    x_des[2] = M_PI / 12;  // Yaw orientation

    x_des[3] =
        -0.1;  //;0.75; // Set desired z height to be 0.75m from the ground
    x_des[5] =
        0.75;  //;0.75; // Set desired z height to be 0.75m from the ground

    X_ref = Eigen::VectorXd(horizon * 13);
    X_ref.setZero();
    for (int i = 0; i < horizon; i++) {
        X_ref.segment(13 * i, 13) = x_des;
        // X_ref[13*i + 5] = x_des[5]; // Set constant desired z-height
    }

#ifdef MPC_PRINT_ALL
    std::cout << "X_ref " << std::endl;
    std::cout << X_ref.transpose() << std::endl;
#endif
}

void get_force_constraints(const int& n_Fr, Eigen::MatrixXd& CMat,
                           Eigen::VectorXd& cvec) {
    // Unilateral constraints on the force vector
    // CMat * u + cvec >= 0
    double mu = 0.9;      // coefficient of friction
    double fz_max = 500;  // maximum z reaction force for one force vector.

    Eigen::MatrixXd cfmat(
        6, 3);  // the constraint matrix for a single force vector
    Eigen::VectorXd cfvec(
        6);  // the constraint vector for a single force vector

    cfmat.setZero();
    cfvec.setZero();

    // -mu*fz <= fx <= mu*fz
    cfmat(0, 0) = -1;
    cfmat(0, 2) = mu;
    cfmat(1, 0) = 1;
    cfmat(1, 2) = mu;

    // -mu*fz <= fy <= mu*fz
    cfmat(2, 1) = -1;
    cfmat(2, 2) = mu;
    cfmat(3, 1) = 1;
    cfmat(3, 2) = mu;

    // fz_min = 0.0 <= fz <= fz_max
    cfmat(4, 2) = -1.0;
    cfvec[4] = fz_max;  // -fz + fz_max >= 0
    cfmat(5, 2) = 1.0;

#ifdef MPC_PRINT_ALL

    std::cout << "cfmat " << std::endl;
    std::cout << cfmat << std::endl;

    std::cout << "cfvec " << std::endl;
    std::cout << cfvec << std::endl;
#endif

    // Populate constraint matrix
    CMat.setZero();
    cvec.setZero();
    for (int i = 0; i < n_Fr; i++) {
        CMat.block(6 * i, 3 * i, 6, 3) = cfmat;
        cvec.segment(6 * i, 6) = cfvec;
    }

#ifdef MPC_PRINT_ALL
    std::cout << "CMat " << std::endl;
    std::cout << CMat << std::endl;

    std::cout << "cvec " << std::endl;
    std::cout << cvec << std::endl;
#endif
}

void get_qp_constraints(const int& horizon, const Eigen::MatrixXd& CMat,
                        const Eigen::VectorXd& cvec, Eigen::MatrixXd& Cqp,
                        Eigen::VectorXd& cvec_qp) {
    Cqp.setZero();
    cvec_qp.setZero();

    int num_rows = CMat.rows();
    int num_cols = CMat.cols();

    // Set the force constraints over the horizon
    // To DO: add 0 upperbound when reaction force should be 0.0 depending on
    // the gate cycle.
    for (int i = 0; i < horizon; i++) {
        Cqp.block(i * num_rows, i * num_cols, num_rows, num_cols) = CMat;
        cvec_qp.segment(i * num_rows, num_rows) = cvec;
    }

#ifdef MPC_PRINT_ALL
    std::cout << "Cqp" << std::endl;
    std::cout << Cqp.rows() << "x" << Cqp.cols() << std::endl;
    std::cout << Cqp << std::endl;

    std::cout << "cvec_qp" << std::endl;
    std::cout << cvec_qp << std::endl;
#endif
}

void get_qp_costs(const int& n, const int& m, const int& horizon,
                  const Eigen::VectorXd& vecS_cost, const double& control_alpha,
                  Eigen::MatrixXd& Sqp, Eigen::MatrixXd& Kqp) {
    Eigen::MatrixXd S_cost = vecS_cost.asDiagonal();
    Sqp.setZero();
    for (int i = 0; i < horizon; i++) {
        Sqp.block(n * i, n * i, n, n) = S_cost;
    }

    Kqp = control_alpha * Eigen::MatrixXd::Identity(m * horizon, m * horizon);

#ifdef MPC_PRINT_ALL
    std::cout << "S_cost" << std::endl;
    std::cout << S_cost << std::endl;

    std::cout << "Sqp" << std::endl;
    std::cout << Sqp << std::endl;

    std::cout << "Kqp" << std::endl;
    std::cout << Kqp << std::endl;
#endif
}

void solve_mpc_qp(const Eigen::MatrixXd& Aqp, const Eigen::MatrixXd& Bqp,
                  const Eigen::VectorXd& X_ref, const Eigen::VectorXd& x0,
                  const Eigen::MatrixXd& Sqp, const Eigen::MatrixXd& Kqp,
                  const Eigen::MatrixXd& Cqp, const Eigen::VectorXd& cvec_qp,
                  Eigen::VectorXd& f_vec_out) {
#ifdef MPC_TIME_ALL
    std::chrono::high_resolution_clock::time_point t_qp_prep_start;
    std::chrono::high_resolution_clock::time_point t_qp_prep_end;
    double dur_qp_prep = 0.0;

    std::chrono::high_resolution_clock::time_point t_qp_mult_start;
    std::chrono::high_resolution_clock::time_point t_qp_mult_end;
    double dur_qp_mult = 0.0;

    t_qp_prep_start = std::chrono::high_resolution_clock::now();
    t_qp_mult_start = std::chrono::high_resolution_clock::now();
#endif

    Eigen::MatrixXd H(Bqp.cols(), Bqp.cols());
    Eigen::VectorXd g_qp(Bqp.cols());

    // For some reason g_qp evaluates slow unless we do these operations first
    // std::cout << "x0" << x0.transpose() << std::endl;
    Eigen::MatrixXd BqpT = Bqp.transpose();
    Eigen::MatrixXd m1;
    m1.noalias() = BqpT * Sqp;
    Eigen::MatrixXd m2;
    m2.noalias() = 2.0 * m1;
    Eigen::VectorXd v1 = (Aqp * x0).eval() - X_ref;

    H = 2.0 * (m1 * Bqp + Kqp);
    g_qp.noalias() = m1 * v1;

#ifdef MPC_TIME_ALL
    t_qp_mult_end = std::chrono::high_resolution_clock::now();
#endif

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

    // std::cout << "g" << std::endl;
    // std::cout << g.transpose() << std::endl;

    // Quadprog matrices vectors
    GolDIdnani::GMatr<double> G, CE, CI;
    GolDIdnani::GVect<double> g0, ce0, ci0, x;
    int n_quadprog_ = 1;
    int p_quadprog_ = 0;
    int m_quadprog_ = 0;

    // Quadprog size setup
    n_quadprog_ = Bqp.cols();      // number of decision variables
    m_quadprog_ = 0;               // number of equality constraints
    p_quadprog_ = cvec_qp.size();  // number of inequality constraints
                                   // (unilateral force constraints).

    // std::cout << "n_quadprog_" << n_quadprog_ << std::endl;
    // std::cout << "p_quadprog_" << p_quadprog_ << std::endl;

    f_vec_out.resize(n_quadprog_);

    x.resize(n_quadprog_);  // Decision Variables
    // Objective
    G.resize(n_quadprog_, n_quadprog_);
    g0.resize(n_quadprog_);
    // Equality Constraints
    CE.resize(n_quadprog_, m_quadprog_);
    ce0.resize(m_quadprog_);
    // Inequality Constraints
    CI.resize(n_quadprog_, p_quadprog_);
    ci0.resize(p_quadprog_);

    // Solve the following QP:
    // min (1/2)*U^T*H*U + U^T*g
    // st. C*U <= cvec_qp
    // where U is the decision variable (vector of forces)

    // Populate G Cost Matrix
    // G = P
    for (int i = 0; i < n_quadprog_; i++) {
        for (int j = 0; j < n_quadprog_; j++) {
            G[i][j] = H(i, j);
        }
    }

    // Populate g0 cost vector
    for (int i = 0; i < n_quadprog_; i++) {
        g0[i] = g_qp[i];
    }

    // No equality constraints. So CE and ce0 are untouched.

    // Populate Inequality Constraint Matrix
    for (int i = 0; i < p_quadprog_; i++) {
        for (int j = 0; j < n_quadprog_; j++) {
            CI[j][i] = Cqp(i, j);
        }
    }

    // Populate Inequality Constraint Vector
    // ci0 = h_
    for (int i = 0; i < p_quadprog_; i++) {
        ci0[i] = cvec_qp[i];
    }

#ifdef MPC_TIME_ALL

    t_qp_prep_end = std::chrono::high_resolution_clock::now();
    dur_qp_mult = std::chrono::duration_cast<std::chrono::duration<double> >(
                      t_qp_mult_end - t_qp_mult_start)
                      .count();
    dur_qp_prep = std::chrono::duration_cast<std::chrono::duration<double> >(
                      t_qp_prep_end - t_qp_prep_start)
                      .count();

    std::cout << "QP mult took " << dur_qp_mult << " seconds." << std::endl;
    std::cout << "QP mult rate " << (1.0 / dur_qp_mult) << " Hz" << std::endl;
    std::cout << "QP prep took " << dur_qp_prep << " seconds." << std::endl;
    std::cout << "QP prep rate " << (1.0 / dur_qp_prep) << " Hz" << std::endl;

    // Time the QP solve
    std::chrono::high_resolution_clock::time_point t_qp_solve_start;
    std::chrono::high_resolution_clock::time_point t_qp_solve_end;
    double dur_qp_solve = 0.0;

    // Start
    t_qp_solve_start = std::chrono::high_resolution_clock::now();
#endif

    double qp_result = solve_quadprog(G, g0, CE, ce0, CI, ci0, x);

#ifdef MPC_TIME_ALL
    // End
    t_qp_solve_end = std::chrono::high_resolution_clock::now();
    dur_qp_solve = std::chrono::duration_cast<std::chrono::duration<double> >(
                       t_qp_solve_end - t_qp_solve_start)
                       .count();
#endif

    // Populate qd result from QP answer
    for (int i = 0; i < n_quadprog_; i++) {
        f_vec_out[i] = x[i];
    }
    // std::cout << "qp_result = " << qp_result << std::endl;
    // std::cout << "f_vec_out = " << f_vec_out.transpose() << std::endl;

#ifdef MPC_TIME_ALL
    std::cout << "QP took " << dur_qp_solve << " seconds." << std::endl;
    std::cout << "QP rate " << (1.0 / dur_qp_solve) << " Hz" << std::endl;
#endif
}

void print_f_vec(int& horizon, int& n_Fr, const Eigen::VectorXd& f_vec_out) {
    for (int i = 0; i < horizon; i++) {
        std::cout << "horizon:" << i + 1 << std::endl;

        for (int j = 0; j < n_Fr; j++) {
            std::cout << "  f" << j << ":"
                      << f_vec_out.segment(3 * i * n_Fr + 3 * j, 3).transpose()
                      << std::endl;
        }
    }
}

void solve_mpc(const Eigen::VectorXd& x0, const Eigen::VectorXd& X_des,
               const Eigen::MatrixXd& r_feet, const double& robot_mass,
               const Eigen::MatrixXd& I_body, const int& horizon,
               const double& mpc_dt, Eigen::VectorXd& f_vec_out) {
#ifdef MPC_TIME_ALL
    // Time the QP solve
    std::chrono::high_resolution_clock::time_point t_mat_setup_start;
    std::chrono::high_resolution_clock::time_point t_mat_setup_end;
    double dur_mat_setup = 0.0;
    // Start
    t_mat_setup_start = std::chrono::high_resolution_clock::now();
#endif

    int n_Fr = r_feet.cols();

    // create continuous time state space matrices
    double psi_yaw = x0[2];
    Eigen::MatrixXd A(13, 13);
    A.setZero();
    Eigen::MatrixXd B(13, 3 * n_Fr);
    B.setZero();
    int n = A.cols();
    int m = B.cols();
    cont_time_state_space(robot_mass, I_body, psi_yaw, r_feet, A, B);

    // create discrete time state space matrices
    Eigen::MatrixXd Adt(n, n);
    Adt.setZero();
    Eigen::MatrixXd Bdt(n, m);
    Bdt.setZero();
    discrete_time_state_space(mpc_dt, A, B, Adt, Bdt);

    // create A and B QP matrices
    Eigen::MatrixXd Aqp(n * horizon, n);
    Aqp.setZero();
    Eigen::MatrixXd Bqp(n * horizon, m * horizon);
    Bqp.setZero();
    qp_matrices(horizon, Adt, Bdt, Aqp, Bqp);

    // create force constraint matrices and vectors
    Eigen::MatrixXd CMat(
        6 * n_Fr,
        3 * n_Fr);  // the QP constraint matrix for n_Fr reaction forces
    Eigen::VectorXd cvec(
        6 * n_Fr);  // the constraint matrix for n_Fr reaction forces
    get_force_constraints(n_Fr, CMat, cvec);

    Eigen::MatrixXd Cqp(horizon * 6 * n_Fr, horizon * 3 * n_Fr);
    Eigen::VectorXd cvec_qp(horizon * 6 * n_Fr);
    get_qp_constraints(horizon, CMat, cvec, Cqp, cvec_qp);

    // Cost matrices (Values similar to cheetah 3)
    Eigen::MatrixXd Sqp(n * horizon, n * horizon);
    Eigen::MatrixXd Kqp(m * horizon, m * horizon);

    Eigen::VectorXd vecS_cost(n);
    //        <<  th1,  th2,  th3,  px,  py,  pz,   w1,  w2,   w3,   dpx,  dpy,
    //        dpz,  g
    // vecS_cost << 0.25, 0.25, 10.0, 2.0, 2.0, 50.0, 0.05, 0.05, 0.30, 0.20,
    // 0.2, 0.10, 0.0;
    vecS_cost << 1.0, 1.0, 10.0, 2.0, 2.0, 50.0, 0.05, 0.05, 0.30, 0.20, 0.2,
        0.10, 0.0;
    Eigen::MatrixXd S_cost = vecS_cost.asDiagonal();
    double control_alpha = 4e-5;
    get_qp_costs(n, m, horizon, vecS_cost, control_alpha, Sqp, Kqp);

#ifdef MPC_TIME_ALL
    // End
    t_mat_setup_end = std::chrono::high_resolution_clock::now();
    dur_mat_setup = std::chrono::duration_cast<std::chrono::duration<double> >(
                        t_mat_setup_end - t_mat_setup_start)
                        .count();

    std::cout << "QP matrices setup took " << dur_mat_setup << " seconds."
              << std::endl;
    std::cout << "QP matrices setup rate " << (1.0 / dur_mat_setup) << " Hz"
              << std::endl;

    std::cout << "robot state x_cur:" << x0.transpose() << std::endl;
    std::cout << "            x_des:" << X_des.head(n).transpose() << std::endl;
    std::cout << "horizon steps = " << horizon
              << ", horizon_time = " << horizon * mpc_dt << std::endl;
#endif

    // Solve MPC
    solve_mpc_qp(Aqp, Bqp, X_des, x0, Sqp, Kqp, Cqp, cvec_qp, f_vec_out);
}

void simulate_mpc() {
    // System Params
    double robot_mass = 10;  // kg mass of the robot
    Eigen::MatrixXd I_body =
        Eigen::MatrixXd::Identity(3, 3);  // Body Inertia matrix
    I_body(0, 0) = 5;
    I_body(0, 1) = 2;
    I_body(1, 0) = 2;
    I_body(1, 1) = 5;
    I_body(2, 2) = 2.5;

    // starting robot state
    // Current reduced state of the robot
    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    Eigen::VectorXd x0(13);
    x0.setZero();
    x0[2] = 0.0;   // M_PI/24.0;; // Yaw orientation
    x0[3] = 0.0;   // x translation error
    x0[4] = 0.0;   // y translation error
    x0[5] = 0.75;  // Height
    x0[12] = -9.81;

    // Feet Configuration
    // Set Foot contact locations
    Eigen::MatrixXd r_feet(3, 4);  // Each column is a reaction force in x,y,z
    r_feet.setZero();
    double foot_length = 0.05;   // 5cm distance between toe and heel
    double nominal_width = 0.1;  // 10cm distance between left and right feet

    // Flat ground contact, z = 0.0
    // 2 Contact Configuration
    // // Right Foot
    // r_feet(0, 0) = 0.0; // x
    // r_feet(1, 0) = -nominal_width/2.0; //y
    // // Left Foot
    // r_feet(0, 1) = 0.0;  // x
    // r_feet(1, 1) = nominal_width/2.0; //y

    // 4 Contact Configuration
    // Right Foot Front
    r_feet(0, 0) = foot_length / 2.0;     // x
    r_feet(1, 0) = -nominal_width / 2.0;  // y
    // Right Foot Back
    r_feet(0, 1) = -foot_length / 2.0;    // x
    r_feet(1, 1) = -nominal_width / 2.0;  // y
    // Left Foot Front
    r_feet(0, 2) = foot_length / 2.0;    // x
    r_feet(1, 2) = nominal_width / 2.0;  // y
    // Left Foot Back
    r_feet(0, 3) = -foot_length / 2.0;   // x
    r_feet(1, 3) = nominal_width / 2.0;  // y

    int horizon = 10;
    double mpc_dt = 0.025;

    int n_Fr = r_feet.cols();  // Number of contacts
    int n = 13;
    int m = 3 * n_Fr;

    Eigen::VectorXd f_vec_out(m * horizon);
    Eigen::MatrixXd f_Mat(3, n_Fr);
    f_Mat.setZero();

    // Get desired reference
    Eigen::VectorXd X_des(n * horizon);
    get_desired_x(horizon, X_des);

    // Solve the MPC
    solve_mpc(x0, X_des, r_feet, robot_mass, I_body, horizon, mpc_dt,
              f_vec_out);
    print_f_vec(horizon, n_Fr, f_vec_out);
    // Populate force output from 1 horizon.
    assemble_vec_to_matrix(3, n_Fr, f_vec_out.head(3 * n_Fr), f_Mat);

    // Simulate MPC for one time step
    double sim_dt = 1e-3;
    Eigen::VectorXd x_prev(n);
    x_prev = x0;
    Eigen::VectorXd x_next(n);
    x_next.setZero();
    integrate_robot_dynamics(sim_dt, x_prev, f_Mat, r_feet, robot_mass, I_body,
                             x_next);

    // Simulate MPC for x seconds
    double total_sim_time = 7.0;
    int sim_steps = static_cast<int>(total_sim_time / sim_dt);
    std::cout << "sim_steps:" << sim_steps << std::endl;

    double last_control_time = 0.0;
    double cur_time = 0.0;

    std::cout << "x_start:" << x0.transpose() << std::endl;
    Eigen::VectorXd f_cmd(12);
    f_cmd.setZero();

    bool print_for_plots = false;

    if (print_for_plots) {
        printf(
            "t, r, p, y, x, y, z, wx, wy, wz, dx, dy, dz, f0x, f0y, f0z, f1x, "
            "f1y, f1z, f2x, f2y, f2z, f3x, f3y, f3z\n");
    }
    for (int i = 0; i < sim_steps; i++) {
        // Solve new MPC every mpc control tick
        if ((cur_time - last_control_time) > mpc_dt) {
            solve_mpc(x_prev, X_des, r_feet, robot_mass, I_body, horizon,
                      mpc_dt, f_vec_out);
            assemble_vec_to_matrix(3, n_Fr, f_vec_out.head(3 * n_Fr), f_Mat);
            last_control_time = cur_time;
            f_cmd.head(m) = f_vec_out.head(m);

            if (!print_for_plots) {
                std::cout << "  Computed MPC Forces" << std::endl;
                printf("  f0:(%0.3f,%0.3f,%0.3f)\n", f_cmd[0], f_cmd[1],
                       f_cmd[2]);
                printf("  f1:(%0.3f,%0.3f,%0.3f)\n", f_cmd[3], f_cmd[4],
                       f_cmd[5]);
                if (n_Fr > 2) {
                    printf("  f2:(%0.3f,%0.3f,%0.3f)\n", f_cmd[6], f_cmd[7],
                           f_cmd[8]);
                    printf("  f3:(%0.3f,%0.3f,%0.3f)\n", f_cmd[9], f_cmd[10],
                           f_cmd[11]);
                }
            }
        }

        // Integrate the robot dynamics
        integrate_robot_dynamics(sim_dt, x_prev, f_Mat, r_feet, robot_mass,
                                 I_body, x_next);
        // Update x
        x_prev = x_next;
        // Increment time
        cur_time += sim_dt;

        if (print_for_plots) {
            printf(
                "%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f, "
                "%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f,%0.3f,%0.3f, "
                "%0.3f,%0.3f,%0.3f, %0.3f,%0.3f,%0.3f, %0.3f,%0.3f,%0.3f \n",
                cur_time, x_next[0], x_next[1], x_next[2], x_next[3], x_next[4],
                x_next[5], x_next[6], x_next[7], x_next[8], x_next[9],
                x_next[10], x_next[11], f_cmd[0], f_cmd[1], f_cmd[2], f_cmd[3],
                f_cmd[4], f_cmd[5], f_cmd[6], f_cmd[7], f_cmd[8], f_cmd[9],
                f_cmd[10], f_cmd[11]);
        } else {
            printf(
                "t:%0.3f, (R,P,Y):(%0.3f, %0.3f, %0.3f), (x,y,z):(%0.3f, "
                "%0.3f, %0.3f), (wx, wy, wz):(%0.3f, %0.3f, %0.3f), (dx, dy, "
                "dz):(%0.3f, %0.3f, %0.3f) \n",
                cur_time, x_next[0], x_next[1], x_next[2], x_next[3], x_next[4],
                x_next[5], x_next[6], x_next[7], x_next[8], x_next[9],
                x_next[10], x_next[11]);
        }

        // std::cout << "    f:" << f_vec_out.head(m).transpose() << std::endl;
        // for(int j = 0; j < n_Fr; j++){
        //   std::cout << "    f" << j << ":" << f_vec_out.segment(3*j,
        //   3).transpose() << std::endl;
        // }
        // std::cout << "cur_time:" << cur_time << "  last_control_time:" <<
        // last_control_time << std::endl;
    }
    std::cout << "x_des" << X_des.tail(n).transpose() << std::endl;
}

int main(int argc, char** argv) {
    simulate_mpc();
    return 0;
}


#ifndef HH_PRE_HPP_INCLUDED
#define HH_PRE_HPP_INCLUDED

#include <vector>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>

//UNITS: sec, V, A, M

//DECLARATIONS FOR HH_PRE_MODEL

class HH_PRE
{
   private:
      const double pi = boost::math::constants::pi<double>();
      double Cm = 1.0;   // F/m^2 Specific capacitance
      //----Reversal potentials for Na, K, channels in mV
      double ENa = 50.0;   // Reversal potential for Sodium currents in Volts
      double EK = -85.0;    // Reversal potential for Potassium currents in Volts
      double ELeak = -81; // Reversal potential for Leak currents in Volts

      double GNa = 100;   // Maximum conductance for Sodium channels S/m^2
      double GK = 36;    // Maximum conductance for Potassium channels S/m^2
      double GLeak = 0.5;  // Maximum conductance for Leak channels S/m^2

   public:
      double IExt = 0.0; //extern current in Amps
      double Area = 0.0;
      double Cap = 0.0;
      double V = 0.0;      // Membrane voltage V
      /* NEURON class explicit constructor */
      HH_PRE( double Area_ = 500E-08): Area(Area_)
      {
         Cap = Cm * Area;
      };
      //---Equations for bouton Na Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
      double alpha_Na_act(const double V);
      double beta_Na_act(const double V);
      double alpha_Na_inact(const double V);
      double beta_Na_inact(const double V);
      //---Equations for bouton K Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
      double alpha_K_act(const double V);
      double beta_K_act(const double V);
      //---Declaration of the ODE solver
      template <class State, class Deriv >
      void operator() ( const State &x, Deriv &dxdt , const double  t );
};
//Function declarations
//------------- Na+ channels --- implimented from classical HH (1952)
double HH_PRE::alpha_Na_act(const double V)
{
   double a = 0.1;
   double b = 45;
   double c = 10;
   double val = (- a * ( (V + b) / (exp( -(V + b) / c ) - 1 ) ) );

   double A = 93.82;
   double B = -105;
   double C = 17;
   double VAL = (- A * ( (V + B) / (exp( -(V + B) / C ) - 1 ) ) );
   return (VAL);
}
double HH_PRE::beta_Na_act(const double V)
{
   double a = 4;
   double b = 70;
   double c = 18;
   double val = ( a * ( exp( - (V + b) / c )) );

   double A = 0.168;
   double C = 23.27;
   double VAL = ( A *  exp( - V / C ) );

   return (VAL);
}
double HH_PRE::alpha_Na_inact(const double V)
{
   double val = 0.07 * exp( -( V + 70) /20 );

   double A = 0.0001;
   double C = 18.706;
   double VAL = ( A *  exp( - (V + 0) / C ) );

   return (VAL);
}
double HH_PRE::beta_Na_inact(const double V)
{
   double val = ( 1 / ( exp( - (V + 40) / 10) + 1) );

   double A = 6.6;
   double B = 17.6;
   double C = 13.3;
   double VAL = ( A / ( (exp( -(V + B) / C ) + 1 ) ) );

   return (VAL);
}
//--------K+ channel alpha & beta
double HH_PRE::alpha_K_act(const double V)
{
   double val = -0.01 * (V + 60) / ( exp( -(V +  60) / 10) - 1 );
   double VAL = -0.01 * (V + 55) / ( exp( -(V +  55) / 10) - 1 );
   return (VAL);
};
double HH_PRE::beta_K_act(const double V)
{
   double val = 0.125 * exp (- (V + 70) / 80);
   double VAL = 0.125 * exp (- (V + 65) / 80);
   return (VAL);
};
//-------HH_PRE class ODE Function

template <class State, class Deriv >
void HH_PRE::operator() ( const State &x, Deriv &dxdt , const double t )
{
   //typename boost::range_iterator< const State >::type x_it = boost::begin( x_ );
   //typename boost::range_iterator< Deriv >::type dxdt_it = boost::begin( dxdt_ );

   dxdt[0] = ( alpha_Na_act(x[3]) * (1 - x[0]) ) - (beta_Na_act(x[3]) * x[0]); // Na activation
   dxdt[1] = ( alpha_Na_inact(x[3]) * (1 - x[1]) ) - (beta_Na_inact(x[3]) * x[1]); // Na inactivation
   dxdt[2] = ( alpha_K_act(x[3]) * (1 - x[2]) ) - (beta_K_act(x[3]) * x[2]); // K activation
   double INa =   -(Area * GNa * pow(x[0],3) * x[1] * (x[3] - ENa) );
   double IK =   -(Area * GK * pow(x[2],4) * (x[3] - EK) );
   double ILeak = -(Area * GLeak *  (x[3] - ELeak) );
   dxdt[3] = (1/Cap) * (INa + IK + ILeak  + (IExt * Area)  );
};

#endif // HH_PRE_HPP_INCLUDED


#include "icp.h"

#include <iostream>

#include <Eigen/Dense>

using FloatType = double;
using IndexType = int64_t;
using EigenMatrix = Eigen::MatrixXd;
using EigenVector = Eigen::VectorXd;

#include "common/io.h"
#include "common/kdtree.h"
#include "common/debug.h"
#include "svd.h"

// Custom struct for KD tree
struct Point : public Vec3 {
    Point() {}
    Point(const Vec3 &v, int index = -1) : Vec3(v), i(index) {}
    int i;
};

// Rodrigues rotation matrix
EigenMatrix rodrigues(const EigenVector &w, const double theta) {
    const double c = std::cos(theta);
    const double s = std::sin(theta);

    const double wx = w(0);
    const double wy = w(1);
    const double wz = w(2);
    EigenMatrix K(3, 3);
    K << 0.0, -wz, wy, wz, 0.0, -wx, -wy, wx, 0.0;

    EigenMatrix I = EigenMatrix::Identity(3, 3);
    return I + K * s + (K * K) * (1.0 - c);
}

//! single point to point ICP step
void point2pointICP_step(const std::vector<Vec3> &target, const std::vector<Vec3> &source, EigenMatrix *rotMat,
                         EigenVector *trans) {
    // Construct Kd-tree to find closest points
    KDTree<Vec3> tree;
    tree.construct(target);

    // {{ NOT_IMPL_ERROR();

    // Match closest point pairs
    // Hint:
    // Construct here that two matrices X and P,
    // whose elements are positions of source vertices (X),
    // and those for closest points of the sources (P)
    const int nPoints = (int)source.size();
    EigenMatrix X(nPoints, 3);
    EigenMatrix P(nPoints, 3);
    for (int i = 0; i < nPoints; i++) {
        const Vec3 &v = source[i];
        const Vec3 u = tree.nearest(v);
        X.row(i) << v.x, v.y, v.z;
        P.row(i) << u.x, u.y, u.z;
    }

    // Subtract xMean, pMean from X, P
    // Hint:
    // Row-wise or column-wise mean (also sum, max, min etc.) of Eigen matrices
    // can be easily calculate using "rowwise()" or "colwise()".
    const EigenVector xMean = X.colwise().mean();
    const EigenVector pMean = P.colwise().mean();
    X.rowwise() -= xMean.transpose();
    P.rowwise() -= pMean.transpose();

    // Solve with SVD to obtain R
    // Hint:
    // An SVD method "eigenSVD" is already given in "svd.h".
    // See its arguments carefully when using it.
    const EigenMatrix M = P.transpose() * X;
    EigenMatrix U, V;
    EigenVector sigma;
    eigenSVD(M, U, sigma, V);
    const double detVU = (U * V.transpose()).determinant();

    // Revise sign of determinant
    EigenVector diagH(3);
    diagH << 1.0, 1.0, detVU;

    // Output
    // Hint:
    // Be careful that arguments for output matrices and vectors
    // are represented as "pointers". Therefore, you should insert
    // values for them with "asterisk" like "*lhs = rhs".
    *rotMat = U * diagH.asDiagonal() * V.transpose();
    *trans = pMean - (*rotMat) * xMean;

    // }}
}

//! single point to plane ICP step
void point2planeICP_step(const std::vector<Vec3> &target, const std::vector<Vec3> &targetNorm,
                         const std::vector<Vec3> &source, Eigen::MatrixXd *rotMat, Eigen::VectorXd *trans) {
    // Construct Kd-tree to find closest points
    std::vector<Point> points;
    for (int i = 0; i < (int)target.size(); i++) {
        points.emplace_back(target[i], i);
    }

    KDTree<Point> tree;
    tree.construct(points);

    // {{ NOT_IMPL_ERROR();

    // Construct linear system
    // Hint:
    // prepare matrix A and vector b
    // A is 6x6 matrix, and b is 6-D vector
    const int nPoints = (int)source.size();
    EigenMatrix A(6, 6);
    EigenVector b(6);
    A.setZero();
    b.setZero();
    for (int i = 0; i < nPoints; i++) {
        const Vec3 &x = source[i];
        const Point pt = tree.nearest(x);
        const Vec3 &p = Vec3(pt.x, pt.y, pt.z);
        const Vec3 &n = targetNorm[pt.i];
        const Vec3 x_cross_n = cross(x, n);
        EigenVector v(6);
        v << x_cross_n.x, x_cross_n.y, x_cross_n.z, n.x, n.y, n.z;
        A += v * v.transpose();
        b += v * (dot(n, p - x));
    }

    // Solve
    // Hint:
    // Use "Eigen::PartialPivLU" to solve the system
    Eigen::PartialPivLU<EigenMatrix> lu(A);
    EigenVector u = lu.solve(b);

    // Substitute to instances a, t
    // Hint:
    // Eigen matrices and vectors can be initialized
    // with its elements by "<<" operator.
    EigenVector a(3);
    a << u(0), u(1), u(2);
    EigenVector t(3);
    t << u(3), u(4), u(5);

    // Reproduce rotation matrix
    // Hint:
    // Remember that vector "a" is the multiple of
    // the direction of rotation axis and
    // the degree of rotation angle.
    const double theta = a.norm();
    const EigenVector w = a / theta;
    *rotMat = rodrigues(w, theta);
    *trans = t;

    // }}
}

void rigidICP(const std::vector<Vec3> &target, const std::vector<Vec3> &targetNorm, std::vector<Vec3> &source,
              std::vector<Vec3> &sourceNorm, ICPMetric metric, int maxIters, double tolerance, bool verbose) {
    // Point-to-point ICP
    for (int it = 0; it < maxIters; it++) {
        Eigen::MatrixXd R(3, 3);
        Eigen::VectorXd t(3);
        switch (metric) {
        case ICPMetric::Point2Point:
            point2pointICP_step(target, source, &R, &t);
            break;

        case ICPMetric::Point2Plane:
            point2planeICP_step(target, targetNorm, source, &R, &t);
            break;

        default:
            throw std::runtime_error("Unknown ICP metric type!");
        }

        // Apply rigid transformation
        for (int i = 0; i < (int)source.size(); i++) {
            Eigen::VectorXd v(3);
            v << source[i].x, source[i].y, source[i].z;
            Eigen::VectorXd u = R * v + t;
            source[i] = Vec3(u(0), u(1), u(2));

            Eigen::VectorXd n(3);
            n << sourceNorm[i].x, sourceNorm[i].y, sourceNorm[i].z;
            Eigen::VectorXd m = R * n;
            sourceNorm[i] = Vec3(m(0), m(1), m(2));
        }

        // Check current error
        const double error = t.norm() + (R - Eigen::MatrixXd::Identity(3, 3)).norm();

        // Report
        if (verbose) {
            printf("*** %d iteration ***\n", it + 1);
            printf("R = \n");
            std::cout << R << std::endl;
            printf("t = \n");
            std::cout << t << std::endl;
            printf("error = %f\n", error);
            printf("\n");
        }

        char outfile[256];
        sprintf(outfile, "intermediate_%03d.off", it);
        write_off(std::string(outfile), source, sourceNorm);

        if (error < tolerance) {
            break;
        }
    }
}


#include <doctest/doctest.h>

#include <non_lin_optim/solvelin.h>
#include <non_lin_optim/version.h>

#include <iostream>
#include <Eigen/Dense>

bool print = false;

template<typename T, typename U>
void f(const Eigen::MatrixBase<T>& A,
       const Eigen::MatrixBase<U>& b,
       const Eigen::MatrixBase<U>& x,
       bool positive){

    auto x_hat = solvelin::lu::fullPiv(A, b);
    CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
    if(print) std::cout << "x_hat lu::fullPiv \n" << x_hat << std::endl;

    x_hat = solvelin::qr::householderQr(A, b);
    CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
    if(print) std::cout << "x_hat qr::householderQr \n" << x_hat << std::endl;

    x_hat = solvelin::qr::colPivHouseholderQr(A, b);
    CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
    if(print) std::cout << "x_hat qr::colPivHouseholderQr \n" << x_hat << std::endl;

    x_hat = solvelin::qr::fullPivHouseholderQr(A, b);
    CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
    if(print) std::cout << "x_hat qr::fullPivHouseholderQr \n" << x_hat << std::endl;

    x_hat = solvelin::qr::completeOrthogonalDecomposition(A, b);
    CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
    if(print) std::cout << "x_hat qr::completeOrthogonalDecomposition \n" << x_hat << std::endl;

    if(positive){
        x_hat = solvelin::cholesky::llt(A, b);
        CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
        if(print) std::cout << "x_hat cholesky::llt \n" << x_hat << std::endl;

        x_hat = solvelin::cholesky::ldlt(A, b);
        CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
        if(print) std::cout << "x_hat cholesky::ldlt \n" << x_hat << std::endl;
    }else{
        CHECK_THROWS(solvelin::cholesky::llt(A, b));
        if(print) std::cout << "cholesky::llt cannot solve non-positive matrices!\n" << std::endl;

        CHECK_THROWS(solvelin::cholesky::ldlt(A, b));
        if(print) std::cout << "cholesky::ldlt cannot solve non-positive matrices!\n" << std::endl;
    }

    x_hat = solvelin::svd::bdc(A, b);
    CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
    if(print) std::cout << "x_hat svd::bdc \n" << x_hat << std::endl;

    x_hat = solvelin::svd::jacobi(A, b);
    CHECK((x_hat-x).cwiseAbs().maxCoeff() == doctest::Approx(0).epsilon(0.0001));
    if(print) std::cout << "x_hat svd::jacobi \n" << x_hat << std::endl;
}

TEST_CASE("Positive definite") {

    Eigen::MatrixXf A(3,3);
    Eigen::Vector3f b;
    Eigen::Vector3f x;
    A << 2,-1,0,  -1,2,-1,  0,-1,2;
    b << 3, 3, 4;
    x << 4.75, 6.5, 5.25;

    if(print) std::cout << "Positive definite" << std::endl;
    f(A, b, x, true);
}

TEST_CASE("Normal case") {

    Eigen::MatrixXf A(3,3);
    Eigen::Vector3f b;
    Eigen::Vector3f x;
    A << 1,2,3,  4,5,6,  7,8,10;
    b << 3, 3, 4;
    x << -2, 1, 1;

    if(print) std::cout << "Normal case" << std::endl;
    f(A, b, x, false);
}

TEST_CASE("Singular Matrix") {

    if(print) std::cout << "Singular Matrix" << std::endl;

    Eigen::MatrixXf A(3,3);
    Eigen::Vector3f b;
    A << 9,17,1, 12,18,9, 11,13,14;
    b << 18, 0, 18;

    auto x_hat = solvelin::lu::fullPiv(A, b);
    if(print) std::cout << "x_hat lu::fullPiv \n" << x_hat << std::endl;

    x_hat = solvelin::MoorePenrose(A, b);
    if(print) std::cout << "x_hat MoorePenrose \n" << x_hat << std::endl;

    x_hat = solvelin::qr::householderQr(A, b);
    if(print) std::cout << "x_hat qr::householderQr \n" << x_hat << std::endl;

    x_hat = solvelin::qr::colPivHouseholderQr(A, b);
    if(print) std::cout << "x_hat qr::colPivHouseholderQr \n" << x_hat << std::endl;

    x_hat = solvelin::qr::fullPivHouseholderQr(A, b);
    if(print) std::cout << "x_hat qr::fullPivHouseholderQr \n" << x_hat << std::endl;

    x_hat = solvelin::qr::completeOrthogonalDecomposition(A, b);
    if(print) std::cout << "x_hat qr::completeOrthogonalDecomposition \n" << x_hat << std::endl;
    /*
    x_hat = solvelin::cholesky::llt(A, b);
    if(print) std::cout << "x_hat cholesky::llt \n" << x_hat << std::endl;

    x_hat = solvelin::cholesky::ldlt(A, b);
    if(print) std::cout << "x_hat cholesky::ldlt \n" << x_hat << std::endl;
    */
    x_hat = solvelin::svd::bdc(A, b);
    if(print) std::cout << "x_hat svd::bdc \n" << x_hat << std::endl;

    x_hat = solvelin::svd::jacobi(A, b);
    if(print) std::cout << "x_hat svd::jacobi \n" << x_hat << std::endl;
}

TEST_CASE("Dynamic") {

    Eigen::MatrixXd A(3,3);
    Eigen::VectorXd b(3);
    Eigen::VectorXd x(3);
    A << 1,2,3,  4,5,6,  7,8,10;
    b << 3, 3, 4;
    x << -2, 1, 1;

    if(print) std::cout << "Dynamic matrix" << std::endl;
    f(A, b, x, false);
}

TEST_CASE("Version") {
    static_assert(std::string_view(NON_LIN_OPTIM_VERSION) == std::string_view("1.0"));
    CHECK(std::string(NON_LIN_OPTIM_VERSION) == std::string("1.0"));
}


/*
 utils.cxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <boost/range/numeric.hpp>
#include <cmath>

#include "utils.hxx"

double det3(ublas::matrix<double> a) {
  /* Fast Determinant of 3x3 Matrix */
  double det = a(0,0)*(a(1,1)*a(2,2) - a(1,2)*a(2,1))
             - a(0,1)*(a(1,0)*a(2,2) - a(1,2)*a(2,0))
             + a(0,2)*(a(1,0)*a(2,1) - a(1,1)*a(2,0));
  return det;
}

ublas::vector<double> cross3(ublas::vector<double> a,
                             ublas::vector<double> b) {

  /* Cross Product for Three-Dimensional Vectors */
  if (a.size() != 3 or b.size() != 3) {
    std::cout << "Exit(1): Vector dimensions not compatible for cross product\n";
    exit(1);
  }
  ublas::vector<double> c(3);
  c(0) = a(1)*b(2) - a(2)*b(1);
  c(1) = a(2)*b(0) - a(0)*b(2);
  c(2) = a(0)*b(1) - a(1)*b(0);
  return c;
}

ublas::matrix<double> inv3(ublas::matrix<double> a) {

  /* Matrix Inversion for Three-Dimensional Vectors */
  ublas::matrix<double> ia(a.size1(),a.size2());
  double det = det3(a);

  ia(0,0) = a(1,1)*a(2,2) - a(1,2)*a(2,1);
  ia(0,1) = a(0,2)*a(2,1) - a(0,1)*a(2,2);
  ia(0,2) = a(0,1)*a(1,2) - a(0,2)*a(1,1);

  ia(1,0) = a(1,2)*a(2,0) - a(1,0)*a(2,2);
  ia(1,1) = a(0,0)*a(2,2) - a(0,2)*a(2,0);
  ia(1,2) = a(0,2)*a(1,0) - a(0,0)*a(1,2);

  ia(2,0) = a(1,0)*a(2,1) - a(1,1)*a(2,0);
  ia(2,1) = a(0,1)*a(2,0) - a(0,0)*a(2,1);
  ia(2,2) = a(0,0)*a(1,1) - a(1,0)*a(0,1);

  return ia/det;
}

long linear_search(const std::vector<double>& vec,
		   double a,
		   double tol) {

  for (long i = 0; i < vec.size(); i++) {
    double value = fabs(vec[i] - a);
    if (value < tol) {
      return i;
    }
  }

  return vec.size();
}

//inline long product(ublas::vector<long> vec) {
//  return boost::accumulate(vec,1,std::multiplies<long>());
//}


#include "openbr/plugins/openbr_internal.h"
#include "openbr/core/opencvutils.h"
#include "openbr/core/eigenutils.h"

#include <opencv2/contrib/contrib.hpp>

#include <Eigen/Dense>

using namespace Eigen;
using namespace cv;

namespace br
{

class ShapeAxisRatioTransform : public UntrainableTransform
{
    Q_OBJECT

    void project(const Template &src, Template &dst) const
    {
        dst = src;

        Mat indices;
        findNonZero(src,indices);

        dst.m() = Mat(1,1,CV_32FC1);

        if (indices.total() > 0) {
            MatrixXd data(indices.total(),2);

            for (size_t i=0; i<indices.total(); i++) {
                data(i,0) = indices.at<Point>(i).y;
                data(i,1) = indices.at<Point>(i).x;
            }

            MatrixXd centered = data.rowwise() - data.colwise().mean();
            MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1);

            SelfAdjointEigenSolver<Eigen::MatrixXd> eSolver(cov);
            MatrixXd D = eSolver.eigenvalues();

            if (eSolver.info() == Success)
                dst.m().at<float>(0,0) = D(0)/D(1);
            else
                dst.file.fte = true;
        } else {
            dst.file.fte = true;
            qWarning("No mask content for %s.",qPrintable(src.file.baseName()));
        }
    }
};

BR_REGISTER(Transform, ShapeAxisRatioTransform)

} // namespace br

#include "imgproc/shapeaxisratio.moc"