/**
 *  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;
}


// 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) 2020-2021, Marco Sánchez Beeckman
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <array>
#include <chrono>
#include <iostream>
#include <limits>
#include <random>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <opencv2/core.hpp>

#include "Homography.h"

namespace tfg {

/**
 * Obtain weights that minimize the smooth truncated quadratic cost given by equation (2) in Szeliski's paper.
 * The formula for these weights is shown in equation (3).
 * @param residuals2 Squared track residuals, ordered by track.
 * @param tau2 Tau squared, an inlier threshold for equations (2) to (4) in Szeliski's paper.
 * @return The squared weights, ordered by track.
 */
std::vector<float> getWeights2(std::vector<float> &residuals2, float tau2) {
    std::vector<float> weights2;
    weights2.reserve(residuals2.size());
    for(unsigned int i = 0; i < residuals2.size(); i++) {
        float boundedResidual2 = residuals2[i] < tau2 ? residuals2[i] : tau2;
        float weight2 = 1 - boundedResidual2/tau2;
        weights2.push_back(weight2);
    }
    return weights2;
}

std::vector<float> getWeightsFromInliers(std::vector<std::vector<int>> &inliers, std::shared_ptr<tfg::TrackTable> &trackTable) {
    std::vector<float> weights(trackTable->numberOfTracks(), 0);
    std::vector<unsigned int> timesCountedAsInlier(trackTable->numberOfTracks(), 0);
    for(unsigned int f = 0; f < inliers.size(); f++) {
        std::vector<unsigned int> trajectories = trackTable->trajectoriesInFrame(f);
        for(unsigned int i = 0; i < inliers[f].size(); i++) {
            timesCountedAsInlier[trajectories[inliers[f][i]]]++;
        }
    }

    for(unsigned int t = 0; t < weights.size(); t++) {
        const unsigned int trackDuration = trackTable->durationOfTrack(t);
        // weights[t] = timesCountedAsInlier[t] == 0 ? 0 : 1;
        weights[t] = timesCountedAsInlier[t] == trackDuration - 1 ? 1 : 0;
    }

    return weights;
}

/**
 * Obtain the homography that fits a model the best given a set of origin points and another set of destinations, using RANSAC.
 * @param p0 Origin points.
 * @param p1 Destination points.
 * @param n Number of points.
 * @param niter Number of iterations for RANSAC.
 * @param tolerance Maximum distance in pixels to consider a point an inlier.
 * @param H Output homography.
 * @param inliers Output list of indexes that correspond to those of the tracks that are inliers.
 */
void computeHomographyRANSAC(const std::vector<cv::Vec2f> &p0, const std::vector<cv::Vec2f> &p1, int n, int niter, float tolerance, cv::Matx33f &H, std::vector<int> &inliers) {

    float tolerance2 = tolerance * tolerance;
    unsigned int maxInliers = 0;
    std::vector<int> bestInliers;

    std::array<int, 4> randomIndices;
    std::random_device randomDevice;
    std::mt19937 mersenneTwister(randomDevice());
    std::uniform_int_distribution<int> uniformIntDist(0, n-1);

    for(int iter = 0; iter < niter; iter++) {

        // Sample 4 different numbers between 0 and n-1
        for(int i = 0; i < 4; i++) {
            int randomNumber = uniformIntDist(mersenneTwister);
            while(std::find(randomIndices.begin(), randomIndices.end(), randomNumber) != randomIndices.end()) {
                randomNumber = uniformIntDist(mersenneTwister);
            }
            randomIndices[i] = randomNumber;
        }

        // Obtain the 4 points corresponding to the random indices
        std::vector<cv::Vec2f> pointsL(4);
        std::vector<cv::Vec2f> pointsR(4);
        for(int i = 0; i < 4; i++) {
            pointsL[i] = p0[randomIndices[i]];
            pointsR[i] = p1[randomIndices[i]];
        }

        // Compute homography using Least Squares method
        cv::Matx33f Haux;
        std::vector<unsigned int> zeros(4, 0);
        std::vector<float> ones(1, 1.0f);
        tfg::computeHomographyWLS(pointsL, pointsR, 4, zeros, ones, Haux);

        // Obtain the inliers for this iteration
        std::vector<int> iterationInliers;
        iterationInliers.reserve(n);
        for(int i = 0; i < n; i++) {
            cv::Vec3f pointOrigin;
            cv::Vec3f pointDestination;
            pointOrigin(0) = p0[i](0);      pointOrigin(1) = p0[i](1);      pointOrigin(2) = 1.0f;
            pointDestination(0) = p1[i](0); pointDestination(1) = p1[i](1); pointDestination(2) = 1.0f;

            cv::Vec3f pred = Haux * pointOrigin;

            if(pred(2) == 0.0f) continue;

            pred(0) = pred(0)/pred(2); pred(1) = pred(1)/pred(2); pred(2) = 1.0f;
            float reprojectionError2 = cv::norm(pointDestination - pred, cv::NORM_L2SQR);
            if(reprojectionError2 < tolerance2) {
                iterationInliers.push_back(i);
            }
        }

        // If the number of inliers has increased from the best iteration, update it along with the homography
        const unsigned int numInliers = iterationInliers.size();
        if(numInliers > maxInliers) {
            maxInliers = numInliers;
            bestInliers.swap(iterationInliers);

            H(0,0) = Haux(0,0); H(0,1) = Haux(0,1); H(0,2) = Haux(0,2);
            H(1,0) = Haux(1,0); H(1,1) = Haux(1,1); H(1,2) = Haux(1,2);
            H(2,0) = Haux(2,0); H(2,1) = Haux(2,1); H(2,2) = Haux(2,2);
        }

    }
    inliers.swap(bestInliers);
}

