#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
#include <fstream>
#include "CommonCurve.h" //std::left std::setw std::setfill
#include "CeresCost.h"
#include <boost/program_options.hpp>
#include <algorithm>
#include <iterator>
#include <filesystem>
namespace fs = std::filesystem;
using namespace boost;
namespace po = boost::program_options;

int main ( int argc, char** argv )
{
    std::string dataFile = "readDataCeres.txt";
    std::string parameter = "parametersCeres.txt";
    std::string path = "./result/";
    // create directory if not exist
    struct stat info;
    if( stat( path.c_str(), &info ) != 0 )fs::create_directories(path);
    std::string pathRealData = path + za::getCurrentTime() + dataFile;
    std::string pathParameter = path + za::getCurrentTime() + parameter;

    // True parameter value to be estimated
    double a, b, c;

    // Total number of data point
    int N, iterate ;

    // Noise Sigma value
    double w_sigma;
    // Cammand line parser
	try
		{
			po::options_description desc("Allowed options");
			desc.add_options()
				("help,h", "produce help message")
				("first,a", po::value<double>(),
					"first parameter")
                ("second,b", po::value<double>(),
					"second parameter")
				("third,c", po::value<double>(),
					"third parameter")
                ("number,n", po::value<int>(&N)->default_value(100)->implicit_value(500),
					"number of data")
                ("iteration,i", po::value<int>(&iterate)->default_value(100)->implicit_value(200),
					"number of iterations")
				("noise,s", po::value<double>(&w_sigma)->default_value(1)->implicit_value(5),
					"noise added");

			/* Point out that all unknown values should be converted to the value of the "input-file" option.
					* Also use the command_line_parser class instead of parse_command_line */
			po::variables_map vm;
			po::store(po::parse_command_line(argc, argv, desc), vm);
			po::notify(vm);

			if (vm.count("help"))
			{
				std::cout << "Usage: options_description [options]\n";
				std::cout << desc;
				return 0;
			}
 			if (vm.count("first") )
        	{

                a =  vm["first"].as<double>();
        	}
        	else
       		{
            	std::cout << "First parameter not set.\n";
				return -1;
        	}
 			if (vm.count("second") )
        	{

                b =  vm["second"].as<double>();
        	}
        	else
       		{
            	std::cout << "Second parameter not set.\n";
				return -1;
        	}
 			if (vm.count("third") )
        	{

                a =  vm["third"].as<double>();
        	}
        	else
       		{
            	std::cout << "Third parameter not set.\n";
				return -1;
        	}
        }// end of try

        catch(std::exception& e)
		{
			std::cout << e.what() << "\n";
			return 1;
		}

    // OpenCV random number generator
    cv::RNG rng;

    // abc Estimated value of the parameter
    double abc[3] = {0,0,0};

   // set of x and y data
    std::vector<double> x_data, y_data;
    std::ofstream myfile;
    const int nameWidth     = 6;
    const int numWidth      = 8;
    myfile.open (&pathRealData[0]);
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    za::printElement("|", 1, myfile);
    za::printElement("N", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("X", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("Y", 10, myfile);
    za::printElement("|", 1, myfile);
    myfile <<"\n";
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    std::cout<<"Generating data: \n";

    for ( int i=0; i<N; i++ )
    {
        // x data
        double x = i/100.0;
        x_data.push_back ( x );

        // y data
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );

        za::printElement("|", 1, myfile);
        za::printElement(i+1, nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(x_data[i], nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(y_data[i], 10, myfile);
        za::printElement("|", 1, myfile);
        myfile <<"\n";
        for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
        myfile <<"\n";

    }
    myfile.close();
    // Constructing the Least Squares Problem
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
    // Add an error term to the question
    // Use automatic derivation, template parameters:
    // error type, output dimension, input dimension, dimension should be consistent with the previous struct
        problem.AddResidualBlock (
            new ceres::AutoDiffCostFunction<za::CURVE_FITTING_COST, 1, 3> (
                new za::CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,            // Kernel function, not used here, empty
            abc                 // Parameters to be estimated
        );
    }

    //Configure solver

    ceres::Solver::Options options;

    // How to solve the incremental equation
    options.linear_solver_type = ceres::DENSE_QR;

    // Output to std::cout
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = iterate;

    // Optimization information
    ceres::Solver::Summary summary;

    // Timer to compute solving time
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

    // Start to optimize
    ceres::Solve ( options, &problem, &summary );
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>( t2-t1 );
    std::cout<<"solve time cost = "<<time_used.count()<<" seconds. \n";
    std::ofstream mySol;
    mySol.open (&pathParameter[0]);
    // Output result
    std::cout<<summary.BriefReport() << "\n";
    mySol<<"Estimated a,b,c =";
    for ( auto val:abc ) mySol<<val<<" ";
    mySol<<"\n";
    mySol.close();

    return 0;
}


#include <cstdint>
#include <memory>
#include <vector>
#include <string>
#include <numeric>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <fstream>
#include <omp.h>
#include <valarray>

#include <typeinfo>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <posit/posit>
// Posit Arithmetic FPGA accelerator library
#include <positarith.h>

#include "main.hpp"
#include "defines.hpp"
#include "utils.hpp"
#include "blas.hpp"
#include "vector_utils.hpp"
#include "matrix_utils.hpp"

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

void normalizeVector(vector<posit<NBITS,ES>>& vec) {
    posit<NBITS,ES> sum;
    for(posit<NBITS,ES>& el : vec) {
        sum += el * el;
    }
    double sq = sqrt((double)sum);
    for(posit<NBITS,ES>& el : vec) {
        el = el / sq;
    }
}

template<typename T>
void normalizeVector(valarray<T>& vec) {
    T sum;
    for(T& el : vec) {
        sum += el * el;
    }
    T sq = sqrt((T)sum);
    for(T& el : vec) {
        el = el / sq;
    }
}

vector<posit<NBITS, ES> > sumProj(vector<vector<posit<NBITS,ES> > >& v, vector<vector<posit<NBITS,ES> > >& u, int i) {
        int k = 0;
        vector<posit<NBITS,ES> > result(u[0].size());

        while (k < i)
        {
                posit<NBITS,ES> dot_u_u;
                vector_dot(u[k], u[k], dot_u_u);

                posit<NBITS,ES> dot_v_u;
                vector_dot(v[i], u[k], dot_v_u);

                posit<NBITS,ES> factor = dot_v_u / dot_u_u;

                vector<posit<NBITS,ES> > factor_u;
                vector_mult(u[k], factor, factor_u);
                vector_add(result, factor_u, result);

                k++;
        }

        return result;
}

template<typename T>
valarray<T> sumProj(valarray<valarray<T>> v, valarray<valarray<T>> u, int i) {
    int k = 0;
    valarray<T> result(v[0].size());

    while (k < i)
    {
        T dot_u_u = (u[k] * u[k]).sum();
        T dot_v_u = (v[i] * u[k]).sum();
        T factor = dot_v_u / dot_u_u;
        valarray<T> factor_u = factor * u[k];
        result += factor * u[k];
        k++;
    }

    return result;
}

// valarray<posit<NBITS,ES>> sumProj(valarray<valarray<posit<NBITS,ES>>> v, valarray<valarray<posit<NBITS,ES>>> u, int i) {
//     int k = 0;
//     valarray<posit<NBITS,ES>> result(v[0].size());
//
//     while (k < i)
//     {
//         posit<NBITS,ES> dot_u_u = (u[k] * u[k]).sum();
//         posit<NBITS,ES> dot_v_u = (v[i] * u[k]).sum();
//         posit<NBITS,ES> factor = dot_v_u / dot_u_u;
//         valarray<posit<NBITS,ES>> factor_u = factor * u[k];
//         result += factor * u[k];
//
//         k++;
//     }
//
//     return result;
// }

int main(int argc, char ** argv)
{
    vector<int> lengths = {10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000, 2000, 3000, 4000, 5000, 10000, 20000, 30000, 40000, 50000, 100000, 200000, 300000, 400000, 500000, 1000000};

    std::string t_dot, t_sum, t_add, t_add_scalar, t_subtract, t_subtract_scalar, t_mult, t_mult_scalar;

    for(int length : lengths) {
        t_dot = test_dot_product(length);
        t_sum = test_sum(length);
        t_add = test_add(length);
        t_add_scalar = test_add_scalar(length);
        t_subtract = test_subtract(length);
        t_subtract_scalar = test_subtract_scalar(length);
        t_mult = test_mult(length);
        t_mult_scalar = test_mult_scalar(length);

        ofstream outfile("positarith_es" + std::to_string(ES) + "_" + std::to_string(length) + ".txt", ios::out);
        outfile << t_dot << endl;
        outfile << t_sum << endl;
        outfile << t_add << endl;
        outfile << t_add_scalar << endl;
        outfile << t_subtract << endl;
        outfile << t_subtract_scalar << endl;
        outfile << t_mult << endl;
        outfile << t_mult_scalar << endl;
        outfile.close();
    }

    // vector<int> vectors_vec = {2, 5, 10, 50, 100};
    // vector<int> gram_length = {10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000, 2000, 3000, 4000, 5000, 10000, 20000, 30000, 40000, 50000, 100000};
    //
    // std::string t_gram;
    //
    // for(int vectors : vectors_vec) {
    //     for(int length : gram_length) {
    //         t_gram = test_gram(3, length);
    //
    //         ofstream outfile("positarith_gram_es" + std::to_string(ES) + "_" + std::to_string(vectors) + "_" + std::to_string(length) + ".txt", ios::out);
    //         outfile << t_gram << endl;
    //         outfile.close();
    //     }
    // }

    return 0;
}

std::string test_dot_product(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Dot Product
    std::vector<posit<NBITS,ES>> vec1, vec2;
    posit<NBITS,ES> result;

    for(i = 0; i < length; i++) {
        vec1.push_back(1);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_dot(vec1, vec2, result);

    start = omp_get_wtime();
    posit<NBITS,ES> res = 0;
    // for(i = 0; i < length; i++) {
    //     res = res + vec1[i] * vec2[i];
    // }
    res = sw::hprblas::dot(length, vec1, 1, vec2, 1);
    stop = omp_get_wtime();
    t_sw = stop - start;

    float res_f = 0;
    std::vector<float> vec1_f, vec2_f;
    for(i = 0; i < length; i++) {
        vec1_f.push_back(1);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        #pragma omp for reduction(+:res_f)
        for(i = 0; i < length; i++) {
            res_f += vec1_f[i] * vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(1);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec = res_dec + vec1_dec[i] * vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_add(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Add
    std::vector<posit<NBITS,ES>> vec1, vec2;
    std::vector<posit<NBITS,ES>> result;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_add(vec1, vec2, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] + vec2[i];
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f, vec2_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] + vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] * vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_add_scalar(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Add Scalar
    std::vector<posit<NBITS,ES>> vec1;
    std::vector<posit<NBITS,ES>> result;
    posit<NBITS,ES> scalar;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    scalar = 5;
    t_fpga = 0;//vector_add(vec1, scalar, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] + scalar;
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] + 5.0;
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] + 5.0;
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_subtract(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Subtract
    std::vector<posit<NBITS,ES>> vec1, vec2;
    std::vector<posit<NBITS,ES>> result;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_sub(vec1, vec2, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] - vec2[i];
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f, vec2_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] - vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] - vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_subtract_scalar(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Subtract Scalar
    std::vector<posit<NBITS,ES>> vec1;
    std::vector<posit<NBITS,ES>> result;
    posit<NBITS,ES> scalar;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    scalar = 5;
    t_fpga = 0;//vector_sub(vec1, scalar, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] - scalar;
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] - 5.0;
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] - 5.0;
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_sum(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Sum
    std::vector<posit<NBITS,ES>> vec1;
    posit<NBITS,ES> result;

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    t_fpga = 0;//vector_sum(vec1, result);

    start = omp_get_wtime();
    // for(i = 0; i < length; i++) {
    //     result = result + vec1[i];
    // }
    result = sw::hprblas::asum(length, vec1, 1);
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f;
    float res_f = 0.0;
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f = res_f + vec1_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec;
    // cpp_dec_float_100 res_dec = 0.0;
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec = res_dec + vec1_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_mult(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double stop, start;
    // Vector Multiplication
    std::vector<posit<NBITS,ES>> vec1, vec2;
    std::vector<posit<NBITS,ES>> result;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
        vec2.push_back(i);
    }
    t_fpga = 0;//vector_mult(vec1, vec2, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++) {
        result[i] = vec1[i] * vec2[i];
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec1_f, vec2_f;
    std::vector<float> res_f;
    res_f.resize(length);
    for(i = 0; i < length; i++) {
        vec1_f.push_back(i);
        vec2_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++) {
            res_f[i] = vec1_f[i] * vec2_f[i];
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec1_dec, vec2_dec;
    // std::vector<cpp_dec_float_100> res_dec;
    // res_dec.resize(length);
    // for(i = 0; i < length; i++) {
    //     vec1_dec.push_back(i);
    //     vec2_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++) {
    //     res_dec[i] = vec1_dec[i] * vec2_dec[i];
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

std::string test_mult_scalar(int length) {
    int i;
    double t_fpga, t_sw, t_float;
    double start, stop;
    // Vector Multiplication Scalar
    std::vector<posit<NBITS,ES>> vec1;
    std::vector<posit<NBITS,ES>> result;
    posit<NBITS,ES> scalar;
    result.resize(length);

    for(i = 0; i < length; i++){
        vec1.push_back(i);
    }
    scalar = 5;
    t_fpga = 0;//vector_mult(vec1, scalar, result);

    start = omp_get_wtime();
    for(i = 0; i < length; i++){
        result[i] = vec1[i] * scalar;
    }
    stop = omp_get_wtime();
    t_sw = stop - start;

    std::vector<float> vec_f;
    std::vector<float> result_f;
    result_f.resize(length);
    for(i = 0; i < length; i++){
        vec_f.push_back(i);
    }
    start = omp_get_wtime();
    #pragma omp parallel private(i) num_threads(8)
    {
        for(i = 0; i < length; i++){
            result_f[i] = vec_f[i] * 5.0;
        }
    }
    stop = omp_get_wtime();
    t_float = stop - start;

    // std::vector<cpp_dec_float_100> vec_dec;
    // std::vector<cpp_dec_float_100> result_dec;
    // result_dec.resize(length);
    // for(i = 0; i < length; i++){
    //     vec_dec.push_back(i);
    // }
    // for(i = 0; i < length; i++){
    //     result_dec[i] = vec_dec[i] * 5.0;
    // }

    return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float);
}

// template<typename T>
// cpp_dec_float_100 aggregateGram(vector<vector<T > >& u) {
//     cpp_dec_float_100 a = 0.0;
//     for(vector<T>& u_vec : u) {
//         a += (cpp_dec_float_100)u_vec.sum();
//     }
//     return a;
// }
//
// template<typename T>
// cpp_dec_float_100 aggregateGram(valarray<valarray<T > >& u) {
//     cpp_dec_float_100 a = 0.0;
//     for(valarray<T>& u_vec : u) {
//         a += (cpp_dec_float_100)u_vec.sum();
//     }
// }

// std::string test_gram(int n, int m) {
//     int i;
//     double stop, start;
//     double t_sw = 0.0, t_fpga = 0.0, t_float = 0.0;
//     cpp_dec_float_100 a_dec = 0.0, a_sw = 0.0, a_fpga = 0.0, a_float = 0.0;
//     cpp_dec_float_100 da_sw = 0.0, da_hw = 0.0, da_float = 0.0;
//
//     // Gram Matrix Test
//
//     // POSIT FPGA
//     vector<vector<posit<NBITS, ES> > > v(n);
//     vector<vector<posit<NBITS, ES> > > u(n); // Orthogonal set
//
//     for(i = 0; i < n; i++) {
//         u[i].resize(m);
//     }
//
//     // // Fill vectors
//     // for(i = 0; i < n; i++) {
//     //     vector<posit<NBITS, ES>> vec(m);
//     //     for(int j = 0; j < m; j++) {
//     //         vec[j] = sqrt(2)/2;
//     //     }
//     //     v[i] = vec;
//     // }
//     //
//     // // Start
//     // start = omp_get_wtime();
//     // u[0] = v[0];
//     // i = 0;
//     // do {
//     //         vector<posit<NBITS,ES> > sum = sumProj(v, u, i);
//     //         vector_sub(v[i], sum, u[i]);
//     //         i++;
//     // } while(i < n);
//     // stop = omp_get_wtime();
//     // t_fpga = stop - start;
//     //
//     // a_fpga = aggregateGram(u);
//
//     // POSIT SW
//     valarray<valarray<posit<NBITS,ES> > > v_posit(n);
//     valarray<valarray<posit<NBITS,ES> > > u_posit(n);
//     u_posit.resize(n);
//     // Fill vectors
//     for(i = 0; i < n; i++) {
//         valarray<posit<NBITS,ES>> vec(m);
//         for(int j = 0; j < m; j++) {
//             vec[j] = sqrt(2)/2;
//         }
//         v_posit[i] = vec;
//     }
//
//     for(i = 0; i < n; i++) {
//         u_posit[i].resize(m);
//     }
//
//     start = omp_get_wtime();
//     u_posit[0] = v_posit[0];
//     i = 0;
//     do {
//             valarray<posit<NBITS,ES>> sum = sumProj(v_posit, u_posit, i);
//             u_posit[i] = v_posit[i] - sum;
//             i++;
//     } while(i < n);
//     stop = omp_get_wtime();
//     t_sw = stop - start;
//
//     a_sw = aggregateGram(u_posit);
//
//     // FLOAT
//     valarray<valarray<float > > v_float(n);
//     valarray<valarray<float > > u_float(n);
//
//     // Fill vectors
//     for(i = 0; i < n; i++) {
//         valarray<float> vec(m);
//         for(int j = 0; j < m; j++) {
//             vec[j] = sqrt(2)/2;
//         }
//         v_float[i] = vec;
//     }
//
//     for(i = 0; i < n; i++) {
//         u_float[i].resize(m);
//     }
//
//     start = omp_get_wtime();
//     u_float[0] = v_float[0];
//     i = 0;
//     do {
//             valarray<float> sum = sumProj(v_float, u_float, i);
//             u_float[i] = v_float[i] - sum;
//             i++;
//     } while(i < n);
//     stop = omp_get_wtime();
//     t_float = stop - start;
//
//     a_float = aggregateGram(u_float);
//
//     // cpp_dec_float_100
//     valarray<valarray<cpp_dec_float_100 > > v_dec(n);
//     valarray<valarray<cpp_dec_float_100 > > u_dec(n);
//
//     // Fill vectors
//     for(i = 0; i < n; i++) {
//         valarray<cpp_dec_float_100> vec(m);
//         for(int j = 0; j < m; j++) {
//             vec[j] = sqrt(2)/2;
//         }
//         v_dec[i] = vec;
//     }
//
//     for(i = 0; i < n; i++) {
//         u_dec[i].resize(m);
//     }
//
//     u_dec[0] = v_dec[0];
//     i = 0;
//     do {
//             valarray<cpp_dec_float_100> sum = sumProj(v_dec, u_dec, i);
//             u_dec[i] = v_dec[i] - sum;
//             i++;
//     } while(i < n);
//
//     a_dec = aggregateGram(u_dec);
//
//     // Calculate decimal accuracy
//     da_hw = decimal_accuracy(a_dec, a_fpga);
//     da_sw = decimal_accuracy(a_dec, a_sw);
//     da_float = decimal_accuracy(a_dec, a_float);
//
//     return to_string_precision(t_fpga) + "," + to_string_precision(t_sw) + "," + to_string_precision(t_float) + "," + to_string_precision(da_hw) + "," + to_string_precision(da_sw) + "," + to_string_precision(da_float);
// }


/**
 * @file
 * @author lkoppel
 */

#ifndef WAVE_GEOMETRY_UTIL_MATH_HPP
#define WAVE_GEOMETRY_UTIL_MATH_HPP

#include <Eigen/Core>
#include <random>

namespace wave {

/** Generate a random real number on the closed interval [a, b] */
template <typename Real>
Real uniformRandom(Real a, Real b) {
    // Use Mersenne Twister pseudo-random number generator, seeded by system random device
    static std::mt19937 rng{std::random_device{}()};

    // The distribution is normally open on one end [a, b). Do this to close it
    const auto closed_b = std::nextafter(b, std::numeric_limits<Real>::max());

    // Pick a real from the distribution
    return std::uniform_real_distribution<Real>{a, closed_b}(rng);
}

/** Generate a random unit quaternion on SO(3)
 *
 * Implements Algorithm 2 from Kuffner, James J. "Effective Sampling and Distance Metrics
 * for 3D Rigid Body Path Planning."
 * https://www.ri.cmu.edu/pub_files/pub4/kuffner_james_2004_1/kuffner_james_2004_1.pdf
 */
template <typename Real>
Eigen::Quaternion<Real> randomQuaternion() {
    const Real s = uniformRandom(Real{0}, Real{1});
    const Real s1 = std::sqrt(1 - s);
    const Real s2 = std::sqrt(s);
    const Real t1 = Real{2 * M_PI} * uniformRandom(Real{0}, Real{1});
    const Real t2 = Real{2 * M_PI} * uniformRandom(Real{0}, Real{1});
    return Eigen::Quaternion<Real>{
      std::cos(t2) * s2, std::sin(t1) * s1, std::cos(t1) * s1, std::sin(t2) * s2};
}

/** Go from a skew-symmetric (cross) matrix to a compact vector
 *
 * Also known as the "vee" operator.
 */
template <typename Derived>
auto uncrossMatrix(const Eigen::MatrixBase<Derived> &skew)
  -> Eigen::Matrix<typename Eigen::internal::traits<Derived>::Scalar, 3, 1> {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
    return Eigen::Matrix<typename Eigen::internal::traits<Derived>::Scalar, 3, 1>{
      skew(2, 1), skew(0, 2), skew(1, 0)};
}

}  // namespace wave

#endif  // WAVE_GEOMETRY_UTIL_MATH_HPP


//
//  EVD.hpp
//
//  Created by r. on 08/05/14
//

#ifndef round1_EVD_hpp
#define round1_EVD_hpp

// GSL
#include <gsl/gsl_eigen.h>

// Boost
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

//#include <boost/numeric/ublas/vector_sparse.hpp>
//#include <boost/numeric/ublas/vector_proxy.hpp>
//#include <boost/numeric/ublas/matrix_sparse.hpp>
//#include <boost/numeric/ublas/operation.hpp>
//#include <boost/numeric/ublas/operation_sparse.hpp>

namespace gsl
{
	struct EVD
	{
	public:
		typedef boost::numeric::ublas::matrix<double> matrix;
		typedef boost::numeric::ublas::vector<double> vector;
	public:
		// These should not be changed from outside
		unsigned int n;
		matrix V;
		vector d;
	public:
		EVD() : n(0), V(0,0), d(0) { }
	private:
		/*//
		 void testV(const matrix& AE, const matrix& ME) const
		 {
		 namespace ublas = boost::numeric::ublas;

		 vector u(n);
		 for (unsigned int i = 0; i != u.size(); ++i) u(i) = i;
		 double j = 1, k = 2;

		 cout << "A: " << AE << endl;
		 cout << "M: " << ME << endl;
		 cout << "V: " << V << endl;
		 cout << "d: " << d << endl;
		 matrix W = ublas::trans(V);
		 matrix AV = ublas::prod(AE, V), MV = ublas::prod(ME, V);
		 cout << "Vt A V: " << ublas::prod(W, AV) << endl;
		 cout << "Vt M V: " << ublas::prod(W, MV) << endl;

		 vector f1(n);
		 {
		 f1 = ublas::prod(j*AE + k*ME, u);
		 }
		 vector f2(n);
		 {
		 f2 = ublas::prod(ME, u);
		 f2 = ublas::prod(ublas::trans(V), f2);
		 for (unsigned int i = 0; i != f2.size(); ++i)
		 {
		 f2(i) *= (j * d(i) + k);
		 }
		 f2 = ublas::prod(V, f2);
		 f2 = ublas::prod(ME, f2);
		 }
		 cout << f1 << endl;
		 cout << f2 << endl;
		 cout << "testV residual: " << ublas::norm_inf(f1 - f2) << endl;
		 assert(ublas::norm_inf(f1 - f2) <= 1e-6);
		 }
		 //*/

	public:
		EVD(const matrix& AE, const matrix& ME)
		:
		n(0), V(0,0), d(0)
		// Pre: AE and ME are square matrices of equal size
		{
			// Thanks to:
			// http://sector7.xray.aps.anl.gov/~dohnarms/programming/gsl/gsl-ref.pdf
			// http://www.ryolab.com/soft/aper/dd/d99/_2utility_2algorithm_2eigen_2eigentest_2main_8cpp-example.html

			n = (unsigned int)(AE.size1());

			assert((AE.size1() == n) && (AE.size2() == n));
			assert((ME.size1() == n) && (ME.size2() == n));

			gsl_eigen_gensymmv_workspace * w = gsl_eigen_gensymmv_alloc(n);

			matrix A(n, n); A = AE;
			matrix M(n, n); M = ME;
			gsl_matrix_view a = gsl_matrix_view_array(&A.data()[0], n, n);
			gsl_matrix_view m = gsl_matrix_view_array(&M.data()[0], n, n);
			gsl_matrix * U = gsl_matrix_alloc(n , n);
			gsl_vector * c = gsl_vector_alloc(n);
			gsl_eigen_gensymmv(&a.matrix, &m.matrix, c, U, w);
			gsl_eigen_gensymmv_sort(c, U, GSL_EIGEN_SORT_ABS_ASC);

			V = matrix(n, n);
			d = vector(n);
			std::copy(U->data, U->data + (n*n), &(V.data()[0]));
			std::copy(c->data, c->data + n, &(d.data()[0]));

			gsl_vector_free(c);
			gsl_matrix_free(U);
			gsl_eigen_gensymmv_free(w);

			// M-normalize V
			for (unsigned int i = 0; i != n; ++i)
			{
				namespace ublas = boost::numeric::ublas;
				auto c = ublas::column(V, i);
				double norm = ublas::inner_prod(c, ublas::prod(ME, c));
				//cout << "norm: " << norm << endl;
				c *= (1/sqrt(norm));
			}

			//testV(AE, ME);
		}

		EVD& operator=(const EVD&) = default;
	};
} // namespace tools

#endif


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

#include "base/fit_plane.h"

#include <Eigen/Dense>

namespace mjmech {
namespace base {

Plane FitPlane(const std::vector<Eigen::Vector3d>& points) {
  Eigen::MatrixXd A(points.size(), 3);
  Eigen::MatrixXd B(points.size(), 1);

  for (size_t i = 0; i < points.size(); i++) {
    A(i, 0) = points[i].x();
    A(i, 1) = points[i].y();
    A(i, 2) = 1.0;
    B(i) = points[i].z();
  }

  Eigen::MatrixXd result = A.bdcSvd(
      Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
  return Plane{result(0), result(1), result(2)};
}

}
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"

namespace math = boost::math::constants;

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

struct err
{
  double infty=0.;
  double one=0.;
  double dt=0. , dx=0. , dv=0.;
  std::size_t n=16;
  std::size_t nb_iter=0;
};

std::ostream &
operator << ( std::ostream & os , err const& e ) {
  os << e.n << " " << " " << e.nb_iter << " " << e.dx << " " << e.dt << " " << e.one << " " << e.infty;
  return os;
}

err
rotation ( std::size_t N , int q=-2 )
{
	std::size_t Nx = N, Nv = N;
	field<double,1> f(boost::extents[Nv][Nx]);
  field<double,1> f_sol(boost::extents[Nv][Nx]);

	f.range.v_min = -10.; f.range.v_max = 10;
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;
	f.range.x_min = -10.; f.range.x_max = 10.;
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;

	const double dt = (math::pi<double>()*std::pow(2,q))*f.step.dv/f.range.v_max;

	ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] =  Vk(k); }
  ublas::vector<double> E (Nx,0.); for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = -Xi(i); }

	const double l = f.range.x_max-f.range.x_min;
	ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i] = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] = std::exp(-SQ(Xi(i)-3)/0.5 - SQ(Vk(k))/2.);
      f_sol[k][i] = f[k][i];
    }
  }

  double Tf = 2*math::pi<double>();
  int i_t=0;

  while ( i_t*dt < Tf ) {
    field<double,1> Edvf = weno::trp_v(f,E);
    field<double,1> f1=f,f2=f;
	  fft::spectrum hf(Nx),hf1(Nx),hf2(Nx),hEdvf(Nx);

	  for ( auto k=0 ; k<f.size(0) ; ++k ) {
	  	hf.fft(&(f[k][0]));
	  	hEdvf.fft(&(Edvf[k][0]));

	  	for ( auto i=0 ; i<Nx ; ++i ) {
	  		auto re = hf[i][fft::re] , im = hf[i][fft::im];
	  		hf1[i][fft::re] = std::cos(v(k)*kx[i]*dt)*(re-dt*hEdvf[i][fft::re]) + std::sin(v(k)*kx[i]*dt)*(im-dt*hEdvf[i][fft::im]);
	  		hf1[i][fft::im] = std::cos(v(k)*kx[i]*dt)*(im-dt*hEdvf[i][fft::im]) - std::sin(v(k)*kx[i]*dt)*(re-dt*hEdvf[i][fft::re]);
	  	}
	  	hf1.ifft(&(f1[k][0]));
	  }

    Edvf = weno::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        auto re = hf[i][fft::re] , im = hf[i][fft::im];
        hf2[i][fft::re] = 0.75*(std::cos(0.5*v(k)*kx[i]*dt)*re + std::sin(0.5*v(k)*kx[i]*dt)*im) + 0.25*( std::cos(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::re]-dt*hEdvf[i][fft::re]) - std::sin(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::im]-dt*hEdvf[i][fft::im]) );
        hf2[i][fft::im] = 0.75*(std::cos(0.5*v(k)*kx[i]*dt)*im - std::sin(0.5*v(k)*kx[i]*dt)*re) + 0.25*( std::cos(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::im]-dt*hEdvf[i][fft::im]) + std::sin(0.5*v(k)*kx[i]*dt)*(hf1[i][fft::re]-dt*hEdvf[i][fft::re]) );
      }
      hf2.ifft(&(f2[k][0]));
    }

    Edvf = weno::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        auto re = hf[i][fft::re] , im = hf[i][fft::im];
        hf[i][fft::re] = (1./3.)*(std::cos(v(k)*kx[i]*dt)*re + std::sin(v(k)*kx[i]*dt)*im) + (2./3.)*( std::cos(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::re]-dt*hEdvf[i][fft::re]) + std::sin(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::im]-dt*hEdvf[i][fft::im]) );
        hf[i][fft::im] = (1./3.)*(std::cos(v(k)*kx[i]*dt)*im - std::sin(v(k)*kx[i]*dt)*re) + (2./3.)*( std::cos(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::im]-dt*hEdvf[i][fft::im]) - std::sin(0.5*v(k)*kx[i]*dt)*(hf2[i][fft::re]-dt*hEdvf[i][fft::re]) );
      }
      hf.ifft(&(f[k][0]));
    }

    ++i_t;
	}

  err e;
  e.dt = dt;
  e.dx = f.step.dx;
  e.dv = f.step.dv;
  e.n = N;
  e.nb_iter = i_t;
  for ( auto k=0 ; k<Nv ; ++k ) {
    for (auto i=0 ; i<Nx ; ++i ) {
      e.one += std::abs(f[k][i]-f_sol[k][i])*f.step.dv*f.step.dx;
      e.infty = std::max(std::abs(f[k][i]-f_sol[k][i]),e.infty);
    }
  }

	return e;
}

