#include <vmmlib/vector.hpp>
#include <vmmlib/math.hpp>

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

using namespace vmml;

BOOST_AUTO_TEST_CASE(vector_base)
{
    vector< 4, double > v;
    double data[] = { 1, 2, 3, 4 };

    v.iter_set( data, data+4 );

    // tests copyFrom1DimCArray function
    size_t tmp = 1;
    for( size_t index = 0; index < 4; ++index, ++tmp )
    {
        BOOST_CHECK(v.at( index ) == tmp);
    }

    tmp = 4;
    float dataf[] = { 4, 3, 2, 1 };
    v.iter_set( dataf, dataf + 4 );
    for( size_t index = 0; index < 4; ++index, --tmp )
    {
        BOOST_CHECK(v.at( index ) == tmp);
    }
}

BOOST_AUTO_TEST_CASE(vector_plus)
{
    vector< 4, double > v;
    double data[] = { 1, 2, 3, 4 };

    // tests operator+ function
    vector< 4, double > v_other;
    vector< 4, double > v_result;

    v = data;

    double datad[] = { 4, 3, 2, 1 };
    v_other = datad;

    v_result = v + v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == 5);
    }

    v_result = v;
    v_result += v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == 5);
    }

    v = data;
    v_result = v + 2.;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == index + 3);
    }

    v_result = v;
    v_result += 2;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == index + 3);
    }
}

BOOST_AUTO_TEST_CASE(vector_minus)
{
    vector< 4, double > v;
    double data[] = { 1, 2, 3, 4 };

    // tests operator- function
    vector< 4, double > v_other;
    vector< 4, double > v_result;
    v = data;

    double datad[] = { 1, 2, 3, 4 };
    v_other = datad;

    v_result = v - v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == 0);
    }

    v_result = v;
    v_result -= v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == 0);
    }

    v_result = v - 1.0;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == index);
    }

    v_result = v;
    v_result -= 1.0;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == index);
    }
}

BOOST_AUTO_TEST_CASE(vector_times)
{
    vector< 4, double > v;
    double data[] = { 1, 2, 3, 4 };

    // tests operator* function
    vector< 4, double > v_other;
    vector< 4, double > v_result;

    v = data;

    double datad[] = { 24, 12, 8, 6 };
    v_other = datad;

    v_result = v * v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == 24);
    }

    v_result = v;
    v_result *= v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == 24);
    }

    v_result = v * 2.0;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == v.at( index ) * 2.0);
    }

    v_result = v;
    v_result *= 2.0;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(v_result.at( index ) == v.at( index ) * 2.0);
    }
}

BOOST_AUTO_TEST_CASE(vector_div)
{
    vector< 4, double > v;
    double data[] = { 1, 2, 3, 4 };

    // tests operator/ function
    vector< 4, double > v_other;
    vector< 4, double > v_result;

    v = data;

    double datad[] = { 2, 4, 6, 8 };
    v_other = datad;

    v_result = v / v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(( v_result.at( index ) - 0.5 ) < 1e-12);
    }

    v_result = v;
    v_result /= v_other;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(( v_result.at( index ) - 0.5 ) < 1e-12);
    }

    v_result = v / 1.5;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(( v_result.at( index ) - ( v.at( index ) / 1.5 ) ) < 1e-12);
    }

    v_result = v;
    v_result /= 1.5;
    for( size_t index = 0; index < 4; ++index )
    {
        BOOST_CHECK(( v_result.at( index ) - ( v.at( index ) / 1.5 ) ) < 1e-12);
    }
}

BOOST_AUTO_TEST_CASE(vector_norm)
{
    vector< 4, double > v;
    double data[] = { 1, 2, 3, 4 };

    // tests norm / normSquared (length/lengthSquared) computation
    vector< 4, double > vec;
    vec = data;

    double normSquared = vec.squared_length();
    BOOST_CHECK(normSquared == 1 * 1 + 2 * 2 + 3 * 3 + 4 * 4);

    double norm = vec.length();
    BOOST_CHECK(sqrt( normSquared ) == norm);

    // tests normalize
    vec = data;
    vec.normalize();
    BOOST_CHECK_CLOSE( vec.length(), 1.0, 0.0000001 );

    // constructor tests
    double vData[] = { 1, 2, 3, 4 };
    vector< 4, double > v4( 1, 2, 3, 4 );

    vector< 2, double > v2C;
    v2C = vData;
    vector< 2, double > v2( 1, 2 );

    BOOST_CHECK(v2 == v2C );

    vector< 3, double > v3C;
    v3C = vData;
    vector< 3, double > v3( 1, 2, 3 );

    BOOST_CHECK(v3 == v3C );

    vector< 4, double > v4C;
    v4C = vData;

    BOOST_CHECK(v4 == v4C);

    double vData2[] = { 23, 23, 23, 23 };
    v4C = vData2;

    vector< 4, double > v4_( 23 );
    BOOST_CHECK(v4_ == v4C);

    v3 = vData;
    v4C = vData;
    vector< 4, double > v4from3_1( v3, vData[ 3 ] );
    BOOST_CHECK(v4from3_1 == v4C);

    double hvData[] = { 1., 2., 3., 0.25 };
    double xvData[] = { 4.0, 8.0, 12.0 };

    vector< 4, double > homogenous;
    homogenous.iter_set( hvData, hvData + 4 );
    vector< 3, double > nonh;
    nonh.iter_set( xvData, xvData + 3 );

    vector< 4, double > htest( nonh );

    // to-homogenous-coordinates ctor
    BOOST_CHECK((htest == vector< 4, double >( 4, 8., 12., 1. ) ));

    vector< 3, double > nhtest( homogenous );

    // from homogenous-coordiates ctor
    BOOST_CHECK(nhtest == nonh );

    // set tests
    vec.set( 2, 3, 4, 5 );
    vector< 4, double > vecCorrect;
    double vCData[] = { 2, 3, 4, 5 };
    vecCorrect = vCData;
    BOOST_CHECK(vec == vecCorrect);

    vec.set( 2 );

    double vCData2[] = { 2, 2, 2, 2 };
    vecCorrect = vCData2;
    BOOST_CHECK( vec == vecCorrect );

    vector< 3, double > v1( 2, 3, 4 );

    // component accessors
    vector< 4, double > vd( 1, 2, 3, 4 );
    BOOST_CHECK( vd.x() == 1 && vd.y() == 2 && vd.z() == 3 && vd.w() == 4 );
}

BOOST_AUTO_TEST_CASE(vector_dot)
{
    // dot product
    vector< 3, float > v0( 1, 2, 3 );
    vector< 3, float > v1( -6, 5, -4 );
    BOOST_CHECK( v0.dot( v1 ) == -8 );
}

BOOST_AUTO_TEST_CASE(vector_cross)
{
    // cross product
    vector< 3, float > v0( 1, 2, 3 );
    vector< 3, float > v1( -6, 5, -4 );
    vector< 3, float > vcorrect( -23, -14, 17 );
    BOOST_CHECK(v0.cross( v1 ) == vcorrect);

    // ???
    vector< 4, float > vf( -1.0f, 3.0f, -99.0f, -0.9f );
    vector< 4, size_t > vui( 0, 5, 2, 4 );

    size_t index = vf.find_min_index();
    float f = vf.find_min();

    BOOST_CHECK( index == 2 && f == -99.0f );

    index = vf.find_max_index();
    f = vf.find_max();
    BOOST_CHECK( index == 1 && f == 3.0f );

    index = vui.find_min_index();
    size_t ui = vui.find_min();
    BOOST_CHECK( index == 0 && ui == 0 );

    index = vui.find_max_index();
    ui = vui.find_max();
    BOOST_CHECK( index == 1 && ui == 5 );
}

BOOST_AUTO_TEST_CASE(vector_tbd1)
{
    vector< 4, float > v1( -1.0f, 3.0f, -99.0f, -0.9f );
    float f = 4.0f;
    vector< 4, float > v_scaled = f * v1;

    BOOST_CHECK(v_scaled == (vector< 4, float >( -4.0f, 12.0f, -396.0f, -3.6f ) ));

    // ???
    vector< 3, float > vf( 3.0, 2.0, 1.0 );
    vector< 3, double > vd( vf );
    vector< 3, double >::const_iterator it = vd.begin(), it_end = vd.end();
    vector< 3, float >::const_iterator fit = vf.begin();
    for( ; it != it_end; ++it, ++fit )
    {
        BOOST_CHECK(*it == *fit);
    }
    vd = vf;
    for( ; it != it_end; ++it, ++fit )
    {
        BOOST_CHECK(*it == *fit);
    }
}

BOOST_AUTO_TEST_CASE(vector_tbd2)
{
    // ???
    vector< 4, float > vf( 3.0, 2.0, 1.0, 1.0 );
    vector< 3, float >& v3 = vf.get_sub_vector< 3 >();
    BOOST_CHECK(v3.x() == vf.x() && v3.y() == vf.y());
    v3.normalize();

    BOOST_CHECK(v3.x() == vf.x() && v3.y() == vf.y());

    //elementwise sqrt
    vector< 4, float > vsq( 9.0, 4.0, 1.0, 2.0 );
    vector< 4, float > vsq_check( 3.0, 2.0, 1.0, 1.414213538169861 );
    vsq.sqrt_elementwise();
    BOOST_CHECK(vsq == vsq_check);

    //elementwise sqrt
    vector< 4, float > vr( 9.0, 4.0, 1.0, 2.0 );
    vector< 4, float > vr_check( 0.1111111119389534, 0.25, 1, 0.5 );
    vr.reciprocal();
    BOOST_CHECK(vr == vr_check);
}

BOOST_AUTO_TEST_CASE(vector_l2norm)
{
    vector< 4, float > vr( 9.0, 4.0, 1.0, 2.0 );
    double v_norm_check = 10.09950493836208;
    double v_norm = vr.norm();

    BOOST_CHECK((v_norm - v_norm_check) < 0.0001);
}


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <algorithm>
#include "class.hpp"
#include <unistd.h>
#include <limits.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/MatOp/SparseGenRealShiftSolve.h>
using namespace std;

PARAMETERS::PARAMETERS(string parameter_file, string gmsh_file)
{
    cout << "\n";
    check_mpi();
    meshfile = gmsh_file;
    cout << "PARAMETERS::PARAMETERS gmsh_file: " << meshfile << endl;
    does_file_exist(meshfile.c_str(), "Called from PARAMETERS::read_calc_parameters");
    read_calc_parameters(parameter_file);
    verify_parameters();
    get_problem_dimension_msh();
    calc_cloud_num_points();
    calc_polynomial_term_exponents();
    cout << "\n";
}

