/*-------------ConjGrad.cpp---------------------------------------------------//
*
*              Conjugate Gradient Method
*
* Purpose: This file intends to solve a simple linear algebra equation:
*              Ax = b
*              Where A is a square Matrix of dimension N and x, b are vectors
*              of length N.
*
*   Notes: This will use the eigen LA library for solving things.
*
*-----------------------------------------------------------------------------*/

#include <iostream>
#include <Eigen/Core>
#include <memory>
#include <cassert>
#include <cmath>

using namespace Eigen;

// Function for Conj Gradient -- All the big stuff happens here
void conjgrad(const Matrix2d &A, const Vector2d &b, Vector2d &x_0,
              double thresh);

/*----------------------------------------------------------------------------//
* MAIN
*-----------------------------------------------------------------------------*/

int main(){

    double thresh = 0.001;

    Matrix2d a(2,2);
    Vector2d b, x;

    a(0,0) = 4; a(1,0) = 1; a(0,1) = 1; a(1,1) = 3;
    x(0) = 2; x(1) = 1;
    b(0) = 1; b(1) = 2;

    conjgrad(a, b, x, thresh);

    std::cout << "Conjugate Gradient output with threshold " << thresh << '\n';

    std::cout << x(0) << '\t' << x(1) <<'\n';

}

/*----------------------------------------------------------------------------//
* SUBROUTINES
*-----------------------------------------------------------------------------*/

// Function for Conj Gradient -- All the big stuff happens here
void conjgrad(const Matrix2d &A, const Vector2d &b, Vector2d &x_0,
              double thresh){

    Vector2d r, p;
    double alpha, diff, beta, temp1, temp2;

    // definingin first r
    Vector2d r_0 = b - (A * x_0);

    // setting x arbitrarily high to start
    Vector2d x = x_0 * 4;

    // *grumble... gustorn... grumble*
    Vector2d p_0 = r_0;

    diff = (x - x_0).norm();
    while (diff > thresh){

        // Note, matmul will output a Matrix2d with one element.
        temp1 = r_0.transpose() * r_0;
        temp2 = p_0.transpose() * A * p_0;
        alpha = temp1 / temp2;

        x = x_0 + (p_0 * alpha);
        r = r_0 - (A * alpha * p_0);

        temp1 = r.transpose() * r;
        temp2 = r_0.transpose() * r_0;

        beta = temp1 / temp2;

        p = r + (p_0 * beta);

        diff = (x - x_0).norm();;

        // set all the values to naughts
        x_0 = x;
        r_0 = r;
        p_0 = p;
    }
}


#include<iostream>
#include<fstream>
#include<vector>
#include <Eigen/Dense>
#include<string>

using Eigen::MatrixXd;
using namespace std;

void print_vector(vector<vector<double> > v);
void print_matrix_vector(vector<double> v);
MatrixXd* read_matrix(string filename);
void write_matrix(string filename, MatrixXd m);

int main(){

    MatrixXd* data = read_matrix("83ppl.csv");

    // Data arrangement:
    int Nsize = (*data).rows();
    MatrixXd x_data(Nsize, 1);
    MatrixXd y_data(Nsize, 1);
    x_data << (*data).col(3);
    y_data << (*data).col(2);
    x_data = x_data.array() / y_data.array();

    // Linear model:--------------------------------------------------------------------
    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data, MatrixXd::Ones(Nsize,1);
    // b << y_data;

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;

    // double m = X(0);
    // double c = X(1);
    // cout<<"slope m: "<<m<<endl<<"const c: "<<c<<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = m*i + c;
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("linear_model.txt", Yi_Xi);

    // Exp model--------------------------------------------------------------------

    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data, MatrixXd::Ones(Nsize,1);
    // b << y_data.array().log();

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;

    // long double a1 = exp(X(1)), b1 = X(0);
    // cout<<"Exp a: "<<  a1 <<"Exp b: "<< b1<<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = a1 * exp(b1*i);
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("exp_model.txt", Yi_Xi);

    // x-Exp model--------------------------------------------------------------------

    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data.array().log() , MatrixXd::Ones(Nsize,1);
    // b << y_data.array().log();

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;

    // double a1 = exp(X(1)), b1 = X(0);
    // cout<<"x-Exp a: "<< a1  <<"x-Exp b: "<< b1 <<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = a1 * pow(i, b1);
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("x_exp_model.txt", Yi_Xi);

    // Loagrithmic model--------------------------------------------------------------------

    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data.array().log() , MatrixXd::Ones(Nsize,1);
    // b << y_data;

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;
    // double a1 = X(0);
    // double b1 = X(1);
    // cout<<"Log a: "<<  a1 <<"Log b: "<< b1 <<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = a1 * log(i) + b1;
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("log_model.txt", Yi_Xi);

    // inv model--------------------------------------------------------------------

    int Msize = 1;
    MatrixXd A(Nsize,Msize);
    MatrixXd b(Nsize,1);

    A << MatrixXd::Ones(Nsize,1);
    b << y_data.array() * x_data.array();

    MatrixXd At = A.transpose();
    MatrixXd X(Msize,1);
    X = (At*A).inverse() * At * b;
    double a1 = X(0);
    cout<<"inv a: "<< a1 <<endl;

    MatrixXd Yi_Xi(350,2);
    for(int i=0; i<350; i++){
        Yi_Xi(i,0) = a1 / i;
        Yi_Xi(i,1) = i;
    }
    write_matrix("inv_model.txt", Yi_Xi);

    return 0;
}

// Supporting functions used:

MatrixXd* read_matrix(string filename){
    ifstream myfile;
    myfile.open(filename);
    if (!myfile.is_open()) throw runtime_error("Could not open file");
    string row_item;
    vector<vector<double> > rows;
    string line;
    while(myfile.good()){
        vector<double> row;
        getline(myfile, line);
        stringstream s(line);
        while (getline(s,row_item,',')){
            row.push_back(stod(row_item));
        }
        rows.push_back(row);
    }
    myfile.close();
    int nrows,ncols;
    nrows = rows.end() - rows.begin();
    ncols = rows[0].end() - rows[0].begin();
    MatrixXd* m = new MatrixXd(nrows,ncols);
    for(int i=0; i<nrows; i++){
        for (int j=0; j<ncols; j++){
            (*m)(i,j) = rows[i][j];
        }
    }
    // cout<<"The following is a matrix of "<<nrows<<" rows and "<<ncols<<" coloumns: "<<endl<<*m<<endl;
    return m;
}

void write_matrix(string filename, MatrixXd m){
    ofstream(myfile);
    myfile.open(filename);
    for(int i=0; i<m.rows(); i++){
        for(int j=0; j<m.cols(); j++){
            myfile << m(i,j) <<",";
        }
        myfile << endl;
    }
}

void print_vector(vector<vector<double> > v){
    vector<vector<double> >::iterator mvp;
    vector<double>::iterator vp;
    for(mvp= v.begin(); mvp!=v.end(); mvp++){
        for(vp = mvp->begin(); vp!= mvp->end(); vp++){
            cout<<*vp<<" ";
        }
        cout<<endl;
    }
}
void print_matrix_vector(vector<double> v){
    vector<double>::iterator it;
    for(it= v.begin(); it!= v.end(); it++){
        cout<<*it<<" ";
    }cout<<endl;
}


//####### Test module for picsar_tables ####################################

//Define Module name
 #define BOOST_TEST_MODULE "containers/picsar_tables"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <vector>
#include <algorithm>
#include <array>

#include "picsar_tables.hpp"

#include "picsar_span.hpp"

using namespace picsar::multi_physics::containers;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-12;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-5;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

double linear_function(double x)
{
    const double m = -3.0;
    const double q = 1.7;
    return m*x + q;
}

double linear_function(double x, double y)
{
    const double a = 2.2;
    const double b = -1.7;
    const double c = -3.1;
    return a*x + b*y + c;
}

const double xmin = -10.0;
const double xmax = 10.0;
const double ymin = -8.0;
const double ymax = 8.0;
const int xsize = 100;
const int ysize = 100;

equispaced_1d_table<double, std::vector<double> > make_1d_table()
{
    std::vector<double> data(xsize);
    std::generate(data.begin(), data.end(),
        [&, n = 0]() mutable {
            double x = xmin+((xmax-xmin)*(n++))/(xsize-1);
            return linear_function(x);
        });

    return equispaced_1d_table<double, std::vector<double>>(xmin, xmax,data);
}

equispaced_2d_table<double, std::vector<double> > make_2d_table()
{
    std::vector<double> data(xsize*ysize);
    for (int i = 0; i < xsize; ++i){
        for (int j = 0; j < ysize; ++j){
            double x = xmin+i*(xmax-xmin)/(xsize-1);
            double y = ymin+j*(ymax-ymin)/(ysize-1);
            data[i*ysize+j] = linear_function(x, y);
        }
    }

    return equispaced_2d_table<double, std::vector<double>>(
        xmin, xmax, ymin, ymax, xsize, ysize, data);
}

// ------------- Tests --------------

void check_table_1d(
    const equispaced_1d_table<double, std::vector<double> >& tab)
{
    const auto rxmin = tab.get_x_min();
    BOOST_CHECK_EQUAL(rxmin, xmin);
    const auto rxmax = tab.get_x_max();
    BOOST_CHECK_EQUAL(rxmax, xmax);
    const auto rxsize = tab.get_x_size();
    BOOST_CHECK_EQUAL(rxsize, xmax-xmin);
    const auto rhowmany =  tab.get_how_many_x();
    BOOST_CHECK_EQUAL(rhowmany, xsize);
    const auto rdx = tab.get_dx();
    BOOST_CHECK_EQUAL(rdx, (xmax-xmin)/(xsize-1));

    const auto first_val = tab.get_values_reference()[0];
    BOOST_CHECK_EQUAL(first_val, linear_function(xmin));

    const auto x0 = tab.get_x_coord(0);
    const auto x1 = tab.get_x_coord(xsize/2);
    const auto x2 = tab.get_x_coord(xsize-1);
    const auto x1exp = (xsize/2)*(xmax - xmin)/(xsize-1) + xmin;
    BOOST_CHECK_SMALL((x0-xmin)/xmin, tolerance<double>());
    BOOST_CHECK_SMALL((x1 - x1exp)/x1exp, tolerance<double>());
    BOOST_CHECK_SMALL((x2 - xmax)/xmax, tolerance<double>());

    const auto x3 = 0.732*(x1-x0) + x0;
    const auto x4 = 0.118*(x2-x1) + x1;

    const auto v0 = tab.get_val(0);
    const auto v1 = tab.get_val(xsize/2);
    const auto v2 = tab.get_val(xsize-1);
    BOOST_CHECK_EQUAL(v0, linear_function(x0));
    BOOST_CHECK_EQUAL(v1, linear_function(x1));
    BOOST_CHECK_EQUAL(v2, linear_function(x2));

    const auto xarr = std::array<double, 5>{x0,x1,x2,x3,x4};
    for (const auto& xx : xarr)
    {
        const auto val = tab.interp(xx);
        const auto expected = linear_function(xx);
        BOOST_CHECK_SMALL((val - expected)/expected, tolerance<double>());
    }

    const auto all_coords = tab.get_all_coordinates();
    BOOST_CHECK_EQUAL(all_coords.size(), xsize);
    for (int i = 0; i < xsize; ++i){
        BOOST_CHECK_EQUAL(all_coords[i], tab.get_x_coord(i));
    }
}