int
main ( int argc , char** argv )
{
  for ( std::size_t exp=5 ; exp<11 ; ++exp ) {
    std::size_t n=1<<exp;
    err e = rotation(n);
    std::cout << e << std::endl;
  }

  return 0;
}


/*
discrete fourier transform for extension fields
*/

#ifndef DFT
#define DFT

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <vector>
#include <cassert>
#include "polynomial.hpp"

using namespace std;

// class to compute the powers of an element a in an extention field,
// via a lookup table
template<class co_t>
class fieldpower{

public:
unsigned n; // max power
co_t nm;

private:
co_t a; // a is an element of order n
co_t am; // the inverse of a; also has order n
vector<co_t> powers_a; // the lookup table for the powers of a
vector<co_t> powers_am; // the lookup table for the powers of a^(-1)

public:
	fieldpower(){};
	fieldpower(const unsigned n_, const co_t& a_){
		a = a_;
		n = n_;

		nm = co_t(n);
		nm = pow( (co_t) nm,-1  );

		// initialize powers_a
		powers_a.resize( n );
		powers_a[0] = co_t(1);
		powers_a[1] =	a;
		for(int i =2;i < powers_a.size();++i)
			powers_a[i] = powers_a[i-1]*a;

		// initialize powers_am
		am = pow( (co_t) a,-1);
		powers_am.resize( n );
		powers_am[0] = co_t(1);
		powers_am[1] =	am;
		for(int i =2;i < powers_am.size();++i)
			powers_am[i] = powers_am[i-1]*am;

		cout << "filedpower: lookup tables generated.. " << endl;
	}