void PARAMETERS::read_calc_parameters(string parameter_file)
{
    does_file_exist(parameter_file.c_str(), "Called from PARAMETERS::read_calc_parameters");
    char ctemp[5000], ctemp_2[100];
    string stemp;
    int itemp;
    double dtemp;
    FILE *file;
    file = fopen(parameter_file.c_str(), "r");
    cout << endl;
    fscanf(file, "%[^,],%i\n", ctemp, &poly_deg);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, poly_deg);
    fscanf(file, "%[^,],%i\n", ctemp, &phs_deg);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, phs_deg);
    fscanf(file, "%[^,],%lf\n", ctemp, &cloud_size_multiplier);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, cloud_size_multiplier);

    fscanf(file, "%[^,],%i\n", ctemp, &nt);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, nt);
    fscanf(file, "%[^,],%lf\n", ctemp, &Courant);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, Courant);
    fscanf(file, "%[^,],%s\n", ctemp, ctemp_2);
    solver_type = ctemp_2;
    printf("PARAMETERS::read_calc_parameters Read %s = %s\n", ctemp, solver_type.c_str());
    fscanf(file, "%[^,],%lf\n", ctemp, &steady_tolerance);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, steady_tolerance);
    fscanf(file, "%[^,],%lf\n", ctemp, &solver_tolerance);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, solver_tolerance);
    fscanf(file, "%[^,],%i\n", ctemp, &euclid_precond_level_hypre);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, euclid_precond_level_hypre);
    fscanf(file, "%[^,],%i\n", ctemp, &gmres_kdim);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, gmres_kdim);
    fscanf(file, "%[^,],%lf\n", ctemp, &precond_droptol);
    printf("PARAMETERS::read_calc_parameters Read %s = %g\n", ctemp, precond_droptol);
    fscanf(file, "%[^,],%i\n", ctemp, &n_iter);
    printf("PARAMETERS::read_calc_parameters Read %s = %i\n", ctemp, n_iter);

    cout << endl;
}

void PARAMETERS::verify_parameters()
{
    if (poly_deg < 2 || poly_deg > 15)
    {
        printf("\n\nERROR from PARAMETERS::verify_parameters poly_deg should be in range [2, 15]; current value: %i\n\n", poly_deg);
        throw bad_exception();
    }
    if (phs_deg != 3 && phs_deg != 5 && phs_deg != 7 && phs_deg != 9 && phs_deg != 11)
    {
        printf("\n\nERROR from PARAMETERS::verify_parameters phs_deg should be 3, 5, 7, 9, or 11; current value: %i\n\n", phs_deg);
        throw bad_exception();
    }
    if (strcmp(solver_type.c_str(), "hypre_ilu_gmres") != 0 && strcmp(solver_type.c_str(), "eigen_direct") != 0 && strcmp(solver_type.c_str(), "eigen_ilu_bicgstab") != 0)
    {
        cout << "\n\nERROR from PARAMETERS::verify_parameters solver_type should be either hypre_ilu_gmres, eigen_ilu_bicgstab or eigen_direct; current value: " << solver_type << "\n\n";
        throw bad_exception();
    }

    if (meshfile.find_last_of(".") + 1 == meshfile.size())
    {
        cout << "\n\nERROR from PARAMETERS::verify_parameters extension of meshfile should be msh; \nUnable to find extension in the meshfile: " << meshfile << "\n\n";
        throw bad_exception();
    }
    string extension = meshfile.substr(meshfile.find_last_of(".") + 1, meshfile.size());
    if (extension != "msh")
    {
        cout << "\n\nERROR from PARAMETERS::verify_parameters extension of meshfile should be msh; \nCurrent extension: " << extension << " of the meshfile: " << meshfile << "\n\n";
        throw bad_exception();
    }

    output_file_prefix = meshfile;
    if (output_file_prefix.find_last_of("/") < output_file_prefix.size()) //remove location details from mesh file name if required
        output_file_prefix = output_file_prefix.substr(output_file_prefix.find_last_of("/") + 1, output_file_prefix.size());
    if (output_file_prefix.find_last_of(".") < output_file_prefix.size()) //remove extension from mesh file name if required
        output_file_prefix.replace(output_file_prefix.begin() + output_file_prefix.find_last_of("."), output_file_prefix.end(), "");
    output_file_prefix += "_polydeg_" + to_string(poly_deg);
    cout << "PARAMETERS::verify_parameters output_file_prefix: " << output_file_prefix << endl;
}

void PARAMETERS::calc_cloud_num_points()
{
    if (dimension == 2)
        num_poly_terms = ((int)(0.5 * (poly_deg + 1) * (poly_deg + 2)));
    else
        num_poly_terms = ((int)((poly_deg + 1) * (poly_deg + 2) * (poly_deg + 3) / 6)); //sum(0.5*(1:poly_deg+1).*(2:poly_deg+2)) = (poly_deg + 1) * (poly_deg + 2) * (poly_deg + 3) / 6
    cloud_size = (int)(ceil(cloud_size_multiplier * num_poly_terms));
    cout << "PARAMETERS::calc_cloud_num_points num_poly_terms: " << num_poly_terms << ", cloud_size: " << cloud_size << endl;
}

void PARAMETERS::get_problem_dimension_msh()
{ //identify whether its a 2D or 3D gmsh grid

    clock_t start = clock();
    int dim = 2;
    FILE *file;
    int itemp, ncv, cv_type, eof_flag = 0;
    double dtemp;
    char temp[50];
    file = fopen(meshfile.c_str(), "r");
    while (true)
    {
        eof_flag = fscanf(file, "%s ", temp);
        if (strcmp(temp, "$MeshFormat") == 0)
            break;
        if (eof_flag < 0)
        {
            printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh $MeshFormat not found in %s file\n\n", meshfile.c_str());
            throw bad_exception();
        }
    }
    // fscanf(file, "%s", temp);
    fgets(temp, 50, file);
    if (strcmp(temp, "2.2 0 8\n") != 0)
    {
        printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh MeshFormat should be 2.2 0 8; but %s file has %s format instead\n\n", meshfile.c_str(), temp);
        throw bad_exception();
    }
    eof_flag = 0;
    while (true)
    {
        eof_flag = fscanf(file, "%s ", temp);
        if (strcmp(temp, "$Nodes") == 0)
            break;
        if (eof_flag < 0)
        {
            printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh $Nodes not found in %s file\n\n", meshfile.c_str());
            throw bad_exception();
        }
    }
    eof_flag = 0;
    while (true)
    {
        eof_flag = fscanf(file, "%s ", temp);
        if (strcmp(temp, "$Elements") == 0)
            break;
        if (eof_flag < 0)
        {
            printf("\n\nERROR from PARAMETERS::get_problem_dimension_msh $Elements not found in %s file\n\n", meshfile.c_str());
            throw bad_exception();
        }
    }
    fscanf(file, "%i ", &ncv);
    // cout << "get_problem_dimension_msh ncv = " << ncv << endl;
    //reference: http://www.manpagez.com/info/gmsh/gmsh-2.2.6/gmsh_63.php
    for (int icv = 0; icv < ncv; icv++)
    {
        fscanf(file, "%i ", &itemp);   //cv number
        fscanf(file, "%i ", &cv_type); //cv type
        if (cv_type == 4 || cv_type == 5 || cv_type == 6 || cv_type == 7)
        { //4-node tetrahedron || 8-node hexahedron || 6-node prism || 5-node pyramid
            dim = 3;
            break; //3D element found
        }
        else if (cv_type == 11 || cv_type == 12 || cv_type == 13 || cv_type == 14)
        { //10-node tetrahedron || 27-node hexahedron || 18-node prism || 14-node pyramid
            dim = 3;
            break; //3D element found
        }
        else if (cv_type == 17 || cv_type == 18 || cv_type == 19)
        { //20-node hexahedron || 15-node prism || 13-node pyramid
            dim = 3;
            break; //3D element found
        }
        else if (cv_type == 29 || cv_type == 30 || cv_type == 31)
        { //20-node tetrahedron || 35-node tetrahedron || 56-node tetrahedron
            dim = 3;
            break; //3D element found
        }
        fscanf(file, "%*[^\n]\n"); //skip reading remaining row
    }
    dimension = dim;
    cout << "PARAMETERS::get_problem_dimension_msh problem dimension: " << dimension << endl;
}

void PARAMETERS::calc_polynomial_term_exponents()
{
    polynomial_term_exponents.resize(num_poly_terms, dimension);
    Eigen::MatrixXi deg_temp;
    if (dimension == 2)
    {
        polynomial_term_exponents.row(0) = Eigen::MatrixXi::Zero(1, dimension);                                        //constant term
        polynomial_term_exponents.block(1, 0, dimension, dimension) = Eigen::MatrixXi::Identity(dimension, dimension); //two linear terms
        int previous_index = 1;
        for (int deg = 2; deg <= poly_deg; deg++)
        { //quadratic onwards terms
            deg_temp.resize(deg + 1, dimension);
            for (int i = previous_index; i < previous_index + deg; i++)
            {
                deg_temp(i - previous_index, 0) = 1 + polynomial_term_exponents(i, 0); //exponents of 'x'
                deg_temp(i - previous_index, 1) = polynomial_term_exponents(i, 1);     //exponents of 'y'
            }
            deg_temp(deg, 0) = 0;
            deg_temp(deg, 1) = deg;
            previous_index = previous_index + deg;
            polynomial_term_exponents.block(previous_index, 0, deg + 1, dimension) = deg_temp;
        }
    }
    else
    {
        polynomial_term_exponents.row(0) = Eigen::MatrixXi::Zero(1, dimension);                                        //constant term
        polynomial_term_exponents.block(1, 0, dimension, dimension) = Eigen::MatrixXi::Identity(dimension, dimension); //three linear terms
        int previous_index = 1;
        for (int deg = 2; deg <= poly_deg; deg++)
        { //quadratic onwards terms
            deg_temp.resize(0.5 * (deg + 1) * (deg + 2), dimension);
            for (int i = previous_index; i < previous_index + (deg * (deg + 1) / 2); i++)
            {
                deg_temp(i - previous_index, 0) = 1 + polynomial_term_exponents(i, 0); //exponents of 'x'
                deg_temp(i - previous_index, 1) = polynomial_term_exponents(i, 1);     //exponents of 'y'
                deg_temp(i - previous_index, 2) = polynomial_term_exponents(i, 2);     //exponents of 'z'
            }
            for (int i = previous_index + (0.5 * deg * (deg - 1)); i < previous_index + (0.5 * deg * (deg + 1)); i++)
            {
                deg_temp(i - previous_index + deg, 0) = polynomial_term_exponents(i, 0);     //exponents of 'x'
                deg_temp(i - previous_index + deg, 1) = 1 + polynomial_term_exponents(i, 1); //exponents of 'y'
                deg_temp(i - previous_index + deg, 2) = polynomial_term_exponents(i, 2);     //exponents of 'z'
            }
            deg_temp((0.5 * (deg + 1) * (deg + 2)) - 1, 0) = 0;
            deg_temp((0.5 * (deg + 1) * (deg + 2)) - 1, 1) = 0;
            deg_temp((0.5 * (deg + 1) * (deg + 2)) - 1, 2) = deg;
            previous_index = previous_index + (0.5 * deg * (deg + 1));
            polynomial_term_exponents.block(previous_index, 0, 0.5 * (deg + 1) * (deg + 2), dimension) = deg_temp;
        }
    }
}