void check_table_2d(
    const equispaced_2d_table<double, std::vector<double> >& tab)
{
    const auto rxmin = tab.get_x_min();
    BOOST_CHECK_EQUAL(rxmin, xmin);
    const auto rxmax = tab.get_x_max();
    BOOST_CHECK_EQUAL(rxmax, xmax);
    const auto rhowmany_x =  tab.get_how_many_x();
    BOOST_CHECK_EQUAL(rhowmany_x, xsize);
    const auto rxsize =  tab.get_x_size();
    BOOST_CHECK_EQUAL(rxsize, xmax-xmin);
    const auto rdx =  tab.get_dx();
    BOOST_CHECK_EQUAL(rdx, (xmax-xmin)/(rhowmany_x-1));
    const auto rymin = tab.get_y_min();
    BOOST_CHECK_EQUAL(rymin, ymin);
    const auto rymax = tab.get_y_max();
    BOOST_CHECK_EQUAL(rymax, ymax);
    const auto rhowmany_y =  tab.get_how_many_y();
    BOOST_CHECK_EQUAL(rhowmany_y, ysize);
    const auto rysize =  tab.get_y_size();
    BOOST_CHECK_EQUAL(rysize, ymax-ymin);
    const auto rdy =  tab.get_dy();
    BOOST_CHECK_EQUAL(rdy, (ymax-ymin)/(rhowmany_y-1));

    const auto first_val = tab.get_values_reference()[0];
    BOOST_CHECK_EQUAL(first_val, linear_function(xmin, ymin));

    const auto x0 = tab.get_x_coord(0);
    const auto x1 = tab.get_x_coord(xsize/2);
    const auto x2 = tab.get_x_coord(xsize-1);
    const auto x1exp = (xsize/2)*(xmax - xmin)/(xsize-1) + xmin;
    BOOST_CHECK_SMALL((x0-xmin)/xmin, tolerance<double>());
    BOOST_CHECK_SMALL((x1 - x1exp)/x1exp, tolerance<double>());
    BOOST_CHECK_SMALL((x2 - xmax)/xmax, tolerance<double>());

    const auto x3 = 0.732*(x1-x0) + x0;
    const auto x4 = 0.118*(x2-x1) + x1;
    const auto xarr = std::array<double, 5>{x0,x1,x2,x3,x4};

    const auto y0 = tab.get_y_coord(0);
    const auto y1 = tab.get_y_coord(ysize/2);
    const auto y2 = tab.get_y_coord(ysize-1);
    const auto y1exp = (ysize/2)*(ymax - ymin)/(ysize-1) + ymin;
    BOOST_CHECK_SMALL((y0-ymin)/ymin, tolerance<double>());
    BOOST_CHECK_SMALL((y1 - y1exp)/y1exp, tolerance<double>());
    BOOST_CHECK_SMALL((y2 - ymax)/ymax, tolerance<double>());

    const auto v00 = tab.get_val(0, 0);
    const auto v10 = tab.get_val(xsize/2, 0);
    const auto v20 = tab.get_val(xsize-1, 0);
    const auto v01 = tab.get_val(0, ysize/2);
    const auto v11 = tab.get_val(xsize/2, ysize/2);
    const auto v21 = tab.get_val(xsize-1, ysize/2);
    const auto v02 = tab.get_val(0, ysize-1);
    const auto v12 = tab.get_val(xsize/2, ysize-1);
    const auto v22 = tab.get_val(xsize-1, ysize-1);
    BOOST_CHECK_EQUAL(v00, linear_function(x0,y0));
    BOOST_CHECK_EQUAL(v10, linear_function(x1,y0));
    BOOST_CHECK_EQUAL(v20, linear_function(x2,y0));
    BOOST_CHECK_EQUAL(v01, linear_function(x0,y1));
    BOOST_CHECK_EQUAL(v11, linear_function(x1,y1));
    BOOST_CHECK_EQUAL(v21, linear_function(x2,y1));
    BOOST_CHECK_EQUAL(v02, linear_function(x0,y2));
    BOOST_CHECK_EQUAL(v12, linear_function(x1,y2));
    BOOST_CHECK_EQUAL(v22, linear_function(x2,y2));

    const auto y3 = 0.8569*(y1-y0) + y0;
    const auto y4 = 0.3467*(y2-y1) + y1;
    const auto yarr = std::array<double, 5>{y0,y1,y2,y3,y4};

    for (const auto& xx : xarr)
    {
        for (const auto& yy : yarr)
        {
            const auto val = tab.interp(xx, yy);
            const auto expected = linear_function(xx, yy);
            BOOST_CHECK_SMALL((val - expected)/expected, tolerance<double>());
        }
    }

    const auto all_coords = tab.get_all_coordinates();
    int count = 0;
    BOOST_CHECK_EQUAL(all_coords.size(), xsize*ysize);
    for (int i = 0; i < xsize; ++i){
        for (int j = 0; j < ysize; ++j){
            auto cc = all_coords[count++];
            BOOST_CHECK_EQUAL(cc[0], tab.get_x_coord(i));
            BOOST_CHECK_EQUAL(cc[1], tab.get_y_coord(j));
        }
    }
}

void check_table_2d_interp_one_coord(
        const equispaced_2d_table<double, std::vector<double> >& tab)
{
    const auto x0 = tab.get_x_coord(0);
    const auto x1 = tab.get_x_coord(xsize/2);
    const auto x2 = tab.get_x_coord(xsize-1);
    const auto x3 = 0.732*(x1-x0) + x0;
    const auto x4 = 0.118*(x2-x1) + x1;
    const auto xarr = std::array<double, 5>{x0,x1,x2,x3,x4};
    const auto jarr = std::array<int, 5>{0,1,5,27,ysize-1};

    for (auto xx : xarr){
        for (auto jj : jarr)
        {
            const auto yy = tab.get_y_coord(jj);
            const auto res = tab.interp_first_coord(xx, jj);
            const auto exp = linear_function(xx, yy);
            BOOST_CHECK_SMALL((res - exp)/exp, tolerance<double>());
        }
    }

    const auto y0 = tab.get_y_coord(0);
    const auto y1 = tab.get_y_coord(ysize/2);
    const auto y2 = tab.get_y_coord(ysize-1);
    const auto y3 = 0.8569*(y1-y0) + y0;
    const auto y4 = 0.3467*(y2-y1) + y1;
    const auto yarr = std::array<double, 5>{y0,y1,y2,y3,y4};
    const auto iarr = std::array<int, 5>{0,1,5,27,xsize-1};

    for (auto yy : yarr){
        for (auto ii : iarr)
        {
            const auto xx = tab.get_x_coord(ii);
            const auto res = tab.interp_second_coord(ii, yy);
            const auto exp = linear_function(xx, yy);
            BOOST_CHECK_SMALL((res - exp)/exp, tolerance<double>());
        }
    }
}

// ***Test equispaced_1d_table constructor and getters
BOOST_AUTO_TEST_CASE( picsar_equispaced_1d_table_constructor_getters)
{
    auto tab_1d = make_1d_table();
    const auto const_tab_1d = make_1d_table();
    auto copy_tab_1d = tab_1d;

    check_table_1d(tab_1d);
    check_table_1d(const_tab_1d);
    check_table_1d(copy_tab_1d);
}

// ***Test equispaced_1d_table setter
BOOST_AUTO_TEST_CASE( picsar_equispaced_1d_table_constructor_setter)
{
    auto tab_1d = make_1d_table();
    const auto val = 1000.0;
    const int where = 10;
    tab_1d.set_val(where, val);
    const auto x10 = tab_1d.get_x_coord(where);
    const auto res =tab_1d.interp(x10);

    BOOST_CHECK_SMALL((res - val)/val, tolerance<double>());

}

// ***Test equispaced_1d_table equality
BOOST_AUTO_TEST_CASE( picsar_equispaced_1d_table_equality)
{
    auto tab_1d = make_1d_table();
    auto tab_1d_2 = make_1d_table();
    BOOST_CHECK_EQUAL(tab_1d == tab_1d_2, true);

    tab_1d.set_val(1, 3.14);
    BOOST_CHECK_EQUAL(tab_1d == tab_1d_2, false);
}

// ***Test equispaced_1d_table serialization
BOOST_AUTO_TEST_CASE( picsar_equispaced_1d_table_serialization)
{
    auto tab_1d = make_1d_table();
    auto raw_data = tab_1d.serialize();
    auto tab_1d_2 = equispaced_1d_table<double, std::vector<double>>{raw_data};

    BOOST_CHECK_EQUAL(tab_1d.get_x_min(), tab_1d_2.get_x_min());
    BOOST_CHECK_EQUAL(tab_1d.get_x_max(), tab_1d_2.get_x_max());
    BOOST_CHECK_EQUAL(tab_1d.get_x_size(), tab_1d_2.get_x_size());
    BOOST_CHECK_EQUAL(tab_1d.get_how_many_x(), tab_1d_2.get_how_many_x());

    for(int i = 0; i < tab_1d.get_how_many_x(); ++i){
        BOOST_CHECK_EQUAL(tab_1d.get_val(i), tab_1d_2.get_val(i));
    }
}

// ***Test equispaced_2d_table constructor and getters
BOOST_AUTO_TEST_CASE( picsar_equispaced_2d_table_constructor_getters)
{
    auto tab_2d = make_2d_table();
    const auto const_tab_2d = make_2d_table();
    auto copy_tab_2d = tab_2d;

    check_table_2d(tab_2d);
    check_table_2d(const_tab_2d);
    check_table_2d(copy_tab_2d);
}

// ***Test equispaced_2d_table setter
BOOST_AUTO_TEST_CASE( picsar_equispaced_2d_table_interp_one_coord)
{
    const auto const_tab_2d = make_2d_table();

    check_table_2d_interp_one_coord(const_tab_2d);
}

// ***Test equispaced_2d_table setter
BOOST_AUTO_TEST_CASE( picsar_equispaced_2d_table_constructor_setter)
{
    auto tab_2d = make_2d_table();
    const auto val = 1000.0;
    const int i = 10;
    const int j = 20;
    tab_2d.set_val(i,j,val);
    const auto xx = tab_2d.get_x_coord(i);
    const auto yy = tab_2d.get_y_coord(j);
    const auto res =tab_2d.interp(xx,yy);

    BOOST_CHECK_SMALL((res - val)/val, tolerance<double>());
}

// ***Test equispaced_2_table equality
BOOST_AUTO_TEST_CASE( picsar_equispaced_2d_table_equality)
{
    auto tab_2d = make_2d_table();
    auto tab_2d_2 = make_2d_table();
    BOOST_CHECK_EQUAL(tab_2d == tab_2d_2, true);

    tab_2d.set_val(0, 10.0);
    BOOST_CHECK_EQUAL(tab_2d == tab_2d_2, false);
}

// ***Test equispaced_2d_table serialization
BOOST_AUTO_TEST_CASE( picsar_equispaced_2d_table_serialization)
{
    auto tab_2d = make_2d_table();
    auto raw_data = tab_2d.serialize();
    auto tab_2d_2 = equispaced_2d_table<double, std::vector<double>>{raw_data};

    BOOST_CHECK_EQUAL(tab_2d.get_x_min(), tab_2d_2.get_x_min());
    BOOST_CHECK_EQUAL(tab_2d.get_x_max(), tab_2d_2.get_x_max());
    BOOST_CHECK_EQUAL(tab_2d.get_y_min(), tab_2d_2.get_y_min());
    BOOST_CHECK_EQUAL(tab_2d.get_y_max(), tab_2d_2.get_y_max());
    BOOST_CHECK_EQUAL(tab_2d.get_x_size(), tab_2d_2.get_x_size());
    BOOST_CHECK_EQUAL(tab_2d.get_y_size(), tab_2d_2.get_y_size());
    BOOST_CHECK_EQUAL(tab_2d.get_how_many_x(), tab_2d_2.get_how_many_x());
    BOOST_CHECK_EQUAL(tab_2d.get_how_many_y(), tab_2d_2.get_how_many_y());

    for(int i = 0; i < tab_2d.get_how_many_x()*tab_2d.get_how_many_x(); ++i){
        BOOST_CHECK_EQUAL(tab_2d.get_values_reference()[i],
            tab_2d_2.get_values_reference()[i]);
    }
}


#include <chrono>
#include <functional>
#include <iostream>
#include <vector>

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

std::vector<Eigen::Triplet<double>> MakeTripletList(int n) {
	int nnz = 3 * n - 2;
	std::vector<Eigen::Triplet<double>> tripletList(nnz);

	for(int i = 0; i < n; i++) {
		tripletList.push_back(Eigen::Triplet<double>(i, i, 2));

		if(i > 0) {
			tripletList.push_back(Eigen::Triplet<double>(i, i - 1, -1));
		}

		if(i < n - 1) {
			tripletList.push_back(Eigen::Triplet<double>(i, i + 1, -1));
		}
	}

	return tripletList;
}

double Runtime(const std::function<void(void)> &f) {
	double runtime = std::numeric_limits<double>::max();
	std::chrono::duration<double, std::ratio<1> > duration;
	std::chrono::time_point<std::chrono::high_resolution_clock> start, end;

	for(int i = 0; i < 10; i++) {
		start = std::chrono::high_resolution_clock::now();
		f();
		end = std::chrono::high_resolution_clock::now();

		duration = end - start;
		runtime = std::min(runtime, duration.count());
	}

	return runtime;
}

template <class T>
std::ostream & operator<< (std::ostream &os, const std::vector<T> &v) {
	os << "[";
	if (!v.empty()) {
		os << v[0];
		for (int i = 1; i < v.size(); ++i) os << ", " << v[i];
	}
    os << "]";

    return os;
}