	co_t copow(int powe) const {
		if(powe >= 0){
			return powers_a[ powe % n ];
		} else
			return powers_am[(-powe) % n ];
	}

};

template<class co_t>
void dft_primitive( const vector<co_t>& xv, vector<co_t>& Xv, fieldpower<co_t>& fp, int prefac){
	//assert(xv.size() == Xv.size());
	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		Xv[j] = co_t(0);
		for(int i=0;i < xv.size(); ++i){
			//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
			if(prefac<0)
				Xv[j] += fp.nm*xv[i]*fp.copow(-i*j);
			else
				Xv[j] += xv[i]*fp.copow(i*j);
		}
	}
};

// prefac = 1: normal fft, prefac =-1: inverse fft
template<class co_t>
void fft( const vector<co_t>& xv, vector<co_t>& Xv, const fieldpower<co_t>& fp, int prefac, unsigned Q, unsigned P){
	assert(Q*P == xv.size());

	// compute y_p^(r)
	vector<vector< co_t> > Y(Q,vector< co_t >(P, co_t(0) ));
	for(unsigned r=0;r<Q;++r){
		// for each r, a Fourier transform..
		for(unsigned p=0;p<P;++p){
			for(unsigned q=0;q<Q;++q){
				Y[r][p] += xv[P*q+p]*fp.copow(prefac*P*q*r);
			}
			Y[r][p] *= fp.copow(prefac*p*r);
		}
	}
	// compute A_{Qs+r}
	for(unsigned r=0;r<Q;r++){
	for(unsigned s=0;s<P;s++){
		Xv[Q*s+r] = co_t(0);
		for(unsigned p=0;p<P;++p){
			Xv[Q*s+r] += Y[r][p]*fp.copow(prefac*Q*s*p);
		}
		if(prefac == -1) Xv[Q*s+r] *= fp.nm;
	}
	}
}

////////////////////////////////////////////////
// FFT implementaion, uses fieldpower
template<class co_t>
class DFT_FFT{
private:
fieldpower<co_t> fp;
unsigned n; // the length of the dft
unsigned P;
unsigned Q;

public:
	DFT_FFT(const unsigned n_, const co_t& a_,unsigned P_, unsigned Q_){
		P = P_;
		Q = Q_;
		fp = fieldpower<co_t>(n_,a_);
	}
	DFT_FFT(){};

	void dft( const vector<co_t>& xv, vector<co_t>& Xv) const {
		fft<co_t>(xv,Xv,fp,1,P,Q);
	};

	void idft(vector<co_t>& xv, const vector<co_t>& Xv) const {
		fft<co_t>(Xv,xv,fp,-1,P,Q);
	}

};

////////////////////////////////////////////////