void PARAMETERS::calc_dt(Eigen::SparseMatrix<double, Eigen::RowMajor> &grad_x, Eigen::SparseMatrix<double, Eigen::RowMajor> &grad_y, Eigen::SparseMatrix<double, Eigen::RowMajor> &grad_z, Eigen::SparseMatrix<double, Eigen::RowMajor> &laplacian, double u0, double v0, double w0, double alpha)
{
    Eigen::VectorXcd eigval_grad_x, eigval_grad_y, eigval_grad_z, eigval_laplacian;
    eigval_grad_x = calc_largest_magnitude_eigenvalue(grad_x);
    eigval_grad_y = calc_largest_magnitude_eigenvalue(grad_y);
    if (dimension == 3)
        eigval_grad_z = calc_largest_magnitude_eigenvalue(grad_z);
    eigval_laplacian = calc_largest_magnitude_eigenvalue(laplacian);
    grad_x_eigval_real = eigval_grad_x[0].real(), grad_x_eigval_imag = eigval_grad_x[0].imag();
    grad_y_eigval_real = eigval_grad_y[0].real(), grad_y_eigval_imag = eigval_grad_y[0].imag();
    if (dimension == 3)
        grad_z_eigval_real = eigval_grad_z[0].real(), grad_z_eigval_imag = eigval_grad_z[0].imag();
    laplace_eigval_real = eigval_laplacian[0].real(), laplace_eigval_imag = eigval_laplacian[0].imag();
    printf("\nPARAMETERS::calc_dt Eigenvalue with largest magnitude: grad_x: (%g, %g)\n", eigval_grad_x[0].real(), eigval_grad_x[0].imag());
    printf("PARAMETERS::calc_dt Eigenvalue with largest magnitude: grad_y: (%g, %g)\n", eigval_grad_y[0].real(), eigval_grad_y[0].imag());
    if (dimension == 3)
        printf("PARAMETERS::calc_dt Eigenvalue with largest magnitude: grad_z: (%g, %g)\n", eigval_grad_z[0].real(), eigval_grad_z[0].imag());
    printf("PARAMETERS::calc_dt Eigenvalue with largest magnitude: laplacian: (%g, %g)\n", eigval_laplacian[0].real(), eigval_laplacian[0].imag());

    double evalues = u0 * sqrt((eigval_grad_x[0].real() * eigval_grad_x[0].real()) + (eigval_grad_x[0].imag() * eigval_grad_x[0].imag()));
    evalues += v0 * sqrt((eigval_grad_y[0].real() * eigval_grad_y[0].real()) + (eigval_grad_y[0].imag() * eigval_grad_y[0].imag()));
    if (dimension == 3)
        evalues += w0 * sqrt((eigval_grad_z[0].real() * eigval_grad_z[0].real()) + (eigval_grad_z[0].imag() * eigval_grad_z[0].imag()));
    evalues += alpha * sqrt((eigval_laplacian[0].real() * eigval_laplacian[0].real()) + (eigval_laplacian[0].imag() * eigval_laplacian[0].imag()));
    dt = 2.0 / evalues; //forward Euler
    dt = dt * Courant;
    printf("PARAMETERS::calc_dt Courant: %g, dt: %g seconds\n\n", Courant, dt);
}


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

#include "fw/math.h"

#include <boost/test/auto_unit_test.hpp>

using namespace moteus;

namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE(log2f_approx_test, * boost::unit_test::tolerance(1e-3f)) {
  const float test_values[] = {
    1.0,
    0.5,
    0.25,
    1.01,
    1.02,
    2.0,
    1.5,
    3.0,
    4.0,
    20.0,
    31.0,
    32.1,
  };
  for (const auto value : test_values) {
    BOOST_TEST_CONTEXT(value) {
      BOOST_TEST(std::abs(std::log2(value) - log2f_approx(value)) < 2e-3);
      // This has a relative tolerance instead of an absolute one.
      BOOST_TEST(pow2f_approx(log2f(value)) == value);
    }
  }

  BOOST_TEST(pow2f_approx(0.0024f) == 1.00166f);
  BOOST_TEST(pow2f_approx(log2f_approx(0.01)) == 0.01,
             boost::test_tools::tolerance(3e-2));
}