int main() {
	// print small example of the tridiagonal matrix
	int m = 4;
	std::vector<Eigen::Triplet<double>> tripletList = MakeTripletList(m);
	Eigen::SparseMatrix<double> S_(m, m);
	S_.setFromTriplets(tripletList.begin(), tripletList.end());
	std::cout << "If n = " << m << ", then T equals" << std::endl;
	std::cout << Eigen::MatrixXd(S_) << std::endl;

	// matrix sizes for benchmark
	std::vector<int> N = {64, 128, 256, 512};
	std::cout << "LU decomposition of T, where n = " << N << std::endl;

	// set up variables for runtime measurement
	std::vector<double> runtimeSparse;
	std::vector<double> runtimeDense;

	for (int n : N) {
		tripletList = MakeTripletList(n);

		// sparse LU decomposition
		Eigen::SparseMatrix<double> S(n, n);
		S.setFromTriplets(tripletList.begin(), tripletList.end());

		runtimeSparse.push_back(Runtime([&]() {
			Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
			solver.analyzePattern(S);
			solver.factorize(S);
		}));

		// dense LU decomposition
		Eigen::MatrixXd D(S);

		runtimeDense.push_back(Runtime([&]() {
			D.fullPivLu();
		}));
	}

	std::cout << "Runtime in seconds using storage format..." << std::endl;
	std::cout << "...sparse: " << runtimeSparse << std::endl;
	std::cout << "...dense:  " << runtimeDense << std::endl;

	return 0;
}


#include "MathUtil.h"
#include <Eigen/Geometry>
#include "Intersection.h"

namespace ptgl {

// https://github.com/s-nakaoka/choreonoid/src/Util/EigenUtil.cpp
Eigen::Matrix3d rotFromRpy(double r, double p, double y)
{
    const double cr = cos(r);
    const double sr = sin(r);
    const double cp = cos(p);
    const double sp = sin(p);
    const double cy = cos(y);
    const double sy = sin(y);

    Eigen::Matrix3d R;
    R << cp*cy, sr*sp*cy - cr*sy, cr*sp*cy + sr*sy,
         cp*sy, sr*sp*sy + cr*cy, cr*sp*sy - sr*cy,
         -sp  , sr*cp           , cr*cp;

    return R;
}

// https://github.com/s-nakaoka/choreonoid/src/Util/EigenUtil.cpp
Eigen::Vector3d omegaFromRot(const Eigen::Matrix3d& R)
{
    double alpha = (R(0,0) + R(1,1) + R(2,2) - 1.0) / 2.0;

    if(fabs(alpha - 1.0) < 1.0e-6) {   //th=0,2PI;
        return Eigen::Vector3d::Zero();

    } else {
        double th = acos(alpha);
        double s = sin(th);

        if (s < std::numeric_limits<double>::epsilon()) {   //th=PI
            return Eigen::Vector3d( sqrt((R(0,0)+1)*0.5)*th, sqrt((R(1,1)+1)*0.5)*th, sqrt((R(2,2)+1)*0.5)*th );
        }

        double k = -0.5 * th / s;

        return Eigen::Vector3d((R(1,2) - R(2,1)) * k,
                       (R(2,0) - R(0,2)) * k,
                       (R(0,1) - R(1,0)) * k);
    }
}

Eigen::Matrix3d rotFromAxisFromTo(const Eigen::Vector3d& from, const Eigen::Vector3d& to, const Eigen::Vector3d& axis)
{
    Eigen::Vector3d p0 = ptgl::calcIntersectionPointAndLine(from, Eigen::Vector3d::Zero(), axis);
    Eigen::Vector3d from0 = from - p0;
    Eigen::Vector3d to0 = to - p0;
    return rotFromTo(from0, to0);
}

Eigen::Matrix3d rotFromToSet(const Eigen::Vector3d& vecA0, const Eigen::Vector3d& vecB0, const Eigen::Vector3d& vecA1, const Eigen::Vector3d& vecB1)
{
    Eigen::Matrix3d R0 = ptgl::rotFromTo(vecA0, vecA1);
    Eigen::Vector3d tempVecB = R0 * vecB0;
    return rotFromAxisFromTo(tempVecB, vecB1, vecA1) * R0;
}

double angleFromAxisFromTo(const Eigen::Vector3d& from, const Eigen::Vector3d& to, const Eigen::Vector3d& axis) {
    Eigen::Vector3d p0 = ptgl::calcIntersectionPointAndLine(from, Eigen::Vector3d::Zero(), axis);
    Eigen::Vector3d from0 = from - p0;
    Eigen::Vector3d to0 = to - p0;

    Eigen::Quaterniond q;
    q.setFromTwoVectors(from0, to0);

    Eigen::AngleAxisd aa(q);
    Eigen::Vector3d c = aa.axis();
    double angle = aa.angle();

    Eigen::Vector3d d1 = c - axis;
    Eigen::Vector3d d2 = c + axis;
    return d1.squaredNorm() < d2.squaredNorm() ? angle : -angle;
};

double angleFromDirectionFromTo(const Eigen::Vector3d& from, const Eigen::Vector3d& to, const Eigen::Vector3d& direction) {
    Eigen::Quaterniond q;
    q.setFromTwoVectors(from, to);

    Eigen::AngleAxisd aa(q);
    Eigen::Vector3d c = aa.axis();
    double angle = aa.angle();

    Eigen::Vector3d d1 = c - direction;
    Eigen::Vector3d d2 = c + direction;
    return d1.squaredNorm() < d2.squaredNorm() ? angle : -angle;
};

// glm/gtc/matrix_transform.inl ortho
Eigen::Matrix4d ortho(double left, double right, double bottom, double top)
{
    Eigen::Matrix4d Result(Eigen::Matrix4d::Identity());
    Result(0,0) = 2.0 / (right - left);
    Result(1,1) = 2.0 / (top - bottom);
    Result(0,3) = - (right + left) / (right - left);
    Result(1,3) = - (top + bottom) / (top - bottom);
    return Result;
}

// glm/gtc/matrix_transform.inl  orthoRH_NO
Eigen::Matrix4d ortho(double left, double right, double bottom, double top, double zNear, double zFar)
{
    Eigen::Matrix4d Result(Eigen::Matrix4d::Identity());
    Result(0,0) = 2.0 / (right - left);
    Result(1,1) = 2.0 / (top - bottom);
    Result(2,2) = - 2.0 / (zFar - zNear);
    Result(0,3) = - (right + left) / (right - left);
    Result(1,3) = - (top + bottom) / (top - bottom);
    Result(2,3) = - (zFar + zNear) / (zFar - zNear);
    return Result;
}

// project/unproject

// glm/gtc/matrix_transform.inl  projectNO
// object(x,y,z) -> window(x,y,z)
Eigen::Vector3d project(const Eigen::Vector3d& object_p, const Eigen::Matrix4d& modelMatrix, const Eigen::Matrix4d& projMatrix, const Eigen::Vector4d& viewport)
{
    Eigen::Vector4d tmp = Eigen::Vector4d(object_p(0), object_p(1), object_p(2), 1.0);
    tmp = Eigen::Vector4d(modelMatrix * tmp);
    tmp = Eigen::Vector4d(projMatrix * tmp);

    tmp /= tmp.w();
    tmp = tmp.array() * 0.5 + 0.5;
    tmp[0] = tmp[0] * viewport[2] + viewport[0];
    tmp[1] = tmp[1] * viewport[3] + viewport[1];
    return Eigen::Vector3d(tmp[0], tmp[1], tmp[2]);
}

// glm/gtc/matrix_transform.inl  unProjectNO
// window(x,y,z) -> object(x,y,z)
Eigen::Vector3d unProject(const Eigen::Vector3d& wp, const Eigen::Matrix4d& modelMatrix, const Eigen::Matrix4d& projMatrix, const Eigen::Vector4d& viewport)
{
    Eigen::Matrix4d Inverse = Eigen::Matrix4d(projMatrix * modelMatrix).inverse();

    Eigen::Vector4d tmp = Eigen::Vector4d(wp(0), wp(1), wp(2), 1.0);
    tmp(0) = (tmp(0) - viewport[0]) / viewport[2];
    tmp(1) = (tmp(1) - viewport[1]) / viewport[3];
    tmp = Eigen::Vector4d(tmp.array() * 2.0 - 1.0);

    Eigen::Vector4d obj = Inverse * tmp;
    obj /= obj.w();

    return Eigen::Vector3d(obj[0], obj[1], obj[2]);
}

} /* namespace ptgl */


#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include "mass_matrix.hpp"

//----------------AssembleMatrixBegin----------------
//! Assemble the mass matrix
//! for the linear system.
//!
//!
//! @param[out] triplets will at the end contain the Galerkin matrix
//! @param[in] vertices a list of triangle vertices
//! @param[in] triangles a list of triangles
SparseMatrix assembleMassMatrix(
    const Eigen::MatrixXd &vertices,
    const Eigen::MatrixXi &triangles) {
	std::vector<Triplet> triplets;
	const int            numberOfElements = triangles.rows();
	SparseMatrix         M(vertices.rows(), vertices.rows());

	// (write your solution here)
	triplets.reserve(numberOfElements * 3 * 3);
	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = triangles.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));
		const auto &c = vertices.row(indexSet(2));

		Eigen::Matrix3d massMatrix;
		computeMassMatrix(massMatrix, a, b, c);

		for (int n = 0; n < 3; ++n) {
			for (int m = 0; m < 3; ++m) {
				triplets.emplace_back(indexSet(n), indexSet(m), massMatrix(n, m));
			}
		}
	}

	M.setFromTriplets(triplets.begin(), triplets.end());

	return M;
}
//----------------AssembleMatrixEnd----------------


/*=================================================================
* Create rotation around a line by an angle theta
*
* according to David Legland's matlab toolbox: geom3d
*
* Junjie Cao, 2013, jjcao1231@gmail.com
*
=================================================================*/

#include <mex.h>
#include <Eigen/Dense>
#include <vector>

Eigen::Matrix4d create_translation3d(Eigen::Vector3d center)
{
	double dx(center.coeff(0)),dy(center.coeff(1)),dz(center.coeff(2));
	Eigen::Matrix4d trans;
	trans << 1,0,0,dx,
		    0,1,0,dy,
			0,0,1,dz,
			0,0,0,1;
	return trans;
}
Eigen::Matrix4d recenter_transform3d(Eigen::Matrix4d &transfo, Eigen::Vector3d& center)
{
	//% remove former translation part
	Eigen::Matrix4d res = Eigen::Matrix4d::Identity();
	res.block(0, 0, 3, 3) = transfo.block(0, 0, 3, 3);

	//% create translations
	Eigen::Matrix4d t1 = create_translation3d(-center);
	Eigen::Matrix4d t2 = create_translation3d(center);

	//% compute translated transform
	res = t2*res*t1;
	return res;
}
Eigen::Matrix4d create_rotation3d_line_angle(Eigen::Vector3d& center,Eigen::Vector3d& v, double theta)
{
	//% normalize vector
	v.normalize();

	//% compute projection matrix P and anti-projection matrix
	Eigen::Matrix3d P = v * v.transpose();
	Eigen::Matrix3d Q;
	Q << 0, -v.coeff(2), v.coeff(1),
		v.coeff(2), 0, -v.coeff(0),
		-v.coeff(1), v.coeff(0), 0;
	Eigen::Matrix3d I = Eigen::Matrix3d::Identity();

	//% compute vectorial part of the transform
	Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();
	mat.block(0, 0, 3, 3) = P + (I - P)*cos(theta) + Q*sin(theta);

	//% add translation coefficient
	mat = recenter_transform3d(mat, center);

	return mat;
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
	////////////////// parse input
	if ( nrhs != 3)
		mexErrMsgTxt("3 arguments needed!");

	double *center_, *vector_, *theta_;
	center_ = mxGetPr(prhs[0]);
	vector_ = mxGetPr(prhs[1]);
	theta_ = mxGetPr(prhs[2]);

	if ( mxGetN(prhs[0]) != 3)
		mexErrMsgTxt("center must be 1*3 matrix!");
	if ( mxGetN(prhs[1]) != 3)
		mexErrMsgTxt("vector must be 1*3 matrix!");

	Eigen::Vector3d center = Eigen::Map<Eigen::Vector3d>(center_);
	Eigen::Vector3d v = Eigen::Map<Eigen::Vector3d>(vector_);

	/////////////////////////////
	Eigen::Matrix4d rot = create_rotation3d_line_angle(center,v,*theta_);

	//////////////////////// output
	plhs[0] = mxCreateDoubleMatrix(4,4,mxREAL);
	double *rot_ = mxGetPr(plhs[0]);
	Eigen::Map<Eigen::Matrix4d>(rot_, 4, 4) = rot;  // may contain mem error
}