// DFT implementation of certain lenth that uses a precomputed lookup table to compute the powers of a
template<class co_t>
class DFT_LA{
private:
co_t a; // the Fourier kernal
co_t am;
unsigned n; // the length of the dft
co_t nm;
vector<co_t> powers_a; // the lookup table for the powers of a
vector<co_t> powers_am; // the lookup table for the powers of a^(-1)

public:
	DFT_LA(const unsigned n_, const co_t& a_){
		a = a_;
		n = n_;

		nm = co_t(n);
		nm = pow( (co_t) nm,-1  );

		// initialize powers_a
		powers_a.resize( ((n-1)*(n-1)+1) );
		powers_a[0] = co_t(1);
		powers_a[1] =	a;
		for(int i =2;i < powers_a.size();++i)
			powers_a[i] = powers_a[i-1]*a;

		// initialize powers_am
		am = pow( (co_t) a,-1);
		powers_am.resize( ((n-1)*(n-1)+1) );
		powers_am[0] = co_t(1);
		powers_am[1] =	am;
		for(int i =2;i < powers_am.size();++i)
			powers_am[i] = powers_am[i-1]*am;

		cout << "lookup tables generated.. " << endl;
	}

	void dft( vector<co_t>& xv, vector<co_t>& Xv) const {
		//assert(xv.size() == Xv.size());
		for(int j=0; j < Xv.size();++j){
			// compute Xv[j]
			Xv[j] = co_t(0);
			for(int i=0;i < xv.size(); ++i){
				//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
				Xv[j] += xv[i]*powers_a[i*j];
			}
		}
	};

	void idft(vector<co_t>& xv, vector<co_t>& Xv) const {
		assert(xv.size() == Xv.size());
		for(int j=0; j < xv.size();++j){
			// compute Xv[j]
			//cout << "adf"<< endl;
			xv[j] = co_t(0);
			//cout << j << endl;
			for(int i=0;i < Xv.size(); ++i){
				//cout << "--" << i << endl;
				//cout << i << "-- Xv: " <<  Xv[i] << " powers " << powers_am[i*j] << endl;
				//cout << "-- xv: " << xv[j] << endl;
				xv[j] += nm*Xv[i]*powers_am[i*j];
			}
		}
	}

};

// primitive DFT
template<class co_t>
class DFT_PRIM{
private:
co_t a; // the Fourier kernal
co_t am;
unsigned n; // the length of the dft
co_t nm;

public:
	DFT_PRIM(){};
	DFT_PRIM(const unsigned n_, const co_t& a_){
		a = a_;
		n = n_;

		nm = co_t(n);
		nm = pow( (co_t) nm,-1  );
	}

	void dft( vector<co_t>& xv, vector<co_t>& Xv) const {

		//assert(xv.size() == Xv.size());
		for(int j=0; j < Xv.size();++j){
			// compute Xv[j]
			Xv[j] = co_t(0);
			for(int i=0;i < xv.size(); ++i){
				//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
				Xv[j] += xv[i]*pow( (co_t) a, i*j);
			}
		}
	};

	void idft(vector<co_t>& xv, vector<co_t>& Xv) const {
		for(int j=0; j < Xv.size();++j){
			// compute Xv[j]
			xv[j] = co_t(0);
			for(int i=0;i < xv.size(); ++i)
				xv[j] += nm*Xv[i]*pow( (co_t) a,i*j);
		}
	}

};

// to do: implement the faster FFT
template<class coeff_t>
void dft( vector<coeff_t>& xv, vector<coeff_t>& Xv, coeff_t a ){

	cout << "startcomplookup" << endl;
	unsigned nn = Xv.size();
	//compute lookup table
	vector<coeff_t> powers( ((nn-1)*(nn-1)+1) );
	powers[0] = coeff_t(1);
	powers[1] =	a;
	for(int i =2;i < powers.size();++i)
		powers[i] = powers[i-1]*a;
	cout << "finishcomplookup" << endl;

	assert(xv.size() == Xv.size());
	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		Xv[j] = coeff_t(0);
		for(int i=0;i < xv.size(); ++i){
			//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
			Xv[j] += xv[i]*powers[i*j];
		}
	}
};

// nm: n^-1 ,where n is the lenght of the dft
// am: a^-1, where a is an element of order n
/*
template <class coeff_t>
void idft(vector<coeff_t>& xv, vector<coeff_t>& Xv,coeff_t am, coeff_t nm ){
	assert(xv.size() == Xv.size());
	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		xv[j] = coeff_t(0);
		for(int i=0;i < xv.size(); ++i)
			xv[j] += nm*Xv[i]*pow( (coeff_t) am, (int)  i*j);
	}

}
*/

template <class coeff_t>
void idft(vector<coeff_t>& xv, vector<coeff_t>& Xv,coeff_t a ){
	assert(xv.size() == Xv.size());

	unsigned nn = xv.size();
	coeff_t n = coeff_t(nn);
	cout << "n: " << n << endl;
	n = pow(n,-1);

	//compute lookup table
	cout << "startcomplookup" << endl;
	coeff_t am = pow( (coeff_t) a,-1);
	vector<coeff_t> powers( ((nn-1)*(nn-1)+1) );
	powers[0] = coeff_t(1);
	powers[1] =	am;
	for(int i =2;i < powers.size();++i)
		powers[i] = powers[i-1]*am;

	cout << "finishlookuptable.. " << endl;

	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		xv[j] = coeff_t(0);
		for(int i=0;i < xv.size(); ++i){

			//xv[j] += n*Xv[i]*pow( (coeff_t) am, (int)  i*j);
			xv[j] += n*Xv[i]*powers[i*j];
		}
	}

}

#endif


//
// Created by Vlad Argunov on 17/01/2022.
//

#include "OptimalExecution.h"

#include <iostream>
#include <Eigen/Dense>
#include <random>
#include <string>
#include <cmath>
#include "Simulation.h"

OptimalExecution::OptimalExecution(double a_coeff_, double b_coeff_, double sigma_coeff_, double k_coeff_, double phi_coeff_) {
    a_coeff = a_coeff_;
    b_coeff = b_coeff_;
    sigma_coeff = sigma_coeff_;
    k_coeff = k_coeff_;
    phi_coeff = phi_coeff_;
}

void OptimalExecution::compute_liquidation(const std::string& trading_speed, double impact_nonlinearity) {
    int num_rows = num_diff_q + 1;
    int num_cols = num_diff_t + 1;
    sim = Simulation();
    value_matrix.resize(num_rows, num_cols);
    optimal_speed_matrix.resize(num_rows, num_cols);
    double diff_q = 1.0 / num_diff_q;
    double diff_t = 1.0 / num_diff_t;
    double quantity;
    double h_diff_q_up;
    double h_diff_q_down;
    double partial_diff_q;

    Eigen::VectorXd diffusion; diffusion.resize(num_cols - 1);
    diffusion = sim.generate_normal_variable(num_cols - 1, 0, pow(diff_t, 2));

    stock.resize(num_cols); inventory.resize(num_cols);
    cash.resize(num_cols); optimal_speed.resize(num_cols);
    value_full.resize(num_cols);

    if (trading_speed == "Nonlinear" || trading_speed == "Linear") {

        // That part computes the matrix of value_matrix function for each value of time and quantity.
        for (int col = (num_cols - 1); col > -1; --col) { // Time space
            for (int row = 0; row < num_rows; ++row) { // Stock space
                quantity = row * diff_q;

                if (trading_speed == "Linear"){
                    double gamma = sqrt(phi_coeff / k_coeff);
                    double zeta = (a_coeff - 0.5 * b_coeff + sqrt(k_coeff * phi_coeff)) / (a_coeff - 0.5 * b_coeff - sqrt(k_coeff * phi_coeff));
                    double remaining_time = (num_cols - col) * diff_t;
                    optimal_speed_matrix(row, col) = gamma * (zeta * exp(gamma * remaining_time) + exp(- gamma * remaining_time)) / (zeta * exp(gamma) - exp(- gamma));
                }

                if (col == (num_cols - 1)){
                    value_matrix(row, col) = - a_coeff * quantity * quantity;

                    if (trading_speed == "Nonlinear") {
                        optimal_speed_matrix(row, col) = pow(abs(-(b_coeff * quantity - 2 * a_coeff * quantity) / ((1 + a_coeff) * k_coeff))
                                ,1.0 / impact_nonlinearity);
                    }
                } else {

                    if (row == 0){
                        h_diff_q_up =  value_matrix(row + 1, col + 1);
                        h_diff_q_down =  value_matrix(row, col + 1);
                        partial_diff_q = (h_diff_q_up - h_diff_q_down) / (diff_q);
                    } else if (row == (num_rows - 1)) {
                        h_diff_q_up =  value_matrix(row, col + 1);
                        h_diff_q_down =  value_matrix(row - 1, col + 1);
                        partial_diff_q = (h_diff_q_up - h_diff_q_down) / (diff_q);
                    } else {
                        h_diff_q_up =  value_matrix(row + 1, col + 1);
                        h_diff_q_down =  value_matrix(row - 1, col + 1);
                        partial_diff_q = (h_diff_q_up - h_diff_q_down) / (2 * diff_q);
                    }

                    value_matrix(row, col) = value_matrix(row, col + 1) -
                            diff_t * (phi_coeff * pow(quantity, 2) - a_coeff * k_coeff *
                            pow(abs(-(b_coeff * quantity + partial_diff_q ) / ((1 + a_coeff) * k_coeff))
                                ,1.0 + 1.0/ impact_nonlinearity));

                    if (trading_speed == "Nonlinear"){
                        optimal_speed_matrix(row, col) = pow(abs(-(b_coeff * quantity + partial_diff_q) / ((1 + a_coeff) * k_coeff))
                                ,1.0 / impact_nonlinearity);
                    }

                }
            }
        }

        // That part simulates stock, cash, and inventory processes
        for (int step = 0; step < num_cols; ++step) {
            if (step == 0){
                inventory(step) = 1;
                stock(step) = 1;
                cash(step) = 0;

                optimal_speed(step) = optimal_speed_matrix(num_rows - 1, step);
                value_full(step) = cash(step) + inventory(step) * stock(step) + value_matrix(num_rows - 1, step);

            } else {
                inventory(step) = std::max(inventory(step - 1) - optimal_speed(step - 1) * diff_t, 0.0);
                stock(step) = stock(step - 1) - b_coeff * optimal_speed(step - 1) * diff_t
                        + sigma_coeff * diffusion(step - 1);
                cash(step) = cash(step - 1) + (stock(step - 1)
                        - k_coeff * pow(optimal_speed(step - 1), impact_nonlinearity)) * optimal_speed(step - 1) * diff_t;

                // Determine the relevant trading speed and value function
                for (int row = 0; row < num_rows; ++row) {
                    if ((row * diff_q <= inventory(step)) && ((row + 1) * diff_q > inventory(step))){
                        if (row != (num_rows - 1)){
                            optimal_speed(step) =
                                    (((row + 1) * diff_q - inventory(step)) / diff_q) * optimal_speed_matrix(row, step) +
                                    ((inventory(step) - row * diff_q) / diff_q) * optimal_speed_matrix(row + 1, step);

                            value_full(step) = cash(step) + inventory(step) * stock(step) +
                                    (((row + 1) * diff_q - inventory(step)) / diff_q) * value_matrix(row, step) +
                                    ((inventory(step) - row * diff_q) / diff_q) * value_matrix(row + 1, step);
                        } else {
                            optimal_speed(step) = optimal_speed_matrix(num_rows - 1, step);
                            value_full(step) = cash(step) + inventory(step) * stock(step) + value_matrix(num_rows - 1, step);
                        }

                    }
                }
            }
        }
    }

    if (inventory(num_cols - 1) > 0.0){
        cash(num_cols - 1) += inventory(num_cols - 1) * (stock(num_cols - 1) - a_coeff * inventory(num_cols - 1));
    }
    }