BOOST_AUTO_TEST_CASE(WrapZeroToTwoPiTest) {
  for (float v = -50.0f; v <= 50.0f; v += 0.001f) {
    BOOST_TEST_CONTEXT(v) {
      auto result = WrapZeroToTwoPi(v);
      const auto oracle_neg_pos = std::atan2(std::sin(v), std::cos(v));
      const auto corrected =
          (oracle_neg_pos > 0.0f) ?
          oracle_neg_pos :
          (2.0f * M_PI + oracle_neg_pos);
      BOOST_TEST(std::abs(corrected - result) < 0.001f);
    }
  }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2018 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include "cmsspread.hpp"
#include "utilities.hpp"

#include <ql/cashflows/cmscoupon.hpp>
#include <ql/cashflows/lineartsrpricer.hpp>
#include <ql/experimental/coupons/cmsspreadcoupon.hpp>
#include <ql/experimental/coupons/lognormalcmsspreadpricer.hpp>
#include <ql/indexes/swap/euriborswap.hpp>
#include <ql/math/array.hpp>
#include <ql/math/comparison.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/math/randomnumbers/sobolrsg.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/swaption/swaptionconstantvol.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actual360.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace boost::accumulators;

namespace {
struct TestData {
    TestData() {
        refDate = Date(23, February, 2018);
        Settings::instance().evaluationDate() = refDate;

        yts2 = Handle<YieldTermStructure>(
            ext::make_shared<FlatForward>(refDate, 0.02, Actual365Fixed()));

        swLn = Handle<SwaptionVolatilityStructure>(
            ext::make_shared<ConstantSwaptionVolatility>(
                refDate, TARGET(), Following, 0.20, Actual365Fixed(),
                ShiftedLognormal, 0.0));
        swSln = Handle<SwaptionVolatilityStructure>(
            ext::make_shared<ConstantSwaptionVolatility>(
                refDate, TARGET(), Following, 0.10, Actual365Fixed(),
                ShiftedLognormal, 0.01));
        swN = Handle<SwaptionVolatilityStructure>(
            ext::make_shared<ConstantSwaptionVolatility>(
                refDate, TARGET(), Following, 0.0075, Actual365Fixed(), Normal,
                0.01));

        reversion = Handle<Quote>(ext::make_shared<SimpleQuote>(0.01));
        cmsPricerLn =
            ext::make_shared<LinearTsrPricer>(swLn, reversion, yts2);
        cmsPricerSln =
            ext::make_shared<LinearTsrPricer>(swSln, reversion, yts2);
        cmsPricerN = ext::make_shared<LinearTsrPricer>(swN, reversion, yts2);

        correlation = Handle<Quote>(ext::make_shared<SimpleQuote>(0.6));
        cmsspPricerLn = ext::make_shared<LognormalCmsSpreadPricer>(
            cmsPricerLn, correlation, yts2, 32);
        cmsspPricerSln = ext::make_shared<LognormalCmsSpreadPricer>(
            cmsPricerSln, correlation, yts2, 32);
        cmsspPricerN = ext::make_shared<LognormalCmsSpreadPricer>(
            cmsPricerN, correlation, yts2, 32);
    }

    SavedSettings backup;
    Date refDate;
    Handle<YieldTermStructure> yts2;
    Handle<SwaptionVolatilityStructure> swLn, swSln, swN;
    Handle<Quote> reversion, correlation;
    ext::shared_ptr<CmsCouponPricer> cmsPricerLn, cmsPricerSln, cmsPricerN;
    ext::shared_ptr<CmsSpreadCouponPricer> cmsspPricerLn, cmsspPricerSln,
        cmsspPricerN;
};
} // namespace

void CmsSpreadTest::testFixings() {
    BOOST_TEST_MESSAGE("Testing fixings of cms spread indices...");

    TestData d;

    ext::shared_ptr<SwapIndex> cms10y =
        ext::make_shared<EuriborSwapIsdaFixA>(10 * Years, d.yts2, d.yts2);
    ext::shared_ptr<SwapIndex> cms2y =
        ext::make_shared<EuriborSwapIsdaFixA>(2 * Years, d.yts2, d.yts2);
    ext::shared_ptr<SwapSpreadIndex> cms10y2y =
        ext::make_shared<SwapSpreadIndex>("cms10y2y", cms10y, cms2y);

    Settings::instance().enforcesTodaysHistoricFixings() = false;

    BOOST_CHECK_THROW(cms10y2y->fixing(d.refDate - 1), QuantLib::Error);
    BOOST_REQUIRE_NO_THROW(cms10y2y->fixing(d.refDate));
    BOOST_CHECK_EQUAL(cms10y2y->fixing(d.refDate),
                      cms10y->fixing(d.refDate) - cms2y->fixing(d.refDate));
    cms10y->addFixing(d.refDate, 0.05);
    BOOST_CHECK_EQUAL(cms10y2y->fixing(d.refDate),
                      cms10y->fixing(d.refDate) - cms2y->fixing(d.refDate));
    cms2y->addFixing(d.refDate, 0.04);
    BOOST_CHECK_EQUAL(cms10y2y->fixing(d.refDate),
                      cms10y->fixing(d.refDate) - cms2y->fixing(d.refDate));
    Date futureFixingDate = TARGET().adjust(d.refDate + 1 * Years);
    BOOST_CHECK_EQUAL(cms10y2y->fixing(futureFixingDate),
                      cms10y->fixing(futureFixingDate) -
                          cms2y->fixing(futureFixingDate));
    IndexManager::instance().clearHistories();

    Settings::instance().enforcesTodaysHistoricFixings() = true;
    BOOST_CHECK_THROW(cms10y2y->fixing(d.refDate), QuantLib::Error);
    cms10y->addFixing(d.refDate, 0.05);
    BOOST_CHECK_THROW(cms10y2y->fixing(d.refDate), QuantLib::Error);
    cms2y->addFixing(d.refDate, 0.04);
    BOOST_CHECK_EQUAL(cms10y2y->fixing(d.refDate),
                      cms10y->fixing(d.refDate) - cms2y->fixing(d.refDate));
    IndexManager::instance().clearHistories();
}

namespace {
Real mcReferenceValue(const ext::shared_ptr<CmsCoupon>& cpn1,
                      const ext::shared_ptr<CmsCoupon>& cpn2, const Real cap,
                      const Real floor,
                      const Handle<SwaptionVolatilityStructure>& vol,
                      const Real correlation) {
    Size samples = 1000000;
    accumulator_set<double, stats<tag::mean> > acc;
    Matrix Cov(2, 2);
    Cov(0, 0) = vol->blackVariance(cpn1->fixingDate(), cpn1->index()->tenor(),
                                   cpn1->indexFixing());
    Cov(1, 1) = vol->blackVariance(cpn2->fixingDate(), cpn2->index()->tenor(),
                                   cpn2->indexFixing());
    Cov(0, 1) = Cov(1, 0) = std::sqrt(Cov(0, 0) * Cov(1, 1)) * correlation;
    Matrix C = pseudoSqrt(Cov);

    Array atmRate(2), adjRate(2), avg(2), volShift(2);
    atmRate[0] = cpn1->indexFixing();
    atmRate[1] = cpn2->indexFixing();
    adjRate[0] = cpn1->adjustedFixing();
    adjRate[1] = cpn2->adjustedFixing();
    if (vol->volatilityType() == ShiftedLognormal) {
        volShift[0] = vol->shift(cpn1->fixingDate(), cpn1->index()->tenor());
        volShift[1] = vol->shift(cpn2->fixingDate(), cpn2->index()->tenor());
        avg[0] =
            std::log((adjRate[0] + volShift[0]) / (atmRate[0] + volShift[0])) -
            0.5 * Cov(0, 0);
        avg[1] =
            std::log((adjRate[1] + volShift[1]) / (atmRate[1] + volShift[1])) -
            0.5 * Cov(1, 1);
    } else {
        avg[0] = adjRate[0];
        avg[1] = adjRate[1];
    }

    InverseCumulativeNormal icn;
    SobolRsg sb_(2, 42);
    Array w(2), z(2);
    for (Size i = 0; i < samples; ++i) {
        std::vector<Real> seq = sb_.nextSequence().value;
        std::transform(seq.begin(), seq.end(), w.begin(), icn);
        z = C * w + avg;
        for (Size i = 0; i < 2; ++i) {
            if (vol->volatilityType() == ShiftedLognormal) {
                z[i] =
                    (atmRate[i] + volShift[i]) * std::exp(z[i]) - volShift[i];
            }
        }
        acc(std::min(std::max(z[0] - z[1], floor), cap));
    }
    return mean(acc);
} // mcReferenceValue
} // namespace

void CmsSpreadTest::testCouponPricing() {
    BOOST_TEST_MESSAGE("Testing pricing of cms spread coupons...");

    TestData d;
    Real tol = 1E-6; // abs tolerance coupon rate

    ext::shared_ptr<SwapIndex> cms10y =
        ext::make_shared<EuriborSwapIsdaFixA>(10 * Years, d.yts2, d.yts2);
    ext::shared_ptr<SwapIndex> cms2y =
        ext::make_shared<EuriborSwapIsdaFixA>(2 * Years, d.yts2, d.yts2);
    ext::shared_ptr<SwapSpreadIndex> cms10y2y =
        ext::make_shared<SwapSpreadIndex>("cms10y2y", cms10y, cms2y);

    Date valueDate = cms10y2y->valueDate(d.refDate);
    Date payDate = valueDate + 1 * Years;
    ext::shared_ptr<CmsCoupon> cpn1a =
        ext::shared_ptr<CmsCoupon>(new CmsCoupon(
            payDate, 10000.0, valueDate, payDate, cms10y->fixingDays(), cms10y,
            1.0, 0.0, Date(), Date(), Actual360(), false));
    ext::shared_ptr<CmsCoupon> cpn1b = ext::shared_ptr<CmsCoupon>(
        new CmsCoupon(payDate, 10000.0, valueDate, payDate, cms2y->fixingDays(),
                      cms2y, 1.0, 0.0, Date(), Date(), Actual360(), false));
    ext::shared_ptr<CmsSpreadCoupon> cpn1 =
        ext::shared_ptr<CmsSpreadCoupon>(new CmsSpreadCoupon(
            payDate, 10000.0, valueDate, payDate, cms10y2y->fixingDays(),
            cms10y2y, 1.0, 0.0, Date(), Date(), Actual360(), false));
    BOOST_CHECK(cpn1->fixingDate() == d.refDate);
    cpn1a->setPricer(d.cmsPricerLn);
    cpn1b->setPricer(d.cmsPricerLn);
    cpn1->setPricer(d.cmsspPricerLn);

#ifndef __FAST_MATH__
    constexpr Real eqTol = 100*QL_EPSILON;
#else
    constexpr Real eqTol = 1e-13;
#endif
    BOOST_CHECK_CLOSE(cpn1->rate(), cpn1a->rate() - cpn1b->rate(), eqTol);
    cms10y->addFixing(d.refDate, 0.05);
    BOOST_CHECK_CLOSE(cpn1->rate(), cpn1a->rate() - cpn1b->rate(), eqTol);
    cms2y->addFixing(d.refDate, 0.03);
    BOOST_CHECK_CLOSE(cpn1->rate(), cpn1a->rate() - cpn1b->rate(), eqTol);
    IndexManager::instance().clearHistories();

    ext::shared_ptr<CmsCoupon> cpn2a = ext::shared_ptr<CmsCoupon>(
        new CmsCoupon(Date(23, February, 2029), 10000.0,
                      Date(23, February, 2028), Date(23, February, 2029), 2,
                      cms10y, 1.0, 0.0, Date(), Date(), Actual360(), false));
    ext::shared_ptr<CmsCoupon> cpn2b = ext::shared_ptr<CmsCoupon>(
        new CmsCoupon(Date(23, February, 2029), 10000.0,
                      Date(23, February, 2028), Date(23, February, 2029), 2,
                      cms2y, 1.0, 0.0, Date(), Date(), Actual360(), false));

    ext::shared_ptr<CappedFlooredCmsSpreadCoupon> plainCpn =
        ext::shared_ptr<CappedFlooredCmsSpreadCoupon>(
            new CappedFlooredCmsSpreadCoupon(
                Date(23, February, 2029), 10000.0, Date(23, February, 2028),
                Date(23, February, 2029), 2, cms10y2y, 1.0, 0.0, Null<Rate>(),
                Null<Rate>(), Date(), Date(), Actual360(), false));
    ext::shared_ptr<CappedFlooredCmsSpreadCoupon> cappedCpn =
        ext::shared_ptr<CappedFlooredCmsSpreadCoupon>(
            new CappedFlooredCmsSpreadCoupon(
                Date(23, February, 2029), 10000.0, Date(23, February, 2028),
                Date(23, February, 2029), 2, cms10y2y, 1.0, 0.0, 0.03,
                Null<Rate>(), Date(), Date(), Actual360(), false));
    ext::shared_ptr<CappedFlooredCmsSpreadCoupon> flooredCpn =
        ext::shared_ptr<CappedFlooredCmsSpreadCoupon>(
            new CappedFlooredCmsSpreadCoupon(
                Date(23, February, 2029), 10000.0, Date(23, February, 2028),
                Date(23, February, 2029), 2, cms10y2y, 1.0, 0.0, Null<Rate>(),
                0.01, Date(), Date(), Actual360(), false));
    ext::shared_ptr<CappedFlooredCmsSpreadCoupon> collaredCpn =
        ext::shared_ptr<CappedFlooredCmsSpreadCoupon>(
            new CappedFlooredCmsSpreadCoupon(
                Date(23, February, 2029), 10000.0, Date(23, February, 2028),
                Date(23, February, 2029), 2, cms10y2y, 1.0, 0.0, 0.03, 0.01,
                Date(), Date(), Actual360(), false));

    cpn2a->setPricer(d.cmsPricerLn);
    cpn2b->setPricer(d.cmsPricerLn);
    plainCpn->setPricer(d.cmsspPricerLn);
    cappedCpn->setPricer(d.cmsspPricerLn);
    flooredCpn->setPricer(d.cmsspPricerLn);
    collaredCpn->setPricer(d.cmsspPricerLn);

    BOOST_CHECK_SMALL(
        std::abs(plainCpn->rate() - mcReferenceValue(cpn2a, cpn2b, QL_MAX_REAL,
                                                     -QL_MAX_REAL, d.swLn,
                                                     d.correlation->value())),
        tol);
    BOOST_CHECK_SMALL(
        std::abs(cappedCpn->rate() - mcReferenceValue(cpn2a, cpn2b, 0.03,
                                                      -QL_MAX_REAL, d.swLn,
                                                      d.correlation->value())),
        tol);
    BOOST_CHECK_SMALL(
        std::abs(flooredCpn->rate() -
                 mcReferenceValue(cpn2a, cpn2b, QL_MAX_REAL, 0.01, d.swLn,
                                  d.correlation->value())),

        tol);
    BOOST_CHECK_SMALL(
        std::abs(collaredCpn->rate() -
                 mcReferenceValue(cpn2a, cpn2b, 0.03, 0.01, d.swLn,
                                  d.correlation->value())),
        tol);

    cpn2a->setPricer(d.cmsPricerSln);
    cpn2b->setPricer(d.cmsPricerSln);
    plainCpn->setPricer(d.cmsspPricerSln);
    cappedCpn->setPricer(d.cmsspPricerSln);
    flooredCpn->setPricer(d.cmsspPricerSln);
    collaredCpn->setPricer(d.cmsspPricerSln);

    BOOST_CHECK_SMALL(
        std::abs(plainCpn->rate() - mcReferenceValue(cpn2a, cpn2b, QL_MAX_REAL,
                                                     -QL_MAX_REAL, d.swSln,
                                                     d.correlation->value())),
        tol);
    BOOST_CHECK_SMALL(
        std::abs(cappedCpn->rate() - mcReferenceValue(cpn2a, cpn2b, 0.03,
                                                      -QL_MAX_REAL, d.swSln,
                                                      d.correlation->value())),
        tol);
    BOOST_CHECK_SMALL(
        std::abs(flooredCpn->rate() -
                 mcReferenceValue(cpn2a, cpn2b, QL_MAX_REAL, 0.01, d.swSln,
                                  d.correlation->value())),

        tol);
    BOOST_CHECK_SMALL(
        std::abs(collaredCpn->rate() -
                 mcReferenceValue(cpn2a, cpn2b, 0.03, 0.01, d.swSln,
                                  d.correlation->value())),
        tol);

    cpn2a->setPricer(d.cmsPricerN);
    cpn2b->setPricer(d.cmsPricerN);
    plainCpn->setPricer(d.cmsspPricerN);
    cappedCpn->setPricer(d.cmsspPricerN);
    flooredCpn->setPricer(d.cmsspPricerN);
    collaredCpn->setPricer(d.cmsspPricerN);

    BOOST_CHECK_SMALL(
        std::abs(plainCpn->rate() - mcReferenceValue(cpn2a, cpn2b, QL_MAX_REAL,
                                                     -QL_MAX_REAL, d.swN,
                                                     d.correlation->value())),
        tol);
    BOOST_CHECK_SMALL(
        std::abs(cappedCpn->rate() - mcReferenceValue(cpn2a, cpn2b, 0.03,
                                                      -QL_MAX_REAL, d.swN,
                                                      d.correlation->value())),
        tol);
    BOOST_CHECK_SMALL(std::abs(flooredCpn->rate() -
                               mcReferenceValue(cpn2a, cpn2b, QL_MAX_REAL, 0.01,
                                                d.swN, d.correlation->value())),

                      tol);
    BOOST_CHECK_SMALL(std::abs(collaredCpn->rate() -
                               mcReferenceValue(cpn2a, cpn2b, 0.03, 0.01, d.swN,
                                                d.correlation->value())),
                      tol);
}

test_suite* CmsSpreadTest::suite() {
    auto* suite = BOOST_TEST_SUITE("CmsSpreadTest");
    suite->add(QUANTLIB_TEST_CASE(&CmsSpreadTest::testFixings));
    suite->add(QUANTLIB_TEST_CASE(&CmsSpreadTest::testCouponPricing));
    return suite;
}


//
// Created by lejonmcgowan on 4/16/16.
//

#include "Transform.h"
#include <Eigen/Geometry>
#include <util/MathUtils.h>
Transform::Transform()
{

}

void Transform::addTranslate(const Eigen::Vector3f& translate)
{
    Eigen::Affine3f affineTranslate(Eigen::Translation3f(translate[0], translate[1], translate[2]));
    transformations.push_back(affineTranslate);
    setDirty();
}

void Transform::addRotate(const Eigen::Vector3f& rotate)
{
    Eigen::Affine3f rotx(Eigen::AngleAxisf(MathHelper::toRadians(rotate[0]), Eigen::Vector3f(1, 0, 0)));
    Eigen::Affine3f roty(Eigen::AngleAxisf(MathHelper::toRadians(rotate[1]), Eigen::Vector3f(0, 1, 0)));
    Eigen::Affine3f rotz(Eigen::AngleAxisf(MathHelper::toRadians(rotate[2]), Eigen::Vector3f(0, 0, 1)));
    transformations.push_back(rotz * roty * rotx);
    setDirty();
}

void Transform::addScale(const Eigen::Vector3f& scale)
{
    Eigen::Affine3f affineScale(Eigen::Scaling(scale[0], scale[1], scale[2]));
    transformations.push_back(affineScale);
    setDirty();
}

Eigen::Matrix4f Transform::getTransformMatrix()
{
    if (dirty)
    {
        Eigen::Matrix4f finalTransform;
        finalTransform.setIdentity();
        for (auto currentTransform: transformations)
            finalTransform = currentTransform.matrix() * finalTransform;
        transform = finalTransform;
    }
    dirty = false;
    return transform;
}

void Transform::setDirty()
{
    dirty = true;
    identity = false;
}

Ray Transform::transformRay(const Ray& ray)
{
    Ray transformRay;
    Eigen::Vector4f vectorO, vectorD;
    vectorO << ray.origin[0], ray.origin[1], ray.origin[2], 1;
    vectorD << ray.direction[0], ray.direction[1], ray.direction[2], 0;

    vectorO = getTransformMatrix() * vectorO;
    vectorD = getTransformMatrix() * vectorD;

    transformRay.origin << vectorO[0], vectorO[1], vectorO[2];
    transformRay.direction << vectorD[0], vectorD[1], vectorD[2];
    transformRay.direction.normalize();

    return transformRay;
}

Eigen::Vector3f Transform::transformPoint(const Eigen::Vector3f& point)
{
    Eigen::Vector3f transformPoint;
    Eigen::Vector4f vector;
    vector << point[0], point[1], point[2], 1;

    vector = getTransformMatrix() * vector;

    transformPoint << vector[0], vector[1], vector[2];

    return transformPoint;
}


//
// Copyright 2021 Sayan Chaudhuri <sayanchaudhuri758@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_FAST_FEATURE_DETECTOR_HPP
#define BOOST_GIL_IMAGE_PROCESSING_FAST_FEATURE_DETECTOR_HPP

#include <boost/gil/image.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/locator.hpp>
#include <boost/gil/point.hpp>
#include <algorithm>
#include <cmath>
#include <vector>

namespace boost { namespace gil {
namespace detail {

/// \brief Implements the FAST corner detection algorithm by Edward Rosten.
///       Algorithm :-
///       1.Consider a circle of 16 pixels around a given pixel(Bresenham circle)
///       2.Decide a threshold t
///       3.Let I_p be the intensity of the central pixel.
///       4.Find if there are n consecutive pixels in those 16 pixels where the intensities of all
///         pixels are <I_p-t or >I_p+t.Here n is taken as 9.
///       5.For step 4, to detect that a pixel is not a corner,check first whether the intensities
///         at pixels from 0 to 6 (with a step of 2)or the 8th pixel from each of them satisfies
///         condition of step 4. If not,check the same condition on odd numbered pixels or their
///         corresponding 8th pixel.
///       6.If the checks in step 5 is not affirmative, the given pixel cannot
///         be a corner.
///       7.Otherwise, proceed to find whether there are n consecutive pixels to satisfy the
///         criterion. If yes, p is a corner,else no.

///       @param buffer -type of source image.Must be grayscale
///       @param r      -row index of the candidate pixel
///       @param c      -column index of candidate pixel
///       @param points -pixel locations for bresenham circle around the given
///                       pixel
///       @param t      -threshold
template <typename SrcView>
bool fast_feature_detector(
    SrcView const& buffer,
    std::size_t r,
    std::size_t c,
    std::vector<point_t>& points,
    int t)
{
    auto src_loc = buffer.xy_at(c, r);
    std::vector<int> threshold_indicator;
    std::vector<int> intensity_array(16);
    std::vector<decltype(src_loc.cache_location(0, -1))> pointers(16);
    // storing intensities of pixels on circumference beforehand to decrease runtime
    for (std::ptrdiff_t i = 0; i < 16; i++)
    {
        pointers[i]        = src_loc.cache_location(points[i][0], points[i][1]);
        intensity_array[i] = src_loc[pointers[i]];
    }
    // calculating the flags to be used during segment test
    auto const I_p = buffer(point_t(c, r));

    std::transform(
        intensity_array.begin(),
        intensity_array.end(),
        back_inserter(threshold_indicator),
        [low = I_p - t, hi = I_p + t](auto const& intensity) {
            if (intensity < low)
                return -1;
            else if (intensity > hi)
                return 1;
            else
                return 0;
        });

    std::transform(
        intensity_array.begin(),
        intensity_array.end(),
        back_inserter(threshold_indicator),
        [low = I_p - t, hi = I_p + t](auto const& intensity) {
            if (intensity < low)
                return -1;
            else if (intensity > hi)
                return 1;
            else
                return 0;
        });

    // high speed test for eliminating non-corners
    for (std::ptrdiff_t i = 0; i <= 6; i += 2)
    {
        if (threshold_indicator[i] == 0 && threshold_indicator[i + 8] == 0)
            return false;
    }
    for (std::ptrdiff_t i = 1; i <= 7; i += 2)
    {
        if (threshold_indicator[i] == 0 && threshold_indicator[i + 8] == 0)
            return false;
    }

    // final segment test
    bool is_feature_point =
        threshold_indicator.end() !=
            std::search_n(threshold_indicator.begin(), threshold_indicator.end(), 9, -1) ||
        threshold_indicator.end() !=
            std::search_n(threshold_indicator.begin(), threshold_indicator.end(), 9, 1);
    return is_feature_point;
}

///\brief assigns a score to each detected corner to measure their degree of
/// cornerness.
///           Algorithm
///           Perform a binary search on threshold t to find out the maximum
///           threshold for which a corner remains a corner
///
///           @param src         -type of input image
///           @param i           -row index of the detected corner
///           @param j           -column index of the detected corner
///           @param points      -pixel locations for bresenham circle around
///                               the given pixel
///           @param threshold   -initial threshold given as input
template <typename SrcView>
std::size_t calculate_score(
    SrcView const& src,
    std::size_t i,
    std::size_t j,
    std::vector<point_t>& points,
    std::size_t threshold)
{
    std::size_t low  = threshold;
    std::size_t high = 255;
    // score measure used= highest threshold for which a corner remains a corner.
    // The cornerness of a corner decreases with increasing threshold
    while (high - low > 1)
    {
        std::size_t mid = (low + high) / 2;
        if (fast_feature_detector(src, i, j, points, mid))
        {
            low = mid;
        }
        else
        {
            high = mid - 1;
        }
    }
    return low - 1;
}
}  // namespace detail

/// \brief public function for using fast feature detector
///        @param src         -type of input image
///        @param keypoints   -vector for storing the locations of
///                            keypoints(corners)
///        @param scores      -vector for scores of each detected keypoint
///        @param nonmax      -indicates whether to perform nonmaximum
///                            suppression or not
///        @param threshold   -initial threshold given as input
template <typename SrcView>
void fast(
    SrcView const& src,
    std::vector<point_t>& keypoints,
    std::vector<std::size_t>& scores,
    bool nonmax           = true,
    std::size_t threshold = 10)
{
    // coordinates of a bresenham circle of radius 3
    std::vector<point_t> final_points_clockwise{
        point_t(3, 0),
        point_t(3, 1),
        point_t(2, 2),
        point_t(1, 3),
        point_t(0, 3),
        point_t(-1, 3),
        point_t(-2, 2),
        point_t(-3, 1),
        point_t(-3, 0),
        point_t(-3, -1),
        point_t(-2, -2),
        point_t(-1, -3),
        point_t(0, -3),
        point_t(1, -3),
        point_t(2, -2),
        point_t(3, -1)};
    // FAST features only calculated on grayscale images
    auto input_image_view = color_converted_view<gray8_pixel_t>(src);
    gray8_image_t fast_image(src.dimensions());
    // scores to be used during nonmaximum suppression
    gray8_view_t fast_score_matrix = view(fast_image);
    fill_pixels(fast_score_matrix, gray8_pixel_t(0));
    std::vector<point_t> kp;

    for (std::size_t i = 3; i < src.height() - 3; i++)
    {
        for (std::size_t j = 3; j < src.width() - 3; j++)
        {
            if (detail::fast_feature_detector(
                    input_image_view, i, j, final_points_clockwise, threshold))
            {
                kp.push_back(point_t(j, i));
            }
        }
    }

    for (auto u : kp)
    {
        int score = 0;
        score     = detail::calculate_score(
            input_image_view, u[1], u[0], final_points_clockwise, threshold);
        fast_score_matrix(u[0], u[1])[0] = gray8_pixel_t(score);
    }

    for (auto u : kp)
    {
        std::size_t i     = u[1];
        std::size_t j     = u[0];
        std::size_t score = 0;
        score             = int(fast_score_matrix(j, i)[0]);
        // performing nonmaximum suppression
        if (!nonmax || score > fast_score_matrix(j - 1, i)[0] &&
                           score > fast_score_matrix(j + 1, i)[0] &&
                           score > fast_score_matrix(j - 1, i - 1)[0] &&
                           score > fast_score_matrix(j, i - 1)[0] &&
                           score > fast_score_matrix(j + 1, i - 1)[0] &&
                           score > fast_score_matrix(j - 1, i + 1)[0] &&
                           score > fast_score_matrix(j, i + 1)[0] &&
                           score > fast_score_matrix(j + 1, i + 1)[0])
        {
            keypoints.push_back(u);
            scores.push_back(score);
        }
    }
}

}}  // namespace boost::gil
#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/integer_list.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/pair.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
        return if_(x == int_<0>, nothing, just(pair(x, x - int_<1>)));
    };

    BOOST_HANA_CONSTANT_ASSERT(
        unfoldr<IntegerList>(f, int_<10>) == integer_list<int, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1>
    );
    //! [main]
}