/**
 * @file TestProblem01.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-06
 */

#pragma once

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

#include <solver/interface/NlpDescription.hpp>

namespace nlp_test_problems
{

  class TestProblem01 : public solver::NlpDescription
  {
    public:
	  TestProblem01(){};
      virtual ~TestProblem01(){};

      // definition of problem size
      void getNlpParameters(int& n_vars, int& n_cons)
      {
        n_vars = 4;
        n_cons = 2;
      }

      // definition of problem box constraints
      void getNlpBounds(int n_vars, int /*n_cons*/, double* x_l, double* x_u, double* g_l, double* g_u)
      {
        // lower and upper bounds on variables
        for (int id=0; id<n_vars; id++)
        {
          x_l[id] = 1.0;
          x_u[id] = 5.0;
        }

    	    // lower and upper bounds on inequality constraints
    	    g_l[0] = 25;
    	    g_u[0] = std::numeric_limits<double>::infinity();

    	    // lower and upper bounds on equality constraints
    	    g_l[1] = g_u[1] = 40.0;
      }

      // definition of starting point
      void getStartingPoint(int /*n_vars*/, double* x)
      {
    	    x[0] = 1.0;
    	    x[1] = 5.0;
    	    x[2] = 5.0;
    	    x[3] = 1.0;
      }

      // definition of objective function
      double evaluateObjective(int /*n_vars*/, const double* x)
      {
    	    return x[0]*x[3]*( x[0]+x[1]+x[2] ) + x[2];
      }

      // definition of constraints function
      void evaluateConstraintsVector(int /*n_vars*/, int /*n_cons*/, const double* x, double* constraints)
      {
    	    constraints[0] = x[0]*x[1]*x[2]*x[3];
    	    constraints[1] = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3];
      }
  };
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;
#include "boundary.hpp"
#include "mass_matrix_assembly.hpp"
#include "neumann_boundary_assemble.hpp"
#include "stiffness_matrix_assembly.hpp"

//! Evolves the solution in time using the DIRK2 method.
//!
//! @param vertices the list of vertices that build the triangular mesh
//! @param triangles the list of indices that represent the triangular mesh
//! @param u0 the initial data
//! @param gamma the parameter gamma (has to do with the boundary conditions)
//! @param m number of timesteps to perform
//! @returns a pair which contains as first parameter the solution at time = 1,
//!          and second parameter the energy evolving over time.
std::pair<Eigen::VectorXd, std::vector<double>> radiativeTimeEvolutionImplicit(const Eigen::MatrixXd &vertices,
                                                                               const Eigen::MatrixXi &triangles,
                                                                               const Eigen::VectorXd &u0,
                                                                               const double           gamma,
                                                                               const int              m) {
	std::vector<double> energy;
	Eigen::VectorXd     u(u0.size());
	u = u0;

	double dt     = 1.0 / m;
	double lambda = 1 - 0.5 * std::sqrt(2);

	// Get the edges, this is used to compute the neumann boundary matrix
	Eigen::MatrixXi edges = getBoundaryEdges(vertices, triangles);

	// Define the needed matrices. Note that these are constant in time,
	// so they can be defined before the time loop. Also set up the solver
	// (write your solution here)

	SparseMatrix M = assembleMassMatrix(vertices, triangles);
	SparseMatrix A = assembleStiffnessMatrix(vertices, triangles);
	SparseMatrix B = assembleBoundaryMatrix(vertices, edges, gamma);
	A += B;

	Eigen::SimplicialLDLT<SparseMatrix> Solver;
	Solver.compute(M + dt * lambda * A);

	if (Solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose matix for SDIRK");
	}

	auto solveY1 = [&](const Eigen::VectorXd &mu) -> Eigen::VectorXd {
		return Solver.solve(M * mu);
	};

	auto solveY2 = [&](const Eigen::VectorXd &mu, const Eigen::VectorXd &Y1) -> Eigen::VectorXd {
		return Solver.solve(M * mu - dt * (1 - lambda) * A * Y1);
	};

	Eigen::SimplicialLDLT<SparseMatrix> SolverM;
	SolverM.compute(M);

	if (SolverM.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose matrix M");
	}

	Eigen::VectorXd Y1(u0.rows());
	Eigen::VectorXd Y2(u0.rows());
	for (int timestep = 0; timestep < m; ++timestep) {
		// Do one step forward in time
		// (write your solution here)
		Y1 = solveY1(u);
		Y2 = solveY2(u, Y1);
		u  = SolverM.solve(M * u - dt * A * ((1 - lambda) * Y1 + lambda * Y2)).eval();
		// Compute the energy for this level
		energy.push_back(u.sum() / u.rows());
	}

	return std::make_pair(u, energy);
}


#include <Eigen/Dense>
#include <cmath>
#include <vector>
#include "Adagrad.hpp"

#define EIGEN_MPL2_ONLY
using namespace Eigen;
using namespace std;

double Adagrad::sigma(const MatrixXd& _x, int i){
  return sigmoid(_x.row(i).dot(w));
}

double Adagrad::sigmoid(double z){
  return 1.0 / (1.0 + exp(-z));
}

Adagrad::Adagrad(int _N,int _d,MatrixXd _x,VectorXd _label,double _C,double _eta,int iter):
  N(_N),
  d(_d),
  X(_x),
  label(_label),
  C(_C),
  eta(_eta),
  iteration(iter),
  E(VectorXd::Zero(d)),
  w(VectorXd::Random(d))
{}

double Adagrad::Acc(vector<double>& pred,VectorXd &l){
  int t =0;
  double loss =0;
  for(int i = 0; i< pred.size();i++){
    loss += (l(i) - pred[i]) * (l(i) - pred[i]);
    int s = pred[i] > 0.5 ? 1 : 0;
    if(s == l(i)){
      t++;
    }
  }
  return (double)t/pred.size();
}

void Adagrad::train(){
  for(int iter = 0; iter< iteration; iter++){
    for(int i = 0; i < N; i++){
      double pred = sigma(X,i);
      for(int idx = 0; idx < d; idx++){
        if(X(i,idx) != 0){
          double grad = (pred - label(i)) * X(i,idx) + C * w(idx);
          E(idx) +=grad * grad;
          w(idx) -= (eta/sqrt(E(idx)))*grad;
        }
      }
    }
    vector<double> ret;
    predict(X,label,ret);
    iterscores.push_back(Acc(ret,label));
  }
}
void Adagrad::predict(MatrixXd& _x,VectorXd& _l,vector<double>& ret){
  for(int i = 0; i < _x.rows(); i++){
    ret.push_back(sigma(_x,i));
  }
}


#pragma once

#include <Eigen/Dense>

using Eigen::MatrixBase;
using Eigen::MatrixXd;
using Eigen::Ref;
using Eigen::VectorXd;

struct Params {
    VectorXd weight;
    double bias;
};

namespace LinearRegression {

class Core {
public:
    Core(double learning_rate, double weight_decay) noexcept;

    const Params& get_params() const;

    void set_params(const Params& params);

    void init_params(const Ref<const MatrixXd> input, const Ref<const VectorXd> target);

    VectorXd compute_prediction(const Ref<const MatrixXd> input) const;

    double compute_cost(const Ref<const VectorXd> prediction, const Ref<const VectorXd> target) const;

    void optimize_step(const Ref<const MatrixXd> input, const Ref<const VectorXd> prediction, const Ref<const VectorXd> target);

private:
    Params compute_grads(const Ref<const MatrixXd> input, const Ref<const VectorXd> error) const;

    void update_params(const Params& grads);

    Params params;
    const double learning_rate;
    const double weight_decay;
};

}


// Released under the terms of the BSD License
// (C) 2014-2017
//   Analog Devices, Inc
//   Kevin Mehall <km@kevinmehall.net>
//   Ian Daniher <itdaniher@gmail.com>

#include <cmath>
#include <vector>

#include "debug.hpp"
#include <libsmu/libsmu.hpp>
#include <boost/math/constants/constants.hpp>

const double PI = boost::math::constants::pi<double>();

using namespace smu;