void OptimalExecution::set_num_steps(int num_diff_t_, int num_diff_q_) {
    num_diff_t = num_diff_t_;
    num_diff_q = num_diff_q_;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
#define CROCODDYL_CORE_UTILS_MATH_HPP_

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

template <typename MatrixType>
MatrixType pseudoInverse(const MatrixType& a, double epsilon = std::numeric_limits<double>::epsilon()) {
  Eigen::JacobiSVD<MatrixType> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
  double tolerance =
      epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
  return svd.matrixV() *
         (svd.singularValues().array().abs() > tolerance)
             .select(svd.singularValues().array().inverse(), 0)
             .matrix()
             .asDiagonal() *
         svd.matrixU().adjoint();
}

#endif  // CROCODDYL_CORE_UTILS_MATH_HPP_


#include "ros_utils.hpp"
#include "kinematics.hpp"
#include <boost/array.hpp>

#include <casadi/casadi.hpp>

#include <mhe_estimator/CanData.h>
#include <mhe_estimator/ArticulatedAngles.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "ackermann_msgs/AckermannDrive.h"

namespace mhe_estimator
{
    using namespace casadi;

    void mheSetupCar(casadi::Function& solver, const mhe_estimator::CarParams& params, const mhe_estimator::MheParams& mheParams,const long unsigned int N_mhe)
    {

        double A = N_mhe;
        double B = N_mhe+1;
        SX theta = SX::sym("theta"); SX x = SX::sym("x"); SX y = SX::sym("y"); SX beta0 = SX::sym("beta0");
        SX u1 = SX::sym("u1"); SX u2 = SX::sym("u2");

        SX states = vertcat(theta, x, y, beta0);
        SX controls = vertcat(u1, u2);  //u1 =dbeta     u2= linear vel
        unsigned int n_states = states.size1(); unsigned int n_controls = controls.size1();

        SX rhs = vertcat(u2*(1/params.L)*tan(beta0), u2*cos(theta), u2*sin(theta), u1); //system r.h.s  theta, x, y, beta0
        Function f = Function("f",{states,controls}, {rhs}); //nonlinear mapping function f(x,u)

        SX U = SX::sym("U",n_controls,A);     //A = N_MHE
        SX X = SX::sym("X",n_states,B);       //B = N_MHE +1
        SX P = SX::sym("P",n_states,B+A+B+A); //state,control,covOfState,covOfControl

        SX obj = 0;
        SX g;

        for(int k = 0; k < B; k++) // Create states objective function
        {
            SX h_x = vertcat(X(0,k), X(1,k), X(2,k), X(3,k));
            SX y_tilde = vertcat(P(0,k), P(1,k), P(2,k), P(3,k));
            SX V = SX::zeros(n_states,n_states);
            V(0,0) = P(0,B+A+k);
            V(1,1) = P(1,B+A+k);
            V(2,2) = P(2,B+A+k);
            V(3,3) = P(3,B+A+k);
            obj = obj + mtimes(mtimes(((y_tilde - h_x).T()), V), (y_tilde - h_x));
        }
        for(int k = 0; k < A; k++) // Create control objective function
        {
            SX con = vertcat(U(0,k), U(1,k));
            SX u_tilde = vertcat(P(0,B+k), P(1,B+k));
            SX W = SX::zeros(n_controls,n_controls);
            W(0,0) = P(0,B+A+B+k);
            W(1,1) = P(1,B+A+B+k);
            obj = obj + mtimes(mtimes(((u_tilde - con).T()), W), (u_tilde - con));
        }

        for(int k = 0; k < A; k++) //  multiple shooting constraints
        {
            SX con = vertcat(U(0,k), U(1,k));
            SX st = vertcat(X(0,k), X(1,k), X(2,k), X(3,k));
            SX st_next = vertcat(X(0,k+1), X(1,k+1), X(2,k+1), X(3,k+1));
            SXVector func = f(SXVector{st,con});
            SX f_value = SX::vertcat(func);
            SX st_next_euler = st + ((1/mheParams.loopRate)*f_value);
            g = vertcat(g, (st_next-st_next_euler));
        }

        SX OPT_variables; //optimization variable

        for(int j = 0; j < X.size2(); j++) //reshape X and apend
        {
            for(int i = 0; i < X.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, X(i,j));
            }
        }
        for(int j = 0; j < U.size2(); j++) //reshape U and apend
        {
            for(int i = 0; i < U.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, U(i,j));
            }
        }

        SXDict nlp = {{"f", obj}, {"x", OPT_variables}, {"g", g}, {"p", P}};
        Dict opts;
        /*
        opts["qpsol"] = "qpoases";
//      opts["max_iter"] =2;

        opts["print_iteration"] = true;
        Dict qopts;
        qopts["sparse"]=true;

        opts["qpsol_options"] = qopts;

        */
        opts["ipopt.max_iter"] = 2000;
        opts["ipopt.print_level"] = 0;
        opts["print_time"] = false;
        opts["ipopt.acceptable_tol"] = 1e-6;
        opts["ipopt.acceptable_obj_change_tol"] = 1e-4;
        opts["ipopt.print_timing_statistics"] = "no";
        opts["ipopt.print_info_string"] = "no";

        solver = nlpsol("nlpsol", "ipopt", nlp, opts);

        //solver = nlpsol("nlpsol", "sqpmethod", nlp, opts);

    }

    void mheSetupTrailer(casadi::Function& solver,const mhe_estimator::CarParams& params,const mhe_estimator::MheParams& mheParams,const long unsigned int N_mhe)
    {
        double A = N_mhe;
        double B = N_mhe+1;
        SX beta1 = SX::sym("beta1"); SX theta = SX::sym("theta"); SX x = SX::sym("x"); SX y = SX::sym("y"); SX beta0 = SX::sym("beta0");
        SX u1 = SX::sym("u1"); SX u2 = SX::sym("u2");

        SX states1 = vertcat(beta1,theta, x);
        SX states2 = vertcat(y, beta0);
        SX states = vertcat(states1, states2); // due to casadi limitation
        SX controls = vertcat(u1, u2);  //u1 =dbeta     u2= linear vel
        unsigned int n_states = states.size1(); unsigned int n_controls = controls.size1();

        SX rhs;
        SX k1 = (1/params.L1)*tan(beta1 - atan((params.Lh1/params.L)*tan(beta0)));
        if(!params.moveGuidancePoint)  //OneTrailerKinematicsGPRear
        {
            SX dq0 = u2 * (sin(beta1)/params.Lh1 - (1 + (params.L1/params.Lh1)*cos(beta1))*(k1)); //beta1
            SX dq1 = u2 * k1; //theta
            SX dq2 = u2 * cos(theta); //x
            SX dq3 = u2 * sin(theta); //y
            SX dq4 = u1; //beta0

            SX rhs1 = vertcat(dq0, dq1, dq2); //due to casadi limitation
            SX rhs2 = vertcat(dq3, dq4);
            rhs = vertcat(rhs1, rhs2); //system r.h.s
        }
        else    //OneTrailerKinematicsGPFront
        {
            SX dq0 = u2 * (sin(beta1)/params.Lh1 - (1 + (params.L1/params.Lh1)*cos(beta1))*k1); //beta1
            SX dq1 = u2 * ( -(params.L1/params.Lh1)*cos(beta1)*k1 + sin(beta1)/params.Lh1 ); //theta
            SX dq2 = u2 * cos(theta) * ( params.L1*sin(beta1)*k1 + cos(beta1) ); //x
            SX dq3 = u2 * sin(theta) * ( params.L1*sin(beta1)*k1 + cos(beta1) ); //y
            SX dq4 = u1; //beta0

            SX rhs1 = vertcat(dq0, dq1, dq2); //due to casadi limitation
            SX rhs2 = vertcat(dq3, dq4);
            rhs = vertcat(rhs1, rhs2); //system r.h.s
        }

        //ROS_INFO_STREAM("rhstRAILER: " << rhs <<" ");

        Function f = Function("f",{states,controls}, {rhs}); //nonlinear mapping function f(x,u)

        SX U = SX::sym("U",n_controls,A); //A = N_MHE
        SX X = SX::sym("X",n_states,B); //B = N_MHE +1
        SX P = SX::sym("P",n_states,B+A+B+A);

        SX obj = 0;
        SX g;

        for(int k = 0; k < B; k++) // Create states objective function
        {
            SX h_x1 = vertcat(X(0,k), X(1,k), X(2,k));
            SX h_x2 = vertcat(X(3,k), X(4,k) );
            SX h_x = vertcat(h_x1, h_x2);

            SX y_tilde1 = vertcat(P(0,k), P(1,k), P(2,k));
            SX y_tilde2 = vertcat(P(3,k), P(4,k));
            SX y_tilde = vertcat(y_tilde1, y_tilde2);

            SX V = SX::zeros(n_states,n_states);
            V(0,0) = P(0,B+A+k);
            V(1,1) = P(1,B+A+k);
            V(2,2) = P(2,B+A+k);
            V(3,3) = P(3,B+A+k);
            V(4,4) = P(4,B+A+k);
            obj = obj + mtimes(mtimes(((y_tilde - h_x).T()), V), (y_tilde - h_x));
        }
        for(int k = 0; k < A; k++) // Create control objective function
        {
            SX con = vertcat(U(0,k), U(1,k));
            SX u_tilde = vertcat(P(0,B+k), P(1,B+k));
            SX W = SX::zeros(n_controls,n_controls);
            W(0,0) = P(0,B+A+B+k);
            W(1,1) = P(1,B+A+B+k);
            obj = obj + mtimes(mtimes(((u_tilde - con).T()), W), (u_tilde - con));
        }

        for(int k = 0; k < A; k++) //  multiple shooting constraints
        {
            SX con = vertcat(U(0,k), U(1,k));

            SX st1 = vertcat(X(0,k), X(1,k), X(2,k));
            SX st2 = vertcat(X(3,k), X(4,k));
            SX st = vertcat(st1,st2);

            SX st_next1 = vertcat(X(0,k+1), X(1,k+1), X(2,k+1));
            SX st_next2 = vertcat(X(3,k+1), X(4,k+1));
            SX st_next = vertcat(st_next1, st_next2);

            SXVector func = f(SXVector{st,con});
            SX f_value = SX::vertcat(func);
            SX st_next_euler = st + ((1/mheParams.loopRate)*f_value);
            g = vertcat(g, (st_next-st_next_euler));
        }

        //ROS_INFO_STREAM("g " << g <<" ");

        SX OPT_variables; //optimization variable

        for(int j = 0; j < X.size2(); j++) //reshape X and apend
        {
            for(int i = 0; i < X.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, X(i,j));
            }
        }
        for(int j = 0; j < U.size2(); j++) //reshape U and apend
        {
            for(int i = 0; i < U.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, U(i,j));
            }
        }
        //ROS_INFO_STREAM("g.size " << g.size1() <<" ");

        SXDict nlp = {{"f", obj}, {"x", OPT_variables}, {"g", g}, {"p", P}};
        Dict opts;

//        opts["qpsol"] = "gurobi";
//        opts["hessian_approximation"] = "limited-memory";

//        opts["max_iter"] =10;

//        opts["print_iteration"] = true;
//        opts["warn_initial_bounds"] = true;
//        Dict qopts;
//       qopts["gurobi.Threads"]=4;
//       qopts["verbose"]=false;

////        qopts["sparse"]=true;
////         qopts["printLevel"]="low";
//        qopts["linsol_plugin"]="gurobi";