/**
 * \file boost/numeric/ublasx/operation/sign.hpp
 *
 * \brief Implement SIGN function through \c std::signbit for a vector or matrix expression.
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author comcon1 based on code of Marco Guazzone
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_SIGN_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SIGN_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_sign_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef typename type_traits<signature_argument_type>::real_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_sign_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef typename type_traits<signature_argument_type>::real_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename RealType>
BOOST_UBLAS_INLINE
RealType sign(RealType v) {
        return ( -(RealType)( ::std::signbit(v) ) + 0.5 ) * 2.0;
}

} // Namespace detail

/**
 * \brief Applies the \c std::sign function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::sign to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_sign_functor_traits<VectorExprT>::result_type sign(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_sign_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_sign_functor_traits<VectorExprT>::signature_result_type signature_result_type;

	return expression_type(ve(), detail::sign<signature_result_type>);
}

/**
 * \brief Applies the \c std::sign function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::sign to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_sign_functor_traits<MatrixExprT>::result_type sign(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_sign_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_sign_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

	return expression_type(me(), detail::sign<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SIGN_HPP


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock.
//  Copyright 2012 Phil Endecott
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include "arithmetic_backend.hpp"
#include <boost/chrono.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <fstream>
#include <iomanip>

template<class Clock>
struct stopwatch {
    typedef typename Clock::duration duration;
    stopwatch() {
        m_start = Clock::now();
    }
    duration elapsed() {
        return Clock::now() - m_start;
    }
    void reset() {
        m_start = Clock::now();
    }

private:
    typename Clock::time_point m_start;
};

// Custom 128-bit maths used for exact calculation of the Delaunay test.
// Only the few operators actually needed here are implemented.

struct int128_t {
    int64_t high;
    uint64_t low;

    int128_t() {
    }
    int128_t(int32_t i) : high(i >> 31), low(static_cast<int64_t>(i)) {
    }
    int128_t(uint32_t i) : high(0), low(i) {
    }
    int128_t(int64_t i) : high(i >> 63), low(i) {
    }
    int128_t(uint64_t i) : high(0), low(i) {
    }
};

inline int128_t operator<<(int128_t val, int amt) {
    int128_t r;
    r.low = val.low << amt;
    r.high = val.low >> (64 - amt);
    r.high |= val.high << amt;
    return r;
}

inline int128_t& operator+=(int128_t& l, int128_t r) {
    l.low += r.low;
    bool carry = l.low < r.low;
    l.high += r.high;
    if (carry)
        ++l.high;
    return l;
}

inline int128_t operator-(int128_t val) {
    val.low = ~val.low;
    val.high = ~val.high;
    val.low += 1;
    if (val.low == 0)
        val.high += 1;
    return val;
}

inline int128_t operator+(int128_t l, int128_t r) {
    l += r;
    return l;
}

inline bool operator<(int128_t l, int128_t r) {
    if (l.high != r.high)
        return l.high < r.high;
    return l.low < r.low;
}

inline int128_t mult_64x64_to_128(int64_t a, int64_t b) {
    // Make life simple by dealing only with positive numbers:
    bool neg = false;
    if (a < 0) {
        neg = !neg;
        a = -a;
    }
    if (b < 0) {
        neg = !neg;
        b = -b;
    }

    // Divide input into 32-bit halves:
    uint32_t ah = a >> 32;
    uint32_t al = a & 0xffffffff;
    uint32_t bh = b >> 32;
    uint32_t bl = b & 0xffffffff;

    // Long multiplication, with 64-bit temporaries:

    //            ah al
    //          * bh bl
    // ----------------
    //            al*bl   (t1)
    // +       ah*bl      (t2)
    // +       al*bh      (t3)
    // +    ah*bh         (t4)
    // ----------------

    uint64_t t1 = static_cast<uint64_t>(al) * bl;
    uint64_t t2 = static_cast<uint64_t>(ah) * bl;
    uint64_t t3 = static_cast<uint64_t>(al) * bh;
    uint64_t t4 = static_cast<uint64_t>(ah) * bh;

    int128_t r(t1);
    r.high = t4;
    r += int128_t(t2) << 32;
    r += int128_t(t3) << 32;

    if (neg)
        r = -r;

    return r;
}

template<class R, class T>
BOOST_FORCEINLINE void mul_2n(R& r, const T& a, const T& b) {
    r = a;
    r *= b;
}

template<class B, nil::crypto3::multiprecision::expression_template_option ET, class T>
BOOST_FORCEINLINE void mul_2n(nil::crypto3::multiprecision::number<B, ET>& r, const T& a, const T& b) {
    multiply(r, a, b);
}

BOOST_FORCEINLINE void mul_2n(int128_t& r, const boost::int64_t& a, const boost::int64_t& b) {
    r = mult_64x64_to_128(a, b);
}

template<class Traits>
inline bool delaunay_test(int32_t ax, int32_t ay, int32_t bx, int32_t by, int32_t cx, int32_t cy, int32_t dx,
                          int32_t dy) {
    // Test whether the quadrilateral ABCD's diagonal AC should be flipped to BD.
    // This is the Cline & Renka method.
    // Flip if the sum of the angles ABC and CDA is greater than 180 degrees.
    // Equivalently, flip if sin(ABC + CDA) < 0.
    // Trig identity: cos(ABC) * sin(CDA) + sin(ABC) * cos(CDA) < 0
    // We can use scalar and vector products to find sin and cos, and simplify
    // to the following code.
    // Numerical robustness is important.  This code addresses it by performing
    // exact calculations with large integer types.
    //
    // NOTE: This routine is limited to inputs with up to 30 BIT PRECISION, which
    // is to say all inputs must be in the range [INT_MIN/2, INT_MAX/2].

    typedef typename Traits::i64_t i64;
    typedef typename Traits::i128_t i128;

    i64 cos_abc, t;
    mul_2n(cos_abc, (ax - bx), (cx - bx));    // subtraction yields 31-bit values, multiplied to give 62-bit values
    mul_2n(t, (ay - by), (cy - by));
    cos_abc += t;    // addition yields 63 bit value, leaving one left for the sign

    i64 cos_cda;
    mul_2n(cos_cda, (cx - dx), (ax - dx));
    mul_2n(t, (cy - dy), (ay - dy));
    cos_cda += t;

    if (cos_abc >= 0 && cos_cda >= 0)
        return false;
    if (cos_abc < 0 && cos_cda < 0)
        return true;

    i64 sin_abc;
    mul_2n(sin_abc, (ax - bx), (cy - by));
    mul_2n(t, (cx - bx), (ay - by));
    sin_abc -= t;

    i64 sin_cda;
    mul_2n(sin_cda, (cx - dx), (ay - dy));
    mul_2n(t, (ax - dx), (cy - dy));
    sin_cda -= t;

    i128 sin_sum, t128;
    mul_2n(sin_sum, sin_abc, cos_cda);    // 63-bit inputs multiplied to 126-bit output
    mul_2n(t128, cos_abc, sin_cda);
    sin_sum += t128;    // Addition yields 127 bit result, leaving one bit for the sign

    return sin_sum < 0;
}

struct dt_dat {
    int32_t ax, ay, bx, by, cx, cy, dx, dy;
};

typedef std::vector<dt_dat> data_t;
data_t data;

template<class Traits>
void do_calc(const char* name) {
    std::cout << "Running calculations for: " << name << std::endl;

    stopwatch<boost::chrono::high_resolution_clock> w;

    boost::uint64_t flips = 0;
    boost::uint64_t calcs = 0;

    for (int j = 0; j < 1000; ++j) {
        for (data_t::const_iterator i = data.begin(); i != data.end(); ++i) {
            const dt_dat& d = *i;
            bool flip = delaunay_test<Traits>(d.ax, d.ay, d.bx, d.by, d.cx, d.cy, d.dx, d.dy);
            if (flip)
                ++flips;
            ++calcs;
        }
    }
    double t = boost::chrono::duration_cast<boost::chrono::duration<double>>(w.elapsed()).count();

    std::cout << "Number of calculations = " << calcs << std::endl;
    std::cout << "Number of flips = " << flips << std::endl;
    std::cout << "Total execution time = " << t << std::endl;
    std::cout << "Time per calculation = " << t / calcs << std::endl << std::endl;
}

template<class I64, class I128>
struct test_traits {
    typedef I64 i64_t;
    typedef I128 i128_t;
};

dt_dat generate_quadrilateral() {
    static boost::random::mt19937 gen;
    static boost::random::uniform_int_distribution<> dist(INT_MIN / 2, INT_MAX / 2);

    dt_dat result;

    result.ax = dist(gen);
    result.ay = dist(gen);
    result.bx = boost::random::uniform_int_distribution<>(result.ax, INT_MAX / 2)(gen);    // bx is to the right of ax.
    result.by = dist(gen);
    result.cx = dist(gen);
    result.cy = boost::random::uniform_int_distribution<>(result.cx > result.bx ? result.by : result.ay, INT_MAX / 2)(
        gen);    // cy is below at least one of ay and by.
    result.dx = boost::random::uniform_int_distribution<>(result.cx, INT_MAX / 2)(gen);    // dx is to the right of cx.
    result.dy = boost::random::uniform_int_distribution<>(result.cx > result.bx ? result.by : result.ay, INT_MAX / 2)(
        gen);    // cy is below at least one of ay and by.

    return result;
}

static void load_data() {
    for (unsigned i = 0; i < 100000; ++i)
        data.push_back(generate_quadrilateral());
}

int main() {
    using namespace nil::crypto3::multiprecision;
    std::cout << "loading data...\n";
    load_data();

    std::cout << "calculating...\n";

    do_calc<test_traits<boost::int64_t, boost::int64_t>>("int64_t, int64_t");
    do_calc<test_traits<number<arithmetic_backend<boost::int64_t>, et_off>,
                        number<arithmetic_backend<boost::int64_t>, et_off>>>(
        "arithmetic_backend<int64_t>, arithmetic_backend<int64_t>");
    do_calc<test_traits<boost::int64_t, number<arithmetic_backend<boost::int64_t>, et_off>>>(
        "int64_t, arithmetic_backend<int64_t>");
    do_calc<test_traits<number<cpp_int_backend<64, 64, nil::crypto3::multiprecision::signed_magnitude,
                                               nil::crypto3::multiprecision::unchecked, void>,
                               et_off>,
                        number<cpp_int_backend<64, 64, nil::crypto3::multiprecision::signed_magnitude,
                                               nil::crypto3::multiprecision::unchecked, void>,
                               et_off>>>("multiprecision::int64_t, nil::crypto3::multiprecision::int64_t");

    do_calc<test_traits<boost::int64_t, ::int128_t>>("int64_t, int128_t");
    do_calc<test_traits<boost::int64_t, nil::crypto3::multiprecision::int128_t>>(
        "int64_t, nil::crypto3::multiprecision::int128_t");
    do_calc<test_traits<boost::int64_t, number<cpp_int_backend<128, 128, nil::crypto3::multiprecision::signed_magnitude,
                                                               nil::crypto3::multiprecision::unchecked, void>,
                                               et_on>>>("int64_t, int128_t (ET)");
    do_calc<test_traits<number<cpp_int_backend<64, 64, nil::crypto3::multiprecision::signed_magnitude,
                                               nil::crypto3::multiprecision::unchecked, void>,
                               et_off>,
                        nil::crypto3::multiprecision::int128_t>>("multiprecision::int64_t, nil::crypto3::multiprecision::int128_t");

    do_calc<test_traits<boost::int64_t, cpp_int>>("int64_t, cpp_int");
    do_calc<test_traits<boost::int64_t, number<cpp_int_backend<>, et_off>>>("int64_t, cpp_int (no ET's)");
    do_calc<test_traits<boost::int64_t, number<cpp_int_backend<128>>>>("int64_t, cpp_int(128-bit cache)");
    do_calc<test_traits<boost::int64_t, number<cpp_int_backend<128>, et_off>>>(
        "int64_t, cpp_int (128-bit Cache no ET's)");

    return 0;
}


#include <iostream>

#define _mm256_set_m128d(vh, vl) \
	_mm256_insertf128_pd(_mm256_castpd128_pd256(vl), (vh), 1)

#include <Eigen/Dense>
#include <chrono>
#include <iomanip>
#include <vector>

using namespace Eigen;
using namespace std;

double timeProduct(int n) {

	MatrixXf a = MatrixXf::Random(n, n);
	MatrixXf b = MatrixXf::Random(n, n);
	b = a*b;
	auto t2 = chrono::high_resolution_clock::now();
	for (int i = 0; i < 10; ++i) {
		b = a*b;
	}
	auto t3 = chrono::high_resolution_clock::now();
	double duration = chrono::duration_cast<chrono::microseconds>(t3-t2).count()/1000.0;
	return duration;
}
int main() {

	// auto t1 = chrono::high_resolution_clock::now();
	// int n = 10000;
	// MatrixXf a = MatrixXf::Random(n, n);
	// MatrixXf b = MatrixXf::Random(n, n);
	// MatrixXf c = MatrixXf::Random(n, n);
	// a = a*1;
	// b = b*1;
	// c = c*1;

	// auto t2 = chrono::high_resolution_clock::now();
	// c = a*b;
	// auto t3 = chrono::high_resolution_clock::now();
	// auto durationAll = chrono::duration_cast<chrono::microseconds>(t2-t1).count()/1000.0;
	// auto duration = chrono::duration_cast<chrono::microseconds>(t3-t2).count()/1000.0;
	// cout << "n = " << n << "\n";
	// cout << "Allocation time:\n\t";
	// cout << durationAll << " ms\n";
	// cout << "Product time:\n\t";
	// cout << duration << "ms\n";

	vector<int> sizes;
	vector<double> times;
	for (int i = 0; i < 100; ++i) {
		sizes.push_back(100 + i*10);
		times.push_back(timeProduct(100+i*10));
	}
	for (auto t : times)
		cout << t << " ";
	cout << endl;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_MEMORY_IS_POWER_OF_2_HPP_INCLUDED
#define BOOST_SIMD_MEMORY_IS_POWER_OF_2_HPP_INCLUDED

/*!
  @file
  @brief Defines and implements is_power_of_2
**/