/**
 * Compute an homography using Weighted Least Squares, i.e. minimize ||WAh||, whose solution is the eigenvector with smallest eigenvalue of the matrix AtWtWA.
 * @param p0 Origin points.
 * @param p1 Destination points.
 * @param n Number of points.
 * @param trajectories The index of the trajectory corresponding to each one of the points.
 * @param weights2 The weights of each track, squared.
 * @param H Output homography.
 */
void computeHomographyWLS(const std::vector<cv::Vec2f> &p0, const std::vector<cv::Vec2f> &p1, int n, const std::vector<unsigned int> &trajectories, const std::vector<float> &weights2, cv::Matx33f &H) {

    // Normalize the left and right observations
    std::vector<cv::Vec2f> pnorm0;
    std::vector<cv::Vec2f> pnorm1;
    cv::Vec2f centerL(0, 0), centerR(0, 0);
    cv::Vec2f scaleL(0, 0), scaleR(0, 0);
    tfg::isotropicNormalization(p0, pnorm0, centerL, scaleL);
    tfg::isotropicNormalization(p1, pnorm1, centerR, scaleR);

    ////////////////////////// Minimization problem || Ah || /////////////////////////////

    Eigen::Matrix3d Tpl, Timinv;

    // Similarity transformation of the plane
    Tpl(0, 0) = scaleL(0); Tpl(0, 1) = 0.0;       Tpl(0, 2) = -scaleL(0)*centerL(0);
    Tpl(1, 0) = 0.0;       Tpl(1, 1) = scaleL(1); Tpl(1, 2) = -scaleL(1)*centerL(1);
    Tpl(2, 0) = 0.0;       Tpl(2, 1) = 0.0;       Tpl(2, 2) = 1.0;

    // Inverse similarity transformation of the image
    Timinv(0, 0) = 1.0/scaleR(0); Timinv(0, 1) = 0.0;           Timinv(0, 2) = centerR(0);
    Timinv(1, 0) = 0.0;           Timinv(1, 1) = 1.0/scaleR(1); Timinv(1, 2) = centerR(1);
    Timinv(2, 0) = 0.0;           Timinv(2, 1) = 0.0;           Timinv(2, 2) = 1.0;

    // Build At*Wt*W*A
    Eigen::Matrix<double, 9, 9> AtA = Eigen::Matrix<double, 9, 9>::Zero();
    for(int i = 0; i < n; i++) {
        float xpl = pnorm0[i](0), ypl = pnorm0[i](1),
                xim = pnorm1[i](0), yim = pnorm1[i](1);

        std::array<float, 9> row1 = {0.0f, 0.0f, 0.0f, -xpl, -ypl, -1.0f, yim * xpl, yim * ypl, yim};
        std::array<float, 9> row2 = {xpl, ypl, 1.0f, 0.0f, 0.0f, 0.0f, -xim * xpl, -xim * ypl, -xim};
        float w2 = weights2[trajectories[i]];

        // Lower half only, since SelfAdjointEigenSolver does not use the upper half
        for(int j = 0; j < 9; j++) {
            for(int k = j; k < 9; k++) {
                AtA(k, j) += w2 * row1[j] * row1[k] + w2 * row2[j] * row2[k];
            }
        }
    }

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 9, 9>> eigenSolver;
    eigenSolver.compute(AtA);
    Eigen::Matrix3d V(eigenSolver.eigenvectors().col(0).data()); V.transposeInPlace();

    // Denormalize H = Timinv * V.col(imin)* Tpl;
    Eigen::Matrix3d Vdenorm = Timinv * (V * Tpl);

    // Divide every entry by H(2, 2) so the bottom right entry is 1
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            H(i, j) = static_cast<float>(Vdenorm(i, j)/Vdenorm(2, 2));
        }
    }
}