//        opts["qpsol_options"] = qopts;
//       opts["qpsol_options"] = qopts;

        opts["ipopt.max_iter"] = 2000;
        opts["ipopt.print_level"] = 0;
        opts["print_time"] = false;
        opts["ipopt.acceptable_tol"] = 1e-6;
        opts["ipopt.acceptable_obj_change_tol"] = 1e-4;
        opts["ipopt.print_timing_statistics"] = "no";
        opts["ipopt.print_info_string"] = "no";

        solver = nlpsol("nlpsol", "ipopt", nlp, opts);
    }

  ////////////////////////////////////////////////////////////////////////////////////////////////////////
    template <class T, class T2, long unsigned int A, long unsigned int B>
    void estimateMhe(casadi::DM& argx0, const boost::array<T, B>& q4wLoc,
        const boost::array<T2, A>& control2w,const boost::array<T, B>& q4wCov,
        const boost::array<T2, A>& control2wCov, const mhe_estimator::CarParams& carParams,
        const mhe_estimator::MheParams& mheParams,const casadi::Function& solver)
    {
        unsigned int n_states = 4; unsigned int n_controls = 2;
        std::map<std::string, DM> arg, res;
        arg["lbg"] = SX::zeros(n_states*A); //size = n_state * N_Mhe
        arg["ubg"] = SX::zeros(n_states*A);

        std::vector<double> lbx((n_controls*A)+(n_states*B), 0.0);
        std::vector<double> ubx((n_controls*A)+(n_states*B), 0.0);

        for(int i = 0; i < (n_states*B); i += n_states)
        {
            lbx[i] = carParams.lowerTh;  //theta lower bound
            lbx[i+1]   = carParams.lowerX;    //x lower bound
            lbx[i+2] = carParams.lowerY;  //y lower bound
            lbx[i+3] = -carParams.steeringLimit;

            ubx[i] = carParams.upperTh;   //theta upper bound
            ubx[i+1]   = carParams.upperX;    //x upper bound
            ubx[i+2] = carParams.upperY;  //y upper bound
            ubx[i+3] = carParams.steeringLimit; //beta upper bound

        }
        for(int i = ((n_states*B)); i < ((n_controls*A)+(n_states*B)); i += n_controls)
        {
            lbx[i] = -carParams.SteeringVelLimit;    //v lower bound
            lbx[i+1] = -carParams.LinearVelLimit;  //beta lower bound

            ubx[i] = carParams.SteeringVelLimit;    //v upper bound
            ubx[i+1] = carParams.LinearVelLimit;  //beta upper bound

        }
        arg["lbx"] = lbx;
        arg["ubx"] = ubx;
        //--------------ALL OF THE ABOVE IS JUST A PROBLEM SET UP-------------//

        DM argp = SX::zeros(n_states,B+A+B+A);
        for(int j = 0; j < B; j++) //measured states
        {
            boost::array<double, 4> qloc = q4wLoc[j];
            argp(0,j) = qloc[0];
            argp(1,j) = qloc[1];
            argp(2,j) = qloc[2];
            argp(3,j) = qloc[3];
        }
        for(int j = 0; j < A; j++) //measured controls
        {
            boost::array<double, 2> qctr = control2w[j];
            //argp(0,j + B) = 0.0;   //we do not have measured control dbeta
            argp(1,B+j) = qctr[1];
        }
        for(int j = 0; j < B; j++) //state cov matrix
        {
            boost::array<double, 4> qCov = q4wCov[j];
            argp(0,B+A+j) = qCov[0];
            argp(1,B+A+j) = qCov[1];
            argp(2,B+A+j) = qCov[2];
            argp(3,B+A+j) = qCov[3];
        }
        for(int j = 0; j < A; j++) //control cov matrix
        {
            boost::array<double, 2> ctrCov = control2wCov[j];
            argp(0,B+A+B+j) = 0.0;   //we do not have measured control dbeta
            argp(1,B+A+B+j) = ctrCov[1];
        }

        arg["p"] = argp;
        arg["x0"] = argx0; //Comes as function parameter

        res = solver(arg); // solve MHE
        argx0 = res.at("x");

    }

    template <class T, class T2, long unsigned int A, long unsigned int B>
    void estimateMheTrailer(casadi::DM& argx0Trailer,const boost::array<T, B>& q5wLocTrailer,
        const boost::array<T2, A>& control2wTrailer,const boost::array<T, B>& q5wCovTrailer,
        const boost::array<T2, A>& control2wCovTrailer,const mhe_estimator::CarParams& carParams,
        const mhe_estimator::MheParams& mheParams,const casadi::Function& solver)  //check the ref and const
    {

        unsigned int n_states = 5; unsigned int n_controls = 2;
        std::map<std::string, DM> arg, res;
        arg["lbg"] = SX::zeros(n_states*A); //size = n_state * N_Mhe
        arg["ubg"] = SX::zeros(n_states*A);

        std::vector<double> lbx((n_controls*A)+(n_states*B), 0.0);
        std::vector<double> ubx((n_controls*A)+(n_states*B), 0.0);

        for(int i = 0; i < (n_states*B); i += n_states)
        {
            lbx[i] = -carParams.trailer1Limit;
            lbx[i+1] = carParams.lowerTh;   //theta lower bound
            lbx[i+2] = carParams.lowerX;    //x lower bound
            lbx[i+3] = carParams.lowerY;    //y lower bound
            lbx[i+4] = -carParams.steeringLimit;

            ubx[i] = carParams.trailer1Limit;
            ubx[i+1] = carParams.upperTh;   //theta upper bound
            ubx[i+2] = carParams.upperX;    //x upper bound
            ubx[i+3] = carParams.upperY;    //y upper bound
            ubx[i+4] = carParams.steeringLimit;
        }

        ////////////////////////////////////////////

        for(int i = (n_states*B); i < ((n_controls*A)+(n_states*B)); i += n_controls)
        {
            lbx[i] = -carParams.SteeringVelLimit;    //dbeta lower bound
            lbx[i+1] = -carParams.LinearVelLimit;  //linear.vel lower bound

            ubx[i] = carParams.SteeringVelLimit;  //dbeta upper bound
            ubx[i+1] = carParams.LinearVelLimit;  //linear.vel upper bound
        }
        arg["lbx"] = lbx;
        arg["ubx"] = ubx;
        //--------------ALL OF THE ABOVE IS JUST A PROBLEM SET UP-------------//

        DM argp = SX::zeros(n_states,B+A+B+A);
        for(int j = 0; j < B; j++) //measured state
        {
            boost::array<double, 5> qloc = q5wLocTrailer[j];
            argp(0,j) = qloc[0]; //beta1
            argp(1,j) = qloc[1]; //theta
            argp(2,j) = qloc[2]; //x
            argp(3,j) = qloc[3]; //y
            argp(4,j) = qloc[4]; //beta0
        }
        for(int j = 0; j < A; j++) //measured control
        {
            boost::array<double, 2> qctr = control2wTrailer[j];
            //argp(0,j+B) = 0.0;   //we do not have measured control dbeta
            argp(1,j+B) = qctr[1];
        }
        for(int j = 0; j < B; j++) //state cov matrix
        {
            boost::array<double, 5> qCov = q5wCovTrailer[j];
            argp(0,B+A+j) = qCov[0]; //beta1
            argp(1,B+A+j) = qCov[1]; //theta
            argp(2,B+A+j) = qCov[2]; //x
            argp(3,B+A+j) = qCov[3]; //y
            argp(4,B+A+j) = qCov[4]; //beta0
        }
        for(int j = 0; j < A; j++) //control cov matrix
        {
            boost::array<double, 2> ctrCov = control2wCovTrailer[j];
            argp(0,B+A+B+j) = 0.0;   //we do not have measured control dbeta
            argp(1,B+A+B+j) = ctrCov[1];
        }

        arg["p"] = argp;
        arg["x0"] = argx0Trailer; //Comes as parameter

        try
        {
          res = solver(arg); // solve MHE
          argx0Trailer = res.at("x");
        } catch (casadi::CasadiException& ex)
        {
          ROS_ERROR_STREAM(ex.what());

        }

    }

    void estimateEst(Vec3& qCarEst,const Vec3& qLoc,const Vec3& qPred,const MheParams& estParams)
    {

        qCarEst[0] = estParams.WeightTh*qPred[0] + (1-estParams.WeightTh)*qLoc[0];
        qCarEst[1] = estParams.WeightPos*qPred[1] + (1-estParams.WeightPos)*qLoc[1];
        qCarEst[2] = estParams.WeightPos*qPred[2] + (1-estParams.WeightPos)*qLoc[2];

    }

    void estimateEstTrailer(Vec4& qTrailerEst,const Vec4& qTrailerLoc,const Vec4& qTrailerPred,const MheParams& estParams)
    {   qTrailerEst[0] = estParams.WeightTrailer1*qTrailerPred[0] + (1-estParams.WeightTrailer1)*qTrailerLoc[0];
        qTrailerEst[1] = estParams.WeightTh*qTrailerPred[1] + (1-estParams.WeightTh)*qTrailerLoc[1];
        qTrailerEst[2] = estParams.WeightPos*qTrailerPred[2] + (1-estParams.WeightPos)*qTrailerLoc[2];
        qTrailerEst[3] = estParams.WeightPos*qTrailerPred[3] + (1-estParams.WeightPos)*qTrailerLoc[3];
    }

}


///////////////////////////////////////////////////////////////////////////////
// kernel::example::mono_bw_kernel_rp.cpp                                    //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/special_functions/fpclassify.hpp> //needed?
#include <boost/math/tools/precision.hpp>

#include <boost/statistics/detail/kernel/kernels/scalar/gaussian.hpp>
#include <boost/statistics/detail/kernel/kernels/multivariate/mono_bw.hpp>
#include <boost/statistics/detail/kernel/estimation/rp_visitor.hpp>
#include <boost/statistics/detail/kernel/estimation/estimator.hpp>
#include <libs/statistics/detail/kernel/example/scalar_rp.h>

void example_mv_mono_bw_rp(std::ostream& out){
    out << "-> example_mono_bw_kernel_rp : ";

    // This example shows how to compute a Rosenblatt-Parzen estimate of the
    // density, p(x). The type used for each data-unit, x, is a vector of
    // doubles, and the kernel uses the same bandwidth throughout all
    // coordinates

    using namespace boost;
    namespace kernel = boost::statistics::detail::kernel;

    // Types
    typedef double                                          val_;
    typedef std::vector<val_>                               vec_;
    typedef vec_                                            x_;
    typedef std::vector<x_>                                 dataset_;
    typedef mt19937                                         urng_;
    typedef normal_distribution<val_>                       norm_;
    typedef variate_generator<urng_&,norm_>                 gen_;
    typedef kernel::scalar::gaussian_kernel<val_>                  gauss_k_;

    const unsigned dim = 2;
    typedef kernel::multivariate::mono_bw_kernel<gauss_k_,dim> mono_bw_kernel_k_;
    // Use of a const reference is not necessary but probably improves speed
    typedef kernel::rp_visitor<mono_bw_kernel_k_,const x_&>  rp_visitor_;

    // Constants
    const val_ bandwidth = 0.5;
    const val_ eps = math::tools::epsilon<val_>();
    const unsigned n = 10;

    // Generate n samples, each drawn from prod{N(0,1):i=1,...,dim}
    dataset_ dataset; dataset.reserve(n);
    vec_ vec_rp; vec_rp.reserve(n);
    urng_ urng;
    norm_ norm;
    gen_ gen(urng,norm);
    for(unsigned i = 0; i<n; i++){
        vec_ tmp(dim);
        std::generate_n(
            boost::begin(tmp),
            dim,
            gen
        );
        dataset.push_back( tmp );
    }

    // Density estimate for each x in dataset
    BOOST_FOREACH(const x_& x,dataset){
        val_ rp = std::for_each(
            boost::begin(dataset),
            boost::end(dataset),
            rp_visitor_(bandwidth,x)
        ).estimate();
        vec_rp.push_back(rp);
    }
    typedef sub_range<dataset_> sub_;
    typedef kernel::estimator<
        sub_,
        kernel::rp_visitor,
        mono_bw_kernel_k_
    > estimator_;
    estimator_ estimator(bandwidth);
    estimator.train(sub_(dataset));
    vec_ vec_rp2; vec_rp2.reserve(n);

    // Same as previous but calls estimator instead of for_each
    for(unsigned i = 0; i<n; i++){
        x_ x = dataset[i];
        val_ rp = vec_rp[i];
        val_ rp2 = estimator.predict(x);
        BOOST_ASSERT(fabs(rp-rp2)<eps);
    }
    out << "<-" << std::endl;
}


#include "Eigen/Dense"
#include <Eigen/Eigenvalues>
#include <cmath>
#include <vector>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>

typedef Eigen::Matrix< double , Eigen::Dynamic , Eigen::Dynamic > MatrixXd;

void load_int_scalar(int &in, std::string filename) {
  std::ifstream input_file(filename);
  input_file >> in;
}

void load_double_scalar(double &in, std::string filename) {
  std::ifstream input_file(filename);
  input_file >> in;
}

void load_double(std::vector<double> &in, std::string filename) {
  std::ifstream input_file(filename);
  double temp_val;
  std::string line;
  while (std::getline(input_file, line)) {
    std::istringstream ss(line);
    while (ss >> temp_val) {
      in.push_back(temp_val);
    }
  }
}

size_t idx2(size_t &i, size_t &j) {
  if (i >= j) {
    return i * (i + 1) / 2 + j;
  } else {
    return j * (j + 1) / 2 + i;
  }
}

size_t idx4(size_t &i, size_t &j, size_t &k, size_t &l) {
  size_t temp_ij = idx2(i, j);
  size_t temp_kl = idx2(k, l);
  return idx2(temp_ij, temp_kl);
}