#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd
{
  /*!
    @brief Power of two compliance predicate for integers

    Checks if a value is a non-zero power of two.

    @par Semantic:

    For any integer @c v :

    @code
    bool r = is_power_of_2(v);
    @endcode

    is equivalent to

    @code
    bool r = (!(value & (value - 1)) && value);
    @endcode

    If @c a is not a power of two, an assertion is triggered.

    @usage{memory/is_power_of_2.cpp}

    @param value  Value to check

    @return A boolean indicating if @c value is a non-zero power of two
  **/
  template<typename Integer> BOOST_FORCEINLINE
  bool is_power_of_2(Integer value)
  {
    return (!(value & (value - 1)) && value);
  }
} }

#endif


//
// Created by vogier on 21/04/2020.
//

#include "Trajectory.h"
#include <Eigen/Geometry>

using namespace vcl;

/** Function returning the index i such that t \in [v[i].t,v[i+1].t] */
size_t index_at_value(float t, vcl::buffer<keyframe> const& v);

vcl::vec3 linear_interpolation(float t, float t1, float t2, const vcl::vec3& p1, const vcl::vec3& p2);

void Trajectory::init(vcl::buffer<keyframe> _keyframes)
{
    // Initial Keyframe data vector of (position, time)
    keyframes = std::move(_keyframes);

    // Set timer bounds
    // You should adapt these extremal values to the type of interpolation
    timer.t_min = keyframes[1].t;                   // first time of the keyframe
    timer.t_max = keyframes[keyframes.size()-2].t;  // last time of the keyframe
    timer.t = timer.t_min;
}