/**
 * Normalize points such that their centroid is at the origin, and their average distance to it is sqrt(2).
 * @param points Vector of points.
 * @param normalizedPoints Output vector of normalized points.
 * @param center Output computed centroid of the points.
 * @param scale Output computed scaling factor.
 */
void isotropicNormalization(const std::vector<cv::Vec2f> &points, std::vector<cv::Vec2f> &normalizedPoints, cv::Vec2f &center, cv::Vec2f &scale) {
    const int NUMBER_OF_POINTS = points.size();
    normalizedPoints.clear();
    normalizedPoints.reserve(NUMBER_OF_POINTS);

    // Compute baricenter of the observations
    center(0) = 0.0f; center(1) = 0.0f;
    for(int i = 0; i < NUMBER_OF_POINTS; i++) {
        center = center + points[i];
    }
    center = center / NUMBER_OF_POINTS;

    // Scaling so that the average distance from the center is sqrt(2)
    scale(0) = 0.0f; scale(1) = 0.0f;
    for(int i = 0; i < NUMBER_OF_POINTS; i++) {
        const float obsDistance = cv::norm(points[i] - center);
        scale(0) = scale(0) + obsDistance;
        scale(1) = scale(1) + obsDistance;
    }
    scale(0) = (sqrtf(2.0f) * NUMBER_OF_POINTS) / scale(0);
    scale(1) = (sqrtf(2.0f) * NUMBER_OF_POINTS) / scale(1);

    // Normalize the observations
    for(int i = 0; i < NUMBER_OF_POINTS; i++) {
        normalizedPoints[i](0) = (points[i](0) - center(0)) * scale(0);
        normalizedPoints[i](1) = (points[i](1) - center(1)) * scale(1);
    }
}

/**
 * Perform Iteratively Reweighted Least Squares on a model with already computed initial homographies and weights.
 * @param model The initial model, to be updated on each iteration if an improvement is made.
 * @param trackTable The table with the previously computed tracks.
 * @param weights2 The squared weights of the tracks.
 * @param tau2 Tau squared, an inlier threshold for equations (2) to (4) in Szeliski's paper.
 */
void IRLS(std::shared_ptr<tfg::MotionModel> &model, std::shared_ptr<tfg::TrackTable> &trackTable, std::vector<float> &weights2, float tau2) {
    // std::cout << "First homography of the initial model:" << '\n';
    // model->printHomography(0);

    float s = 1.0f;
    unsigned int k = 0;
    float bestCost = -1;

    std::vector<float> initialWeights2(weights2.size(), 1.0f);
    std::vector<float> refinedWeights2(weights2.size(), 1.0f);
    std::vector<float> bestWeights2(weights2.size(), 1.0f);

    bool converged = false;
    unsigned int numIter = 0;
    initialWeights2.swap(weights2);
    std::shared_ptr<tfg::MotionModel> refinedModel;

    while(numIter < 50 && !converged) {
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

        // Refine model with new weights
        refinedModel = std::make_shared<tfg::MotionModel>(trackTable, tau2);
        refinedModel->fitFromWeights(initialWeights2);

        // Compute residuals of the new model
        std::vector<float> residuals2 = refinedModel->getResiduals2();
        std::vector<float> currentWeights2 = tfg::getWeights2(residuals2, tau2);
        // Update weights based on the current best model and the new one, then get the cost of the model
        for(unsigned int i = 0; i < weights2.size(); i++) {
            refinedWeights2[i] = s*currentWeights2[i] + (1-s)*bestWeights2[i];
        }
        float cost = refinedModel->getCost();

        std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

        std::cout << "(" << k + 1 << ") Iteration cost: " << cost << '\n';
        std::cout << "(" << k + 1 << ") Cost margin: " << std::abs((cost - bestCost) / bestCost) << '\n';
        std::cout << "(" << k + 1 << ") Time: " << (std::chrono::duration_cast<std::chrono::microseconds>(end-begin).count())/1000000.0 << " seconds" << '\n';

        // If the cost has increased, reduce the step. If it has decreased, make the step bigger and update the best model to the new one
        if(cost > bestCost && bestCost >= 0) {
            s = s/4;
        } else {
            if(cost > 0 && std::abs((cost - bestCost) / bestCost) < 0.00001) converged = true;
            bestCost = cost;
            k = k + 1;
            s = 4*s < 1 ? 4*s : 1;
            for(unsigned int i = 0; i < bestWeights2.size(); i++) {
                bestWeights2[i] = initialWeights2[i];
            }
            model = refinedModel;
        }
        initialWeights2.swap(refinedWeights2);
        numIter++;
    }
    weights2.swap(bestWeights2);
}

} // namespace tfg