int main(int argc, char const *argv[]) {
  int iteration_max;
  int num_elec_alpha;
  int num_elec_beta;
  int num_ao;

  load_int_scalar(num_elec_alpha, "../../data/num_elec_alpha.txt");
  load_int_scalar(num_elec_beta, "../../data/num_elec_beta.txt");
  load_int_scalar(iteration_max, "../../data/iteration_max.txt");
  load_int_scalar(num_ao, "../../data/num_ao.txt");

  std::vector<double> S_vec;
  load_double(S_vec, "../../data/S.txt");
  Eigen::Map<MatrixXd> S(S_vec.data(), num_ao, num_ao);

  std::vector<double> T_vec;
  load_double(T_vec, "../../data/T.txt");
  Eigen::Map<MatrixXd> T(T_vec.data(), num_ao, num_ao);

  std::vector<double> V_vec;
  load_double(V_vec, "../../data/V.txt");
  Eigen::Map<MatrixXd> V(V_vec.data(), num_ao, num_ao);

  std::vector<double> eri_vec;
  load_double(eri_vec, "../../data/eri.txt");
  Eigen::Map<MatrixXd> eri(eri_vec.data(),45150,1);

  double convergence_DM;
  load_double_scalar(convergence_DM, "../../data/convergence_DM.txt");
  double convergence_E;
  load_double_scalar(convergence_DM, "../../data/convergence_E.txt");
  double E_nuc;
  load_double_scalar(E_nuc, "../../data/E_nuc.txt");

  MatrixXd D = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd D_last = MatrixXd::Zero(num_ao,num_ao);
  // loop variables
  int iteration_num = 0;
  double E_total = 0.0;
  double E_elec = 0.0;
  double E_elec_last = 0.0;
  double iteration_E_diff = 0.0;
  double iteration_rmsc_dm = 0.0;
  bool converged = false;
  bool exceeded_iterations = false;
  Eigen::SelfAdjointEigenSolver<MatrixXd> eigen_solver(num_ao);
  eigen_solver.compute(S);
  auto s = eigen_solver.eigenvalues().transpose();
  auto L = eigen_solver.eigenvectors();
  MatrixXd X = MatrixXd::Zero(num_ao,num_ao);
  for (size_t i = 0; i < s.size(); i++) {
    X(i, i) = 1.0 / std::sqrt(s(i));
  }
  X = L*X*L.transpose();
  auto H = T + V;
  MatrixXd G = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd F = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd F_prime = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd C = MatrixXd::Zero(num_ao,num_ao);
  while (!converged && !exceeded_iterations) {
    E_elec_last = E_elec;
    D_last = D;

    iteration_num++;
    // form G matrix
    MatrixXd G = MatrixXd::Zero(num_ao,num_ao);
    for (size_t i = 0; i < num_ao; i++) {
      for (size_t j = 0; j < num_ao; j++) {
        for (size_t k = 0; k < num_ao; k++) {
          for (size_t l = 0; l < num_ao; l++) {
            G(i, j) += D(k, l) * ((2.0 * (eri(idx4(i, j, k, l)))) - (eri(idx4(i, k, j, l))));
          }
        }
      }
    }

    F = H + G;
    F_prime = X*F*X;

    eigen_solver.compute(F_prime);
    auto E_orbitals = eigen_solver.eigenvalues().transpose();
    auto C_prime = eigen_solver.eigenvectors();

    C = X*C_prime;
    D.setZero();

    for (size_t i = 0; i < num_ao; i++) {
      for (size_t j = 0; j < num_ao; j++) {
        for (size_t k = 0; k < num_elec_alpha; k++) {
          D(i, j) += C(i, k) * C(j, k);
        }
      }
    }
    E_elec = D.cwiseProduct(H + F).sum();
    iteration_E_diff = std::abs(E_elec - E_elec_last);
    iteration_rmsc_dm = std::sqrt((D-D_last).cwiseProduct(D-D_last).sum());
    if (iteration_E_diff < convergence_E &&
        iteration_rmsc_dm < convergence_DM) {
      converged = true;
    }
    if (iteration_num == iteration_max) {
      exceeded_iterations = true;
    }
  }
  E_total = E_elec + E_nuc;
  printf("%20.15f\n", E_total);
  return 0;
}


#include "Lshape.hpp"
#include "convergence.hpp"
#include "fem_solve.hpp"
#include "stiffness_matrix.hpp"
#include "writer.hpp"
#include <Eigen/Core>
#include <sstream>
#include <tuple>

// constant sigma
double sigma(double x, double y) {
	std::ignore = x;
	std::ignore = y;
	return 1.0;
}

// Data for square
double f_square(double x, double y) {
	return 2 * M_PI * M_PI * sin(M_PI * x) * sin(M_PI * y);
}

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

double uex_square(double x, double y) {
	return sin(M_PI * x) * sin(M_PI * y);
}

Eigen::Vector2d uex_grad_square(double x, double y) {
	Eigen::Vector2d grad;
	grad << M_PI * cos(M_PI * x) * sin(M_PI * y),
	    M_PI * sin(M_PI * x) * cos(M_PI * y);
	return grad;
}

Eigen::Vector2d u_square_ex_grad(double x, double y) {
	Eigen::Vector2d gradient;
	gradient << M_PI * cos(M_PI * x) * sin(M_PI * y), M_PI * sin(M_PI * x) * cos(M_PI * y);
	return gradient;
}

int main(int, char **) {
	try {
		solveL(0.5);

		std::cout << "Convergence Analysis" << std::endl;
		convergenceAnalysis("square", 7, f_square, sigma, g_square, 0, uex_square, uex_grad_square);
		convergenceAnalysis("Lshape", 7, f_lshape, sigma, g_lshape, 0, g_lshape, g_grad_lshape);
	} catch (std::runtime_error &e) {
		std::cerr << "An error occurred. Error message: " << std::endl;
		std::cerr << "    \"" << e.what() << "\"" << std::endl;
		return EXIT_FAILURE;
	} catch (...) {
		std::cerr << "An unknown error occurred." << std::endl;
		throw;
	}

	return EXIT_SUCCESS;
}


/* integrate.hpp header file
 *
 * Copyright Jens Maurer 2000
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * $Id: integrate.hpp,v 1.4 2001/11/19 22:13:04 jmaurer Exp $
 *
 * Revision history
 *   01 April 2001: Modified to use new <boost/limits.hpp> header. (JMaddock)
 */

#ifndef INTEGRATE_HPP
#define INTEGRATE_HPP

#include <boost/limits.hpp>

template<class UnaryFunction>
inline typename UnaryFunction::result_type
trapezoid(UnaryFunction f, typename UnaryFunction::argument_type a,
          typename UnaryFunction::argument_type b, int n)
{
  typename UnaryFunction::result_type tmp = 0;
  for(int i = 1; i <= n-1; ++i)
    tmp += f(a+(b-a)/n*i);
  return (b-a)/2/n * (f(a) + f(b) + 2*tmp);
}

template<class UnaryFunction>
inline typename UnaryFunction::result_type
simpson(UnaryFunction f, typename UnaryFunction::argument_type a,
        typename UnaryFunction::argument_type b, int n)
{
  typename UnaryFunction::result_type tmp1 = 0;
  for(int i = 1; i <= n-1; ++i)
    tmp1 += f(a+(b-a)/n*i);
  typename UnaryFunction::result_type tmp2 = 0;
  for(int i = 1; i <= n ; ++i)
    tmp2 += f(a+(b-a)/2/n*(2*i-1));

  return (b-a)/6/n * (f(a) + f(b) + 2*tmp1 + 4*tmp2);
}

// compute b so that f(b) = y; assume f is monotone increasing
template<class UnaryFunction>
inline typename UnaryFunction::argument_type
invert_monotone_inc(UnaryFunction f, typename UnaryFunction::result_type y,
                    typename UnaryFunction::argument_type lower = -1,
                    typename UnaryFunction::argument_type upper = 1)
{
  while(upper-lower > 1e-6) {
    double middle = (upper+lower)/2;
    if(f(middle) > y)
      upper = middle;
    else
      lower = middle;
  }
  return (upper+lower)/2;
}

// compute b so that  I(f(x), a, b) == y
template<class UnaryFunction>
inline typename UnaryFunction::argument_type
quantil(UnaryFunction f, typename UnaryFunction::argument_type a,
        typename UnaryFunction::result_type y,
        typename UnaryFunction::argument_type step)
{
  typedef typename UnaryFunction::result_type result_type;
  if(y >= 1.0)
    return std::numeric_limits<result_type>::infinity();
  typename UnaryFunction::argument_type b = a;
  for(result_type result = 0; result < y; b += step)
    result += step*f(b);
  return b;
}

#endif /* INTEGRATE_HPP */