void Trajectory::update() {
  timer.update();
  const float t = timer.t;

  // ********************************************* //
  // Compute interpolated position at time t
  // ********************************************* //
  const int idx = index_at_value(t, keyframes);

  // Preparation of data for the linear interpolation
  // Parameters used to compute the linear interpolation
  keyframe const & k0 = keyframes[idx - 1]; // t_{i-1}
  keyframe const & k1 = keyframes[idx]; // = t_i
  keyframe const & k2 = keyframes[idx + 1]; // = t_{i+1}
  keyframe const & k3 = keyframes[idx + 2]; // = t_{i+2}

  // Update position and TNB
  cardinal_spline_interpolation_update(t, k0, k1, k2, k3, trajectory_tension);

}

size_t index_at_value(float t, vcl::buffer<keyframe> const& v)
{
    const size_t N = v.size();
    assert(v.size()>=2);
    assert(t>=v[0].t);
    assert(t<v[N-1].t);

    size_t k=0;
    while( v[k+1].t<t )
        ++k;
    return k;
}

vec3 linear_interpolation(float t, float t1, float t2, const vec3& p1, const vec3& p2)
{
    const float alpha = (t-t1)/(t2-t1);
    const vec3 p = (1-alpha)*p1 + alpha*p2;

    return p;
}

/**
 *
 * Les positions sont données par une interpolation par spline cardinale.
 *
 * Les rotations sont obtenus de la manière suivante :
 *
 * Chaque keyframe se voit associé une direction verticale ("binormale") par cross product
 * entre les vecteurs vers ses voisins. Exemple : si on a trois keyframes successifs
 *      A ---- B ---- C
 * alors la direction verticale en B est donnée par cross(AB,BC).
 * Pour que cette direction verticale pointe "vers le haut" (z positif), on la retourne
 * si son produit scalaire avec le vecteur unitaire z est négatif.
 *
 * La tangente T en un keyframe est donnée par le vecteur reliant ses voisins (AC pour le point B),
 * c'est le même que pour l'interpolation par spline cardianale.
 *
 * Le dernier vecteur est obtenue par cross product : N = cross(B,T). Avec ces trois vecteurs, on
 * forme un quaternion représentant la rotation à ce keyframe.
 *
 * On obtient les rotations intermédiaires entre les keyframes par slerp dans le domaine
 * des quaternions unitaires.
 *
 */

void Trajectory::cardinal_spline_interpolation_update(float t, keyframe const & k0, keyframe const & k1, keyframe const & k2, keyframe const & k3 , float K) {
  auto t0 = k0.t, t1 = k1.t, t2 = k2.t, t3 = k3.t;
  auto const & p0 = k0.p, p1 = k1.p, p2 = k2.p, p3 = k3.p;

  float const s = (t - t1) / (t2 - t1);
  float const s2 = s*s;
  float const s3 = s*s*s;
  vec3 d1 = 2 * K * (p2 - p0) / (t2 - t0);
  vec3 d2 = 2 * K * (p3 - p1) / (t3 - t1);

  // cardinal spline interpolation of position r
  position =  (2 * s3 - 3 * s2 + 1) * p1 + (s3 - 2*s2 + s) * d1 + (-2*s3 + 3*s2) * p2 + (s3 - s2) * d2;

  /*
  tangent = normalize( (1-s) * d1 + s * d2);
  vec3 T = normalize(tangent - tangent.z);
  normal = {-T.y, T.x, 0};
  binormal = cross(tangent,normal);
  */

  // compute quaternion rotation at 1 and 2 and use spherical interpolation.
  Eigen::Quaternionf q1, q2;

  // Ici on approxime T par interpolation linéaire entre d1 et d2.
  {
    vec3 B = normalize(cross(p0 - p1, p2 - p1));
    if (dot(B,{0,0,1}) < 0) {
      B = -B;
    }
    vec3 const T = normalize(d1 - dot(B, d1) * B);
    vec3 const N = cross(B,T);
    Eigen::Matrix3f m;
    m << T.x, N.x, B.x, T.y, N.y, B.y, T.z, N.z, B.z;
    q1 = Eigen::Quaternionf{m};
  }
  {
    //vec3 const B = normalize(vert2);
    vec3 B = normalize(cross(p1 - p2, p3 - p2));
    if (dot(B,{0,0,1}) < 0) {
      B = -B;
    }
    vec3 const T = normalize(d2 - dot(B, d2) * B);
    vec3 const N = cross(B,T);
    Eigen::Matrix3f m;
    m << T.x, N.x, B.x, T.y, N.y, B.y, T.z, N.z, B.z;
    q2 = Eigen::Quaternionf{m};
  }
  Eigen::Quaternionf q {q1.slerp(s, q2)};

  auto m {q.toRotationMatrix()};

  tangent = {m(0,0), m(1,0), m(2,0)};
  normal = {m(0,1), m(1,1), m(2,1)};
  binormal = {m(0,2), m(1,2), m(2,2)};
}


/*
 * Copyright 2020 INRIA
 */

#ifndef __eigenpy_decomposition_llt_hpp__
#define __eigenpy_decomposition_llt_hpp__

#include "eigenpy/eigenpy.hpp"

#include <Eigen/Core>
#include <Eigen/Cholesky>

#include "eigenpy/utils/scalar-name.hpp"

namespace eigenpy
{

  template<typename _MatrixType>
  struct LLTSolverVisitor
  : public boost::python::def_visitor< LLTSolverVisitor<_MatrixType> >
  {

    typedef _MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::RealScalar RealScalar;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,MatrixType::Options> VectorType;
    typedef Eigen::LLT<MatrixType> Solver;

    template<class PyClass>
    void visit(PyClass& cl) const
    {
      namespace bp = boost::python;
      cl
      .def(bp::init<>("Default constructor"))
      .def(bp::init<Eigen::DenseIndex>(bp::arg("size"),
                                       "Default constructor with memory preallocation"))
      .def(bp::init<MatrixType>(bp::arg("matrix"),
                                "Constructs a LLT factorization from a given matrix."))

      .def("matrixL",&matrixL,bp::arg("self"),
           "Returns the lower triangular matrix L.")
      .def("matrixU",&matrixU,bp::arg("self"),
           "Returns the upper triangular matrix U.")
      .def("matrixLLT",&Solver::matrixLLT,bp::arg("self"),
           "Returns the LLT decomposition matrix.",
           bp::return_internal_reference<>())

#if EIGEN_VERSION_AT_LEAST(3,3,90)
      .def("rankUpdate",(Solver& (Solver::*)(const VectorType &, const RealScalar &))&Solver::template rankUpdate<VectorType>,
           bp::args("self","vector","sigma"), bp::return_self<>())
#else
      .def("rankUpdate",(Solver (Solver::*)(const VectorType &, const RealScalar &))&Solver::template rankUpdate<VectorType>,
           bp::args("self","vector","sigma"))
#endif

#if EIGEN_VERSION_AT_LEAST(3,3,0)
      .def("adjoint",&Solver::adjoint,bp::arg("self"),
           "Returns the adjoint, that is, a reference to the decomposition itself as if the underlying matrix is self-adjoint.",
           bp::return_self<>())
#endif

      .def("compute",(Solver & (Solver::*)(const Eigen::EigenBase<MatrixType> & matrix))&Solver::compute,
           bp::args("self","matrix"),
           "Computes the LLT of given matrix.",
           bp::return_self<>())

      .def("info",&Solver::info,bp::arg("self"),
           "NumericalIssue if the input contains INF or NaN values or overflow occured. Returns Success otherwise.")
#if EIGEN_VERSION_AT_LEAST(3,3,0)
      .def("rcond",&Solver::rcond,bp::arg("self"),
           "Returns an estimate of the reciprocal condition number of the matrix.")
#endif
      .def("reconstructedMatrix",&Solver::reconstructedMatrix,bp::arg("self"),
           "Returns the matrix represented by the decomposition, i.e., it returns the product: L L^*. This function is provided for debug purpose.")
      .def("solve",&solve<VectorType>,bp::args("self","b"),
           "Returns the solution x of A x = b using the current decomposition of A.")
      ;
    }

    static void expose()
    {
      static const std::string classname = "LLT" + scalar_name<Scalar>::shortname();
      expose(classname);
    }

    static void expose(const std::string & name)
    {
      namespace bp = boost::python;
      bp::class_<Solver>(name.c_str(),
                         "Standard Cholesky decomposition (LL^T) of a matrix and associated features.\n\n"
                         "This class performs a LL^T Cholesky decomposition of a symmetric, positive definite matrix A such that A = LL^* = U^*U, where L is lower triangular.\n\n"
                         "While the Cholesky decomposition is particularly useful to solve selfadjoint problems like D^*D x = b, for that purpose, we recommend the Cholesky decomposition without square root which is more stable and even faster. Nevertheless, this standard Cholesky decomposition remains useful in many other situations like generalised eigen problems with hermitian matrices.",
                         bp::no_init)
      .def(LLTSolverVisitor());
    }

  private:

    static MatrixType matrixL(const Solver & self) { return self.matrixL(); }
    static MatrixType matrixU(const Solver & self) { return self.matrixU(); }

    template<typename VectorType>
    static VectorType solve(const Solver & self, const VectorType & vec)
    {
      return self.solve(vec);
    }
  };

} // namespace eigenpy