void Signal::constant(std::vector<float>& buf, uint64_t samples, float val)
{
	m_src = CONSTANT;
	m_src_v1 = val;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::square(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase, double duty)
{
	m_src = SQUARE;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;
	m_src_duty = duty;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::sawtooth(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = SAWTOOTH;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::stairstep(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = STAIRSTEP;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::sine(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = SINE;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::triangle(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = TRIANGLE;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

// Internal function to generate waveform values.
float Signal::get_sample()
{
	switch (m_src) {
		case CONSTANT:
			return m_src_v1;

		case SQUARE:
		case SAWTOOTH:
		case SINE:
		case STAIRSTEP:
		case TRIANGLE: {

			auto peak_to_peak = m_src_v2 - m_src_v1;
			auto phase = m_src_phase;
			auto norm_phase = phase / m_src_period;
			if (norm_phase < 0)
				norm_phase += 1;
			m_src_phase = fmod(m_src_phase + 1, m_src_period);

			switch (m_src) {
				case SQUARE:
					return (norm_phase < m_src_duty) ? m_src_v1 : m_src_v2;

				case SAWTOOTH: {
					float int_period = truncf(m_src_period);
					float int_phase = truncf(phase);
					float frac_period = m_src_period - int_period;
					float frac_phase = phase - int_phase;
					float max_int_phase;

					// Get the integer part of the maximum value phase will be set at.
					// For example:
					// - If m_src_period = 100.6, phase first value = 0.3 then
					//   phase will take values: 0.3, 1.3, ..., 98.3, 99.3, 100.3
					// - If m_src_period = 100.6, phase first value = 0.7 then
					//   phase will take values: 0.7, 1.7, ..., 98.7, 99.7
					if (frac_period <= frac_phase)
						max_int_phase = int_period - 1;
					else
						max_int_phase = int_period;
                    auto nphase = int_phase / max_int_phase;
                    if(nphase < 0)
                        nphase += 1;
                    return m_src_v2 - nphase * peak_to_peak;
				}

				case STAIRSTEP:
					return m_src_v2 - floorf(norm_phase * 10) * peak_to_peak / 9;

				case SINE:
                    return m_src_v1 + (1 + cos(norm_phase * 2 * PI)) * peak_to_peak / 2;

				case TRIANGLE:
					return m_src_v1 + fabs(1 - norm_phase * 2) * peak_to_peak;
				default:
					throw std::runtime_error("unknown waveform");
			}
		}
	}
	throw std::runtime_error("unknown waveform");
}


//####### Test module for mathematical constants ##################

//Define Module name
 #define BOOST_TEST_MODULE "math/constants"

#include <cmath>

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include "math_constants.h"

using namespace picsar::multi_physics::math;

// ------------- Tests --------------

// ***Test math constants

template<typename RealType>
void test_case_const_math()
{
    const auto exp_pi =
        static_cast<RealType>(3.14159265358979323846264338327950288);

    const auto exp_zero = static_cast<RealType>(0.0);
    const auto exp_half = static_cast<RealType>(0.5);
    const auto exp_one = static_cast<RealType>(1.0);
    const auto exp_two = static_cast<RealType>(2.0);
    const auto exp_three = static_cast<RealType>(3.0);
    const auto exp_four = static_cast<RealType>(4.0);
    const auto exp_one_third = static_cast<RealType>(1.0/3.0);
    const auto exp_two_thirds = static_cast<RealType>(2.0/3.0);
    const auto exp_five_thirds = static_cast<RealType>(5.0/3.0);

    BOOST_CHECK_EQUAL(pi<RealType>, exp_pi);
    BOOST_CHECK_EQUAL(zero<RealType>, exp_zero);
    BOOST_CHECK_EQUAL(half<RealType>, exp_half);
    BOOST_CHECK_EQUAL(one<RealType>, exp_one);
    BOOST_CHECK_EQUAL(two<RealType>, exp_two);
    BOOST_CHECK_EQUAL(three<RealType>, exp_three);
    BOOST_CHECK_EQUAL(four<RealType>, exp_four);
    BOOST_CHECK_EQUAL(one_third<RealType>, exp_one_third);
    BOOST_CHECK_EQUAL(two_thirds<RealType>, exp_two_thirds);
    BOOST_CHECK_EQUAL(five_thirds<RealType>, exp_five_thirds);
}

BOOST_AUTO_TEST_CASE( picsar_const_math )
{
    test_case_const_math<double>();
    test_case_const_math<float>();
}

// *******************************


#include "Optimize.h"

#include <armadillo>
#include <functional>

arma::vec Gradient_Approximate( const arma::vec & x,
								std::function<double( const arma::vec & )> func,
								double resolution )
{
	arma::vec gradient_vector = arma::zeros( x.size() );
	for( auto i = 0; i < x.size(); i++ )
	{
		arma::vec offset = arma::zeros<arma::vec>( x.size() );
		offset( i ) = resolution;
		//double debug1 = func( x + offset );
		//double debug2 = func( x - offset );

		//gradient_vector( i ) = (func( x + offset ) - func( x - offset )) / (2 * resolution);
		gradient_vector( i ) = (func( x + offset ) - func( x - offset ));// / (2 * resolution);
	}

	return gradient_vector;
}

arma::mat Hessian_Approximate( const arma::vec & x,
							   std::function<double( const arma::vec & )> func,
							   double resolution )
{
	arma::mat hessian_matrix( x.size(), x.size() );
	const double denominator = 4 * resolution * resolution;
	for( int j = 0; j < x.size(); j++ )
	{
		arma::vec offset_j = arma::zeros<arma::vec>( x.size() );
		offset_j( j ) = resolution;
		for( int i = 0; i <= j; i++ )
		{
			arma::vec offset_i = arma::zeros<arma::vec>( x.size() );
			offset_i( i ) = resolution;
			//double debug1 = func( x + offset_i + offset_j );
			//double debug2 = func( x + offset_i - offset_j );
			//double debug3 = func( x - offset_i + offset_j );
			//double debug4 = func( x - offset_i - offset_j );
			double delta_f = func( x + offset_i + offset_j ) - func( x + offset_i - offset_j ) - func( x - offset_i + offset_j ) + func( x - offset_i - offset_j );
			if( abs( delta_f ) < 1E-9 )
				int i = 0;
			//double debug5 = delta_f / denominator;
			double result = delta_f;// / denominator;
			hessian_matrix( i, j ) = result;
			hessian_matrix( j, i ) = result;
		}
	}

	return hessian_matrix;
}

arma::vec Minimize_Function_Starting_Point( std::function<double( const arma::vec & )> function_to_minimize,
											const arma::vec & starting_point,
											int max_iteration_count,
											double attenuation_coefficient,
											double resolution,
											double biggest_step_size,
											std::function<void( arma::vec )> iteration_finished_callback )
{
	arma::vec current_guess = starting_point;
	arma::vec previous_direction = arma::zeros( current_guess.size() );
	for( int i = 0; i < max_iteration_count; i++ )
	{
		std::cout << "current_guess: " << function_to_minimize( current_guess ) << " =";
		for( double x : current_guess )
			std::cout << " " << x;
		std::cout << std::endl;

		arma::vec gradient = Gradient_Approximate( current_guess, function_to_minimize, resolution );
		//gradient = gradient( arma::span( 0, 1 ) );
		//std::cout << "gradient: " << gradient << std::endl;
		//arma::mat hessian = Hessian_Approximate( current_guess, function_to_minimize, resolution );
		////hessian = hessian( arma::span( 0, 1 ), arma::span( 0, 1 ) );
		////std::cout << "hessian: " << hessian << std::endl;
		//arma::mat inverse_hessian;
		//try
		//{
		//	inverse_hessian = arma::inv( hessian );
		//}
		//catch( ... )
		//{
		//	current_guess = 1.1 * current_guess;
		//	continue;
		//}
		//std::cout << "inverse_hessian: " << inverse_hessian << std::endl;

		//arma::vec move_vector = -attenuation_coefficient * (inverse_hessian * gradient) * (2 * resolution);
		//if( arma::norm( move_vector ) > 10 * resolution )
		//	move_vector = move_vector / arma::norm( move_vector ) * 10 * resolution;
		//for( int j = 0; j < current_guess.size(); j++ )
		//{
		//	if( abs( move_vector( j ) ) > 0.0005 * abs( current_guess( j ) ) )
		//		move_vector( j ) = 0.0005 * abs( current_guess( j ) ) * move_vector( j ) / abs( move_vector( j ) );
		//}
		arma::vec to_zero = -gradient / (2 * resolution) / function_to_minimize( current_guess );
		arma::vec move_vector = std::min( arma::norm( to_zero ), biggest_step_size ) * arma::normalise( to_zero );
		//double length = arma::norm( gradient ) / (2 * resolution);
		//arma::vec move_vector = -arma::normalise( gradient ) * std::min( length, 1000 * resolution );
		//arma::vec extend_thing = { 0, 0, 0 };
		//extend_thing( arma::span( 0, 1 ) ) = move_vector;
		//move_vector = extend_thing;
		//move_vector( 2 ) = 0;
		std::cout << "Move vector: ";
		for( double x : move_vector )
			std::cout << " " << x;
		std::cout << std::endl;
		current_guess = current_guess + move_vector;
		iteration_finished_callback( current_guess );
		//std::cout << "current_guess: " << current_guess( 0 ) << " " << current_guess( 1 ) << std::endl;
		if( arma::dot( move_vector, move_vector ) < resolution * resolution )
			break; // Quit out if we are barely moving anymore
		if( arma::dot( move_vector, previous_direction ) < 0 )
			biggest_step_size *= 0.9;

		previous_direction = move_vector;
	}

	return current_guess;
}

arma::mat Minimize_Function( std::function<double( const arma::vec & )> function_to_minimize,
							 const std::vector< std::tuple<double, double> > & bounds,
							 const int max_iteration_count )
{
	return arma::mat();
}

double Newtons_Method( std::function<double( double )> func, std::function<double( double )> derivative, double starting_point, double resolution, int max_iteration_count )
{
	double x_i = starting_point;
	double x_i_old;
	int iteration = 0;
	for( int iteration = 0; iteration < max_iteration_count; iteration++ )
	{
		x_i_old = x_i;
		x_i = x_i - func( x_i ) / derivative( x_i );
		if( abs( x_i - x_i_old ) < resolution )
		{
			return x_i;
		}
	}
}

double Binary_Search( std::function<double( double )> func, double left_most, double right_most, double resolution, int max_iteration_count )
{
	// Binary search
	double left = left_most, right = right_most;
	while( right - left > resolution )
	{
		double center = (right + left) / 2;
		if( func( center ) > 0 )
			right = center;
		else
			left = center;
	}
	double center = (right + left) / 2;

	return center;
}


// -*- coding: utf-16 -*-
#pragma once

/*! @file include/fractions.hpp
 *  This is a C++ Library header.
 */

#include <boost/operators.hpp>
#include <cmath>
#include <numeric>
#include <type_traits>

namespace fun
{

/*!
 * @brief Greatest common divider
 *
 * @tparam _Mn
 * @param[in] __m
 * @param[in] __n
 * @return _Mn
 */
template <typename Mn>
constexpr auto gcd(Mn _m, Mn _n) -> Mn
{
    return _m == 0 ? abs(_n) : _n == 0 ? abs(_m) : gcd(_n, _m % _n);
}

/*!
 * @brief Least common multiple
 *
 * @tparam _Mn
 * @param[in] __m
 * @param[in] __n
 * @return _Mn
 */
template <typename Mn>
constexpr auto lcm(Mn _m, Mn _n) -> Mn
{
    return (_m != 0 && _n != 0) ? (abs(_m) / gcd(_m, _n)) * abs(_n) : 0;
}

template <typename Z>
struct Fraction : boost::totally_ordered<Fraction<Z>,
                      boost::totally_ordered2<Fraction<Z>, Z,
                          boost::multipliable2<Fraction<Z>, Z,
                              boost::dividable2<Fraction<Z>, Z>>>>
{
    Z _numerator;
    Z _denominator;

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     * @param[in] denominator
     */
    constexpr Fraction(Z&& numerator, Z&& denominator) noexcept
        : _numerator {std::move(numerator)}
        , _denominator {std::move(denominator)}
    {
        this->normalize();
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     * @param[in] denominator
     */
    constexpr Fraction(const Z& numerator, const Z& denominator)
        : _numerator {numerator}
        , _denominator {denominator}
    {
        this->normalize();
    }

    constexpr void normalize()
    {
        auto common = gcd(this->_numerator, this->_denominator);
        if (common == Z(1))
        {
            return;
        }
        // if (common == Z(0)) [[unlikely]] return; // both num and den are zero
        if (this->_denominator < Z(0))
        {
            common = -common;
        }
        this->_numerator /= common;
        this->_denominator /= common;
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     */
    constexpr explicit Fraction(Z&& numerator) noexcept
        : _numerator {std::move(numerator)}
        , _denominator(Z(1))
    {
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     */
    constexpr explicit Fraction(const Z& numerator)
        : _numerator {numerator}
        , _denominator(Z(1))
    {
    }

    /*!
     * @brief
     *
     * @return const Z&
     */
    constexpr auto numerator() const -> const Z&
    {
        return _numerator;
    }

    /*!
     * @brief
     *
     * @return const Z&
     */
    constexpr auto denominator() const -> const Z&
    {
        return _denominator;
    }

    /*!
     * @brief
     *
     * @return Fraction
     */
    constexpr auto abs() const -> Fraction
    {
        return Fraction(std::abs(_numerator), std::abs(_denominator));
    }

    /*!
     * @brief
     *
     */
    constexpr void reciprocal()
    {
        std::swap(_numerator, _denominator);
    }

    /*!
     * @brief
     *
     * @return Fraction
     */
    constexpr auto operator-() const -> Fraction
    {
        auto res = Fraction(*this);
        res._numerator = -res._numerator;
        return res;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator+(const Fraction& frac) const -> Fraction
    {
        if (_denominator == frac._denominator)
        {
            return Fraction(_numerator + frac._numerator, _denominator);
        }
        auto d = _denominator * frac._denominator;
        auto n =
            frac._denominator * _numerator + _denominator * frac._numerator;
        return Fraction(n, d);
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator-(const Fraction& frac) const -> Fraction
    {
        return *this + (-frac);
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator*(const Fraction& frac) const -> Fraction
    {
        auto n = _numerator * frac._numerator;
        auto d = _denominator * frac._denominator;
        return Fraction(std::move(n), std::move(d));
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator/(Fraction frac) const -> Fraction
    {
        frac.reciprocal();
        return *this * frac;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator+(const Z& i) const -> Fraction
    {
        auto n = _numerator + _denominator * i;
        return Fraction(std::move(n), _denominator);
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator-(const Z& i) const -> Fraction
    {
        return *this + (-i);
    }

    // /*!
    //  * @brief
    //  *
    //  * @param[in] i
    //  * @return Fraction
    //  */
    // constexpr Fraction operator*(const Z& i) const
    // {
    //     auto n = _numerator * i;
    //     return Fraction(n, _denominator);
    // }

    // /*!
    //  * @brief
    //  *
    //  * @param[in] i
    //  * @return Fraction
    //  */
    // constexpr Fraction operator/(const Z& i) const
    // {
    //     auto d = _denominator * i;
    //     return Fraction(_numerator, d);
    // }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator+=(const Fraction& frac) -> Fraction&
    {
        return *this = *this + frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator-=(const Fraction& frac) -> Fraction&
    {
        return *this = *this - frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator*=(const Fraction& frac) -> Fraction&
    {
        return *this = *this * frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator/=(const Fraction& frac) -> Fraction&
    {
        return *this = *this / frac;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator+=(const Z& i) -> Fraction&
    {
        return *this = *this + i;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator-=(const Z& i) -> Fraction&
    {
        return *this = *this - i;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator*=(const Z& i) -> Fraction&
    {
        const auto common = gcd(i, this->_denominator);
        if (common == Z(1))
        {
            this->_numerator *= i;
        }
        // else if (common == Z(0)) [[unlikely]] // both i and den are zero
        // {
        //     this->_numerator = Z(0);
        // }
        else
        {
            this->_numerator *= (i / common);
            this->_denominator /= common;
        }
        return *this;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator/=(const Z& i) -> Fraction&
    {
        const auto common = gcd(this->_numerator, i);
        if (common == Z(1))
        {
            this->_denominator *= i;
        }
        // else if (common == Z(0)) [[unlikely]] // both i and num are zero
        // {
        //     this->_denominator = Z(0);
        // }
        else
        {
            this->_denominator *= (i / common);
            this->_numerator /= common;
        }
        return *this;
    }

    /*!
     * @brief Three way comparison
     *
     * @param[in] frac
     * @return auto
     */
    template <typename U>
    constexpr auto cmp(const Fraction<U>& frac) const
    {
        // if (_denominator == frac._denominator) {
        //     return _numerator - frac._numerator;
        // }
        return _numerator * frac._denominator - _denominator * frac._numerator;
    }

    constexpr auto operator==(const Fraction<Z>& rhs) const -> bool
    {
        if (this->_denominator == rhs._denominator)
        {
            return this->_numerator == rhs._numerator;
        }

        return (this->_numerator * rhs._denominator) ==
            (this->_denominator * rhs._numerator);
    }

    constexpr auto operator<(const Fraction<Z>& rhs) const -> bool
    {
        if (this->_denominator == rhs._denominator)
        {
            return this->_numerator < rhs._numerator;
        }

        return (this->_numerator * rhs._denominator) <
            (this->_denominator * rhs._numerator);
    }

    /**
     * @brief
     *
     */
    constexpr auto operator==(const Z& rhs) const -> bool
    {
        return this->_denominator == Z(1) && this->_numerator == rhs;
    }

    /**
     * @brief
     *
     */
    constexpr auto operator<(const Z& rhs) const -> bool
    {
        return this->_numerator < (this->_denominator * rhs);
    }

    /**
     * @brief
     *
     */
    constexpr auto operator>(const Z& rhs) const -> bool
    {
        return this->_numerator > (this->_denominator * rhs);
    }

    // /*!
    //  * @brief
    //  *
    //  * @return double
    //  */
    // constexpr explicit operator double()
    // {
    //     return double(_numerator) / _denominator;
    // }

    // /**
    //  * @brief
    //  *
    //  */
    // friend constexpr bool operator<(const Z& lhs, const Fraction<Z>& rhs)
    // {
    //     return lhs * rhs.denominator() < rhs.numerator();
    // }
};

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator+(const Z& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator-(const Z& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return c + (-frac);
}

// /*!
//  * @brief
//  *
//  * @param[in] c
//  * @param[in] frac
//  * @return Fraction<Z>
//  */
// template <typename Z>
// constexpr Fraction<Z> operator*(const Z& c, const Fraction<Z>& frac)
// {
//     return frac * c;
// }

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator+(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator-(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return (-frac) + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator*(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac * c;
}

/*!
 * @brief
 *
 * @tparam _Stream
 * @tparam Z
 * @param[in] os
 * @param[in] frac
 * @return _Stream&
 */
template <typename Stream, typename Z>
auto operator<<(Stream& os, const Fraction<Z>& frac) -> Stream&
{
    os << frac.numerator() << "/" << frac.denominator();
    return os;
}

// For template deduction
// Integral{Z} Fraction(const Z &, const Z &) -> Fraction<Z>;

} // namespace fun


#pragma once
#include <Eigen/Core>
#include <igl/readMESH.h>
#include <iostream>
#include <tuple>

#include "fem_solve.hpp"
#include "writer.hpp"

double f_lshape(double x, double y) {
	std::ignore = x;
	std::ignore = y;
	return 0;
}

double g_lshape(double x, double y) {
	double r = std::sqrt(x * x + y * y);

	double theta = std::atan2(y, x);

	// Adjust for the region where theta < 0
	if (theta < 0) {
		theta += 2 * M_PI;
	}

	return std::pow(r, 2.0 / 3.0) * std::sin(2 * theta / 3);
}

Eigen::Vector2d g_grad_lshape(double x, double y) {
	double r = std::sqrt(x * x + y * y);

	double theta = std::atan2(y, x);

	// Adjust for the region where theta < 0
	if (theta < 0) {
		theta += 2 * M_PI;
	}
	Eigen::Vector2d grad;
	grad << -2.0 / 3.0 * std::pow(r, -1.0 / 3.0) * std::sin(theta / 3), 2.0 / 3.0 * std::pow(r, -1.0 / 3.0) * std::cos(theta / 3);
	return grad;
}

void solveL(double r) {
	std::cout << "Solving L-shape" << std::endl;
	Vector u;

	Eigen::MatrixXd vertices;
	Eigen::MatrixXi triangles;
	Eigen::MatrixXi tetrahedra;

	igl::readMESH(NPDE_DATA_PATH "Lshape_5.mesh", vertices, tetrahedra, triangles);

	solveFiniteElement(u, vertices, triangles, f_lshape, constantFunction, g_lshape, r);

	writeToFile("Lshape_values.txt", u);
	writeMatrixToFile("Lshape_vertices.txt", vertices);
	writeMatrixToFile("Lshape_triangles.txt", triangles);
}


#include "libphysica/Integration.hpp"

#include <algorithm>
#include <iostream>

#include <boost/math/quadrature/gauss.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>

#include "libphysica/Special_Functions.hpp"
#include "libphysica/Statistics.hpp"

namespace libphysica
{
using namespace boost::math::quadrature;

// 1. One-dimensional MC integration
// 1.1 One-dimensional integration via adaptive Simpson method

//Function to return a reasonable precision.
double Find_Epsilon(std::function<double(double)> func, double a, double b, double precision)
{
	double c	   = (a + b) / 2;
	double h	   = b - a;
	double fa	   = func(a);
	double fb	   = func(b);
	double fc	   = func(c);
	double S	   = (h / 6) * (fa + 4 * fc + fb);
	double epsilon = precision * S;
	return epsilon;
}

double Adaptive_Simpson_Integration(std::function<double(double)> func, double a, double b, double epsilon, double S, double fa, double fb, double fc, int bottom, bool& warning)
{
	double c	  = (a + b) / 2;
	double h	  = b - a;
	double d	  = (a + c) / 2;
	double e	  = (b + c) / 2;
	double fd	  = func(d);
	double fe	  = func(e);
	double Sleft  = (h / 12) * (fa + 4 * fd + fc);
	double Sright = (h / 12) * (fc + 4 * fe + fb);
	double S2	  = Sleft + Sright;
	if(bottom <= 0 || fabs(S2 - S) <= 15 * epsilon)	  //15 due to error analysis
	{
		if(bottom <= 0 && fabs(S2 - S) > 15 * epsilon)
			warning = true;
		return S2 + (S2 - S) / 15;
	}
	else
	{
		return Adaptive_Simpson_Integration(func, a, c, epsilon / 2, Sleft, fa, fc, fd, bottom - 1, warning) + Adaptive_Simpson_Integration(func, c, b, epsilon / 2, Sright, fc, fb, fe, bottom - 1, warning);
	}
}

//Recursive functions for one-dimensional integration
void Check_Integration_Limits(double& a, double& b, double& sign)
{
	if(a > b)
	{
		std::cerr << "Warning in libphysica::Integrate(): From the integral from a to b, a>b (a = " << a << ", b = " << b << "). Sign will get swapped." << std::endl;
		std::swap(a, b);
		sign = -1.0;
	}
}

double Integrate(std::function<double(double)> func, double a, double b, double epsilon, int maxRecursionDepth)
{
	double sign = +1.0;
	if(a == b)
		return 0.0;
	else
		Check_Integration_Limits(a, b, sign);
	double c	  = (a + b) / 2;
	double h	  = b - a;
	double fa	  = func(a);
	double fb	  = func(b);
	double fc	  = func(c);
	double S	  = (h / 6) * (fa + 4 * fc + fb);
	bool warning  = false;
	double result = Adaptive_Simpson_Integration(func, a, b, fabs(epsilon), S, fa, fb, fc, maxRecursionDepth, warning);
	if(warning)
	{
		std::cout << "Warning in libphysica::Integrate(): Numerical integration on the interval (" << a << "," << b << ") did not converge to the desired precision." << std::endl;
		std::cout << "\tDesired precision: " << Round(fabs(epsilon)) << " Result: " << Round(result) << std::endl;
	}
	if(std::isnan(result))
		std::cout << "Warning in libphysica::Integrate(): Result is nan." << std::endl;
	else if(std::isinf(result))
		std::cout << "Warning in libphysica::Integrate(): Result is inf." << std::endl;
	return sign * result;
}

// 1.2 1D integration with boost functions
double Integrate(std::function<double(double)> func, double a, double b, const std::string& method)
{
	double sign = 1.0;
	if(a == b)
		return 0.0;
	else
		Check_Integration_Limits(a, b, sign);
	if(method == "Trapezoidal")
		return sign * trapezoidal(func, a, b);
	else if(method == "Gauss-Legendre")
		return sign * gauss<double, 30>::integrate(func, a, b);
	else if(method == "Gauss-Kronrod")
		return sign * gauss_kronrod<double, 31>::integrate(func, a, b, 5, 1e-9);
	else
	{
		std::cerr << "Error in libphysica::Integrate(): Method " << method << " not recognized." << std::endl;
		std::exit(EXIT_FAILURE);
	}
}

// 2. Multidimensional integration
// 2.1 Multidimensional integration via nesting 1D integration
double Integrate_2D(std::function<double(double, double)> func, double x1, double x2, double y1, double y2, const std::string& method)
{
	auto integrand_x = [&func, y1, y2, method](double x) {
		auto integrand_y = [&func, x](double y) {
			return func(x, y);
		};
		return Integrate(integrand_y, y1, y2, method);
	};
	return Integrate(integrand_x, x1, x2, method);
}

double Integrate_3D(std::function<double(double, double, double)> func, double x1, double x2, double y1, double y2, double z1, double z2, const std::string& method)
{
	auto integrand_x = [&func, y1, y2, z1, z2, method](double x) {
		auto integrand_y = [&func, z1, z2, x, method](double y) {
			auto integrand_z = [&func, x, y, method](double z) {
				return func(x, y, z);
			};
			return Integrate(integrand_z, z1, z2, method);
		};
		return Integrate(integrand_y, y1, y2, method);
	};
	return Integrate(integrand_x, x1, x2, method);
}

double Integrate_3D(std::function<double(Vector)> func, double r1, double r2, double costheta_1, double costheta_2, double phi_1, double phi_2, const std::string& method)
{
	auto integrand = [&func](double r, double cos_theta, double phi) {
		Vector rVec = Spherical_Coordinates(r, acos(cos_theta), phi);
		return r * r * func(rVec);
	};
	return Integrate_3D(integrand, r1, r2, costheta_1, costheta_2, phi_1, phi_2, method);
}

// 2.2 Monte Carlo Integration
// Reference: Some of these functions are taken from http://numerical.recipes/webnotes/nr3web9.pdf

// Utility routine used by vegas, to rebin a vector of densities contained in row j of xi into new bins defined by a vector r.
void Rebin(const double rc, const int nd, std::vector<double>& r, std::vector<double>& xin, libphysica::Matrix& xi, const int j)
{
	int i, k = 0;
	double dr = 0.0, xn = 0.0, xo = 0.0;

	for(i = 0; i < nd - 1; i++)
	{
		while(rc > dr)
			dr += r[(++k) - 1];
		if(k > 1)
			xo = xi[j][k - 2];
		xn = xi[j][k - 1];
		dr -= rc;
		xin[i] = xn - (xn - xo) * dr / r[k - 1];
	}
	for(i = 0; i < nd - 1; i++)
		xi[j][i] = xin[i];
	xi[j][nd - 1] = 1.0;
}

double Integrate_MC_Vegas(std::function<double(std::vector<double>&, const double)> func, std::vector<double>& region, const int init, const int ncall, const int itmx, const int nprn)
{
	double integral, chi2a, standard_deviation;
	// Best make everything static, allowing restarts.
	static const int NDMX = 50, MXDIM = 10;
	static const double ALPH = 1.5, TINY = 1.0e-30;
	static int i, it, j, k, mds, nd, ndo, ng, npg;
	static double calls, dv2g, dxg, f, f2, f2b, fb, rc, ti;
	static double tsi, wgt, xjac, xn, xnd, xo, schi, si, swgt;
	static std::vector<int> ia(MXDIM), kg(MXDIM);
	static std::vector<double> dt(MXDIM), dx(MXDIM), r(NDMX), x(MXDIM), xin(NDMX);
	static libphysica::Matrix d(NDMX, MXDIM), di(NDMX, MXDIM), xi(MXDIM, NDMX);

	// Initialize  captive, static random number generator
	std::random_device rd;
	std::mt19937 PRNG(rd());

	int ndim = region.size() / 2;
	if(init <= 0)
	{
		mds = ndo = 1;
		for(j = 0; j < ndim; j++)
			xi[j][0] = 1.0;
	}
	if(init <= 1)
		si = swgt = schi = 0.0;
	if(init <= 2)
	{
		nd = NDMX;
		ng = 1;
		if(mds != 0)
		{
			ng	= int(pow(ncall / 2.0 + 0.25, 1.0 / ndim));
			mds = 1;
			if((2 * ng - NDMX) >= 0)
			{
				mds = -1;
				npg = ng / NDMX + 1;
				nd	= ng / npg;
				ng	= npg * nd;
			}
		}
		for(k = 1, i = 0; i < ndim; i++)
			k *= ng;
		npg	  = std::max(int(ncall / k), 2);
		calls = double(npg) * double(k);
		dxg	  = 1.0 / ng;
		for(dv2g = 1, i = 0; i < ndim; i++)
			dv2g *= dxg;
		dv2g = calls * dv2g * calls * dv2g / npg / npg / (npg - 1.0);
		xnd	 = nd;
		dxg *= xnd;
		xjac = 1.0 / calls;
		for(j = 0; j < ndim; j++)
		{
			dx[j] = region[j + ndim] - region[j];
			xjac *= dx[j];
		}
		if(nd != ndo)
		{
			for(i = 0; i < std::max(nd, ndo); i++)
				r[i] = 1.0;
			for(j = 0; j < ndim; j++)
				Rebin(ndo / xnd, nd, r, xin, xi, j);
			ndo = nd;
		}
		if(nprn >= 0)
		{
			std::cout << " Input parameters for vegas";
			std::cout << "  ndim= " << std::setw(4) << ndim;
			std::cout << "  ncall= " << std::setw(8) << calls << std::endl;
			std::cout << std::setw(34) << "  it=" << std::setw(5) << it;
			std::cout << "  itmx=" << std::setw(5) << itmx << std::endl;
			std::cout << std::setw(34) << "  nprn=" << std::setw(5) << nprn;
			std::cout << "  ALPH=" << std::setw(9) << ALPH << std::endl;
			std::cout << std::setw(34) << "  mds=" << std::setw(5) << mds;
			std::cout << "  nd=" << std::setw(5) << nd << std::endl;
			for(j = 0; j < ndim; j++)
			{
				std::cout << std::setw(30) << " x1[" << std::setw(2) << j;
				std::cout << "]= " << std::setw(11) << region[j] << " xu[";
				std::cout << std::setw(2) << j << "]= ";
				std::cout << std::setw(11) << region[j + ndim] << std::endl;
			}
		}
	}
	for(it = 0; it < itmx; it++)
	{
		ti = tsi = 0.0;
		for(j = 0; j < ndim; j++)
		{
			kg[j] = 1;
			for(i = 0; i < nd; i++)
				d[i][j] = di[i][j] = 0.0;
		}
		for(;;)
		{
			fb = f2b = 0.0;
			for(k = 0; k < npg; k++)
			{
				wgt = xjac;
				for(j = 0; j < ndim; j++)
				{
					xn	  = (kg[j] - libphysica::Sample_Uniform(PRNG)) * dxg + 1.0;
					ia[j] = std::max(std::min(int(xn), NDMX), 1);
					if(ia[j] > 1)
					{
						xo = xi[j][ia[j] - 1] - xi[j][ia[j] - 2];
						rc = xi[j][ia[j] - 2] + (xn - ia[j]) * xo;
					}
					else
					{
						xo = xi[j][ia[j] - 1];
						rc = (xn - ia[j]) * xo;
					}
					x[j] = region[j] + rc * dx[j];
					wgt *= xo * xnd;
				}
				f  = wgt * func(x, wgt);
				f2 = f * f;
				fb += f;
				f2b += f2;
				for(j = 0; j < ndim; j++)
				{
					di[ia[j] - 1][j] += f;
					if(mds >= 0)
						d[ia[j] - 1][j] += f2;
				}
			}
			f2b = sqrt(f2b * npg);
			f2b = (f2b - fb) * (f2b + fb);
			if(f2b <= 0.0)
				f2b = TINY;
			ti += fb;
			tsi += f2b;
			if(mds < 0)
			{
				for(j = 0; j < ndim; j++)
					d[ia[j] - 1][j] += f2b;
			}
			for(k = ndim - 1; k >= 0; k--)
			{
				kg[k] %= ng;
				if(++kg[k] != 1)
					break;
			}
			if(k < 0)
				break;
		}
		tsi *= dv2g;
		wgt = 1.0 / tsi;
		si += wgt * ti;
		schi += wgt * ti * ti;
		swgt += wgt;
		integral = si / swgt;
		chi2a	 = (schi - si * integral) / (it + 0.0001);
		if(chi2a < 0.0)
			chi2a = 0.0;
		standard_deviation = sqrt(1.0 / swgt);
		tsi				   = sqrt(tsi);
		if(nprn >= 0)
		{
			std::cout << " iteration no. " << std::setw(3) << (it + 1);
			std::cout << " : integral = " << std::setw(14) << ti;
			std::cout << " +/- " << std::setw(9) << tsi << std::endl;
			std::cout << " all iterations:  "
					  << " integral =";
			std::cout << std::setw(14) << integral << "+-" << std::setw(9) << standard_deviation;
			std::cout << " chi**2/IT n =" << std::setw(9) << chi2a << std::endl;
			if(nprn != 0)
			{
				for(j = 0; j < ndim; j++)
				{
					std::cout << " DATA FOR axis  " << std::setw(2) << j << std::endl;
					std::cout << "     X      delta i          X      delta i";
					std::cout << "          X       deltai" << std::endl;
					for(i = nprn / 2; i < nd - 2; i += nprn + 2)
					{
						std::cout << std::setw(8) << xi[j][i] << std::setw(12) << di[i][j];
						std::cout << std::setw(12) << xi[j][i + 1] << std::setw(12) << di[i + 1][j];
						std::cout << std::setw(12) << xi[j][i + 2] << std::setw(12) << di[i + 2][j];
						std::cout << std::endl;
					}
				}
			}
		}
		for(j = 0; j < ndim; j++)
		{
			xo		= d[0][j];
			xn		= d[1][j];
			d[0][j] = (xo + xn) / 2.0;
			dt[j]	= d[0][j];
			for(i = 2; i < nd; i++)
			{
				rc			= xo + xn;
				xo			= xn;
				xn			= d[i][j];
				d[i - 1][j] = (rc + xn) / 3.0;
				dt[j] += d[i - 1][j];
			}
			d[nd - 1][j] = (xo + xn) / 2.0;
			dt[j] += d[nd - 1][j];
		}
		for(j = 0; j < ndim; j++)
		{
			rc = 0.0;
			for(i = 0; i < nd; i++)
			{
				if(d[i][j] < TINY)
					d[i][j] = TINY;
				r[i] = pow((1.0 - d[i][j] / dt[j]) /
							   (log(dt[j]) - log(d[i][j])),
						   ALPH);
				rc += r[i];
			}
			Rebin(rc / xnd, nd, r, xin, xi, j);
		}
	}
	return integral;
}

double Integrate_MC_Brute_Force(std::function<double(std::vector<double>&, const double)> func, std::vector<double>& region, const int ncall)
{
	int dim = region.size() / 2.0;
	std::random_device rd;
	std::mt19937 PRNG(rd());

	double volume = 1.0;
	for(int i = 0; i < dim; i++)
		volume *= (region[i + dim] - region[i]);

	double sum = 0.0;
	// double sum_2 = 0.0;
	for(int i = 0; i < ncall; i++)
	{
		std::vector<double> args(dim);
		for(int j = 0; j < dim; j++)
			args[j] = region[j] + libphysica::Sample_Uniform(PRNG) * (region[j + dim] - region[j]);
		double fct = func(args, 0.0);
		sum += volume * fct;
		// sum_2 += volume * volume * fct * fct;
	}
	double integral = sum / ncall;
	// double standard_deviation = sqrt((sum_2 / ncall - integral * integral) / (ncall - 1.0));
	// double chi2a			  = 0.0;
	return integral;
}

double Integrate_MC(std::function<double(std::vector<double>&, const double)> func, std::vector<double>& region, const int ncalls, const std::string& method)
{
	if(method == "Brute force")
		return Integrate_MC_Brute_Force(func, region, ncalls);
	else if(method == "Vegas")
	{
		int init	   = 0;
		const int itmx = 5;
		const int nprn = -1;
		return Integrate_MC_Vegas(func, region, init, ncalls, itmx, nprn);
	}
	else
	{
		std::cerr << "Error in libphysica::Integrate_MC(): Method " << method << " not recognized." << std::endl;
		std::exit(EXIT_FAILURE);
	}
}

}	// namespace libphysica


#include "problemes.h"
#include "arithmetique.h"

#include <boost/range/adaptor/reversed.hpp>

typedef unsigned long long nombre;

ENREGISTRER_PROBLEME(14, "Longest Collatz sequence") {
    // The following iterative sequence is defined for the set of positive integers:
    //
    // n -> n/2 (n is even)
    // n -> 3n + 1 (n is odd)
    //
    // Using the rule above and starting with 13, we generate the following sequence:
    //
    // 13 -> 40 -> 20 -> 10 -> 5 -> 16 -> 8 -> 4 -> 2 -> 1
    // It can be seen that this sequence (starting at 13 and finishing at 1) contains 10 terms.
    // Although it has not been proved yet (Collatz Problem), it is thought that all starting numbers finish at 1.
    //
    // Which starting number, under one million, produces the longest chain?
    //
    // NOTE: Once the chain starts the terms are allowed to go above one million.
    std::map<nombre, nombre> cache;
    cache[1] = 1;

    nombre max_longueur = 1;
    nombre max_nombre = 1;
    for (nombre n = 2; n < 1000000; ++n) {
        std::vector<nombre> chaine;
        chaine.push_back(n);
        nombre p = n;
        while (cache.find(p) == cache.end()) {
            if (p % 2 == 0)
                p /= 2;
            else
                p = 3 * p + 1;
            chaine.push_back(p);
        }

        nombre longueur = cache[p];
        for (const auto &c : boost::adaptors::reverse(chaine)) {
            cache[c] = ++longueur;
        }

        if (cache[n] > max_longueur) {
            max_longueur = cache[n];
            max_nombre = n;
        }
    }

    return std::to_string(max_nombre);
}


#pragma once

#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

//! \file rkintegrator.hpp Solution for Problem 1a, implementing RkIntegrator class

//! \brief Implements a Runge-Kutta explicit solver for a given Butcher tableau for autonomous ODEs
//! \tparam State a type representing the space in which the solution lies, e.g. R^d, represented by e.g. Eigen::VectorXd.
template <class State>
class RKIntegrator {
public:
    //! \brief Constructor for the RK method.
    //! Performs size checks and copies A and b into internal storage
    //! \param[in] A matrix containing coefficents of Butcher tableau, must be (strictly) lower triangular (no check)
    //! \param[in] b vector containing coefficients of lower part of Butcher tableau
    RKIntegrator(const Eigen::MatrixXd & A, const Eigen::VectorXd & b): A_(A),b_(b){
		n=A_.rows();
		assert(n==b_.size() && "size missmatch custom error");
		assert(A_.rows()==A_.cols() && "matrix not square custom error");

        // TODO: implement size checks and initialize internal data (DONE?)
    }

    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using a RK scheme given in the Butcher tableau provided in the
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function. Must have State operator()(State x)
    //! \param[in] f function handle for rhs in y' = f(y), e.g. implemented using lambda funciton
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function>
    std::vector<State> solve(const Function &f, double T, const State & y0, unsigned int N) const {
		double h = T/N;
		std::vector<State> y_f;
		y_f.reserve(N);
		y_f.push_back(y0);

		State ytemp1 = y0;
        State ytemp2 = y0;
        // Pointers to swap previous value
        State * yold = &ytemp1;
        State * ynew = &ytemp2;

		for (int i =1; i<N;++i){
			step(f,h,*yold,*ynew);
			y_f.push_back(*ynew);
			std::swap(yold,ynew);
		}
		return y_f;
        // TODO: implement solver from 0 to T, calling function step appropriately
    }

private:

    //! \brief Perform a single step of the RK method for the solution of the autonomous ODE
    //! Compute a single explicit RK step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs. Must have State operator()(State x)
    //! \param[in] f function handle for ths f, s.t. y' = f(y)
    //! \param[in] h step size
    //! \param[in] y0 initial state
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function>
    void step(const Function &f, double h, const State & y0, State & y1) const {
		y1 = y0;
		std::vector<State> k;
        k.reserve(n);
		for (int i=0;i<n;++i){
			State incr=y0;
			for (int j=0;j<i;++j){
				incr+=h*A_(i,j)*k.at(j);
			}
			k.push_back(f(incr));
			y1+=h*b_(i)*k.back();
		}
        // TODO: implement a single step of the RK method using provided Butcher scheme
    }

    //! TODO: put here suitable internal data storage
    const Eigen::VectorXd b_;
    const Eigen::MatrixXd A_;
    unsigned n;
};

Nummerical Methods for CSE/PS12/rkintegrator_cus.hpp


#pragma once

#include <vector>
#include <cassert>
#include <iostream>
#include <iomanip>
#include <Eigen/Dense>

#include "dampnewton.hpp"

//! \file implicit_rkintegrator.hpp Solution for Problem 1, implementing implicit_RkIntegrator class

//! \brief Compute the Kronecker product $C = A \otimes B$.
//! \param[in] A Matrix $m \times n$
//! \param[in] B Matrix $l \times k$
//! \param[out] C Kronecker product of A and B of dim $ml \times nk$
Eigen::MatrixXd kron(const Eigen::MatrixXd & A, const Eigen::MatrixXd & B)
{
    Eigen::MatrixXd C(A.rows()*B.rows(), A.cols()*B.cols());
    for(unsigned int i = 0; i < A.rows(); ++i) {
        for(unsigned int j = 0; j < A.cols(); ++j) {
            C.block(i*B.rows(),j*B.cols(), B.rows(), B.cols()) = A(i,j)*B;
        }
    }
    return C;
}

//! \brief Implements a Runge-Kutta implicit solver for a given Butcher tableau for autonomous ODEs

class implicit_RKIntegrator {
public:
    //! \brief Constructor for the implicit RK method.
    //! Performs size checks and copies A and b into internal storage
    //! \param[in] A matrix containing coefficents of Butcher tableau, must be (strictly) lower triangular (no check)
    //! \param[in] b vector containing coefficients of lower part of Butcher tableau
    implicit_RKIntegrator(const Eigen::MatrixXd & A, const Eigen::VectorXd & b)
        : A(A), b(b), s(b.size()) {
        assert( A.cols() == A.rows() && "Matrix must be square.");
        assert( A.cols() == b.size() && "Incompatible matrix/vector size.");
    }

    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using an implicit RK scheme given in the Butcher tableau provided in the
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function. Must have Eigen::VectorXd operator()(Eigen::VectorXd x)
    //! \tparam Function2 type for function implementing the Jacobian of f. Must have Eigen::MatrixXd operator()(Eigen::VectorXd x)
    //! \param[in] f function handle for rhs in y' = f(y), e.g. implemented using lambda funciton
    //! \param[in] Jf function handle for Jf, e.g. implemented using lambda funciton
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function, class Function2>
    std::vector<Eigen::VectorXd> solve(const Function &f, const Function2 &Jf, double T, const Eigen::VectorXd & y0, unsigned int N) const {
        // Iniz step size
        double h = T / N;

        // Will contain all steps, reserve memory for efficiency
        std::vector<Eigen::VectorXd> res;
        res.reserve(N+1);

        // Store initial data
        res.push_back(y0);

        // Initialize some memory to store temporary values
        Eigen::VectorXd ytemp1 = y0;
        Eigen::VectorXd ytemp2 = y0;
        // Pointers to swap previous value
        Eigen::VectorXd * yold = &ytemp1;
        Eigen::VectorXd * ynew = &ytemp2;

        // Loop over all fixed steps
        for(unsigned int k = 0; k < N; ++k) {
            // Compute, save and swap next step
            step(f, Jf, h, *yold, *ynew);
            res.push_back(*ynew);
            std::swap(yold, ynew);
        }
        return res;
    }

private:

    //! \brief Perform a single step of the RK method for the solution of the autonomous ODE
    //! Compute a single explicit RK step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs. Must have Eigen::VectorXd operator()(Eigen::VectorXd x)
    //! \tparam Function2 type for function implementing the Jacobian of f. Must have Eigen::MatrixXd operator()(Eigen::VectorXd x)
    //! \param[in] f function handle for ths f, s.t. y' = f(y)
    //! \param[in] Jf function handle for Jf, e.g. implemented using lambda funciton
    //! \param[in] h step size
    //! \param[in] y0 initial Eigen::VectorXd
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function, class Function2>
    void step(const Function &f, const Function2 &Jf, double h, const Eigen::VectorXd & y0, Eigen::VectorXd & y1) const {

        int d = y0.size();

        // Handle for the function F describing the equation satisfied by the stages g
        auto F = [y0, h, d, this, f] (Eigen::VectorXd gv) {
            Eigen::VectorXd Fv = gv;
            for (int j = 0; j < s; j++)
                Fv = Fv - h*kron(A.col(j),Eigen::MatrixXd::Identity(d,d))*f(y0+gv.segment(j*d,d));
            return Fv;
        };

        // Handle for the Jacobian of F.
        auto JF = [y0, h, d, Jf, this] (Eigen::VectorXd gv) {
            Eigen::MatrixXd DF(s*d,s*d);
            for (int j = 0; j < s; j++)
                DF.block(0,j*d,s*d,d) = kron(A.col(j),Eigen::MatrixXd::Identity(d,d))*Jf(y0+gv.segment(j*d,d));
            DF = Eigen::MatrixXd::Identity(s*d,s*d) - h*DF;
            return DF;
        };

        // Obtain stages with damped Newton method
        Eigen::VectorXd gv = Eigen::VectorXd::Zero(s*d);
        dampnewton(F, JF, gv);

        // Calculate y1
        Eigen::MatrixXd K(d,s);
        for (int j = 0; j < s; j++) K.col(j) = f(y0+gv.segment(j*d,d));
        y1 = y0 + h*K*b;
    }

    //! Matrix A in Butcher scheme
    const Eigen::MatrixXd A;
    //! Vector b in Butcher scheme
    const Eigen::VectorXd b;
    //! Size of Butcher matrix and vector A and b
    unsigned int s;
};

Nummerical Methods for CSE/PS14/solutions_ps14/implicit_rkintegrator.hpp


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use the general purpose non-linear
    least squares optimization routines from the dlib C++ Library.

    This example program will demonstrate how these routines can be used for data fitting.
    In particular, we will generate a set of data and then use the least squares
    routines to infer the parameters of the model which generated the data.
*/

#include <dlib/optimization.h>
#include <iostream>
#include <vector>

using namespace std;
using namespace dlib;

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

typedef matrix<double,2,1> input_vector;
typedef matrix<double,3,1> parameter_vector;

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

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
    const input_vector& input,
    const parameter_vector& params
)
{
    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = input(0);
    const double i1 = input(1);

    const double temp = p0*i0 + p1*i1 + p2;

    return temp*temp;
}

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

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

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

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    const double temp = p0*i0 + p1*i1 + p2;

    der(0) = i0*2*temp;
    der(1) = i1*2*temp;
    der(2) = 2*temp;

    return der;
}

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

int main()
{
    try
    {
        // randomly pick a set of parameters to use in this example
        const parameter_vector params = 10*randm(3,1);
        cout << "params: " << trans(params) << endl;

        // Now let's generate a bunch of input/output pairs according to our model.
        std::vector<std::pair<input_vector, double> > data_samples;
        input_vector input;
        for (int i = 0; i < 1000; ++i)
        {
            input = 10*randm(2,1);
            const double output = model(input, params);

            // save the pair
            data_samples.push_back(make_pair(input, output));
        }

        // Before we do anything, let's make sure that our derivative function defined above matches
        // the approximate derivative computed using central differences (via derivative()).
        // If this value is big then it means we probably typed the derivative function incorrectly.
        cout << "derivative error: " << length(residual_derivative(data_samples[0], params) -
                                               derivative(residual)(data_samples[0], params) ) << endl;

        // Now let's use the solve_least_squares_lm() routine to figure out what the
        // parameters are based on just the data_samples.
        parameter_vector x;
        x = 1;

        cout << "Use Levenberg-Marquardt" << endl;
        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               residual_derivative,
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = 1;
        cout << "Use Levenberg-Marquardt, approximate derivatives" << endl;
        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               derivative(residual),
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = 1;
        cout << "Use Levenberg-Marquardt/quasi-newton hybrid" << endl;
        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        solve_least_squares(objective_delta_stop_strategy(1e-7).be_verbose(),
                            residual,
                            residual_derivative,
                            data_samples,
                            x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;

    }
    catch (std::exception& e)
    {
        cout << e.what() << endl;
    }
}

// Example output:
/*
params: 8.40188 3.94383 7.83099

derivative error: 9.78267e-06
Use Levenberg-Marquardt
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.39172e+10
iteration: 3   objective: 1.57036e+09
iteration: 4   objective: 2.66917e+07
iteration: 5   objective: 4741.9
iteration: 6   objective: 0.000238674
iteration: 7   objective: 7.8815e-19
iteration: 8   objective: 0
inferred parameters: 8.40188 3.94383 7.83099

solution error:      0

Use Levenberg-Marquardt, approximate derivatives
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.39172e+10
iteration: 3   objective: 1.57036e+09
iteration: 4   objective: 2.66917e+07
iteration: 5   objective: 4741.87
iteration: 6   objective: 0.000238701
iteration: 7   objective: 1.0571e-18
iteration: 8   objective: 4.12469e-22
inferred parameters: 8.40188 3.94383 7.83099

solution error:      5.34754e-15

Use Levenberg-Marquardt/quasi-newton hybrid
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.3917e+10
iteration: 3   objective: 1.5572e+09
iteration: 4   objective: 2.74139e+07
iteration: 5   objective: 5135.98
iteration: 6   objective: 0.000285539
iteration: 7   objective: 1.15441e-18
iteration: 8   objective: 3.38834e-23
inferred parameters: 8.40188 3.94383 7.83099

solution error:      1.77636e-15
*/


#include "vicon_odom/filter.h"
#include <Eigen/LU> // For matrix inverse

KalmanFilter::KalmanFilter()
{
}

KalmanFilter::KalmanFilter(const KalmanFilter::State_t &state,
                           const KalmanFilter::ProcessCov_t &initial_cov,
                           const KalmanFilter::ProcessCov_t &process_noise,
                           const KalmanFilter::MeasurementCov_t &meas_noise)
{
  x = state;
  P = initial_cov;
  Q = process_noise;
  R = meas_noise;
}

void KalmanFilter::initialize(const State_t &state,
                              const ProcessCov_t &initial_cov,
                              const ProcessCov_t &process_noise,
                              const MeasurementCov_t &meas_noise)
{
  x = state;
  P = initial_cov;
  Q = process_noise;
  R = meas_noise;
}

void KalmanFilter::processUpdate(double dt)
{
  ProcessCov_t A = ProcessCov_t::Identity();
  A.topRightCorner<3,3>() = Eigen::Vector3d(dt, dt, dt).asDiagonal();

  x = A*x;
  P = A*P*A.transpose() + Q;
}

void KalmanFilter::measurementUpdate(const Measurement_t &meas, double dt)
{
  Eigen::Matrix<double, n_meas, n_states> H;
  H.setZero();
  H(0, 0) = 1;
  H(1, 1) = 1;
  H(2, 2) = 1;

  const Eigen::Matrix<double, n_states, n_meas> K = P * H.transpose() *
      (H*P*H.transpose() + R).inverse();
  const Measurement_t inno = meas - H*x;
  x += K*inno;
  P = (ProcessCov_t::Identity() - K*H) * P;
}

void KalmanFilter::setProcessNoise(const ProcessCov_t &process_noise)
{
  Q = process_noise;
}

void KalmanFilter::setMeasurementNoise(const MeasurementCov_t &meas_noise)
{
  R = meas_noise;
}

const KalmanFilter::State_t &KalmanFilter::getState(void)
{
  return x;
}

const KalmanFilter::ProcessCov_t &KalmanFilter::getProcessNoise(void)
{
  return P;
}


#include "Math/math_3dh.hpp"
#include <Eigen/Dense>
using Eigen::MatrixXf;
using Eigen::VectorXf;

namespace math_3dh {
float multiquadric_rbf(float r) {
  float R = 1.0f; // smoothing parameter
  float e = 3.0f; // shape parameter
  return -sqrtf(R + powf(e * r, 2));
}
float rbf_interp(glm::vec2 point, std::vector<glm::vec3> data,
                 float (*rbf)(float)) {
  // Construct A
  size_t n = data.size();
  MatrixXf A(n, n);
  for (size_t j = 0; j < n; j++) {
    for (size_t i = 0; i < n; i++) {
      if (i == j) {
        A(i, j) = 0.0f;
      } else {
        A(i, j) = rbf(glm::length(glm::vec2(data[i]) - glm::vec2(data[j])));
      }
    }
  }
  // Construct z
  VectorXf z(n);
  for (size_t i = 0; i < n; i++) {
    z(i) = data[i].z;
  }
  // Construct p
  VectorXf p(n);
  for (size_t i = 0; i < n; i++) {
    p(i) = rbf(glm::length(point - glm::vec2(data[i])));
  }
  // elevation(point) = p*(A^(-1)z) = p*w
  VectorXf w = A.partialPivLu().solve(z);
  return p.dot(w);
}
} // namespace math_3dh