/*
 * MathUtils.hpp
 *
 *  Created on: August 22, 2020
 *      Author: Quincy Jones
 *
 * Copyright (c) <2020> <Quincy Jones - quincy@implementedrobotics.com/>
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software
 * is furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef NOMAD_COMMON_MATHUTILS_H
#define NOMAD_COMMON_MATHUTILS_H

// C System Files
#include <math.h>

// C++ System Files

// Third Party Includes
#include <Eigen/Dense>
// Project Include Files

namespace Common::Math
{
    template <typename T>
    int sgn(T val)
    {
        return (T(0) < val) - (val < T(0));
    }

    // https://en.wikipedia.org/wiki/Skew-symmetric_matrix#Cross_product
    Eigen::Matrix3d SkewSymmetricCrossProduct(const Eigen::Vector3d& a);

    void rpyToR(Eigen::Matrix3d &R, double *rpy_in);

        // Convert Rotation -> Euler RPY
    Eigen::Vector3d RotationMatrixToEuler(const Eigen::Matrix3d& R);

    // Convert Euler RPY -> Rotation Matrix.  TODO: Add support for rotation order
    Eigen::Matrix3d EulerToRotationMatrix(const Eigen::Vector3d& euler);

    // Convert Euler RPY -> Quaternion.  TODO: Add support for rotation order
    Eigen::Quaterniond EulerToQuaternion(const Eigen::Vector3d& euler);

    // Convert Euler RPY -> Quaternion.  TODO: Add support for rotation order
    Eigen::Vector3d QuaterionToEuler(const Eigen::Quaterniond& q);

    // Compute Orientation Error between 2 Euler orientations
    Eigen::Vector3d ComputeOrientationError(const Eigen::Vector3d& theta_1, const Eigen::Vector3d& theta_2);

    Eigen::Matrix3d RotationX(double rad);
    Eigen::Matrix3d RotationY(double rad);
    Eigen::Matrix3d RotationZ(double rad);

    namespace EigenHelpers
    {

        // Typedefs
        typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrixXd;
        // TODO: Make this more generic
        // Block Matrix Class
        class BlockMatrixXd
        {
        public:
            BlockMatrixXd() {}
            BlockMatrixXd(const unsigned int Rows, const unsigned int Cols, const unsigned int BlockHeight, const unsigned int BlockWidth, const int init_val);
            BlockMatrixXd(const unsigned int Rows, const unsigned int Cols, const unsigned int BlockHeight, const unsigned int BlockWidth, Eigen::MatrixXd &matrix);
            BlockMatrixXd(const unsigned int Rows, const unsigned int Cols, const unsigned int BlockHeight, const unsigned int BlockWidth);

            friend std::ostream &operator<<(std::ostream &os, BlockMatrixXd const &bm) { return os << bm.Matrix_; }
            // Operator Overload:
            void operator()(const unsigned int Row, const unsigned int Col, const Eigen::MatrixXd &block_val);

            // TODO: Fix Templated Hack so we can set this directly... i.e. matrix(1,1) = block
            Eigen::Block<Eigen::MatrixXd, -1, -1, false> operator()(const unsigned int Row, const unsigned int Col);

            // Cast Overload
            operator Eigen::MatrixXd() { return Matrix_; }

            Eigen::MatrixXd MatrixXd() const { return Matrix_; }
            // Set Block Matrix Value
            // TODO: Should be Matrix(1,1) = value
            void SetBlock(const unsigned int Row, const unsigned int Col, const Eigen::MatrixXd &block_val);

            // Fill Diagonal with a block
            void FillDiagonal(const Eigen::MatrixXd &block_val, const int k = 0);

        protected:
            unsigned int Rows_;
            unsigned int Cols_;
            unsigned int BlockWidth_;
            unsigned int BlockHeight_;

            Eigen::MatrixXd Matrix_;

        };
    } // namespace EigenHelpers

} // namespace Common::Math
#endif // NOMAD_COMMON_STATISTICS_H


///
/// File:   eigen.cpp
/// Author: Open Risk
///

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

using namespace std;
using namespace Eigen;

int main(int argc, const char **argv) {

    //
    // Get eigen library version
    //

    std::stringstream ss;
    ss.str("");
    ss << EIGEN_MAJOR_VERSION;
    ss << ".";
    ss << EIGEN_MINOR_VERSION;
    std::string eigen_version = ss.str();
    std::cout << eigen_version << std::endl;

    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "The least-squares solution is:\n"
         << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;

    return 0;
}


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

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	assert(int(size(v)) == m * n);
	Vector w(m * n, typename Vector::value_type(0));

	for (int i= 0; i < m; i++)
	    for (int j= 0; j < n; j++) {
		int k= i * n + j; // offset
		w[k]= 4 * v[k];
		if (i > 0) w[k]-= v[k-n];   // upper neighbor
		if (i < m-1) w[k]-= v[k+n]; // lower neighbor
		if (j > 0) w[k]-= v[k-1];   // left neighbor
		if (j < n-1) w[k]-= v[k+1]; // right neighbor
	    }
	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

int main()
{
    mtl::vampir_trace<9999> tracer;
  // For a more realistic example set size to 1000 or larger
  const int size = 10, N = size * size;

  poisson2D_dirichlet A(size, size);

  itl::pc::identity<poisson2D_dirichlet>     P(A);

  mtl::dense_vector<double> x(N, 1.0), b(N);

  b = A * x;
  x= 0;
  itl::cyclic_iteration<double> iter(b, 100, 1.e-11, 0.0, 5);
  cg(A, x, b, P, iter);

  return 0;
}


// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <tuple>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <fft/fft2.hpp>
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

#include <operators/operators.hpp>
#include <solver/cg.hpp>
#include <solver/ridgelet_solver.hpp>

using namespace std;

typedef RT<> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;
typedef FFTr2c<PlannerR2COD> fft_t;

const double tol = 1e-9;
const int maxit = 400;
double pi = boost::math::constants::pi<double>();
bool save = false;

void dump_frc(const std::vector<rt_coeff_t>& f_rc, const RidgeletFrame& rt, std::string ffname)
{
  const char* fname = ffname.c_str();
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (unsigned int i = 0; i < f_rc.size(); ++i) {
    stringstream ss;
    ss << rt.lambdas()[i];
    string slam = ss.str();
    eigen2hdf::save(file, slam, f_rc[i]);
  }
  H5Fclose(file);
  cout << "Written f(lambda, t) to " << fname << "\n";
}

// --------------------------------------------------------------------------------
// solve in Ridgelet domain
template <typename ARRAY_T>
std::tuple<int, double> solve_rt(
    ARRAY_T& Fh, const RidgeletFrame& rf, const double dt, Eigen::Vector2d& v, bool log)
{
  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows

  RT_t rt(rf);
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  rca_t rt_cell_array(rf);
  auto& rt_coeffs = rt_cell_array.coeffs();
  rt.rt(rt_coeffs, Fh);

  double vx = v[0];
  double vy = v[1];

  // since vx, vy = 0, this has no effect
  double Lx = 1;
  double Ly = 1;

  // initialize operators
  AhAOp AhA(vx, vy, Lx, Ly, Nx, Ny, dt);
  PTransportOp<RT_t> A(rt, AhA, vx, vy);

  RidgeletSolver<rt_coeff_t> rt_solver(rf, vx, vy, dt);
  // assert(vx == 0);
  // assert(vy == 0);
  // have to apply A.T to rhs otherwise!!!

  /////////////
  // b = A*x //
  /////////////
  rca_t x(rf);
  x.resize(rt_cell_array);
  rca_t b(rf);
  b.resize(rt_cell_array);
  x = rt_cell_array;

  // b = A*x
  // Note if v=[0,0]: A is the identity operator
  b = x;

  /* cout << "RT_SOLVER::tol  : " << tol << "\n"; */
  /* cout << "RT_SOLVER::maxit: " << maxit << "\n"; */
  Logger::GetInstance().push_prefix("RTCG");
  rt_solver.set_log(log);
  rt_solver.solve(x, A, b, tol, maxit);
  Logger::GetInstance().clear_prefix();

  if (save) {
    rt.irt(Fh, x.coeffs());
    ARRAY_T Fh2 = ftcut(Fh, Ny / 2, Nx / 2);
    Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> X(Ny / 2, Nx / 2);
    fft_t fft;
    fft.ift(X, Fh2);
    hid_t file = H5Fcreate("cg-result.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(file, "X", X);
    H5Fclose(file);
  }

  return std::make_tuple(rt_solver.iter(), rt_solver.relres());

  /* cout << "RT CG::RELRES " << rt_solver.relres() << "\n" */
  /*      << "RT CG::ITER "   << rt_solver.iter() << "\n"; */
}

// --------------------------------------------------------------------------------
// solve in Ridgelet domain
template <typename ARRAY_T>
std::tuple<int, double> solve_rtnop(
    ARRAY_T& Fh, const RidgeletFrame& rf, const double dt, Eigen::Vector2d& v, bool log)
{
  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows

  RT_t rt(rf);
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  rca_t rt_cell_array(rf);
  auto& rt_coeffs = rt_cell_array.coeffs();
  rt.rt(rt_coeffs, Fh);

  double vx = v[0];
  double vy = v[1];

  // since vx, vy = 0, this has no effect
  double Lx = 1;
  double Ly = 1;

  // initialize operators
  AhAOp AhA(vx, vy, Lx, Ly, Nx, Ny, dt);
  PTransportOpId<RT_t> A(rt, AhA, vx, vy);

  RidgeletSolverNOP<rt_coeff_t> rt_solver(rf, vx, vy, dt);
  // assert(vx == 0);
  // assert(vy == 0);
  // have to apply A.T to rhs otherwise!!!

  /////////////
  // b = A*x //
  /////////////
  rca_t x(rf);
  x.resize(rt_cell_array);
  rca_t b(rf);
  b.resize(rt_cell_array);
  x = rt_cell_array;

  // b = A*x
  // Note if v=[0,0]: A is the identity operator
  b = x;

  /* cout << "RT_SOLVER::tol  : " << tol << "\n"; */
  /* cout << "RT_SOLVER::maxit: " << maxit << "\n"; */
  Logger::GetInstance().push_prefix("RTCGNOP");
  rt_solver.set_log(log);
  rt_solver.solve(x, A, b, tol, maxit);
  Logger::GetInstance().clear_prefix();

  return std::make_tuple(rt_solver.iter(), rt_solver.relres());

  /* cout << "RT CG::RELRES " << rt_solver.relres() << "\n" */
  /*      << "RT CG::ITER "   << rt_solver.iter() << "\n"; */
}

// --------------------------------------------------------------------------------
// solve in Fourier domain
template <typename ARRAY_T>
std::tuple<int, double> solve_ft(ARRAY_T& Fh, const double dt, Eigen::Vector2d& v, bool log)
{
  const unsigned int Nx = Fh.cols();  // #cols
  const unsigned int Ny = Fh.rows();  // #rows

  double vx = v[0];
  double vy = v[1];
  // since vx, vy = 0, this has no effect
  double Lx = 1;
  double Ly = 1;

  // initialize operators
  AhAOp A(vx, vy, Lx, Ly, Nx, Ny, dt);

  /////////////
  // b = A*x //
  /////////////
  ARRAY_T x(Ny, Nx);
  x = Fh;

  ARRAY_T b(Ny, Nx);
  b = x;

  Logger::GetInstance().push_prefix("FTCG");
  CG cg;
  cg.set_log(log);
  cg.solve(x, A, b, tol, maxit);
  Logger::GetInstance().clear_prefix();

  return std::make_tuple(cg.iter(), cg.relres());

  /* cout << "FT CG::RELRES " << cg.relres() << "\n" */
  /*      << "FT CG::ITER "  << cg.iter() << "\n"; */
}

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

  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;
  int nv;
  double dt, r;
  bool log;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("rad,r", po::value<double>(&r)->default_value(1), "|v|")
      /* ("vx", po::value<double>(&vx)->default_value(1), "vx") */
      /* ("vy", po::value<double>(&vy)->default_value(0), "vy") */
      ("dt,t", po::value<double>(&dt)->default_value(0.1), "dt")
      ("nv", po::value<int>(&nv)->default_value(6), "#directons in velocity")
      ("log", po::value<bool>(&log)->default_value(false), "log cg history")
      ("smooth", "use smooth initial condition")
      ("save", "save solution");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  if (vm.count("save")) save = true;

  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n"
       << setw(20) << "dt"
       << ": " << dt << "\n"
       << setw(20) << "|v|"
       << ": " << r << "\n";

  RidgeletFrame rf(Jx, Jy, rho_x, rho_y);

  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows
  cout << "Nx: " << Nx << "\n";
  cout << "Ny: " << Ny << "\n";

  auto phi = 2 * pi * Eigen::VectorXd::LinSpaced(nv + 1, 0, 1);

  for (int i = 0; i < nv; ++i) {
    cout << "---------- phi = " << phi[i] << " ----------\n";
    const double vx = std::cos(phi[i]) * r;
    const double vy = std::sin(phi[i]) * r;
    Eigen::Vector2d v;
    v[0] = vx;
    v[1] = vy;

    Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx, 0, 1);
    Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny, 0, 1);
    array_t F(Ny, Nx);
    F = (-100 * ((yi - 0.5)).cwiseAbs2()).exp().replicate(1, xi.rows()) *
        (-100 * ((xi - 0.5).cwiseAbs2())).exp().transpose().replicate(yi.rows(), 1);
    if (!vm.count("smooth")) {
      F = (((yi - 0.5).replicate(1, xi.rows()) * vy -
            (xi - 0.5).transpose().replicate(yi.rows(), 1) * vx) <=
           0).select(F, Eigen::ArrayXXd::Zero(Ny, Nx));
    }

    hid_t file = H5Fcreate("cg.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(file, "x0", F);
    H5Fclose(file);

    // F.setOnes();

    // ----------------------------------------
    /* double n[2] = {1,1}; */
    /* F = ((n[0]*(yi.replicate(1, xi.rows())-0.5*pi) + n[1]*(xi.transpose().replicate(yi.rows(),
     * 1)-0.5*pi)) > 1e-8) */
    /*   .select(F, array_t::Zero(Ny, Nx)); */
    fft_t fft;
    complex_array_t Fh(Ny, Nx);
    fft.ft(Fh, F, false);

    auto ret_rt = solve_rt(Fh, rf, dt, v, log);
    auto ret_ft = solve_ft(Fh, dt, v, log);
    auto ret_rtnop = solve_rtnop(Fh, rf, dt, v, log);

    cout << "RTCG " << std::scientific << std::setprecision(3) << std::setw(7) << vx << " "
         << std::setw(7) << vy << " " << std::setw(7) << std::get<1>(ret_rt) << " " << std::setw(7)
         << std::get<0>(ret_rt) << "\n";
    cout << "RTCGNOP " << std::scientific << std::setprecision(3) << std::setw(7) << vx << " "
         << std::setw(7) << vy << " " << std::setw(7) << std::get<1>(ret_rtnop) << " "
         << std::setw(7) << std::get<0>(ret_rtnop) << "\n";
    cout << "FTCG " << std::scientific << std::setprecision(3) << std::setw(7) << vx << " "
         << std::setw(7) << vy << " " << std::setw(7) << std::get<1>(ret_ft) << " " << std::setw(7)
         << std::get<0>(ret_ft) << "\n";

    /* std::vector<double> diagp = make_inv_diagonal_preconditioner(rf, dt*vx, dt*vy); */
    /* cout  << "\n"; */
    /* for (int i = 0; i < diagp.size(); ++i) { */
    /*   cout << diagp[i] << "\n"; */
    /* } */
    /* cout << "\n"; */
  }

  return 0;
}