#endif // ifndef __eigenpy_decomposition_llt_hpp__


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschrÃ¤nkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <cassert>
#include <cstdlib>
#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/timer.hpp>

using namespace std;

template <typename Matrix, typename Value, typename Vector>
void test_vector(const Matrix& A, const Value& alpha, const Vector& v, double tol, int i)
{
    Vector diff(A*v-alpha*v), v1(A*v), v2(alpha*v); // , diff(v1-v2);
    if (size(v1) < 17)
	cout << "A*v is     " << v1 << "\nalpha*v is " << v2 << '\n';
    if (two_norm(diff) > tol) cout << "two_norm(difference) of the " << i << "-th eigenvector is " << two_norm(diff) << ", two_norm(A*v) is " << two_norm(v1) << ", two_norm(alpha*v) is " << two_norm(v2) << '\n'; // throw "wrong eigenvector";
}

int main(int argc, char** argv)
{
    using namespace mtl;

    int select= 1, sub= 600;
    if (argc > 1)
	select= atoi(argv[1]);
    assert(select >= 1 && select <= 2);

    if (argc > 2)
	sub= atoi(argv[2]);

    string fname= string("../../../../../branches/data/matrix_market/Partha") + char('0' + select) + ".mtx";

    dense2D<double>    A0(io::matrix_market(fname.c_str())), A(clone(A0[irange(sub)][irange(sub)]));
    //    cout << "Size of A is " << num_rows(A) << " x " << num_cols(A) << '\n';

    boost::timer tri_time;
    dense2D<double>    C(hessenberg_factors(A)), D(clone(bands(C, -1, 2))), Q(num_rows(D), num_rows(D));
    cout << "The tridiagonal matrix is\n" << D[irange(10)][irange(10)] << "This took " << tri_time.elapsed() << "s.\n";

    tri_time.restart();
    dense_vector<double>       lambda(num_rows(D));

    cuppen(D, Q, lambda);
    cout << "Q is\n" << Q[irange(10)][irange(10)] << "This took " << tri_time.elapsed() << "s.\n";
    // std::cout << "The eigenvalues are " << lambda << "\n";
#if 0
    for (unsigned i= 0; i < num_rows(D); i++)
	test_vector(D, lambda[i], mtl::dense_vector<double>(Q[mtl::iall][i]), 1e-4, i);
#endif
    return 0;
}


/**
 * \file boost/numeric/ublasx/operation/rep.hpp
 *
 * \brief Replicate and tile a matrix or a vector.
 *
 * This operation mimic the MATLAB \c repmat function.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_REP_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_REP_HPP

#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

template <typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> rep(matrix_expression<MatrixExprT> const& me, ::std::size_t nr, ::std::size_t nc)
{
	typedef matrix<typename matrix_traits<MatrixExprT>::value_type> result_matrix_type;
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;

	size_type nr_me(num_rows(me));
	size_type nc_me(num_columns(me));

	result_matrix_type res(nr_me*nr, nc_me*nc);

	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			subrange(res, r*nr_me, (r+1)*nr_me, c*nc_me, (c+1)*nc_me) = me;
		}
	}

	return res;
}

template <typename VectorExprT>
matrix<typename vector_traits<VectorExprT>::value_type> rep(vector_expression<VectorExprT> const& ve, ::std::size_t nr, ::std::size_t nc)
{
	typedef matrix<typename vector_traits<VectorExprT>::value_type> result_matrix_type;
	typedef typename vector_traits<VectorExprT>::size_type size_type;

	size_type n_ve(size(ve));

	result_matrix_type res(n_ve*nr, nc);

	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			for (size_type i = 0; i < n_ve; ++i)
			{
				res(r*n_ve+i, c) = ve()(i);
			}
		}
	}

	return res;
}

template <typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> rep(matrix_expression<MatrixExprT> const& me, ::std::size_t n)
{
	return rep(me, n, n);
}

template <typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> rep(vector_expression<MatrixExprT> const& ve, ::std::size_t n)
{
	return rep(ve, n, n);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_REP_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_SQRTSMALLESTPOSVAL_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_SQRTSMALLESTPOSVAL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the square root of the least non zero positive non denormal
    value of the chosen type.

    @par Semantic:

    @code
    T r = Sqrtsmallestposval<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = 1
    else if T is double
      r =  1.491668146240041e-154;
    else if T is float
      r =   1.0842022e-19;
    @endcode

    @return The Sqrtsmallestposval constant for the proper type
  **/
  template<typename T> T Sqrtsmallestposval();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant sqrtsmallestposval.

      @return The Sqrtsmallestposval constant for the proper type
    **/
    Value Sqrtsmallestposval();
  }
} }
#endif

#include <boost/simd/constant/definition/sqrtsmallestposval.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 * @file    testPCGSolver.cpp
 * @brief   Unit tests for PCGSolver class
 * @author  Yong-Dian Jian
 */

#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Matrix.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

#include <iostream>
#include <fstream>

using namespace std;
using namespace gtsam;

const double tol = 1e-3;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( PCGSolver, llt ) {
  Matrix R = (Matrix(3,3) <<
                1., -1., -1.,
                0.,  2., -1.,
                0.,  0.,  1.);
  Matrix AtA = R.transpose() * R;

  Vector Rvector = (Vector(9) << 1., -1., -1.,
                                 0.,  2., -1.,
                                 0.,  0.,  1.);
//  Vector Rvector = (Vector(6) << 1., -1., -1.,
//                                      2., -1.,
//                                           1.);

  Vector b = (Vector(3) << 1., 2., 3.);

  Vector x = (Vector(3) << 6.5, 2.5, 3.) ;

  /* test cholesky */
  Matrix Rhat = AtA.llt().matrixL().transpose();
  EXPECT(assert_equal(R, Rhat, 1e-5));

  /* test backward substitution */
  Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b);
  EXPECT(assert_equal(x, xhat, 1e-5));

  /* test in-place back substitution */
  xhat = b;
  Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat);
  EXPECT(assert_equal(x, xhat, 1e-5));

  /* test triangular matrix map */
  Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
  xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b);
  EXPECT(assert_equal(x, xhat, 1e-5));

}

/* ************************************************************************* */
TEST( PCGSolver, dummy )
{
  LevenbergMarquardtParams paramsPCG;
  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
  pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
  paramsPCG.iterativeParams = pcg;

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  Point2 x0(10,10);
  Values c0;
  c0.insert(X(1), x0);

  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();

  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}

/* ************************************************************************* */
TEST( PCGSolver, blockjacobi )
{
  LevenbergMarquardtParams paramsPCG;
  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
  pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>();
  paramsPCG.iterativeParams = pcg;

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  Point2 x0(10,10);
  Values c0;
  c0.insert(X(1), x0);

  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();

  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}

/* ************************************************************************* */
TEST( PCGSolver, subgraph )
{
  LevenbergMarquardtParams paramsPCG;
  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
  pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
  paramsPCG.iterativeParams = pcg;

  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  Point2 x0(10,10);
  Values c0;
  c0.insert(X(1), x0);

  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();

  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------

#include <simd_bench.hpp>
#include <boost/simd/function/simd/ldexp.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;
namespace bd =  boost::dispatch;

DEFINE_SIMD_BENCH(simd_ldexp, bs::ldexp);

template < int N >
struct lde
{
  template<class T> T operator()(const T & a) const
  {
    return bs::ldexp(a, N);
  }
};
template < int N >
struct ldef
{
  template<class T> T operator()(const T & a) const
  {
    return bs::fast_(bs::ldexp)(a, N);
  }
};

  DEFINE_SIMD_BENCH(simd_lde10, lde<10>());
  DEFINE_SIMD_BENCH(fast_simd_lde10, ldef<10>());

DEFINE_BENCH_MAIN() {
  nsb::for_each<simd_lde10, NS_BENCH_IEEE_TYPES>(-10, 10);
  nsb::for_each<fast_simd_lde10, NS_BENCH_IEEE_TYPES>(-10, 10);
}


/**
 * @file tests/binarize_test.cpp
 * @author Keon Kim
 *
 * Test the Binarize method.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/data/binarize.hpp>
#include <mlpack/core/math/random.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace arma;
using namespace mlpack::data;

BOOST_AUTO_TEST_SUITE(BinarizeTest);

BOOST_AUTO_TEST_CASE(BinerizeOneDimension)
{
  mat input;
  input << 1 << 2 << 3 << endr
        << 4 << 5 << 6 << endr // this row will be tested
        << 7 << 8 << 9;

  mat output;
  const double threshold = 5.0;
  const size_t dimension = 1;
  Binarize<double>(input, output, threshold, dimension);

  BOOST_REQUIRE_CLOSE(output(0, 0), 1, 1e-5); // 1
  BOOST_REQUIRE_CLOSE(output(0, 1), 2, 1e-5); // 2
  BOOST_REQUIRE_CLOSE(output(0, 2), 3, 1e-5); // 3
  BOOST_REQUIRE_SMALL(output(1, 0), 1e-5); // 4 target
  BOOST_REQUIRE_SMALL(output(1, 1), 1e-5); // 5 target
  BOOST_REQUIRE_CLOSE(output(1, 2), 1, 1e-5); // 6 target
  BOOST_REQUIRE_CLOSE(output(2, 0), 7, 1e-5); // 7
  BOOST_REQUIRE_CLOSE(output(2, 1), 8, 1e-5); // 8
  BOOST_REQUIRE_CLOSE(output(2, 2), 9, 1e-5); // 9
}

BOOST_AUTO_TEST_CASE(BinerizeAll)
{
  mat input;
  input << 1 << 2 << 3 << endr
        << 4 << 5 << 6 << endr // this row will be tested
        << 7 << 8 << 9;

  mat output;
  const double threshold = 5.0;

  Binarize<double>(input, output, threshold);

  BOOST_REQUIRE_SMALL(output(0, 0), 1e-5); // 1
  BOOST_REQUIRE_SMALL(output(0, 1), 1e-5); // 2
  BOOST_REQUIRE_SMALL(output(0, 2), 1e-5); // 3
  BOOST_REQUIRE_SMALL(output(1, 0), 1e-5); // 4
  BOOST_REQUIRE_SMALL(output(1, 1), 1e-5); // 5
  BOOST_REQUIRE_CLOSE(output(1, 2), 1.0, 1e-5); // 6
  BOOST_REQUIRE_CLOSE(output(2, 0), 1.0, 1e-5); // 7
  BOOST_REQUIRE_CLOSE(output(2, 1), 1.0, 1e-5); // 8
  BOOST_REQUIRE_CLOSE(output(2, 2), 1.0, 1e-5); // 9
}

BOOST_AUTO_TEST_SUITE_END();


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_PI_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_PI_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Pi generic tag

     Represents the Pi constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Pi, double, 3
                                , 0x40490FDB, 0x400921FB54442D18ULL
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pi, Site> dispatching_Pi(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Pi, Site>();
   }
   template<class... Args>
   struct impl_Pi;
  }
  /*!
    Generates value \f$\pi\f$ that is the half length of a circle of radius one
    ... in normal temperature and pressure conditions.

    @par Semantic:

    @code
    T r = Pi<T>();
    @endcode

    is similar to:

    @code
    T r = T(4*atan(1));
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Pi, Pi)
} }

#include <boost/simd/constant/common.hpp>

#endif