#include "Line2D.h"
#include "Angle.h"
#include "Pose2D.h"
#include "Geometry.h"

#include <cmath>
#include <boost/concept_check.hpp>
#include <limits>

using namespace A2O;
using namespace Eigen;

Line2D::Line2D()
      : _start(0, 0), _extension(1, 0)
{
}

Line2D::Line2D(const double& m,
	       const double& b)
      : _start(0, b), _extension(1, m)
{
}

Line2D::Line2D(const double& xStart,
	       const double& yStart,
	       const Angle& alpha,
	       const double& length)
      : _start(xStart, yStart), _extension(alpha.getVector(length))
{
  if (_extension(0) == 0 && _extension(1) == 0) {
    _extension = Vector2d(1, 0);
  }
}

Line2D::Line2D(const Eigen::Vector2d& start,
	       const Angle& alpha,
	       const double& length)
      : _start(start), _extension(alpha.getVector(length))
{
  validate();
}

Line2D::Line2D(const Vector2d& start,
	       const Vector2d& end)
      : _start(start), _extension(end - start)
{
  validate();
}

Line2D::Line2D(const double& xStart,
	       const double& yStart,
	       const double& xEnd,
	       const double& yEnd)
      : _start(xStart, yStart), _extension(xEnd - xStart, yEnd - yStart)
{
  validate();
}

Line2D::~Line2D()
{
}

void Line2D::validate()
{
  if (_extension(0) == 0 && _extension(1) == 0) {
    _extension = Vector2d(0, 1);
  }
}

const Eigen::Vector2d& Line2D::getStart() const
{
  return _start;
}

const Eigen::Vector2d& Line2D::getExtensionVector() const
{
  return _extension;
}

const Eigen::Vector2d Line2D::getEnd() const
{
  return _start + _extension;
}

const Angle Line2D::getAngle() const
{
  return Angle::to(_extension);
}

const double Line2D::m() const
{
  if (_extension(0) == 0) {
    if (_extension(1) > 0) {
      return std::numeric_limits<double>::max();
    } else {
      return std::numeric_limits<double>::lowest();
    }
  }

  return _extension(1) / _extension(0);
}

const double Line2D::b() const
{
  if (_extension(0) == 0) {
    return 0;
  }

  return _start(1) - ((_extension(1) / _extension(0)) * _start(0));
}

const double Line2D::yValue(const double& x) const
{
  if (_extension(0) == 0) {
    // this line is y-parallel
    return 0;
  }

  return (_extension(1) / _extension(0)) * (x - _start(0)) + _start(1);
}

const double Line2D::xValue(const double& y) const
{
  if (_extension(1) == 0) {
    // this line is x-parallel
    return _start(0);
  }

  return (_extension(0) / _extension(1)) * (y - _start(1)) + _start(0);
}

Pose2D Line2D::getClosestPose(const Vector2d& point)
{

  Angle lineAngle(Angle::to(_extension));
  Angle pointAngle(Angle::to(point - _start));

  Angle offsetAngle (pointAngle - lineAngle);
  double hypotenuse = Geometry::getDistance<double, 2>(point, _start);
  double factor = cos(offsetAngle.rad()) * hypotenuse;
  Vector2d closestPoint (_start + _extension / Geometry::getNorm<double, 2>(_extension) * factor);

  return Pose2D(closestPoint, Angle::to(_extension));
}

