/**
 *  Author: Cedric LE GENTIL
 *
 *  Copyright 2021 Cedric LE GENTIL
 *
 *  This run Monte Carlo simulation experiements for the paper
 *
 *  Disclaimer:
 *  This code is not optimised neither for performance neither for maintainability.
 *  There might still be errors in the code, logic or procedure. Every feedback is welcomed.
 *
 *  For any further question or if you see any problem in that code
 *  le.gentil.cedric@gmail.com
 **/

#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include "sensor_input/imu_simulator.h"
#include "imu_preintegration/preintegration.h"
#include "common/random.h"
#include "common/utils.h"

celib::PreintMeas VanillaPreintegration(celib::ImuData data, double start_t, double t)
{
    celib::PreintMeas output;
    output.delta_R = celib::Mat3::Identity();
    output.delta_v = celib::Vec3::Zero();
    output.delta_p = celib::Vec3::Zero();
    output.cov = celib::Mat9::Zero();

    int counter = 0;
    while(start_t > data.gyr[counter].t)
    {
        counter++;
        if(counter == data.acc.size())
        {
            throw std::range_error("VanillaPreintegration: looks like integration window is out of data");
        }
    }

    double dt = data.gyr[counter].t - start_t;
    celib::Vec3 acc;
    acc << data.acc[counter-1].data[0],
           data.acc[counter-1].data[1],
           data.acc[counter-1].data[2];
    celib::Vec3 gyr;
    gyr << data.gyr[counter-1].data[0],
           data.gyr[counter-1].data[1],
           data.gyr[counter-1].data[2];

    celib::Mat6 cov_imu = celib::Mat6::Identity();
    cov_imu.block<3,3>(0,0) = data.gyr_var*celib::Mat3::Identity();
    cov_imu.block<3,3>(3,3) = data.acc_var*celib::Mat3::Identity();

    while(t > data.gyr[counter].t)
    {
        celib::Vec3 acc_rot = output.delta_R*acc;
        output.delta_p = output.delta_p + (output.delta_v*dt) + (acc_rot*dt*dt/2.0);
        output.delta_v = output.delta_v + (acc_rot*dt);
        celib::Mat3 e_R = celib::ExpMap(gyr*dt);
        celib::Mat3 j_r = celib::JacobianRighthandExpMap<double>(gyr*dt);
        celib::Mat9 A = celib::Mat9::Identity();
        celib::Mat9_6 B = celib::Mat9_6::Zero();
        celib::Mat3 skew_acc = celib::ToSkewSymMat(acc);
        A.block<3,3>(0,0) = e_R.transpose();
        A.block<3,3>(3,0) = -output.delta_R*skew_acc*dt;
        A.block<3,3>(6,0) = -output.delta_R*skew_acc*dt*dt/2.0;
        A.block<3,3>(6,3) = celib::Mat3::Identity();

        B.block<3,3>(0,0) = j_r*dt;
        B.block<3,3>(3,3) = output.delta_R*dt;
        B.block<3,3>(3,6) = output.delta_R*dt*dt/2.0;

        output.delta_R = output.delta_R*e_R;

        output.cov = (A*output.cov*A.transpose()) + (B*cov_imu*B.transpose());

        if(counter >= (data.acc.size()-1))
        {
            throw std::range_error("VanillaPreintegration: looks like integration window is out of data");
        }else{
            counter++;
            acc << data.acc[counter-1].data[0],
                data.acc[counter-1].data[1],
                data.acc[counter-1].data[2];
            gyr << data.gyr[counter-1].data[0],
                data.gyr[counter-1].data[1],
                data.gyr[counter-1].data[2];
            dt = data.acc[counter].t - data.acc[counter-1].t;
            if( (data.acc[counter-1].t < t) && (t < data.acc[counter].t) )
            {
                dt = t - data.acc[counter-1].t;
            }

        }
    }
    return output;
}

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

    celib::PreintOption preint_opt;
    preint_opt.min_freq = 500;

    int nb_monte_carlo = 100;
    double overlap = 0.15;

    // Program options
    boost::program_options::options_description opt_description("Allowed options");
    opt_description.add_options()
        ("help,h", "Produce help message")
        ("experiment,e", boost::program_options::value< int >(), "1 = accuracy metrics (default), 2 = computation time, 3 = noise robustness, 4 = bias correction")
        ("nb_monte_carlo,n", boost::program_options::value< int >(), "(default = 100)")
        ;

    boost::program_options::variables_map var_map;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, opt_description), var_map);
    boost::program_options::notify(var_map);

    // Check help options
    if(var_map.count("help")) {
        std::cout << opt_description << std::endl;
        return 1;
    }

    int experiment_type = 1;
    if(var_map.count("experiment"))
    {
        experiment_type = var_map["experiment"].as<int>();
    }
    if(var_map.count("nb_monte_carlo"))
    {
        nb_monte_carlo = var_map["nb_monte_carlo"].as<int>();
    }

    if(experiment_type == 1)
    { // Metrics for the accuracy  with realistic noise

        std::cout << "ACCURACY EXPERIMENT" << std::endl;
        std::cout.precision(3);

        std::vector<double> durations = {0.05, 0.1, 0.5, 1.0};
        celib::ImuSimulatorOption sim_opt;
        sim_opt.acc_std = 0.04;
        sim_opt.gyr_std = 0.01;

        int nb_methods = 7;

        std::vector<std::vector<std::vector<double> > > rot_error_avg;
        std::vector<std::vector<std::vector<double> > > rot_error_std;
        std::vector<std::vector<std::vector<double> > > vel_error_avg;
        std::vector<std::vector<std::vector<double> > > vel_error_std;
        std::vector<std::vector<std::vector<double> > > pos_error_avg;
        std::vector<std::vector<std::vector<double> > > pos_error_std;
        std::vector<std::vector<std::vector<double> > > rot_rel_error_avg;
        std::vector<std::vector<std::vector<double> > > pos_rel_error_avg;

        std::vector<double> avg_ang_vel;
        std::vector<double> avg_vel;
        for(int type = 0; type < 2; ++type)
        {
            rot_error_avg.push_back(std::vector<std::vector<double> >());
            rot_error_std.push_back(std::vector<std::vector<double> >());
            vel_error_avg.push_back(std::vector<std::vector<double> >());
            vel_error_std.push_back(std::vector<std::vector<double> >());
            pos_error_avg.push_back(std::vector<std::vector<double> >());
            pos_error_std.push_back(std::vector<std::vector<double> >());
            rot_rel_error_avg.push_back(std::vector<std::vector<double> >());
            pos_rel_error_avg.push_back(std::vector<std::vector<double> >());

            avg_vel.push_back(0);
            avg_ang_vel.push_back(0);

            if(type == 1)
            {
                sim_opt.motion_type = "slow";
            }
            else
            {
                sim_opt.motion_type = "fast";
            }
            std::cout << "===============================================" << std::endl;
            std::cout << "Run results for " << sim_opt.motion_type << " motion" << std::endl;
            for(int d = 0; d < durations.size(); ++d)
            {

                rot_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                rot_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                vel_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                vel_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                pos_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                pos_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                rot_rel_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                pos_rel_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                std::cout << "Duration : " << durations[d] << " s" << std::endl;

                std::vector<std::vector<double> > rot_errors(nb_methods);
                std::vector<std::vector<double> > vel_errors(nb_methods);
                std::vector<std::vector<double> > pos_errors(nb_methods);
                std::vector<std::vector<double> > rot_rel_errors(nb_methods);
                std::vector<std::vector<double> > pos_rel_errors(nb_methods);
                for(int i = 0; i < nb_monte_carlo; ++i)
                {
                    std::cout << i << "." << std::flush;
                    celib::ImuSimulator imu_sim(sim_opt);
                    avg_vel[type] += imu_sim.getAvgVel();
                    avg_ang_vel[type] += imu_sim.getAvgAngVel();
                    celib::RandomGenerator rand_gen;
                    double start_t = rand_gen.randUniform(overlap,sim_opt.dataset_length - durations[d] - overlap);
                    double end_t = start_t + durations[d];
                    auto data = imu_sim.get(start_t-overlap, end_t+overlap);

                    double pos_dist = imu_sim.getTranslationDistance(start_t, end_t);
                    double rot_dist = imu_sim.getOrientationDistance(start_t, end_t);

                    for(int method = 0; method < nb_methods; ++method)
                    {
                        celib::PreintMeas preint;
                        if(method == 0)
                        {
                            preint = VanillaPreintegration(data, start_t, end_t);
                        }
                        else
                        {
                            // Create a preintegration object
                            celib::PreintPrior prior;
                            std::vector<std::vector<double> > t;
                            std::vector<double> temp_t;
                            temp_t.push_back(end_t);
                            t.push_back(temp_t);
                            if(method == 1)
                            {
                                preint_opt.type = celib::LPM;
                                preint_opt.train_gpm = false;
                                preint_opt.quantum = -1;
                            }
                            else if(method == 2)
                            {
                                preint_opt.type = celib::GPM;
                                preint_opt.train_gpm = false;
                                preint_opt.quantum = -1;
                            }
                            else if(method == 3)
                            {
                                preint_opt.type = celib::GPM;
                                preint_opt.train_gpm = true;
                                preint_opt.quantum = -1;
                            }
                            else if(method == 4)
                            {
                                preint_opt.type = celib::UGPM;
                                preint_opt.train_gpm = false;
                                preint_opt.quantum = -1;
                            }
                            else if(method == 5)
                            {
                                preint_opt.type = celib::UGPM;
                                preint_opt.train_gpm = true;
                                preint_opt.quantum = -1;
                            }
                            else
                            {
                                preint_opt.type = celib::UGPM;
                                preint_opt.train_gpm = false;
                                preint_opt.quantum = 0.2;
                            }
                            celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
                            preint = imu_preint.get(0,0);

                        }
                        auto error = imu_sim.testPreint(start_t, end_t, preint);
                        rot_errors[method].push_back(error[0]);
                        vel_errors[method].push_back(error[1]);
                        pos_errors[method].push_back(error[2]);
                        rot_rel_errors[method].push_back(error[0]/rot_dist);
                        pos_rel_errors[method].push_back(error[2]/pos_dist);
                    }
                }
                std::cout << std::endl;

                for(int method = 0; method < nb_methods; ++method)
                {
                    for(int i = 0; i < nb_monte_carlo; ++i)
                    {
                        rot_error_avg[type][d][method] += rot_errors[method][i];
                        vel_error_avg[type][d][method] += vel_errors[method][i];
                        pos_error_avg[type][d][method] += pos_errors[method][i];
                        rot_rel_error_avg[type][d][method] += rot_rel_errors[method][i];
                        pos_rel_error_avg[type][d][method] += pos_rel_errors[method][i];
                    }
                    rot_error_avg[type][d][method] /= nb_monte_carlo;
                    vel_error_avg[type][d][method] /= nb_monte_carlo;
                    pos_error_avg[type][d][method] /= nb_monte_carlo;
                    rot_rel_error_avg[type][d][method] /= nb_monte_carlo;
                    pos_rel_error_avg[type][d][method] /= nb_monte_carlo;
                    for(int i = 0; i < nb_monte_carlo; ++i)
                    {
                        rot_error_std[type][d][method] += std::pow(rot_errors[method][i] - rot_error_avg[type][d][method],2);
                        vel_error_std[type][d][method] += std::pow(vel_errors[method][i] - vel_error_avg[type][d][method],2);
                        pos_error_std[type][d][method] += std::pow(pos_errors[method][i] - pos_error_avg[type][d][method],2);
                    }
                    rot_error_std[type][d][method] = std::sqrt(rot_error_std[type][d][method]/nb_monte_carlo);
                    vel_error_std[type][d][method] = std::sqrt(vel_error_std[type][d][method]/nb_monte_carlo);
                    pos_error_std[type][d][method] = std::sqrt(pos_error_std[type][d][method]/nb_monte_carlo);

                }
            }
        }

        std::cout << std::endl << "RESULT " << std::endl << "(Methods: 0 = PM, 1 = LPM, 2 = GPM, 3 = UGPM, 4 = GPM trained, 5 = UGPM trained, 6 = UGPM per chunk" << std::endl;
        for(int type = 0; type < 2; ++type)
        {
            avg_vel[type] /= double(nb_monte_carlo);
            avg_ang_vel[type] /= double(nb_monte_carlo);

            if(type == 1) std::cout << std::endl << std::endl << "Slow motion" << std::endl;
            if(type == 0) std::cout << std::endl << std::endl << "Fast motion" << std::endl;
            std::cout << "Avg velocity : " << avg_vel[type] << " m/s    Avg ang velocity : " << avg_ang_vel[type] << " rad/s" << std::endl;
            std::cout << " Rot and Pos error ====" << std::endl;
            std::cout << "Duration \\ method";
            for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
            std::cout << std::endl;
            for(int d = 0; d < durations.size(); ++d)
            {
                std::cout << "\\\\ \\hline \\hline" << std::endl;
                std::cout << "\\multirow{4}{*}{" << durations[d] << "} & Rot abs. er. [$\\,\\mathrm{mrad}$]      ";
                for(int method = 0; method < nb_methods; ++method)
                {
                    std::cout << " &  " << "\\scriptsize " << rot_error_avg[type][d][method]*1000.0 << " $\\pm$ " << rot_error_std[type][d][method]*1000.0;

                }
                std::cout << std::endl;
                std::cout << "\\\\ & Rot rel. er.      ";
                for(int method = 0; method < nb_methods; ++method)
                {
                    std::cout << " &  " << "\\scriptsize " << rot_rel_error_avg[type][d][method]*100.0 << "\\%";

                }
                std::cout << std::endl;
                std::cout << "\\\\ \\cline{2-9}" << std::endl;
                std::cout << " & Pos abs. er. [$\\,\\mathrm{mm}$]      ";
                for(int method = 0; method < nb_methods; ++method)
                {
                    std::cout << " &  " << "\\scriptsize " << pos_error_avg[type][d][method]*1000.0 << " $\\pm$ " << pos_error_std[type][d][method]*1000.0;
                }
                std::cout << std::endl;
                std::cout << "\\\\ & Pos rel. er.      ";
                for(int method = 0; method < nb_methods; ++method)
                {
                    std::cout << " &  " << "\\scriptsize " << pos_rel_error_avg[type][d][method]*100.0 << "\\%";

                }
                std::cout << std::endl;
            }
            std::cout << std::endl << " Rot error ====" << std::endl;
            std::cout << "Duration \\ method";
            for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
            std::cout << std::endl;
            for(int d = 0; d < durations.size(); ++d)
            {
                std::cout << durations[d] << "\\\\              ";
                for(int method = 0; method < nb_methods; ++method)
                {
                    std::cout << " &  " << rot_error_avg[type][d][method] << " $\\pm$ " << rot_error_std[type][d][method];
                }
                std::cout << std::endl;
            }
            std::cout << std::endl << " Vel error ====" << std::endl;
            std::cout << "Duration \\ method";
            for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
            std::cout << std::endl;
            for(int d = 0; d < durations.size(); ++d)
            {
                std::cout << durations[d] << "\\\\              ";
                for(int method = 0; method < nb_methods; ++method)
                {
                    std::cout << " &  " << vel_error_avg[type][d][method] << " $\\pm$ " << vel_error_std[type][d][method];
                }
                std::cout << std::endl;
            }
            std::cout << std::endl << " Pos error ====" << std::endl;
            std::cout << "Duration \\ method";
            for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
            std::cout << std::endl;
            for(int d = 0; d < durations.size(); ++d)
            {
                std::cout << durations[d] << "              ";
                for(int method = 0; method < nb_methods; ++method)
                {
                    std::cout << " &  " << pos_error_avg[type][d][method] << " $\\pm$ " << pos_error_std[type][d][method];
                }
                std::cout << std::endl;
            }
        }
    }

    if(experiment_type == 2)
    { // Metrics for the computation time  with realistic noise

        std::cout << "COMPUTATION TIME EXPERIMENT" << std::endl;
        std::cout.precision(4);

        std::vector<double> durations = {0.05, 0.1, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5};
        celib::ImuSimulatorOption sim_opt;
        sim_opt.acc_std = 0.04;
        sim_opt.gyr_std = 0.01;

        int nb_methods = 7;

        std::vector<std::vector<double> > time_average;

        celib::StopWatch stop_watch;

        std::cout << "===============================================" << std::endl;
        for(int d = 0; d < durations.size(); ++d)
        {

            time_average.push_back(std::vector<double>(nb_methods,0.0));
            std::cout << "Duration : " << durations[d] << " s" << std::endl;

            for(int i = 0; i < nb_monte_carlo; ++i)
            {
                std::cout << i << "." << std::flush;
                celib::ImuSimulator imu_sim(sim_opt);
                celib::RandomGenerator rand_gen;
                double start_t = rand_gen.randUniform(overlap,sim_opt.dataset_length - durations[d] - overlap);
                double end_t = start_t + durations[d];
                auto data = imu_sim.get(start_t-overlap, end_t+overlap);

                for(int method = 0; method < nb_methods; ++method)
                {
                    celib::PreintMeas preint;
                    double time;
                    stop_watch.reset();
                    if(method == 0)
                    {
                        stop_watch.start();
                        preint = VanillaPreintegration(data, start_t, end_t);
                        time = stop_watch.stop();
                    }
                    else
                    {
                        // Create a preintegration object
                        celib::PreintPrior prior;
                        std::vector<std::vector<double> > t;
                        std::vector<double> temp_t;
                        temp_t.push_back(end_t);
                        t.push_back(temp_t);
                        if(method == 1)
                        {
                            preint_opt.type = celib::LPM;
                            preint_opt.train_gpm = false;
                            preint_opt.quantum = -1;
                        }
                        else if(method == 2)
                        {
                            preint_opt.type = celib::GPM;
                            preint_opt.train_gpm = false;
                            preint_opt.quantum = -1;
                        }
                        else if(method == 3)
                        {
                            preint_opt.type = celib::GPM;
                            preint_opt.train_gpm = true;
                            preint_opt.quantum = -1;
                        }
                        else if(method == 4)
                        {
                            preint_opt.type = celib::UGPM;
                            preint_opt.train_gpm = false;
                            preint_opt.quantum = -1;
                        }
                        else if(method == 5)
                        {
                            preint_opt.type = celib::UGPM;
                            preint_opt.train_gpm = true;
                            preint_opt.quantum = -1;
                        }
                        else
                        {
                            preint_opt.type = celib::UGPM;
                            preint_opt.train_gpm = false;
                            preint_opt.quantum = 0.2;
                        }
                        stop_watch.start();
                        celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
                        preint = imu_preint.get(0,0);
                        time = stop_watch.stop();
                    }
                    time_average[d][method] += time;
                    auto error = imu_sim.testPreint(start_t, end_t, preint);
                }
            }
            std::cout << std::endl;

            for(int method = 0; method < nb_methods; ++method)
            {
                time_average[d][method] /= nb_monte_carlo;
            }
        }

        std::cout << std::endl << "RESULT computation time " << std::endl << "(Methods: 0 = PM, 1 = LPM, 2 = GPM, 3 = UGPM, 4 = GPM trained, 5 = UGPM trained, 6 = UGPM per chunk" << std::endl;
        std::cout << "Duration (ms)\\ method";
        for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
        std::cout << std::endl;
        for(int d = 0; d < durations.size(); ++d)
        {
            std::cout << "\\scriptsize " << durations[d]*1000.0 << "            ";
            for(int method = 0; method < nb_methods; ++method)
            {
                std::cout << " &  \\scriptsize " << time_average[d][method];
            }
            std::cout << std::endl;
        }
    }

    if(experiment_type == 3)
    { // Metrics for the robustness to noise

        std::cout << "NOISE ROBUSTNESS EXPERIMENT" << std::endl;

        {
            celib::ImuSimulatorOption sim_opt;
            std::vector<double> noise_factor = {0.001, 0.33, 0.66, 1, 1.33, 1.66, 2, 2.33, 2.66, 3, 3.33, 3.66, 4, 4.33, 4.66, 5};
            double acc_std = 0.02;
            double gyr_std = 0.01;
            sim_opt.motion_type = "fast";
            double duration = 1;
            int nb_methods = 4;

            std::vector<std::vector<std::vector<double> > > rot_error_avg;
            std::vector<std::vector<std::vector<double> > > rot_error_std;
            std::vector<std::vector<std::vector<double> > > vel_error_avg;
            std::vector<std::vector<std::vector<double> > > vel_error_std;
            std::vector<std::vector<std::vector<double> > > pos_error_avg;
            std::vector<std::vector<std::vector<double> > > pos_error_std;
            for(int type = 0; type < 2; ++type)
            {
                rot_error_avg.push_back(std::vector<std::vector<double> >());
                rot_error_std.push_back(std::vector<std::vector<double> >());
                vel_error_avg.push_back(std::vector<std::vector<double> >());
                vel_error_std.push_back(std::vector<std::vector<double> >());
                pos_error_avg.push_back(std::vector<std::vector<double> >());
                pos_error_std.push_back(std::vector<std::vector<double> >());

                std::cout << "===============================================" << std::endl;
                std::cout << "Run results for " << sim_opt.motion_type << " motion" << std::endl;
                for(int d = 0; d < noise_factor.size(); ++d)
                {

                    if(type == 0)
                    {
                        sim_opt.acc_std = noise_factor[d] * acc_std;
                        sim_opt.gyr_std = 0.00001;
                    }
                    else
                    {
                        sim_opt.acc_std = 0.00001;
                        sim_opt.gyr_std = noise_factor[d] * gyr_std;
                    }

                    rot_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                    rot_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                    vel_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                    vel_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                    pos_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                    pos_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                    std::cout << "Noise factor : " << noise_factor[d] << std::endl;

                    std::vector<std::vector<double> > rot_errors(nb_methods);
                    std::vector<std::vector<double> > vel_errors(nb_methods);
                    std::vector<std::vector<double> > pos_errors(nb_methods);
                    for(int i = 0; i < nb_monte_carlo; ++i)
                    {
                        std::cout << i << "." << std::flush;
                        celib::ImuSimulator imu_sim(sim_opt);
                        celib::RandomGenerator rand_gen;
                        double start_t = rand_gen.randUniform(overlap,sim_opt.dataset_length - duration - overlap);
                        double end_t = start_t + duration;
                        auto data = imu_sim.get(start_t-overlap, end_t+overlap);

                        for(int method = 0; method < nb_methods; ++method)
                        {
                            celib::PreintMeas preint;
                            if(method == 0)
                            {
                                preint = VanillaPreintegration(data, start_t, end_t);
                            }
                            else
                            {
                                // Create a preintegration object
                                celib::PreintPrior prior;
                                std::vector<std::vector<double> > t;
                                std::vector<double> temp_t;
                                temp_t.push_back(end_t);
                                t.push_back(temp_t);
                                if(method == 1)
                                {
                                    preint_opt.type = celib::LPM;
                                    preint_opt.train_gpm = false;
                                }
                                else if(method == 2)
                                {
                                    preint_opt.type = celib::GPM;
                                    preint_opt.train_gpm = true;
                                }
                                else
                                {
                                    preint_opt.type = celib::UGPM;
                                    preint_opt.train_gpm = true;
                                }
                                celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
                                preint = imu_preint.get(0,0);

                            }
                            auto error = imu_sim.testPreint(start_t, end_t, preint);
                            rot_errors[method].push_back(error[0]);
                            vel_errors[method].push_back(error[1]);
                            pos_errors[method].push_back(error[2]);
                        }
                    }
                    std::cout << std::endl;

                    for(int method = 0; method < nb_methods; ++method)
                    {
                        for(int i = 0; i < nb_monte_carlo; ++i)
                        {
                            rot_error_avg[type][d][method] += rot_errors[method][i];
                            vel_error_avg[type][d][method] += vel_errors[method][i];
                            pos_error_avg[type][d][method] += pos_errors[method][i];
                        }
                        rot_error_avg[type][d][method] /= nb_monte_carlo;
                        vel_error_avg[type][d][method] /= nb_monte_carlo;
                        pos_error_avg[type][d][method] /= nb_monte_carlo;
                        for(int i = 0; i < nb_monte_carlo; ++i)
                        {
                            rot_error_std[type][d][method] += std::pow(rot_errors[method][i] - rot_error_avg[type][d][method],2);
                            vel_error_std[type][d][method] += std::pow(vel_errors[method][i] - vel_error_avg[type][d][method],2);
                            pos_error_std[type][d][method] += std::pow(pos_errors[method][i] - pos_error_avg[type][d][method],2);
                        }
                        rot_error_std[type][d][method] = std::sqrt(rot_error_std[type][d][method]/nb_monte_carlo);
                        vel_error_std[type][d][method] = std::sqrt(vel_error_std[type][d][method]/nb_monte_carlo);
                        pos_error_std[type][d][method] = std::sqrt(pos_error_std[type][d][method]/nb_monte_carlo);

                    }
                }
            }
            std::cout << std::endl << "RESULTS " << std::endl << "(Methods: PM, LPM, GPM trained, UGPM trained" << std::endl;
            for(int type = 0; type < 2; ++type)
            {
                if(type == 0) std::cout << std::endl << std::endl << "Acc noise" << std::endl;
                if(type == 1) std::cout << std::endl << std::endl << "Gyr noise" << std::endl;
                std::cout << " Rot error ====" << std::endl;
                for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
                std::cout << std::endl;
                for(int method = 0; method < nb_methods; ++method)
                {
                    for(int d = 0; d < noise_factor.size(); ++d)
                    {
                        if(type==0)
                        {
                                std::cout << " (" << acc_std*noise_factor[d] << "," << rot_error_avg[type][d][method] << ")";
                        }else
                        {
                                std::cout << " (" << gyr_std*noise_factor[d] << "," << rot_error_avg[type][d][method] << ")";
                        }
                    }
                    std::cout << std::endl;
                }
                std::cout << " Pos error ====" << std::endl;
                for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
                std::cout << std::endl;
                for(int method = 0; method < nb_methods; ++method)
                {
                    for(int d = 0; d < noise_factor.size(); ++d)
                    {
                        if(type==0)
                        {
                                std::cout << " (" << acc_std*noise_factor[d] << "," << pos_error_avg[type][d][method] << ")";
                        }else
                        {
                                std::cout << " (" << gyr_std*noise_factor[d] << "," << pos_error_avg[type][d][method] << ")";
                        }
                    }
                    std::cout << std::endl;
                }
            }
        }
        {
            celib::ImuSimulatorOption sim_opt;
            std::vector<double> noise_factor = {0.0001, 0.33, 0.66, 1, 1.33, 1.66, 2, 2.33, 2.66, 3, 3.33, 3.66, 4, 4.33, 4.66, 5};
            double acc_std = 0.02;
            double gyr_std = 0.01;
            sim_opt.motion_type = "slow";
            double duration = 1;
            int nb_methods = 4;

            std::vector<std::vector<std::vector<double> > > rot_error_avg;
            std::vector<std::vector<std::vector<double> > > rot_error_std;
            std::vector<std::vector<std::vector<double> > > vel_error_avg;
            std::vector<std::vector<std::vector<double> > > vel_error_std;
            std::vector<std::vector<std::vector<double> > > pos_error_avg;
            std::vector<std::vector<std::vector<double> > > pos_error_std;
            for(int type = 0; type < 2; ++type)
            {
                rot_error_avg.push_back(std::vector<std::vector<double> >());
                rot_error_std.push_back(std::vector<std::vector<double> >());
                vel_error_avg.push_back(std::vector<std::vector<double> >());
                vel_error_std.push_back(std::vector<std::vector<double> >());
                pos_error_avg.push_back(std::vector<std::vector<double> >());
                pos_error_std.push_back(std::vector<std::vector<double> >());

                std::cout << "===============================================" << std::endl;
                std::cout << "Run results for " << sim_opt.motion_type << " motion" << std::endl;
                for(int d = 0; d < noise_factor.size(); ++d)
                {

                    if(type == 0)
                    {
                        sim_opt.acc_std = noise_factor[d] * acc_std;
                        sim_opt.gyr_std = 0.00001;
                    }
                    else
                    {
                        sim_opt.acc_std = 0.00001;
                        sim_opt.gyr_std = noise_factor[d] * gyr_std;
                    }

                    rot_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                    rot_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                    vel_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                    vel_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                    pos_error_avg[type].push_back(std::vector<double>(nb_methods,0.0));
                    pos_error_std[type].push_back(std::vector<double>(nb_methods,0.0));
                    std::cout << "Noise factor : " << noise_factor[d] << std::endl;

                    std::vector<std::vector<double> > rot_errors(nb_methods);
                    std::vector<std::vector<double> > vel_errors(nb_methods);
                    std::vector<std::vector<double> > pos_errors(nb_methods);
                    for(int i = 0; i < nb_monte_carlo; ++i)
                    {
                        std::cout << i << "." << std::flush;
                        celib::ImuSimulator imu_sim(sim_opt);
                        celib::RandomGenerator rand_gen;
                        double start_t = rand_gen.randUniform(overlap,sim_opt.dataset_length - duration - overlap);
                        double end_t = start_t + duration;
                        auto data = imu_sim.get(start_t-overlap, end_t+overlap);

                        for(int method = 0; method < nb_methods; ++method)
                        {
                            celib::PreintMeas preint;
                            if(method == 0)
                            {
                                preint = VanillaPreintegration(data, start_t, end_t);
                            }
                            else
                            {
                                // Create a preintegration object
                                celib::PreintPrior prior;
                                std::vector<std::vector<double> > t;
                                std::vector<double> temp_t;
                                temp_t.push_back(end_t);
                                t.push_back(temp_t);
                                if(method == 1)
                                {
                                    preint_opt.type = celib::LPM;
                                    preint_opt.train_gpm = false;
                                }
                                else if(method == 2)
                                {
                                    preint_opt.type = celib::GPM;
                                    preint_opt.train_gpm = true;
                                }
                                else
                                {
                                    preint_opt.type = celib::UGPM;
                                    preint_opt.train_gpm = true;
                                }
                                celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
                                preint = imu_preint.get(0,0);

                            }
                            auto error = imu_sim.testPreint(start_t, end_t, preint);
                            rot_errors[method].push_back(error[0]);
                            vel_errors[method].push_back(error[1]);
                            pos_errors[method].push_back(error[2]);
                        }
                    }
                    std::cout << std::endl;

                    for(int method = 0; method < nb_methods; ++method)
                    {
                        for(int i = 0; i < nb_monte_carlo; ++i)
                        {
                            rot_error_avg[type][d][method] += rot_errors[method][i];
                            vel_error_avg[type][d][method] += vel_errors[method][i];
                            pos_error_avg[type][d][method] += pos_errors[method][i];
                        }
                        rot_error_avg[type][d][method] /= nb_monte_carlo;
                        vel_error_avg[type][d][method] /= nb_monte_carlo;
                        pos_error_avg[type][d][method] /= nb_monte_carlo;
                        for(int i = 0; i < nb_monte_carlo; ++i)
                        {
                            rot_error_std[type][d][method] += std::pow(rot_errors[method][i] - rot_error_avg[type][d][method],2);
                            vel_error_std[type][d][method] += std::pow(vel_errors[method][i] - vel_error_avg[type][d][method],2);
                            pos_error_std[type][d][method] += std::pow(pos_errors[method][i] - pos_error_avg[type][d][method],2);
                        }
                        rot_error_std[type][d][method] = std::sqrt(rot_error_std[type][d][method]/nb_monte_carlo);
                        vel_error_std[type][d][method] = std::sqrt(vel_error_std[type][d][method]/nb_monte_carlo);
                        pos_error_std[type][d][method] = std::sqrt(pos_error_std[type][d][method]/nb_monte_carlo);

                    }
                }
            }
            std::cout << std::endl << "RESULTS " << std::endl << "(Methods: PM, LPM, GPM trained, UGPM trained" << std::endl;
            for(int type = 0; type < 2; ++type)
            {
                if(type == 0) std::cout << std::endl << std::endl << "Acc noise" << std::endl;
                if(type == 1) std::cout << std::endl << std::endl << "Gyr noise" << std::endl;
                std::cout << " Rot error ====" << std::endl;
                for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
                std::cout << std::endl;
                for(int method = 0; method < nb_methods; ++method)
                {
                    for(int d = 0; d < noise_factor.size(); ++d)
                    {
                        if(type==0)
                        {
                                std::cout << " (" << acc_std*noise_factor[d] << "," << rot_error_avg[type][d][method] << ")";
                        }else
                        {
                                std::cout << " (" << gyr_std*noise_factor[d] << "," << rot_error_avg[type][d][method] << ")";
                        }
                    }
                    std::cout << std::endl;
                }
                std::cout << " Pos error ====" << std::endl;
                for(int i = 0; i < nb_methods; ++i) std::cout << "      " << i;
                std::cout << std::endl;
                for(int method = 0; method < nb_methods; ++method)
                {
                    for(int d = 0; d < noise_factor.size(); ++d)
                    {
                        if(type==0)
                        {
                                std::cout << " (" << acc_std*noise_factor[d] << "," << pos_error_avg[type][d][method] << ")";
                        }else
                        {
                                std::cout << " (" << gyr_std*noise_factor[d] << "," << pos_error_avg[type][d][method] << ")";
                        }
                    }
                    std::cout << std::endl;
                }
            }
        }
    }

    if(experiment_type == 4)
    { // Metrics for the robustness to noise

        std::cout << "BIAS CORRECTION EXPERIMENT" << std::endl;

        celib::ImuSimulatorOption sim_opt;
        std::vector<double> gyr_bias_norm = {0.01, 0.05, 0.1, 0.2, 0.4, 0.6, 0.8, 1.0};
        std::vector<double> acc_bias_norm = {0.01, 0.05, 0.1, 0.2, 0.4, 0.6, 0.8, 1.0};
        std::vector<double> dt_bias = {0.001, 0.005, 0.01, 0.02, 0.04, 0.06, 0.08 , 0.1};
        sim_opt.acc_std = 0.04;
        sim_opt.gyr_std = 0.01;

        double duration = 1.0;
        std::cout.precision(3);

        std::vector<std::vector<double> > gyr_rot_error_avg;
        std::vector<std::vector<double> > gyr_pos_error_avg;
        std::vector<std::vector<double> > acc_rot_error_avg;
        std::vector<std::vector<double> > acc_pos_error_avg;
        std::vector<std::vector<double> > dt_rot_error_avg;
        std::vector<std::vector<double> > dt_pos_error_avg;
        std::vector<std::vector<double> > gyr_rot_error_raw_avg;
        std::vector<std::vector<double> > gyr_pos_error_raw_avg;
        std::vector<std::vector<double> > acc_rot_error_raw_avg;
        std::vector<std::vector<double> > acc_pos_error_raw_avg;
        std::vector<std::vector<double> > dt_rot_error_raw_avg;
        std::vector<std::vector<double> > dt_pos_error_raw_avg;

        for(int type = 0; type < 2; ++type)
        {
            gyr_rot_error_avg.push_back(std::vector<double>());
            gyr_pos_error_avg.push_back(std::vector<double>());
            acc_rot_error_avg.push_back(std::vector<double>());
            acc_pos_error_avg.push_back(std::vector<double>());
            dt_rot_error_avg.push_back(std::vector<double>());
            dt_pos_error_avg.push_back(std::vector<double>());
            gyr_rot_error_raw_avg.push_back(std::vector<double>());
            gyr_pos_error_raw_avg.push_back(std::vector<double>());
            acc_rot_error_raw_avg.push_back(std::vector<double>());
            acc_pos_error_raw_avg.push_back(std::vector<double>());
            dt_rot_error_raw_avg.push_back(std::vector<double>());
            dt_pos_error_raw_avg.push_back(std::vector<double>());

            if(type == 0)
            {
                sim_opt.motion_type = "slow";
            }
            else
            {
                sim_opt.motion_type = "fast";
            }
            std::cout << "===============================================" << std::endl;
            std::cout << "Run results for " << sim_opt.motion_type << " motion" << std::endl;

            std::vector<std::vector<double> > gyr_rot_error(gyr_bias_norm.size());
            std::vector<std::vector<double> > gyr_pos_error(gyr_bias_norm.size());
            std::vector<std::vector<double> > acc_rot_error(acc_bias_norm.size());
            std::vector<std::vector<double> > acc_pos_error(acc_bias_norm.size());
            std::vector<std::vector<double> > dt_rot_error(acc_bias_norm.size());
            std::vector<std::vector<double> > dt_pos_error(acc_bias_norm.size());
            std::vector<std::vector<double> > gyr_rot_raw_error(gyr_bias_norm.size());
            std::vector<std::vector<double> > gyr_pos_raw_error(gyr_bias_norm.size());
            std::vector<std::vector<double> > acc_rot_raw_error(acc_bias_norm.size());
            std::vector<std::vector<double> > acc_pos_raw_error(acc_bias_norm.size());
            std::vector<std::vector<double> > dt_rot_raw_error(acc_bias_norm.size());
            std::vector<std::vector<double> > dt_pos_raw_error(acc_bias_norm.size());

            for(int i = 0; i < nb_monte_carlo; ++i)
            {
                std::cout << i << "." << std::flush;
                celib::ImuSimulator imu_sim(sim_opt);
                celib::RandomGenerator rand_gen;
                double start_t = rand_gen.randUniform(overlap,sim_opt.dataset_length - duration - overlap);
                double end_t = start_t + duration;
                auto data = imu_sim.get(start_t-overlap, end_t+overlap);
                auto data_save = data;

                double pos_dist = imu_sim.getTranslationDistance(start_t, end_t);
                double rot_dist = imu_sim.getOrientationDistance(start_t, end_t);

                celib::Vec3 unit_vec = celib::Vec3::Random();
                unit_vec = unit_vec / (unit_vec.norm());

                for(int ibw = 0; ibw < gyr_bias_norm.size(); ++ibw)
                {
                    data = data_save;
                    celib::Vec3 bias_vec = gyr_bias_norm[ibw] * unit_vec;
                    for(int idata = 0; idata < data.acc.size(); idata++)
                    {
                        data.gyr[idata].data[0] += bias_vec(0);
                        data.gyr[idata].data[1] += bias_vec(1);
                        data.gyr[idata].data[2] += bias_vec(2);
                    }

                    celib::PreintPrior prior;
                    std::vector<std::vector<double> > t;
                    std::vector<double> temp_t;
                    temp_t.push_back(end_t);
                    t.push_back(temp_t);
                    preint_opt.type = celib::UGPM;
                    preint_opt.train_gpm = true;
                    celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
                    celib::PreintMeas preint = imu_preint.get(0,0);
                    celib::PreintMeas preint_corrected;
                    preint_corrected.delta_R = preint.delta_R * celib::ExpMap(preint.d_delta_R_d_bw * (-bias_vec));
                    preint_corrected.delta_v = preint.delta_v + (preint.d_delta_v_d_bw * (-bias_vec));
                    preint_corrected.delta_p = preint.delta_p + (preint.d_delta_p_d_bw * (-bias_vec));

                    auto raw_error = imu_sim.testPreint(start_t, end_t, preint);
                    gyr_rot_raw_error[ibw].push_back(raw_error[0]/rot_dist);
                    gyr_pos_raw_error[ibw].push_back(raw_error[2]/pos_dist);
                    auto error = imu_sim.testPreint(start_t, end_t, preint_corrected);
                    gyr_rot_error[ibw].push_back(error[0]/rot_dist);
                    gyr_pos_error[ibw].push_back(error[2]/pos_dist);
                }

                for(int ibf = 0; ibf < acc_bias_norm.size(); ++ibf)
                {
                    data = data_save;
                    celib::Vec3 bias_vec = acc_bias_norm[ibf] * unit_vec;
                    for(int idata = 0; idata < data.acc.size(); idata++)
                    {
                        data.acc[idata].data[0] += bias_vec(0);
                        data.acc[idata].data[1] += bias_vec(1);
                        data.acc[idata].data[2] += bias_vec(2);
                    }

                    celib::PreintPrior prior;
                    std::vector<std::vector<double> > t;
                    std::vector<double> temp_t;
                    temp_t.push_back(end_t);
                    t.push_back(temp_t);
                    preint_opt.type = celib::UGPM;
                    preint_opt.train_gpm = true;
                    celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
                    celib::PreintMeas preint = imu_preint.get(0,0);
                    celib::PreintMeas preint_corrected;
                    preint_corrected.delta_v = preint.delta_v + (preint.d_delta_v_d_bf * (-bias_vec));
                    preint_corrected.delta_p = preint.delta_p + (preint.d_delta_p_d_bf * (-bias_vec));

                    auto raw_error = imu_sim.testPreint(start_t, end_t, preint);
                    acc_rot_raw_error[ibf].push_back(raw_error[0]/rot_dist);
                    acc_pos_raw_error[ibf].push_back(raw_error[2]/pos_dist);
                    auto error = imu_sim.testPreint(start_t, end_t, preint_corrected);
                    acc_rot_error[ibf].push_back(error[0]/rot_dist);
                    acc_pos_error[ibf].push_back(error[2]/pos_dist);
                }

                for(int idt = 0; idt < dt_bias.size(); ++idt)
                {
                    data = data_save;
                    for(int idata = 0; idata < data.acc.size(); idata++)
                    {
                        data.acc[idata].t += dt_bias[idt];
                        data.gyr[idata].t += dt_bias[idt];
                    }

                    celib::PreintPrior prior;
                    std::vector<std::vector<double> > t;
                    std::vector<double> temp_t;
                    temp_t.push_back(end_t);
                    t.push_back(temp_t);
                    preint_opt.type = celib::UGPM;
                    preint_opt.train_gpm = true;
                    celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior);
                    celib::PreintMeas preint = imu_preint.get(0,0);
                    celib::PreintMeas preint_corrected;
                    preint_corrected.delta_R = preint.delta_R * celib::ExpMap(preint.d_delta_R_d_t * dt_bias[idt]);
                    preint_corrected.delta_v = preint.delta_v + (preint.d_delta_v_d_t * dt_bias[idt]);
                    preint_corrected.delta_p = preint.delta_p + (preint.d_delta_p_d_t * dt_bias[idt]);

                    auto raw_error = imu_sim.testPreint(start_t, end_t, preint);
                    dt_rot_raw_error[idt].push_back(raw_error[0]/rot_dist);
                    dt_pos_raw_error[idt].push_back(raw_error[2]/pos_dist);
                    auto error = imu_sim.testPreint(start_t, end_t, preint_corrected);
                    dt_rot_error[idt].push_back(error[0]/rot_dist);
                    dt_pos_error[idt].push_back(error[2]/pos_dist);
                }

            }
            std::cout << std::endl;
            for(int ibw = 0; ibw < gyr_bias_norm.size(); ++ibw)
            {
                gyr_rot_error_avg[type].push_back(0);
                gyr_pos_error_avg[type].push_back(0);
                gyr_rot_error_raw_avg[type].push_back(0);
                gyr_pos_error_raw_avg[type].push_back(0);
                for(int isample = 0; isample < nb_monte_carlo; ++isample)
                {
                    gyr_rot_error_avg[type][ibw] += gyr_rot_error[ibw][isample];
                    gyr_pos_error_avg[type][ibw] += gyr_pos_error[ibw][isample];
                    gyr_rot_error_raw_avg[type][ibw] += gyr_rot_raw_error[ibw][isample];
                    gyr_pos_error_raw_avg[type][ibw] += gyr_pos_raw_error[ibw][isample];
                }
                gyr_rot_error_avg[type][ibw] /= nb_monte_carlo;
                gyr_pos_error_avg[type][ibw] /= nb_monte_carlo;

                gyr_rot_error_raw_avg[type][ibw] /= nb_monte_carlo;
                gyr_pos_error_raw_avg[type][ibw] /= nb_monte_carlo;
            }
            for(int ibf = 0; ibf < acc_bias_norm.size(); ++ibf)
            {
                acc_rot_error_avg[type].push_back(0);
                acc_pos_error_avg[type].push_back(0);
                acc_rot_error_raw_avg[type].push_back(0);
                acc_pos_error_raw_avg[type].push_back(0);
                for(int isample = 0; isample < nb_monte_carlo; ++isample)
                {
                    acc_rot_error_avg[type][ibf] += acc_rot_error[ibf][isample];
                    acc_pos_error_avg[type][ibf] += acc_pos_error[ibf][isample];
                    acc_rot_error_raw_avg[type][ibf] += acc_rot_raw_error[ibf][isample];
                    acc_pos_error_raw_avg[type][ibf] += acc_pos_raw_error[ibf][isample];
                }
                acc_rot_error_avg[type][ibf] /= nb_monte_carlo;
                acc_pos_error_avg[type][ibf] /= nb_monte_carlo;
                acc_rot_error_raw_avg[type][ibf] /= nb_monte_carlo;
                acc_pos_error_raw_avg[type][ibf] /= nb_monte_carlo;

            }
            for(int idt = 0; idt < dt_bias.size(); ++idt)
            {
                dt_rot_error_avg[type].push_back(0);
                dt_pos_error_avg[type].push_back(0);
                dt_rot_error_raw_avg[type].push_back(0);
                dt_pos_error_raw_avg[type].push_back(0);
                for(int isample = 0; isample < nb_monte_carlo; ++isample)
                {
                    dt_rot_error_avg[type][idt] += dt_rot_error[idt][isample];
                    dt_pos_error_avg[type][idt] += dt_pos_error[idt][isample];
                    dt_rot_error_raw_avg[type][idt] += dt_rot_raw_error[idt][isample];
                    dt_pos_error_raw_avg[type][idt] += dt_pos_raw_error[idt][isample];
                }
                dt_rot_error_avg[type][idt] /= nb_monte_carlo;
                dt_pos_error_avg[type][idt] /= nb_monte_carlo;
                dt_rot_error_raw_avg[type][idt] /= nb_monte_carlo;
                dt_pos_error_raw_avg[type][idt] /= nb_monte_carlo;

            }
        }

        std::cout << "RESULTS" << std::endl;

        std::cout << std::endl;
        std::cout << ">>>> Gyr bias experiment" << std::endl;
        std::cout << "\\scriptsize Bias norm ";
        for(auto n:gyr_bias_norm)
        {
            std::cout << " & \\scriptsize " << n;
        }
        std::cout << std::endl << std::endl;
        for(int type = 0; type < 2; ++type)
        {
            if(type == 0) std::cout << "\\scriptsize Slow Rot er.";
            if(type == 1) std::cout << "\\scriptsize Fast Rot er.";
            for(int b = 0; b < gyr_bias_norm.size(); ++b)
            {
                std::cout << " & \\scriptsize " << 100*gyr_rot_error_raw_avg[type][b] << " & \\scriptsize " << 100*gyr_rot_error_avg[type][b];
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;
        for(int type = 0; type < 2; ++type)
        {
            if(type == 0) std::cout << "\\scriptsize Slow Pos er.";
            if(type == 1) std::cout << "\\scriptsize Fast Pos er.";
            for(int b = 0; b < gyr_bias_norm.size(); ++b)
            {
                std::cout << " & \\scriptsize " << 100*gyr_pos_error_raw_avg[type][b] << " & \\scriptsize " << 100*gyr_pos_error_avg[type][b];
            }
            std::cout << std::endl;
        }
        std::cout << std::endl << std::endl;

        std::cout << ">>>> Acc bias experiment" << std::endl;
        std::cout << "\\scriptsize Bias norm ";
        for(auto n:acc_bias_norm)
        {
            std::cout << " & \\scriptsize " << n;
        }
        std::cout << std::endl << std::endl;
        for(int type = 0; type < 2; ++type)
        {
            if(type == 0) std::cout << "\\scriptsize Slow Pos er.";
            if(type == 1) std::cout << "\\scriptsize Fast Pos er.";
            for(int b = 0; b < acc_bias_norm.size(); ++b)
            {
                std::cout << " & \\scriptsize " << 100*acc_pos_error_raw_avg[type][b] << " & \\scriptsize " << 100*acc_pos_error_avg[type][b];
            }
            std::cout << std::endl;
        }
        std::cout << std::endl << std::endl;

        std::cout << ">>>> Timeshift experiment" << std::endl;
        std::cout << "\\scriptsize Timeshift ";
        for(auto n:dt_bias)
        {
            std::cout << " & \\scriptsize " << n;
        }
        std::cout << std::endl;
        for(int type = 0; type < 2; ++type)
        {
            if(type == 0) std::cout << "\\scriptsize Slow Rot er.";
            if(type == 1) std::cout << "\\scriptsize Fast Rot er.";
            for(int b = 0; b < dt_bias.size(); ++b)
            {
                std::cout << " & \\scriptsize " << 100*dt_rot_error_raw_avg[type][b] << " & \\scriptsize " << 100*dt_rot_error_avg[type][b];
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;
        for(int type = 0; type < 2; ++type)
        {
            if(type == 0) std::cout << "\\scriptsize Slow Pos er.";
            if(type == 1) std::cout << "\\scriptsize Fast Pos er.";
            for(int b = 0; b < dt_bias.size(); ++b)
            {
                std::cout << " & \\scriptsize " << 100*dt_pos_error_raw_avg[type][b] << " & \\scriptsize " << 100*dt_pos_error_avg[type][b];
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;
    }

    return 0;
}


#include <fstream>
#include <sstream>
#include <iostream>
#include <string>
#include <iomanip>
#include <optional>
#include <cctype>
#include <map>
#include <algorithm>
#include <vector>
#include <range/v3/view.hpp>
#include <range/v3/algorithm.hpp>
#include <boost/dynamic_bitset.hpp>
#include <complex>
using namespace std::literals::complex_literals;
#include "../aoc2020/matrix.h"
#include "../aoc2020/mypair.h"

namespace views = ranges::views;

template<int N>
concept Module4 = N % 4 == 0;

template<int N>
auto rotateSquare(const FixedMatrix<bool, N, N> &m)
{
  FixedMatrix<bool, N, N> r;
  for (int i = 0; i < N; ++i) {
    for (int j = 0; j < N; ++j) {
      r(j, N - 1 - i) = m(i, j);
    }
  }
  return r;
}
template<int N>
auto flipSquare(const FixedMatrix<bool, N, N> &m)
{
  FixedMatrix<bool, N, N> r;
  for (int i = 0; i < N; ++i) {
    std::reverse_copy(m.row(i).begin(), m.row(i).end(), r.row(i));
  }
  return r;
}

template<int N>
auto rotateflipSquare(const FixedMatrix<bool, N, N> &m)
{
  std::array<FixedMatrix<bool, N, N>, 8> r;

  r[0] = m;
  for (int i = 0; i < 3; ++i) {
    r[i + 1] = rotateSquare(r[i]);
  }

  r[4] = flipSquare(m);
  for (int i = 4; i < 7; ++i) {
    r[i + 1] = rotateSquare(r[i]);
  }
  return r;
}

class Game
{
  std::map<FixedMatrix<bool, 2>, FixedMatrix<bool, 3>> ruleB_;
  std::map<FixedMatrix<bool, 3>, FixedMatrix<bool, 4>> ruleC_;
  FixedMatrix<bool, 3> mat_;

  template<int N>
  void insert(std::map<FixedMatrix<bool, N>, FixedMatrix<bool, N + 1>> &m, FixedMatrix<bool, N> const &k, const FixedMatrix<bool, N + 1> &v)
  {
    //std::cout << N << "v" << std::endl;
    //std::cout << v << std::endl;
    auto r = rotateflipSquare(k);
    for (auto square : r) {
      //std::cout << square << std::endl;
      m[square] = v;
    }
  }
  //Matrix r;
  template<int M, int N>
  FixedMatrix<bool, M> subMatrix(const FixedMatrix<bool, N> &input, int m, int n)
  {
    FixedMatrix<bool, M> r;
    for (int i = 0; i < M; ++i) {
      for (int j = 0; j < M; ++j) {
        r(i, j) = input(m + i, n + j);
      }
    }
    return r;
  }
  template<int N, int M>
  void assignsubMatrix(FixedMatrix<bool, N> &input, int m, int n, const FixedMatrix<bool, M> &r)
  {
    for (int i = 0; i < M; ++i) {
      for (int j = 0; j < M; ++j) {
        input(m + i, n + j) = r(i, j);
      }
    }
  }

public:
  void outputrules() const
  {
    std::cout << "rulesB:\n";
    for (auto &[k, v] : ruleB_) {
      std::cout << k << std::endl;
      std::cout << v << std::endl;
      std::cout << "---------\n";
    }
    std::cout << "rulesC:\n";
    for (auto &[k, v] : ruleC_) {
      std::cout << k << std::endl;
      std::cout << v << std::endl;
      std::cout << "---------\n";
    }
  }

  template<int N>
  requires(Module4<N>)
    FixedMatrix<bool, N + N / 2> transform(const FixedMatrix<bool, N> &input)
  {

    std::cout << "transform" << N << std::endl;
    FixedMatrix<bool, N + N / 2> r;
    for (int i = 0; i < N; i += 2) {
      for (int j = 0; j < N; j += 2) {
        FixedMatrix<bool, 2, 2> m = subMatrix<2>(input, i, j);
        std::cout << "--" << i << "," << j << "-input--------" << std::endl;

        std::cout << m << std::endl;
        std::cout << "--" << i * 3 / 2 << "," << j * 3 / 2 << "-output--------" << std::endl;
        std::cout << ruleB_[m] << std::endl;
        assignsubMatrix(r, i * 3 / 2, j * 3 / 2, ruleB_[m]);
      }
    }
    return r;
  }
  template<int N>
  requires((N % 6) == 0 || N == 3 || Module4<N>)
    FixedMatrix<bool, N + N / 3> transform(const FixedMatrix<bool, N> &input)
  {
    std::cout << "transform" << N << std::endl;
    FixedMatrix<bool, N + N / 3> r;
    for (int i = 0; i < N; i += 3) {
      for (int j = 0; j < N; j += 3) {
        FixedMatrix<bool, 3, 3> m = subMatrix<3>(input, i, j);
        std::cout << "--" << i << "," << j << "-input--------" << std::endl;
        std::cout << m << std::endl;
        std::cout << "--" << i * 4 / 3 << "," << j * 4 / 3 << "-output--------" << std::endl;
        std::cout << ruleC_[m] << std::endl;
        assignsubMatrix(r, i * 4 / 3, j * 4 / 3, ruleC_[m]);
      }
    }
    return r;
  }
  Game()
  {
  }
  friend std::istream &operator>>(std::istream &is, Game &g)
  {
    std::string k, v;
    while (is >> k >> v) {
      if (k.size() == 5) {
        FixedMatrix<bool, 2> matk;
        auto mk = k | views::split('/') | views::transform([](auto row) {
          return row | views::transform([](char c) { return c == '#'; });
        });
        {
          auto id = 0;
          for (auto i = mk.begin(); i != mk.end(); ++i, ++id) {
            auto jd = 0;
            for (auto j = (*i).begin(); j != (*i).end(); ++j, ++jd) {
              matk(id, jd) = *j;
            }
          }
        }
        FixedMatrix<bool, 3> matv;
        auto mv = v | views::split('/') | views::transform([](auto row) {
          return row | views::transform([](char c) { return c == '#'; });
        });
        auto id = 0;
        for (auto i = mv.begin(); i != mv.end(); ++i, ++id) {
          auto jd = 0;
          for (auto j = (*i).begin(); j != (*i).end(); ++j, ++jd) {
            int n = *j;
            matv(id, jd) = n;
          }
        }
        g.insert(g.ruleB_, matk, matv);
      } else {
        FixedMatrix<bool, 3> matk;
        auto mk = k | views::split('/') | views::transform([](auto row) {
          return row | views::transform([](char c) { return c == '#'; });
        });
        {
          auto id = 0;
          for (auto i = mk.begin(); i != mk.end(); ++i, ++id) {
            auto jd = 0;
            for (auto j = (*i).begin(); j != (*i).end(); ++j, ++jd) {
              matk(id, jd) = *j;
            }
          }
        }
        FixedMatrix<bool, 4> matv;
        auto mv = v | views::split('/') | views::transform([](auto row) {
          return row | views::transform([](char c) { return c == '#'; });
        });
        auto id = 0;
        for (auto i = mv.begin(); i != mv.end(); ++i, ++id) {
          auto jd = 0;
          for (auto j = (*i).begin(); j != (*i).end(); ++j, ++jd) {
            int n = *j;
            matv(id, jd) = n;
          }
        }
        g.insert(g.ruleC_, matk, matv);
      }
    }

    return is;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    Game g;
    std::ifstream iss(argv[1]);
    iss >> g;
    //g.outputrules();
    //return 0;
    FixedMatrix<bool, 3> mat_;
    mat_(0, 1) = true;
    mat_(1, 2) = true;
    mat_(2, 0) = true;
    mat_(2, 1) = true;
    mat_(2, 2) = true;
    auto A1 = g.transform(mat_);//4
    std::cout << A1 << std::endl;
    auto A2 = g.transform(A1);//6
    std::cout << A2 << std::endl;
    auto A3 = g.transform(A2);
    std::cout << A3 << std::endl;
    auto A4 = g.transform(A3);
    std::cout << A4 << std::endl;
    auto A5 = g.transform(A4);
    std::cout << A5 << std::endl;
    std::cout << std::count(std::begin(A5.array()), std::end(A5.array()), 1) << std::endl;
  }
}


// This is a finite element solver for the stochastic failure of 3D octect-truss
// system under uniaxial tension. Matrix calculations rely on Eigen linear algebra
// library.
//
// Copyright (c) 2019 Wen Luo <wenluo2016@u.northwestern.edu>
//
// 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.

#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <Eigen/Sparse>
#include <algorithm>
#include <fstream>

#include "functions_Geometry.h"
#include "functions_Kernel.h"
#include "functions_Post-processing.h"
#include "functions_Random_vec.h"
#include "functions_Step_control.h"
#include "erf_inv.h"

using namespace std;
using std::vector;
//using namespace Eigen;

//// Debug only --------------//
//#include <chrono>
//using namespace std::chrono;
////--------------------------//

/** ----------------------------------------- Unit System ------------------------------------------- *
  * length:       10^-6 m                                                                             *
  * force:        10^-6 N                                                                             *
  * pressure:     MPa (10^6 Pa)                                                                       *
  * ------------------------------------------------------------------------------------------------- */

/** --------------------------------------- Model Parameters ---------------------------------------- *
  * Young's Modulus:              10^4 MPa                    Link Length:            2 micron        *
  * Mean Tensile Strength:      ~ 10^2 MPa                    Link Section Diameter:  0.5 micron      *
  * ------------------------------------------------------------------------------------------------- */

int main()
{
    /**------------------------------Geometry Definition & Preprocessing-----------------------------**/
    const size_t nx = 8, ny = 4, nz = 4;
    const double l_block = sqrt(2); // 10^-6 m
    const double len = sqrt(2) * l_block;
    /* nodal coordinates */
    vector<vector<double> > coords;
    coords.reserve((nx+3)*(ny+3)*(nz+3)/2);
    SetCoords(nx, ny, nz, l_block, coords);
    /* total number of nodes */
    const size_t n_node = coords.size();

    /* connectivity matrix */
    vector<vector<vector<size_t> > > conn(6);
    /* flattened conn */
    vector<vector<size_t>> conn_flat;
    conn_flat.reserve(6*nx*ny*nz);
    SetConnectivity(len, coords, conn, conn_flat);

//    for(auto it = conn_flat.cbegin(); it != conn_flat.cend(); ++it){
//        cout << (*it)[0] << ", " << (*it)[1] << endl;
//    }

    /* total number of elements */
    const size_t n_ele = conn_flat.size();

    /* write mesh information to VTK_XML file */
//    WriteMeshXML("octet_lattice_CELL.vtu", n_node, n_ele, coords, conn_flat);

    /**-------------------------------------End of Pre-processing-------------------------------------**/
    /**-----------------------------------------------------------------------------------------------**/
    /**--------------------------------------------Kernel---------------------------------------------**/
    /* element stiffness matrix - entry form */
    const double elas = 10000, area = 3.1415926 * 0.5 * 0.5 / 4;

    /* ind_u: list of DOF index of delta_u_0 (prescribed disp.)
     * ind_f: list of DOF index of delta_f_1 (prescribed load) */
    vector<size_t> ind_u, ind_f, ind_RF;
    ind_u.reserve( (nx+1)*(ny+1)+2*(ny+1)*(nz+1)+(nx+1)*(nz+1) );
    ind_f.reserve( 3*n_node - nx*ny+2*ny*nz+nx*nz );
    ind_RF.reserve( (nx+1)*(ny+1) );
    SetDispLoadDomain(nx*l_block, coords, ind_RF, ind_u, ind_f);

    /* global u_0 vector (trial_u0) */
    const size_t size_u = ind_u.size();
    Eigen::VectorXd trial_u0 = Eigen::ArrayXd::Zero(size_u);
    Set_u0(size_u, nx*l_block, coords, ind_u, trial_u0);

    /* global f_1 vector - ALL ZEROS */
    const size_t size_f = ind_f.size();
    Eigen::VectorXd delta_f_1 = Eigen::ArrayXd::Zero(size_f);

    /* permutation vector */
    Eigen::VectorXi perm_Vec(size_u+size_f);
    for (size_t i = 0; i != size_u+size_f; ++i){
        if (i < size_u)
            perm_Vec(i) = ind_u[i];
        else
            perm_Vec(i) = ind_f[i-size_u];
    }

    const size_t n_run = 5;
    const size_t n_batch = 100;
//    const size_t batch_offset = 0*n_run;
    for(size_t run = 0; run != n_run; ++run){
        /* string index of runs for output */
//        string str_run = std::to_string(run+batch_offset);
//        str_run = string(6 - str_run.length(), '0') + str_run;

        /* global stiffness matrix spK*/
        Eigen::SparseMatrix<double> spK_glob(3*n_node,3*n_node); // matrix dimension: dimension of geometry * n_node
        SetStiff_glob(n_ele, elas, area, len, coords, conn_flat, spK_glob);

        /* Variables initialization */
        Eigen::VectorXd u_glob = Eigen::ArrayXd::Zero(size_u+size_f);
        Eigen::VectorXd f_glob = Eigen::ArrayXd::Zero(size_u+size_f);

        vector<double> strain(n_ele), stress(n_ele);
        vector<double> sig_ratio(n_ele);

        // residual element stiffness (damage indicator)
        vector<double> res_Ke(n_ele, elas*area/len);
        // critical elements
        size_t cri_ele_ind_new = n_ele + 10;

        // reaction forces
        double RF = 0;

        // history variables
        size_t n_step = 12*ny*nz;
        Eigen::MatrixXd load_disp = Eigen::MatrixXd::Constant(n_step, 2, 0);

        // random number generation
        vector<double> bound_tensile(n_ele), bound_compressive(n_ele);
//        SetBoundariesWeibull(5, 100, bound_tensile, bound_compressive, 10);
        SetBoundariesGaussWeibull(bound_tensile, bound_compressive, 10);

//        cout << "Bounds = " << endl;
//        for(auto it = bound_tensile.cbegin(); it != bound_tensile.cend(); ++it)
//            cout << *it << endl;

        /** ~~~~~~ Entering step loops ~~~~~~ **/
        for(size_t step = 0; step != n_step; ++step){
//            cout << "------------------------------------" << endl;
//            cout << "STEP = " << step << endl;

            RunElasticSolver(0.01*trial_u0, delta_f_1, ind_u, ind_f, perm_Vec, spK_glob, u_glob, f_glob);

            GetStressStrain_Elastic(elas, area, len, res_Ke, conn_flat, coords, u_glob, strain, stress);

            /* Get loading multiplier */
            GetLoadRatio_Elastic(bound_tensile, bound_compressive, res_Ke, stress, sig_ratio);
//            cout << "RATIO = " << endl;
//            for(auto it = sig_ratio.cbegin(); it != sig_ratio.cend(); ++it)
//                cout << '\t' << *it << endl;

            /* Critical element */
            auto cri_ele_it = std::min_element(sig_ratio.cbegin(), sig_ratio.cend());
            cri_ele_ind_new = distance(sig_ratio.cbegin(), cri_ele_it);
//            cout << "index = " << cri_ele_ind_new << endl;

            /* Correct Field Variables */
            u_glob *= (*cri_ele_it);
            f_glob *= (*cri_ele_it);
            MulVec(strain, *cri_ele_it);
            MulVec(stress, *cri_ele_it);

            /* Update SPK */
            UpdateSPK(cri_ele_ind_new, len, elas*area/len, 0, coords, conn_flat, spK_glob);

            /* Update res_Ke */
            res_Ke[cri_ele_ind_new] = 0;

            /**--------------------------**/
            /**      Record Output       **/
            /**--------------------------**/
            // load-displacement curve
            RF = 0;
            for (auto it = ind_RF.cbegin(); it != ind_RF.cend(); ++it){
                RF += f_glob(*it);
            }
            load_disp(step,0) = u_glob(ind_RF[0]);
            load_disp(step,1) = RF;

            /**--------------------------**/
            /**   Write to Output File   **/
            /**--------------------------**/
//            // Field Variables
//            WriteMeshXML("Brittle_T_"+str_run+"/odb_octet_"+std::to_string(step)+".vtu",
//                         n_node, n_ele, coords, conn_flat);
//            WriteNodeDataXML("Brittle_T_"+str_run+"/odb_octet_"+std::to_string(step)+".vtu",
//                             n_node, u_glob, f_glob);
//            WriteElementDataXML("Brittle_T_"+str_run+"/odb_octet_"+std::to_string(step)+".vtu",
//                                n_ele, strain, stress, res_Ke, bound_tensile);
//            WriteEndXML("Brittle_T_"+str_run+"/odb_octet_"+std::to_string(step)+".vtu");
            /**--------------------------**/
            /* Break Condition */
            if(load_disp(step,1)/load_disp(step,0) < 0.01 * load_disp(0,1)/load_disp(0,0))
                break;

        }/** ~~~~~~ End of step loops ~~~~~~ **/
//        /* Load Displacement Curve */
//        ofstream dbg_f;
//        //dbg_f.open("Brittle_T_"+str_run+"/LoadDisp_Lattice_"+str_run+".dat",ios::out|ios::trunc);
//        dbg_f.open("Brittle_T_results/LoadDisp_octet_"+str_run+".dat",ios::out|ios::trunc);
//        dbg_f << load_disp << endl;
//        dbg_f.close();
        /* maximum loads vector */
        ofstream dbg_f;
        dbg_f.open("Brittle_T_results/PeakLoads_octet_"+std::to_string(n_batch)+".dat",ios::out|ios::app);
        dbg_f << load_disp.colwise().maxCoeff()(1) << endl;
        dbg_f.close();

//        /* first strength vector */
//        dbg_f.open("Brittle_T_results/FirstLoads_octet_"+std::to_string(n_batch)+".dat",ios::out|ios::app);
//        dbg_f << load_disp(0,1) << endl;
//        dbg_f.close();
//
//        /* second strength vector */
//        dbg_f.open("Brittle_T_results/SecondLoads_octet_"+std::to_string(n_batch)+".dat",ios::out|ios::app);
//        dbg_f << load_disp(1,1) << endl;
//        dbg_f.close();
//
//        /* third strength vector */
//        dbg_f.open("Brittle_T_results/ThirdLoads_octet_"+std::to_string(n_batch)+".dat",ios::out|ios::app);
//        dbg_f << load_disp(2,1) << endl;
//        dbg_f.close();
    }
    /* Update new coordinates *///    cout << "minimum_ratio = " << *critical_ele_it << endl;
//    vector<vector<double> > coords_new;
//    get_stress_strain(elas, len, conn_flat, coords, u_glob);

    /**-----------------------------------------End of Kernel------------------------------------------**/
    /**------------------------------------------------------------------------------------------------**/
    /**-------------------------------------------Debugging--------------------------------------------**/

//    cout << "u_1 = " << endl;
//    cout << u_1 << endl;
//    cout << "u_glob = " << endl;
//    cout << u_glob << endl;

//    cout << "Sum of forces = " << temp << endl;
//    ofstream dbg_f;
//    dbg_f.open("dbg_B.dat",ios::out|ios::trunc);
//    dbg_f << '\n' << "Sparse Global Stiffness Matrix:" << '\n'
//          << Eigen::MatrixXd(spK_glob) << endl;
//    dbg_f << load_disp << endl;
//    dbg_f << '\n' << "permutation vector:" << '\n' << perm_Vec << endl;
//    dbg_f.close();

//    for (auto it = ind_u.cbegin(); it != ind_u.cend(); ++it){
//        cout << *it << '\n';
//    }
//    cout << endl;

//    auto start = high_resolution_clock::now();
//    auto stop = high_resolution_clock::now();
//    auto duration = duration_cast<microseconds>(stop - start);
//
//    cout << "Time taken by function: "
//         << duration.count() << " 10^-6s " << endl;

    return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Mengali, G., and A.A. Quarta, Fondamenti di Meccanica del volo Spaziale.
 *      Noomen, R., Lambert targeter Excel file.
 *      Izzo, D., Keplerian_Toolbox.
 *
 *    Notes
 *      The elliptical case was taken from Example 6.1, page 159-162 of ( Mengali, Quarta ). The
 *      hyperbolic case was taken from ( Noomen, R. ). The retrograde and near-pi cases are verified
 *      against values found with the Lambert routine available in the Keplerian_Toolbox from
 *      ESA/ACT. It is assumed that the first two test cases are sufficient to test the radial and
 *      tangential velocity components computations. These are therefore not tested in the last two
 *      test cases.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "tudat/astro/basic_astro/orbitalElementConversions.h"
#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/basics/testMacros.h"

#include "tudat/astro/mission_segments/zeroRevolutionLambertTargeterIzzo.h"

namespace tudat
{
namespace unit_tests
{

using namespace unit_conversions;

//! Test the Izzo Lambert targeting algorithm code.
BOOST_AUTO_TEST_SUITE( test_zero_revolution_lambert_targeter_izzo )

//! Test hyperbolic case.
BOOST_AUTO_TEST_CASE( testHyperbolicCase )
// Copied and slightly adapted from unitTestLambertTargeterIzzo (rev 466)
{
    // Expected test result in meters.
    // Hyperbolic test case (results from excel file [1]).
    const double expectedValueOfSemiMajorAxisHyperbola = -1270129.3602e3;
    const double expectedValueOfRadialSpeedAtDepartureHyperbola = -0.74546e3;
    const double expectedValueOfRadialSpeedAtArrivalHyperbola = 0.69321e3;
    const double expectedValueOfTransverseSpeedAtDepartureHyperbola = 0.15674e3;
    const double expectedValueOfTransverseSpeedAtArrivalHyperbola = 0.10450e3;
    const Eigen::Vector3d expectedInertialVelocityAtDeparture( -745.457, 156.743, 0.0 );
    const Eigen::Vector3d expectedInertialVelocityAtArrival( 104.495, -693.209, 0.0 );

    // Tolerances.
    const double toleranceSemiMajorAxisHyperbola = 1.0e-7;
    const double toleranceVelocity = 1.0e-4;

    // Time conversions.
    const double timeOfFlightInDaysHyperbola = 100.0;
    const double timeOfFlightHyperbola = convertJulianDaysToSeconds( timeOfFlightInDaysHyperbola );

    // Central body gravitational parameter.
    const double earthGravitationalParameter = 398600.4418e9;

    // The starting point is twice as far as L1 and L2, which is not really
    // realistic, but it is not about the case, but about the verification.
    const Eigen::Vector3d positionAtDepartureHyperbola( convertAstronomicalUnitsToMeters( 0.02 ),
                                                        0.0, 0.0 ),
            positionAtArrivalHyperbola( 0.0, convertAstronomicalUnitsToMeters( -0.03 ), 0.0 );

    // Compute Lambert targeting algorithms.
    mission_segments::ZeroRevolutionLambertTargeterIzzo lambertTargeterHyperbola(
                positionAtDepartureHyperbola, positionAtArrivalHyperbola, timeOfFlightHyperbola,
                earthGravitationalParameter );

    // Create local vectors for position and velocity.
    const Eigen::Vector3d positionDepartureHyperbola = positionAtDepartureHyperbola;
    const Eigen::Vector3d velocityDepartureHyperbola =
            lambertTargeterHyperbola.getInertialVelocityAtDeparture( );

    // Test if the computed semi-major axis corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getSemiMajorAxis( ),
                                expectedValueOfSemiMajorAxisHyperbola,
                                toleranceSemiMajorAxisHyperbola );

    // Test if the computed transverse and radial velocity components corresponds to the
    // expected values within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getRadialVelocityAtDeparture( ),
                                expectedValueOfRadialSpeedAtDepartureHyperbola,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getRadialVelocityAtArrival( ),
                                expectedValueOfRadialSpeedAtArrivalHyperbola,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getTransverseVelocityAtDeparture( ),
                                expectedValueOfTransverseSpeedAtDepartureHyperbola,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterHyperbola.getTransverseVelocityAtArrival( ),
                                expectedValueOfTransverseSpeedAtArrivalHyperbola,
                                toleranceVelocity );

    // Check that velocities match expected values within the defined tolerance.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedInertialVelocityAtDeparture,
                                       lambertTargeterHyperbola.getInertialVelocityAtDeparture( ),
                                       toleranceVelocity );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedInertialVelocityAtArrival,
                                       lambertTargeterHyperbola.getInertialVelocityAtArrival( ),
                                       toleranceVelocity );

    // Test if the computed solution is anti-clockwise, if the z-component of the angular momentum
    // (h = r \times v) is positive.
    BOOST_CHECK_GT( positionDepartureHyperbola.cross( velocityDepartureHyperbola ).z( ), 0 );
}

//! Test elliptical case.
BOOST_AUTO_TEST_CASE( testEllipticalCase )
// Copied and slightly adapted from unitTestLambertTargeterIzzo (rev 466)
{
    // Elliptical test case (results from example 6.1 page 159-162 [2]).
    // Set canonical units for Earth (see page 29 [2]).
    double distanceUnit = 6.378136e6;
    double timeUnit = 806.78;

    // Set expected test result in meters.
    double expectedValueOfSemiMajorAxisEllipse = 5.4214 * distanceUnit;
    double expectedValueOfRadialSpeedAtDepartureEllipse = 2.73580e3;
    double expectedValueOfRadialSpeedAtArrivalEllipse = 2.97503e3;
    double expectedValueOfTransverseSpeedAtDepartureEllipse = 6.59430e3;
    double expectedValueOfTransverseSpeedAtArrivalEllipse = 3.29715e3;
    const Eigen::Vector3d expectedInertialVelocityAtDeparture( 2735.8, 6594.3, 0.0 );
    const Eigen::Vector3d expectedInertialVelocityAtArrival( -1367.9, 4225.03, 0.0 );

    // Tolerance in absolute units.
    double toleranceSemiMajorAxisEllipse = 1.0e4;
    double toleranceVelocity = 1.0e-2;

    // Time conversions.
    double timeOfFlightEllipse = 5.0 * timeUnit;

    // Central body gravitational parameter.
    const double earthGravitationalParameter = 398600.4418e9;

    // Elliptical orbit case.
    const Eigen::Vector3d positionAtDepartureEllipse( 2.0 * distanceUnit, 0.0, 0.0 ),
            positionAtArrivalEllipse( 2.0 * distanceUnit, 2.0 * sqrt( 3.0 ) * distanceUnit, 0.0 );

    // Compute Lambert targeting algorithms.
    mission_segments::ZeroRevolutionLambertTargeterIzzo lambertTargeterEllipse(
                positionAtDepartureEllipse, positionAtArrivalEllipse, timeOfFlightEllipse,
                earthGravitationalParameter );

    // Create local vectors for position and velocity.
    const Eigen::Vector3d positionDepartureEllipse = positionAtDepartureEllipse;
    const Eigen::Vector3d velocityDepartureEllipse =
            lambertTargeterEllipse.getInertialVelocityAtDeparture( );

    // Test if the computed semi-major axis corresponds to the expected value within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getSemiMajorAxis( ),
                                expectedValueOfSemiMajorAxisEllipse,
                                toleranceSemiMajorAxisEllipse );

    // Test if the computed transverse and radial velocity components corresponds to the
    // expected values within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getRadialVelocityAtDeparture( ),
                                expectedValueOfRadialSpeedAtDepartureEllipse,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getRadialVelocityAtArrival( ),
                                expectedValueOfRadialSpeedAtArrivalEllipse,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getTransverseVelocityAtDeparture( ),
                                expectedValueOfTransverseSpeedAtDepartureEllipse,
                                toleranceVelocity );
    BOOST_CHECK_CLOSE_FRACTION( lambertTargeterEllipse.getTransverseVelocityAtArrival( ),
                                expectedValueOfTransverseSpeedAtArrivalEllipse,
                                toleranceVelocity );

    // Check that velocities match expected values within the defined tolerance.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedInertialVelocityAtDeparture,
                                       lambertTargeterEllipse.getInertialVelocityAtDeparture( ),
                                       toleranceVelocity );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedInertialVelocityAtArrival,
                                       lambertTargeterEllipse.getInertialVelocityAtArrival( ),
                                       toleranceVelocity );

    // Test if the computed solution is anti-clockwise, if the z-component of the angular momentum
    // (h = r \times v) is positive.
    BOOST_CHECK_GT( positionDepartureEllipse.cross( velocityDepartureEllipse ).z( ), 0 );
}