std::vector< Vector2d > Line2D::getTrail(const Vector2d& start, const Vector2d& end)
{
  Pose2D poseStart = getClosestPose(start);
  Pose2D poseEnd = getClosestPose(end);
  double distance = poseStart.getDistanceTo(poseEnd);
  Vector2d offset = poseStart.getAngle().getVector(0.1 * distance);
  Vector2d point = poseStart.getPosition();
  std::vector<Vector2d> points;
  int i = 0;
  do{
    point += offset;
    points.push_back(point);
  } while(++i < 10);
  return points;
}

bool Line2D::operator==(const Line2D& other) const
{
  return _start == other._start && _extension == other._extension;
}

bool Line2D::operator!=(const Line2D& other) const
{
  return !(*this == other);
}


#ifndef __MOTIONNLP_HPP__
#define __MOTIONNLP_HPP__

#include "IpTNLP.hpp"
#include <math.h>
#include <Eigen/Dense>
#include <iostream>

#include "motion_planner/Agent.hpp"
#include <motion_planner/NumericalIntegrationConstraints.hpp>

using namespace Ipopt;

class MotionNLP: public TNLP
{

public:

  Agent* p_agent;

  /** default constructor */
  MotionNLP();
  MotionNLP(Agent* p_agent);

  /** default destructor */
  virtual ~MotionNLP();

     /**@name Overloaded from TNLP */
     //@{
     /** Method to return some info about the nlp */
     virtual bool get_nlp_info(
        Index&          n,
        Index&          m,
        Index&          nnz_jac_g,
        Index&          nnz_h_lag,
        IndexStyleEnum& index_style
     );

     /** Method to return the bounds for my problem */
     virtual bool get_bounds_info(
        Index   n,
        Number* x_l,
        Number* x_u,
        Index   m,
        Number* g_l,
        Number* g_u
     );

     /** Method to return the starting point for the algorithm */
     virtual bool get_starting_point(
        Index   n,
        bool    init_x,
        Number* x,
        bool    init_z,
        Number* z_L,
        Number* z_U,
        Index   m,
        bool    init_lambda,
        Number* lambda
     );

     /** Method to return the objective value */
     virtual bool eval_f(
        Index         n,
        const Number* x,
        bool          new_x,
        Number&       obj_value
     );

     /** Method to return the gradient of the objective */
     virtual bool eval_grad_f(
        Index         n,
        const Number* x,
        bool          new_x,
        Number*       grad_f
     );

     /** Method to return the constraint residuals */
     virtual bool eval_g(
        Index         n,
        const Number* x,
        bool          new_x,
        Index         m,
        Number*       g
     );

     /** Method to return:
      *   1) The structure of the Jacobian (if "values" is NULL)
      *   2) The values of the Jacobian (if "values" is not NULL)
      */
     virtual bool eval_jac_g(
        Index         n,
        const Number* x,
        bool          new_x,
        Index         m,
        Index         nele_jac,
        Index*        iRow,
        Index*        jCol,
        Number*       values
     );

     /** Method to return:
      *   1) The structure of the Hessian of the Lagrangian (if "values" is NULL)
      *   2) The values of the Hessian of the Lagrangian (if "values" is not NULL)
      */
     virtual bool eval_h(
        Index         n,
        const Number* x,
        bool          new_x,
        Number        obj_factor,
        Index         m,
        const Number* lambda,
        bool          new_lambda,
        Index         nele_hess,
        Index*        iRow,
        Index*        jCol,
        Number*       values
     );

     /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
     virtual void finalize_solution(
        SolverReturn               status,
        Index                      n,
        const Number*              x,
        const Number*              z_L,
        const Number*              z_U,
        Index                      m,
        const Number*              g,
        const Number*              lambda,
        Number                     obj_value,
        const IpoptData*           ip_data,
        IpoptCalculatedQuantities* ip_cq
     );
     //@}
     double getQuadraticCost(const Eigen::Vector3d & final_state,
                                                         const Eigen::Vector3d & target,
                                                         Eigen::Vector3d & gradient_f);
  private:
     /**@name Methods to block default compiler methods.
      *
      * The compiler automatically generates the following three methods.
      *  Since the default compiler implementation is generally not what
      *  you want (for all but the most simple classes), we usually
      *  put the declarations of these methods in the private section
      *  and never implement them. This prevents the compiler from
      *  implementing an incorrect "default" behavior without us
      *  knowing. (See Scott Meyers book, "Effective C++")
      */
     //@{
     MotionNLP(
        const MotionNLP&
     );

     MotionNLP& operator=(
        const MotionNLP&
     );
     //@}
     NumericalIntegrationConstraints integration_constr;

  };

  #endif