//! Test retrograde Earth-Mars transfer.
BOOST_AUTO_TEST_CASE ( testRetrograde )
// Copied and slightly adapted from unitTestLambertTargeterIzzo (rev 466).
{
    // Set tolerance.
    const double tolerance = 1.0e-9;

    // Set positions at departure and arrival.
    /* Values taken from http://ccar.colorado.edu/~rla/lambert_j2000.html for JDi = 2456036 and
     * JDf = 2456336.
     */
    const Eigen::Vector3d positionAtDeparture( -131798187443.90068, -72114797019.4148,
                                               2343782.3918863535 ),
            positionAtArrival( 202564770723.92966, -42405023055.01754, -5861543784.413235 );

    // Set time-of-flight, coherent with initial and final positions.
    const double timeOfFlight = convertJulianDaysToSeconds( 300.0 );

    // Set central body (the Sun) gravitational parameter. Value taken from keptoolbox.
    const double solarGravitationalParameter = 1.32712428e20;

    // Set expected values for inertial velocities. Values obtained with keptoolbox.
    const Eigen::Vector3d expectedInitialVelocity( -14157.8507230353, 28751.266655828,
                                                   1395.46037631136 ),
            expectedFinalVelocity( -6609.91626743654, -22363.5220239692, -716.519714631494 );

    // Compute Lambert solution.
    mission_segments::ZeroRevolutionLambertTargeterIzzo lambertTargeterRetrograde(
                positionAtDeparture, positionAtArrival,
                timeOfFlight, solarGravitationalParameter, true );

    // Retrieve inertial velocities.
    const Eigen::Vector3d initialVelocity =
            lambertTargeterRetrograde.getInertialVelocityAtDeparture( );
    const Eigen::Vector3d finalVelocity = lambertTargeterRetrograde.getInertialVelocityAtArrival( );

    // Check that velocities match expected values within the defined tolerance.
    TUDAT_CHECK_MATRIX_CLOSE( initialVelocity, expectedInitialVelocity, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE( finalVelocity, expectedFinalVelocity, tolerance );
}

//! Test near-pi, circular, coplanar, Earth-Mars transfer.
BOOST_AUTO_TEST_CASE ( testNearPi )
// Copied and slightly adapted from unitTestLambertTargeterIzzo (rev 466).
{
    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Set time-of-flight, coherent with initial and final positions.
    const double timeOfFlight = convertJulianDaysToSeconds( 300.0 );

    // Set central body (the Sun) gravitational parameter. Value taken from keptoolbox.
    const double solarGravitationalParameter = 1.32712428e20;

    // Set Keplerian elements at departure and arrival.
    Eigen::Matrix< double, 6, 1 > keplerianStateAtDeparture, keplerianStateAtArrival;
    keplerianStateAtDeparture << convertAstronomicalUnitsToMeters( 1.0 ), 0.0,
            0.0, 0.0, 0.0, 0.0;
    keplerianStateAtArrival << convertAstronomicalUnitsToMeters( 1.5 ), 0.0, 0.0,
            0.0, 0.0, convertDegreesToRadians( 179.999 );

    //  Convert to Cartesian elements.
    const Eigen::Matrix< double, 6, 1 > cartesianStateAtDeparture =
            orbital_element_conversions::convertKeplerianToCartesianElements(
                keplerianStateAtDeparture, solarGravitationalParameter );
    const Eigen::Matrix< double, 6, 1 > cartesianStateAtArrival =
            orbital_element_conversions::convertKeplerianToCartesianElements(
                keplerianStateAtArrival, solarGravitationalParameter );

    // Extract positions at departure and arrival.
    const Eigen::Vector3d positionAtDeparture = cartesianStateAtDeparture.head( 3 );
    const Eigen::Vector3d positionAtArrival = cartesianStateAtArrival.head( 3 );

    // Set expected values for inertial velocities. Values obtained with keptoolbox.
    const Eigen::Vector3d expectedInitialVelocity( 3160.36638344209, 32627.4771454454, 0.0 ),
            expectedFinalVelocity( 3159.89183582648, -21751.7065841264, 0.0 );

    // Compute Lambert solution.
    mission_segments::ZeroRevolutionLambertTargeterIzzo lambertTargeterRetrograde(
                positionAtDeparture, positionAtArrival, timeOfFlight, solarGravitationalParameter );

    // Retrieve inertial velocities.
    const Eigen::Vector3d initialVelocity =
            lambertTargeterRetrograde.getInertialVelocityAtDeparture( );
    const Eigen::Vector3d finalVelocity = lambertTargeterRetrograde.getInertialVelocityAtArrival( );

    // Check that velocities match expected values within the defined tolerance.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedInitialVelocity, initialVelocity, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedFinalVelocity, finalVelocity, tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat