#pragma once
#include <cmath>
#include <type_traits>
#include <chrono>

#include <boost/chrono.hpp>
#include <QtCore/QMetaType>
#include <QtCore/QDateTime>

namespace QtTools
{
	/// тут ряд функций для преобразования unix time в julian date,
	/// в форматах std::chrono, boost::chrono, QDateTime / double, QDate.

	/// unix time в QDate julian day.
	/// QDate хранит julian day как qint64.
	/// QDATE_JULIAN_DAY_FOR_UNIX_EPOCH должно быть равно 2440588
	const qint64 QDATE_JULIAN_DAY_FOR_UNIX_EPOCH = QDate(1970, 1, 1).toJulianDay();

	/// https://en.wikipedia.org/wiki/Julian_day
	/// 2440587.5
	/// разница в .5 вызвана в отчете точки начала дня:
	/// для julian day это полдень, для unix time это полночь
	const double JULIAN_DAY_FOR_UNIX_EPOCH = 2440587.5;

	/************************************************************************/
	/*                   julian / chrono -> QDate                           */
	/************************************************************************/
	/// QDate по факту обертка вокруг qint64 - in/out параметры не нужны, достаточно return формы
	inline QDate ToQDate(std::chrono::system_clock::time_point point)
	{
		auto days = std::chrono::duration_cast<std::chrono::hours>(point.time_since_epoch()).count() / 24;
		return QDate::fromJulianDay(days + QDATE_JULIAN_DAY_FOR_UNIX_EPOCH);
	}

	inline QDate ToQDate(boost::chrono::system_clock::time_point point)
	{
		auto days = boost::chrono::duration_cast<boost::chrono::hours>(point.time_since_epoch()).count() / 24;
		return QDate::fromJulianDay(days + QDATE_JULIAN_DAY_FOR_UNIX_EPOCH);
	}

	inline QDate ToQDate(double jd)
	{
		/// julian date: день начинается в полдень
		/// но QDate считает только дни(qint64), а unix эпоха считается как 2440588, вместо 2440587.5.
		/// округляем что бы быть консистентными. Возможно лишнее
		return QDate::fromJulianDay(std::round(jd));
	}

	/************************************************************************/
	/*                   julian / chrono -> QDateTime                       */
	/************************************************************************/
	/// QDateTime - qt shared data class, который создается на куче.
	/// предоставляем 2 версии сигнатур с in/out параметром и return формой
	inline void ToQDateTime(std::chrono::system_clock::time_point point, QDateTime & dt)
	{
		auto msec = std::chrono::duration_cast<std::chrono::milliseconds>(point.time_since_epoch());
		dt.setMSecsSinceEpoch(msec.count());
	}

	inline void ToQDateTime(boost::chrono::system_clock::time_point point, QDateTime & dt)
	{
		auto msec = boost::chrono::duration_cast<boost::chrono::milliseconds>(point.time_since_epoch());
		dt.setMSecsSinceEpoch(msec.count());
	}

	inline void ToQDateTime(double jd, QDateTime & dt)
	{
		/// https://en.wikipedia.org/wiki/Julian_day
		/// (JD − 2440587.5) × 86400
		auto msec = static_cast<qint64>((jd - JULIAN_DAY_FOR_UNIX_EPOCH) * 24 * 60 * 60 * 1000);
		dt.setMSecsSinceEpoch(msec);
	}

	inline QDateTime ToQDateTime(std::chrono::system_clock::time_point point)
	{
		auto msec = std::chrono::duration_cast<std::chrono::milliseconds>(point.time_since_epoch());
		return QDateTime::fromMSecsSinceEpoch(msec.count());
	}

	inline QDateTime ToQDateTime(boost::chrono::system_clock::time_point point)
	{
		auto msec = boost::chrono::duration_cast<boost::chrono::milliseconds>(point.time_since_epoch());
		return QDateTime::fromMSecsSinceEpoch(msec.count());
	}

	inline QDateTime ToQDateTime(double jd)
	{
		/// https://en.wikipedia.org/wiki/Julian_day
		/// (JD − 2440587.5) × 86400
		auto msec = static_cast<qint64>((jd - JULIAN_DAY_FOR_UNIX_EPOCH) * 24 * 60 * 60 * 1000);
		return QDateTime::fromMSecsSinceEpoch(msec);
	}

	/************************************************************************/
	/*                  chrono / QDate/QDateTime -> julian day              */
	/************************************************************************/
	inline double ToJulianDay(std::chrono::system_clock::time_point point)
	{
		auto days = std::chrono::duration_cast<std::chrono::hours>(point.time_since_epoch()).count() / 24;
		return days + QDATE_JULIAN_DAY_FOR_UNIX_EPOCH;
	}

	inline double ToJulianDay(boost::chrono::system_clock::time_point point)
	{
		auto days = boost::chrono::duration_cast<boost::chrono::hours>(point.time_since_epoch()).count() / 24;
		return days + QDATE_JULIAN_DAY_FOR_UNIX_EPOCH;
	}

	inline double ToJulianDay(const QDate & date)         { return date.toJulianDay(); }
	inline double ToJulianDay(const QDateTime & date)     { return ToJulianDay(date.date()); }

	/************************************************************************/
	/*          QDate/QDateTime / julian day -> chrono                      */
	/************************************************************************/
	inline std::chrono::system_clock::time_point ToStdChrono(double jd)
	{
		/// https://en.wikipedia.org/wiki/Julian_day
		/// (JD − 2440587.5) × 86400
		auto unix_time = static_cast<qint64>((jd - JULIAN_DAY_FOR_UNIX_EPOCH) * 24 * 60 * 60);
		return std::chrono::system_clock::from_time_t(unix_time);
	}

	inline std::chrono::system_clock::time_point ToStdChrono(QDate date)
	{
		/// https://en.wikipedia.org/wiki/Julian_day
		/// (JD − 2440587.5) × 86400
		auto unix_time = (date.toJulianDay() - QDATE_JULIAN_DAY_FOR_UNIX_EPOCH) * 24 * 60 * 60;
		return std::chrono::system_clock::from_time_t(unix_time);
	}

	inline std::chrono::system_clock::time_point ToStdChrono(const QDateTime & dt)
	{
		return std::chrono::system_clock::from_time_t(dt.toMSecsSinceEpoch() / 1000);
	}

	inline boost::chrono::system_clock::time_point ToBoostChrono(double jd)
	{
		/// https://en.wikipedia.org/wiki/Julian_day
		/// (JD − 2440587.5) × 86400
		auto unix_time = static_cast<qint64>((jd - JULIAN_DAY_FOR_UNIX_EPOCH) * 24 * 60 * 60);
		return boost::chrono::system_clock::from_time_t(unix_time);
	}

	inline boost::chrono::system_clock::time_point ToBoostChrono(QDate date)
	{
		/// https://en.wikipedia.org/wiki/Julian_day
		/// (JD − 2440587.5) × 86400
		auto unix_time = (date.toJulianDay() - QDATE_JULIAN_DAY_FOR_UNIX_EPOCH) * 24 * 60 * 60;
		return boost::chrono::system_clock::from_time_t(unix_time);
	}

	inline boost::chrono::system_clock::time_point ToBoostChrono(const QDateTime & dt)
	{
		return boost::chrono::system_clock::from_time_t(dt.toMSecsSinceEpoch() / 1000);
	}

	/// регистрирует std::chrono::system_clock::time_point/duration в Qt meta system
	void QtRegisterStdChronoTypes();
}

Q_DECLARE_METATYPE(std::chrono::system_clock::time_point);
Q_DECLARE_METATYPE(std::chrono::system_clock::duration);


#include <boost/chrono.hpp>
#include <iostream>

using namespace boost::chrono;

int main()
{
  minutes m{1};
  seconds s{35};

  std::cout << m + s << '\n';
  std::cout << duration_cast<minutes>(m + s) << '\n';
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Filename    : main.cpp                                                                              *
 * Project     : Planewalker - Schnorr-Euchner sphere decoder simulation for space-time lattice codes  *
 * Authors     : Pasi Pyrrö, Oliver Gnilke                                                             *
 * Version     : 1.0                                                                                *
 * Copyright   : Aalto University ~ School of Science ~ Department of Mathematics and Systems Analysis *
 * Date        : 9.1.2017                                                                              *
 * Language    : C++ (2011 or newer standard)                                                          *
 * Description : main program file, handles the simulation setup and main loop                         *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define ARMA_NO_DEBUG /* disable Armadillo bound checks for addiotional speed */

#include <iostream>
#include <iomanip>
#include <armadillo> /* linear algebra library */
#include <complex>
#include <vector>
#include <string>
#include <unordered_map>
#include <algorithm>
#include <random>
#include <chrono>
#include <ratio>
#include <ctime>
#include <csignal>
#include <thread>

#include "misc.hpp"
#include "config.hpp"
#include "algorithms.hpp"
#include "sphdec.hpp"

using namespace std;
using namespace arma;

/* storage for filepaths */
unordered_map<string, string> filenames;

/* Storage for simulation integer parameters */
unordered_map<string, int> params;

/* storage for simulation double parameters */
unordered_map<string, double> dparams;

/* storage for simulation string parameters */
unordered_map<string, string> sparams;

/* 64-bit standard random number generator */
mt19937_64 mersenne_twister {
    static_cast<long unsigned int>(
        chrono::high_resolution_clock::now().time_since_epoch().count() /* seed for rng */
    )
};

/* general purpose large uniform integer distribution (use with modulo) */
uniform_int_distribution<int> uniform_dist(0, 1000000);
uniform_real_distribution<double> uniform_real_dist(0.0, 1.0);
// normal_distribution uniform_dist(0, 1000000);

/* Used for writing the csv output file */
parallel_vector<string> output;

/* flag which indicates early stop of the simulation */
bool exit_flag;

/* Handles task kills (CTRL-C) */
void signal_handler(int signum) {
    exit_flag = true;    /* Terminates simulations */
    this_thread::sleep_for(chrono::milliseconds(1000)); /* Give some time for the simulation to end
                                                           before attempting to output data */
    log_msg("Simulations terminated by user!", "Alert");
    output_data(output); /* output current simulation data */
    exit(signum);        /* exit program */
}

/* The program starts here */
int main(int argc, char** argv)
{
    /* assign signal SIGINT (when CTRL-C is pressed) to signal handler */
    signal(SIGINT, signal_handler);
    /* if this flag is true, the program will exit from simulation loops */
    exit_flag = false;

    /* define default filenames */
    filenames["settings"]         = "settings/settings.ini";
    filenames["settings_default"] = "settings/settings.ini";
    filenames["bases"]            = "bases/bases.txt";
    filenames["log"]              = "logs/log.txt";

    /* settings filename parameter was given */
    if (argc == 2){
        filenames["settings"] = "settings/" + string(argv[1]);
    }
    /* too many parameters were given */
    else if (argc > 2) {
        cout << "Usage: " << argv[0] << " [settings_file*]" << endl;
        exit(0);
    }

    /* This program uses custom logging function which prints info
       in stdout (console output) and also in a log file.
       It can be found from the misc.cpp file */
    log_msg();
    log_msg("Starting program...");

    /* Configure program parameters i.e. occupy the maps listed above
     * with values read from the settings file.
     * Implemented in config.cpp file
     */
    configure();

    /* initialize simulation parameters */
    int m = params["no_of_transmit_antennas"];
    int n = params["no_of_receiver_antennas"];
    int t = params["time_slots"];
    int k = params["no_of_matrices"];
    int q = params["x-PAM"];
    int s = params["codebook_size_exponent"]; /* if spesified, the program will
                                                 try to look for a spherical shaping radius
                                                 so that the codebook has size of 2^s */

    /* Spesify SNR range for the simulation */
    int min = params["snr_min"];
    int max = params["snr_max"];
    int step = params["snr_step"];

    int min_runs = params["simulation_rounds"]; /* run at least this many rounds for each SNR simulation */

    int stat_interval = params["stat_display_interval"];  /* Display intermediate simulation stats every this many rounds */
    int search_density = params["radius_search_density"]; /* Spesifies how many radiuses around the initial guess we try
                                                             to get the codebook size to 2^s */
    search_density = (search_density <= 0) ? 10 : search_density; /* default to 10 */
    int samples = params["energy_estimation_samples"]; /* Take this many random samples from the codebook (negative number means sample all) */
    double P = dparams["spherical_shaping_max_power"]; /* User can also manually spesify the spherical shaping radius */

    string channel_model = sparams["channel_model"]; /* Decides how channel matrix H is generated */

    /* Read the complex basis matrices for the lattice we want to decode in */
    vector<cx_mat> bases = read_matrices(filenames["bases"]);
    /* Another set of basis matrices in case we want to use coset encoding */
    vector<cx_mat> coset_bases;

    /* flag that spesifies if we're doing the so called 'wiretap' simulation
       i.e. all codewords that belong to the same coset have the same meaning
       for this simulation we need another lattice, which is a sublattice of the previous one */
    bool coset_encoding = (filenames.count("cosets") != 0); /* If the user has spesified a coset basis file, assume 'wiretap' simulation */
    bool siso_channel = (channel_model.compare("siso") == 0); /* flag that is true when SISO channel model is used */

    if (!siso_channel && channel_model.compare("mimo") != 0){
        log_msg("Invalid channel model parameter used!", "Error");
        exit(1);
    }

    /* Conditionally read the coset basis matrices from a different basis file */
    if (coset_encoding)
        coset_bases = read_matrices(filenames["cosets"]);

    /* Sanity check: Print all read matrices! */
    log_msg("Read basis matrices:");
    for (auto const &basis : bases){
        log_msg(mat2str(basis), "Raw");
    }
    if (coset_encoding) {
        log_msg("Read coset basis matrices:");
        for (auto const &basis : coset_bases){
            log_msg(mat2str(basis), "Raw");
        }
    }

    /* Generator matrix of the lattice code */
    cx_mat G = create_generator_matrix(bases);

    /* LLL_reduction for G */
    // cout << G << endl;
    // G = LLL_reduction(G);
    // cout << G << endl;
    // bases = generator_to_bases(G);

    /* output LLL reduced basis matrices */
    // auto lll_bases = generator_to_bases(G);
    // for (const auto &lll : lll_bases) {
    //     output_complex_matrix("bases/MIDO_LLL_basis.txt", lll, true);
    // }
    // exit(0);

    /* sublattice generator matrices for coset encoding (e stands for 'Eve') */
    cx_mat invGe, Ge;

    /***** If the Gram matrix (generator times its own transpose) is diagonal,
           the basis for the lattice is orthogonal, Check it like this *****/
    // cout << "Orthogonality check:" << endl;
    // cout << G.t()*G << endl;

    /***** Matrices can be hardcoded in the simulation like so *****/
    // mat coset_multiplier("4 0 0 0;"
    //                      "0 2 0 0;"
    //                      "0 0 2 0;"
    //                      "0 0 0 2");

    // mat coset_multiplier("-2 -2  0  0;"
    //                       "0  0 -2 -1;"
    //                      "-1  1  1 -2;"
    //                       "1 -1  1 -1");

    // mat coset_multiplier("4 2 2 2;"
    //                      "0 2 0 0;"
    //                      "0 0 2 0;"
    //                      "0 0 0 2");

    // mat coset_multiplier("4 0 0 0;"
    //                      "0 4 0 0;"
    //                      "0 0 4 0;"
    //                      "0 0 0 4");

    // mat coset_multiplier("-2 -3  4 -1;"
    //                      " 0 -1  0  3;"
    //                      " 0 -3 -2 -3;"
    //                      "-4 -1  0 -1");

    if (coset_encoding) {
        /* as q-PAM is already a subset of translated 2Z^n lattice,
           we need to multiply the sublattice basis with 2 to make the lattice points comparable */
        Ge = 2*create_generator_matrix(coset_bases); /* Read cosets from file */
        // Ge = 2*G*coset_multiplier; /* Generate cosets from basis matrix with hardcoded transformation matrices */
        invGe = pinv(Ge); /* Coset lattice generator matrix and its (pseudo)inverse */
    }

    /***** You can generate basis matrix txt files like this *****/
    // auto cos_bases = generator_to_bases(0.5*Ge);
    // for (const auto &b : cos_bases) {
    //     output_complex_matrix("bases/alamouti_L5.txt", b, true);
    // }
    // cx_mat T(7,7);
    // for (short j = 0; j < 7; ++j){
    //     T.row(j) = bases[j];
    // }
    // // auto cols = generator_to_bases(T);
    // for (short j = 0; j < 7; ++j) {
    //     output_complex_matrix("bases/krus_7_new.txt", T.col(j), true);
    // }
    // exit(0);

    /***** Another example of matrix outputting: prints the real generator matrix into a txt file *****/
    // output_real_matrix("bases/MIDO_basis.txt", G_real);

    /* Many helper functions have been implemented for matrix manipulation,
       Check algorithms.cpp for a complete list */
    mat G_real = to_real_matrix(G); /* Make a real representation out of G */

    /***** Calculate the lattice constant like this *****/
    // cout << "determinant: " << det(G_real.t()*G_real) << endl;

    mat Q, Rorig; /* Decompose G_real */
    qr_econ(Q, Rorig, G_real);  /* QR-decomposition of G_real (omits zero rows in Rorig) */
    process_qr(Q, Rorig); /* Makes sure Rorig only has positive diagonal elements (this is probably optional) */

    /* Creates a q-PAM symbol set,
       e.g. 4-PAM = {-3, -1, 1, 3}
       Details in algorithms.cpp */
    vector<int> symbset = create_symbolset(q);

    // cout << "floor 6.5: " << nearest_symbol_floor(6.5, symbset) << endl;
    // cout << "floor -10: " << nearest_symbol_floor(-10, symbset) << endl;
    // cout << "floor  10: " << nearest_symbol_floor(10, symbset) << endl;
    // cout << "ceil  5.5: " << nearest_symbol_ceil(5.5, symbset) << endl;
    // cout << "ceil  -10: " << nearest_symbol_ceil(-10, symbset) << endl;
    // cout << "ceil   10: " << nearest_symbol_ceil(10, symbset) << endl;

    int num_points = 0; /* Number of codewords inside the spherical constellation */

    /* try to find radius for the codebook that has atleast 2^s codewords if s is spesified */
    if (s > 0) {
        log_msg("Attempting to estimate squared radius (max power) for codebook of 2^" + to_string(s) + " codewords...");
        double P_estimate = estimate_squared_radius(Rorig, s); /* see algorithms.cpp for details */
        cx_vec lambda_min = shortest_basis_vector(G); /* pick the shortest column vector of G */
        double search_step = frob_norm_squared(lambda_min)/search_density; /* radius search step */
        double rcurr = P_estimate + search_density*search_step/2; /* starting radius */
        log_msg("Initial guesstimate for codebook squared radius: " + to_string(P_estimate));
        log_msg("Radius search step: " + to_string(search_step));
        vector<double> rvec; /* store radius candidates in here (descending order) */
        for (int i = 0; i < search_density + 1 && rcurr > 0.0; i++) {
            rvec.push_back(rcurr);
            rcurr -= search_step;
        }
        /* Counts corresponding number of points inside hypersphere for each radius in rvec */
        vector<int> pvec = count_points_many_radiuses(Rorig, symbset, rvec, vec(k, fill::zeros), k, 0);

        // cout << "rvec size: " << rvec.size() << endl;
        // cout << "pvec size: " << pvec.size() << endl << endl;
        // cout << "rvec: " << vec2str(rvec, rvec.size()) << endl;
        // cout << "pvec: " << vec2str(pvec, pvec.size()) << endl;

        /* Pick the smallest radius that maps to number of codewords in range [2^s, 2^(s+1)] */
        int end_index = (int)pvec.size()-1;
        for (int j = end_index; j >= 0; j--) {
            if (pvec[j] >= (int)pow(2, s) && pvec[j] < (int)pow(2, s+1)) {
                P = rvec[j];
                num_points = pvec[j];
                break;
            }
        }
        /* Evaluate estimation result */
        if (P < 0) {
            log_msg("Estimation failed, using non-spherical shaping...", "Alert");
        } else if (P == dparams["spherical_shaping_max_power"]) {
            log_msg("Estimation failed, using the configured value for squared radius...", "Alert");
        } else {
            dparams["spherical_shaping_max_power"] = P;
        }
    }

    // P = sqrt(P);

    /* Generate the whole codebook or a random sampled subset of it. */
    log_msg("Generating codebook for code energy calculations...");
    vector<pair<vector<int>,cx_mat>> codebook = create_codebook(bases, Rorig, symbset);

    /* Calculate the average and maximum energy of the codewords in our lattice code */
    auto e = code_energy(codebook);

    /* Print some useful information before starting the actual simulation */
    log_msg("", "Raw");
    log_msg("Simulation info");
    log_msg("---------------");
    log_msg("Number of basis matrices (code length): " + to_string(k));
    log_msg("Using " + to_string(q) + "-PAM symbolset: " + vec2str(symbset, q));
    log_msg("Channel model: " + channel_model);
    log_msg("Size of the whole codebook: " + to_string((long long) dparams["codebook_size"]));
    if (samples > 0)
        log_msg("Number of codewords used for energy calculations: " + to_string(codebook.size()));
    log_msg("Average code energy: " + to_string(e.first));
    log_msg("Max code energy: " + to_string(e.second));
    if (P > 0) {
        log_msg("Using codebook spherical shaping squared radius: " + to_string(P));
        // log_msg("Suggested squared radius (max power) for 2^" + to_string(s) +
        //     " (" + to_string((int)pow(2, s)) + ") codewords: " + to_string(P_estimate));
        if (num_points == 0)
            num_points = count_points(Rorig, symbset, P, vec(k, fill::zeros), k, 0);
        log_msg("Number of codewords inside the hypersphere: " + to_string(num_points));
    }
    if (coset_encoding) {
        auto rates = code_rates(2*G, Ge);  /* Calculate code rates, scaling by two needed again because of q-PAM (Ge already scaled) */
        log_msg("Code overall rate: "      + to_string(get<0>(rates)/t) + " bpcu");
        log_msg("Code transmission rate: " + to_string(get<1>(rates)/t) + " bpcu");
        log_msg("Code confusion rate: "    + to_string(get<2>(rates)/t) + " bpcu");
    }
    log_msg("---------------");
    log_msg("", "Raw");

    /* Ask the user whether he/she actually wants to run the simulations after precalculations */
    string answer;
    cout << "Continue to simulation (y/n)? " << endl;
    getline(cin, answer);
    /* Continue only if user types 'y' or 'yes' */
    if (!(answer.compare("y") == 0 || answer.compare("yes") == 0)) {
        log_msg("Program exited successfully!");
        return 0;
    }

    log_msg("Starting simulations... (Press CTRL-C to abort)");

    unordered_map<int, int> required_errors;
    /* if error file is spesified, fill the required_errors map with it
       Otherwise use constant error requirement for each SNR simulation */
    if (filenames.count("error") == 0)
        for (int snr = min; snr <= max; snr += step)
            required_errors[snr] = params["required_errors"];
    else
        required_errors = read_error_requirements(filenames["error"]);

    /* If user has not spesified output filename, generate one automatically */
    if (filenames.count("output") == 0)
        create_output_filename(); /* see misc.cpp for details */

    /* thread safe output string vector, used for csv output */
    output.append("Simulated SNR,Real SNR,Avg Complexity,Max Complexity,Errors,Runs,BLER"); /* add label row */

    log_msg("---------------------------------------------------------------------------------------------------------");
    log_msg("SNR simulation | Real SNR     | Avg Complexity | Max Complexity | Errors     | Runs       | BLER ");
    log_msg("---------------------------------------------------------------------------------------------------------");

    auto start = chrono::high_resolution_clock::now();

    #pragma omp parallel  /* parallelize SNR simulations */
    {
        double Hvar = 1, Nvar = 1;

        /* initialize a bunch of complex matrices used in the simulation */
        cx_mat H(n, m), X(m, t), N(n, t);

        vector<int> x(k), orig(k); /* output and input vectors */

        ostringstream buffer; /* string stream buffer used for log_msg() */

        /* simulation variables */
        int runs = 0;
        int errors = 0;
        int visited_nodes = 0;
        int total_nodes = 0;
        int snr_to_errorbound = 0;

        double sigpow = 0;
        double bler = 0;
        double avg_complex = 0;
        int    max_complex = 0;
        double noisepow = 0;
        double sigsum = 0;
        double noisesum = 0;
        double SNRreal = 0;
        double C = 0.0; /* initial squared radius for the sphere decoder */

        /* pair containing the coefficients and matrix representation of a single codeword */
        pair<vector<int>,cx_mat> codeword;
        vector<pair<vector<int>,cx_mat>> codebook_instance = codebook;
        uniform_int_distribution<int> dist(0, codebook.size()-1);

        /* Simulations main loop (iteration shared among parallel threads)
           Simulations are indexed by their SNR value */
        #pragma omp for schedule(static,1)
        for (int snr = min; snr <= max; snr += step) {

            Hvar = pow(10.0, snr/10.0)*(t/e.first); /* calculate channel matrix variance from SNR */
            /* for the SISO channel model the variance has to be bigger since there are so many entries in H fixed to zero */
            if (siso_channel) Hvar *= 2*n; /* scale variance by factor 2n */
            /* (noise variance is constant one) */

            /* Single SNR simulation loop:
               run until both conditions are satisfied or the simulation is terminated by user */
            snr_to_errorbound = required_errors[snr];
            while (errors < snr_to_errorbound || runs < min_runs) {

                if (exit_flag) break; /* terminate simulations */

                /* Simulation starts by generating a random codeword that we want to send */
                if (samples <= 0) {
                    codeword = codebook_instance[dist(mersenne_twister)];
                } else {
                    if (P <= 0) {
                        codeword = create_random_codeword(bases, symbset);
                    }
                    else {
                        codeword = create_random_spherical_codeword(bases, Rorig, symbset, P);
                    }
                }

                orig = codeword.first;  /* coefficients from the signal set (i.e. data vector) */
                X = codeword.second;    /* Code block we want to send */

                /* Generate random channel matrix according to channel mode */
                if (!siso_channel)
                    H = create_random_matrix(n, m, 0, Hvar);
                else
                    H = create_random_diag_matrix(t, 0, Hvar);

            	N = create_random_matrix(n, t, 0, Nvar); /* Additive complex Gaussian white noise matrix */

                sigpow = frob_norm_squared(H*X);       /* Signal power */
                noisepow = frob_norm_squared(N);       /* Noise power */
                C = noisepow + 1e-3;                   /* initial radius for the sphere decoder (added small "epsilon" to avoid equality comparison) */
                sigsum += sigpow;                      /* sum of signal powers */
                noisesum += noisepow;                  /* sum of noise powers */

                /* wrapper function for the sphere decoder algorithm, see details in sphdec.cpp
                   x is the decoded output vector */
            	x = sphdec_wrapper(bases, Rorig, H, X, N, symbset, visited_nodes, C);

                if (x.empty())
                	log_msg("C = " + to_string(C), "Alert");

                /* Check if the decoded vector is the same as what we sent in the simulation.
                   If we're doing 'wiretap' simulation the checking is done in a different manner */
                if (coset_encoding) {
                    /* see details for coset_check in algorithms.cpp
                       the last argument is the difference vector between x and orig */
                    if (!coset_check(G, invGe, Col<int>(orig) - Col<int>(x))) {
                        errors++;
                    }
                } else {
                    /* simple equality check for normal simulation */
                    if (orig != x) {
                        errors++;
                    }
                }

                /***** the vectors can be printed out nicely like this for debugging reasons *****/
                // cout << endl << "orig = " << vec2str(orig, orig.size()) << endl;
                // cout << "x = "<< vec2str(x, x.size()) << endl << endl;

                /* increase counters */
                total_nodes += visited_nodes;
                if (visited_nodes > max_complex) max_complex = visited_nodes;
                runs++;

                /* print intermediate simulation stats every now and then (if enabled) */
                if (runs % stat_interval == 0 && stat_interval > 0){
                    SNRreal = 10 * log(sigsum / noisesum) / log(10.0);
                    bler = (double)errors/runs;
                    avg_complex = (double)total_nodes/runs;
                    /*log_msg("SNR-simulation " + to_string(snr) + \
                    "\tReal SNR: " + to_string(SNRreal) + \
                    ", BLER: " + to_string(errors) + "/" + to_string(runs) + " (" + to_string(bler) + ")" + \
                    ", Avg Complexity: " + to_string(avg_complex) + \
                    ", Max Complexity: " + to_string(max_complex));*/

                    buffer << std::right << std::setw(14) << snr << " | " << setw(12) << SNRreal << " | "
                           << std::setw(14) << avg_complex << " | " << std::setw(14) << max_complex << " | "
                           << std::setw(10) << errors << " | " << std::setw(10)  << runs << " | " << std::setw(13) << bler;

                    log_msg(buffer.str());
                    buffer.str("");
                }
            }

            /* SNR simulation finished, calculate results... */

            /* Sanity check for the realized SNR, should roughly equal to the configured SNR */
            SNRreal = 10 * log(sigsum / noisesum) / log(10.0);
            /* Block error rate */
            bler = (double)errors/runs;
            /* Average complexity (number of visited search tree nodes) of the sphere decoder */
            avg_complex = (double)total_nodes/runs;

            output.append(to_string(snr) + "," + float2str(SNRreal, 16) + "," + float2str(avg_complex, 16) + "," + to_string(max_complex)
                  + "," + to_string(errors) + "," + to_string(runs) + "," + float2str(bler, 16));

            /*log_msg("SNR-simulation " + to_string(snr) + \
                    "\t[Finished] Real SNR: " + to_string(SNRreal) + \
                    ", BLER: " + to_string(errors) + "/" + to_string(runs) + " (" + to_string(bler) + ")" + \
                    ", Avg Complexity: " + to_string(avg_complex) + \
                    ", Max Complexity: " + to_string(max_complex));*/

            buffer << green << "[Finished] " << std::right << std::setw(3) << snr << " | " << setw(12) << SNRreal << " | "
                   << std::setw(14) << avg_complex << " | " << std::setw(14) << max_complex << " | "
                   << std::setw(10) << errors << " | " << std::setw(10)  << runs << " | " << std::setw(13) << bler << def;

            log_msg(buffer.str());
            buffer.str("");

            /* reset counters after simulation round */
            runs = 0;
            errors = 0;
            sigsum = 0;
            noisesum = 0;
            total_nodes = 0;
        }

    }
    log_msg("---------------------------------------------------------------------------------------------------------");
    log_msg("SNR simulation | Real SNR     | Avg Complexity | Max Complexity | Errors     | Runs       | BLER ");
    log_msg("---------------------------------------------------------------------------------------------------------");
    auto end = chrono::high_resolution_clock::now();
    auto simulation_time = chrono::duration_cast<chrono::duration<double>>(end - start);
    log_msg("Simulations finished in " + to_string(simulation_time.count()) + " seconds.");

    /* output the simulation results in a csv file in /output/ folder */
    output_data(output);

    log_msg("Program exited successfully!");
    return 0;
}


// Authors: Frerk Saxen and Philipp Werner (Frerk.Saxen@ovgu.de, Philipp.Werner@ovgu.de)
// License: BSD 2-Clause "Simplified" License (see LICENSE file in root directory)

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/video/video.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>
#include <chrono>
#include <vector>
#include <string>
#include <algorithm>

//#include <FaceBase/FaceRegistrationTrained.hpp>
//#include <FaceBase/FaceLibDlib.hpp>

#include <dlib/opencv.h>
#include <dlib/gui_widgets.h>
#include <dlib/clustering.h>
#include <dlib/string.h>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>

#include "misc.hpp"

using namespace dlib;
using namespace std;

// ----------------- Face Recognition CNN Architecture --------------------------------------------
template <template <int, template<typename>class, int, typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N, BN, 1, tag1<SUBNET>>>;

template <template <int, template<typename>class, int, typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2, 2, 2, 2, skip1<tag2<block<N, BN, 2, tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block = BN<con<N, 3, 3, 1, 1, relu<BN<con<N, 3, 3, stride, stride, SUBNET>>>>>;

template <int N, typename SUBNET> using ares = relu<residual<block, N, affine, SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block, N, affine, SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256, SUBNET>;
template <typename SUBNET> using alevel1 = ares<256, ares<256, ares_down<256, SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128, ares<128, ares_down<128, SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64, ares<64, ares<64, ares_down<64, SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32, ares<32, ares<32, SUBNET>>>;

using face_rec_net_type = loss_metric<fc_no_bias<128, avg_pool_everything<
	alevel0<
	alevel1<
	alevel2<
	alevel3<
	alevel4<
	max_pool<3, 3, 2, 2, relu<affine<con<32, 7, 7, 2, 2,
	input_rgb_image_sized<150>
	>>>>>>>>>>>>;

void recognizeFaces(const std::string& exdata_dir, const std::string& train_or_val_or_test)
{
	std::string filename_list_filename = exdata_dir + train_or_val_or_test + "_filenames.txt";
	std::string filename_face_detection = exdata_dir + train_or_val_or_test + "_facedet.txt";
	std::string filename_face_recognition = exdata_dir + train_or_val_or_test + "_face_recognition.txt";

	std::string shape_predictor_file = exdata_dir + "spd+all=cascade30+oversampling70+trees1500.dat";
	std::string face_recognition_file = exdata_dir + "dlib_face_recognition_resnet_model_v1.dat";

	const long max_frames = 50;

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

	std::vector<std::string> filename_list;
	misc::read_filename_list(filename_list_filename, filename_list);

	std::vector<std::vector<dlib::rectangle>> face_dets_list;
	misc::read_face_detection(filename_face_detection, face_dets_list);

	DLIB_CASSERT(filename_list.size() == face_dets_list.size(), "List size mismatch: \n\t filename_list.size(): " << filename_list.size() << "\n\t face_dets_list.size(): " << face_dets_list.size() << std::endl);

	shape_predictor sp;
	deserialize(shape_predictor_file) >> sp;

	face_rec_net_type face_rec_net;
	deserialize(face_recognition_file) >> face_rec_net;

	long nrError = 0;

	matrix<dlib::rgb_pixel> img;
	cv::Mat cvImage;
	typedef matrix<float, 0, 1> sample_type;
	std::vector<std::vector<sample_type>> face_descriptors;
	std::vector<matrix<rgb_pixel>> first_frame;

	for(long vid_id = 0; vid_id < filename_list.size(); ++vid_id)
	{
	      const auto vid_filename = filename_list.at(vid_id);

	      std::chrono::steady_clock::time_point time_end = std::chrono::steady_clock::now();
	      int seconds_expired = std::chrono::duration_cast<std::chrono::seconds>(time_end - time_start).count();
	      int minutes_left = vid_id == 0 ? 0 : seconds_expired * (static_cast<double>(filename_list.size() - vid_id) / (double)vid_id / 60.0);
	      std::cout << rpad(cast_to_string(vid_id+1),3) << "/" << filename_list.size() << ". Approx. " << rpad(cast_to_string(minutes_left), 3) << " minutes left. " << "Processing video: " << vid_filename << std::endl;

	      cv::VideoCapture vid(vid_filename);
	      DLIB_CASSERT(vid.isOpened(), "Cannot open video filename : " << vid_filename);

	      const std::vector<dlib::rectangle>& face_dets = face_dets_list.at(vid_id);
	      std::vector<matrix<rgb_pixel>> faces;
	      matrix<rgb_pixel> face_chip;

	      long frame_no = 0;
	      while (vid.read(cvImage) && frame_no / 4.0 < max_frames)
	      {
		      // Take every 4th frame
		      if(frame_no % 4 != 0)
		      {
			      ++frame_no;
			      continue;
		      }

		      // Prepare for next frame
		      dlib::assign_image(img, dlib::cv_image<dlib::bgr_pixel>(cvImage));

		      const auto& face_det = face_dets.at(frame_no);

		      // Get landmarks and face chip
		      auto shape = sp(img, face_det);
		      auto face_details = get_face_chip_details(shape, 150, 0.25);
		      extract_image_chip(img, face_details, face_chip); //, 150, 0.25
 		      faces.push_back(move(face_chip));

		      ++frame_no;
	      }
	      first_frame.push_back(faces.at(0));

	      // Perform face recognition
	      face_descriptors.push_back(face_rec_net(faces));
	}

// 	dlib::deserialize(exdata_dir + "train_face_descriptors.dat") >> face_descriptors >> first_frame;

	typedef std::vector<sample_type> vec_sample_type;

	const long num_clusters = filename_list.size() / 12; // There are always 12 videos of the same person

	auto dist_function = [](const vec_sample_type& a, const vec_sample_type&b)
	{
		std::vector<double> dists;
		for (size_t i = 0; i < a.size(); ++i)
		    for (size_t j = 0; j < b.size(); ++j)
			dists.push_back(length(a[i]-b[j]));
		std::sort(dists.begin(),dists.end());
 		double median = dists.at(dists.size() / 2);
		return 1.0 / (median + 0.00001);
	};

 	std::vector<unsigned long> labels = spectral_cluster(dist_function, face_descriptors, num_clusters);

	// Now let's display the face clustering results on the screen.  It hopefully
	// correctly grouped all the faces.
	std::vector<image_window> win_clusters(num_clusters);
	for (size_t cluster_id = 0; cluster_id < num_clusters; ++cluster_id)
	{
	    std::vector<matrix<rgb_pixel>> temp;
	    for (size_t j = 0; j < labels.size(); ++j)
	    {
		if (cluster_id == labels[j])
		    temp.push_back(first_frame[j]);
	    }
	    win_clusters[cluster_id].set_title("face cluster " + cast_to_string(cluster_id));
	    win_clusters[cluster_id].set_image(tile_images(temp));
	}

	// Open dest filename
	std::ofstream idFile(filename_face_recognition);
	DLIB_CASSERT(idFile.is_open());
	DLIB_CASSERT(labels.size() == filename_list.size());
	for(long vid_id = 0; vid_id < filename_list.size(); ++vid_id)
	{
		  idFile << vid_id << "," << labels[vid_id] << "\n";
	}
	idFile.close();
//  	std::cin.get();

	return;
}


#ifndef PUBLIC_INTERFACE_EIGEN_HPP
#define PUBLIC_INTERFACE_EIGEN_HPP

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::ArrayXd;
using Eigen::ArrayXXd;

using Eigen::MatrixXf;
using Eigen::VectorXf;
using Eigen::ArrayXf;
using Eigen::ArrayXXf;

using Eigen::MatrixXi;
using Eigen::VectorXi;
using Eigen::ArrayXi;
using Eigen::ArrayXXi;

MatrixXd createEigenMat();
VectorXd createEigenVect();
ArrayXXd createEigenArray();
ArrayXd createEigenArrayVect();

MatrixXf createEigenMatf();
VectorXf createEigenVectf();
ArrayXXf createEigenArrayf();
ArrayXf createEigenArrayVectf();

MatrixXi createEigenMati();
VectorXi createEigenVecti();
ArrayXXi createEigenArrayi();
ArrayXi createEigenArrayVecti();

#endif


#include "rts_object_cost_grid_space.h"

#include <boost/graph/adjacency_list.hpp>
#include "cor_data_structure/sources/geometry/uniform_grid_tmpl_impl.h"
#include "cor_data_structure/sources/ai/stack_decoder_tmpl_impl.h"
#include "cor_system/sources/cor_time.h"

namespace cor
{
    namespace cocos2dx_converter
    {

        RtsObjectCostGridSpaceCell::RtsObjectCostGridSpaceCell()
        {
            parent = nullptr;
            wall = rfalse;
            min_cost = FloatMax;
        }

        struct RtsObjectCostGridSpaceItnl
        {
            typedef boost::adjacency_list<boost::vecS, boost::listS, boost::undirectedS,
                RtsObjectCostGridSpaceCellPtr, boost::property<boost::edge_weight_t, RFloat> > Graph;
            typedef std::set<RtsObjectSP> RtsObjectSet;
            typedef data_structure::UniformGridTmpl<RtsObjectCostGridSpaceCell, type::Vector2I> Cells;

            Graph g;
            std::map<RtsObjectCostGridSpaceCellPtr, RtsObjectCostGridSpaceItnl::Graph::vertex_descriptor> vd_map;
            RtsObjectSet objects;
            Cells cells;

            RInt32 wall_kind;
            type::Vector2F scale;
            type::Vector2F offset;

            Collision2dNodeSP collision;
            data_structure::CostGridSpaceSP cost_grid_space;
            RtsObjectGroupSP object_group;

            RtsObjectCostGridSpaceItnl()
            {
                wall_kind = -1;
                scale.set(1.0f, 1.0f);
                offset.set(0.0f, 0.0f);
            }
        };

        RtsObjectCostGridSpace::RtsObjectCostGridSpace() : itnl(new RtsObjectCostGridSpaceItnl())
        {

        }

        RtsObjectCostGridSpace::RtsObjectCostGridSpace(Collision2dNodeSP collision, data_structure::CostGridSpaceSP cost_grid_space, RtsObjectGroupSP object_group) : itnl(new RtsObjectCostGridSpaceItnl())
        {
            itnl->collision = collision;
            itnl->cost_grid_space = cost_grid_space;
            itnl->object_group = object_group;

            auto vsz = itnl->cost_grid_space->get_size();
            itnl->cells.resize(vsz);
            auto& cells = itnl->cells.ref_a();
            RSize ct = 0;
            for(auto& cell : cells)
            {
                cell.position.x = ct % vsz.x;
                cell.position.y = ct / vsz.x;
                cell.a.clear();
                cell.parent = nullptr;
                ct++;
            }
        }

        RtsObjectCostGridSpace::~RtsObjectCostGridSpace()
        {

        }

        void RtsObjectCostGridSpace::set_wall_kind(RInt32 wall_kind)
        {
            itnl->wall_kind = wall_kind;
        }

        void RtsObjectCostGridSpace::set_index_convert(type::Vector2F scale, type::Vector2F offset)
        {
            itnl->scale = scale;
            itnl->offset = offset;
        }

        void RtsObjectCostGridSpace::add(RtsObjectSP o)
        {
            itnl->objects.insert(o);
        }

        void RtsObjectCostGridSpace::remove(RtsObjectSP o)
        {
            itnl->objects.erase(o);
        }

        void RtsObjectCostGridSpace::each_vertices(std::function<void(RtsObjectCostGridSpaceCellPtr)> callback)
        {
            auto va = boost::vertices(itnl->g);
            for(auto v = va.first; v != va.second; v++)
            {
                callback(itnl->g[*v]);
            }
        }

        void RtsObjectCostGridSpace::each_edges(std::function<void(RtsObjectCostGridSpaceCellPtr, RtsObjectCostGridSpaceCellPtr, RFloat)> callback)
        {
            auto ea = boost::edges(itnl->g);
            const auto w = boost::get(boost::edge_weight, itnl->g);
            for(auto e = ea.first; e != ea.second; e++)
            {
                callback(itnl->g[boost::source(*e, itnl->g)], itnl->g[boost::target(*e, itnl->g)], w[*e]);
            }
        }

        void RtsObjectCostGridSpace::each_cell(std::function<void(RtsObjectCostGridSpaceCellPtr)> callback)
        {
            for(auto& cell : itnl->cells.ref_a())
            {
                callback(&cell);
            }
        }

        void RtsObjectCostGridSpace::make_graph()
        {
            auto vsz = itnl->cost_grid_space->get_size();

            auto& g = itnl->g;

            g.clear();

            auto& cells = itnl->cells.ref_a();
            for(auto& cell : cells)
            {
                cell.wall = rfalse;
                cell.min_cost = FloatMax;
                cell.a.clear();
                cell.parent = nullptr;
                cell.root = &cell;
            }

            auto vec_f_to_i = [=](type::Vector2F p){
                return type::Vector2I(RInt32(p.x / itnl->scale.x - itnl->offset.x),
                    RInt32(p.y / itnl->scale.y - itnl->offset.y));
            };

            std::set<RtsObjectCostGridSpaceCellPtr> object_cells;

            for(auto o : itnl->objects)
            {
                auto ip = vec_f_to_i(o->get_position());
                auto& cell = itnl->cells.ref(ip);

                if(o->get_kind() == itnl->wall_kind)
                {
                    cell.wall = rtrue;
                }

                object_cells.insert(&cell);

                cell.a.push_back(o);
            }

            itnl->vd_map.clear();

            for(auto& cell : object_cells)
            {
                auto vd = boost::add_vertex(g);
                itnl->g[vd] = &*cell;
                itnl->vd_map[&*cell] = vd;
            }

            struct State
            {
                RFloat cost;
                RtsObjectCostGridSpaceCellPtr cell;
            };

            typedef data_structure::InstantStackDecoderTmpl<RFloat, State> StackDecoder;

            StackDecoder sd;
            const auto w = boost::get(boost::edge_weight, g);

            auto next_state = [&](RInt32 x, RInt32 y, const State& prev_state, std::function<void(const State&)> f){
                type::Vector2I vi = prev_state.cell->position;
                vi.x += x;
                vi.y += y;

                if(vi.x < 0) return;
                if(vi.y < 0) return;
                if(vi.x >= vsz.x) return;
                if(vi.y >= vsz.y) return;
                State next_state;
                next_state.cost = prev_state.cost + sqrtf(RFloat(x * x + y * y));
                next_state.cell = &itnl->cells.ref(vi);
                if(next_state.cost < next_state.cell->min_cost)
                {
                    next_state.cell->min_cost = next_state.cost;
                    next_state.cell->parent = prev_state.cell;
                    next_state.cell->root = prev_state.cell->root;
                    f(next_state);
                }
                else
                {
                    if(prev_state.cell->root && next_state.cell->root && (
                        prev_state.cell->root != next_state.cell->root
                        ))
                    {

                        auto vd0 = itnl->vd_map[prev_state.cell->root];
                        auto vd1 = itnl->vd_map[next_state.cell->root];
                        auto e = boost::edge(vd0, vd1, g);
                        auto cost = prev_state.cell->min_cost + next_state.cell->min_cost;
                        if(!e.second)
                        {
                            auto ed = boost::add_edge(vd0, vd1,
                                cost, g);
                        }
                        else
                        {
                            if(w[e.first] > cost)
                            {
                                w[e.first] = cost;
                            }
                        }
                    }

                }
            };

            sd.set_func([&](const State& state){
                return state.cost;
            }, [&](const State& state, std::function<void(const State&)> f){
                next_state(-1, 0, state, f);
                next_state(1, 0, state, f);
                next_state(0, -1, state, f);
                next_state(0, 1, state, f);

            }, [&](const State& state){
                return rfalse;
            });

            for(auto& cell : object_cells)
            {
                if(!cell->wall)
                {
                    State state;
                    state.cell = &*cell;
                    state.cost = 0.0f;
                    sd.push(state);
                }

                cell->min_cost = 0.0f;
            }

            sd.search([&](const State& state){
                return rfalse;
            });

        }

    }
}


// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

// std
#include <iostream>
#include "kindyn/robot.hpp"
#include <std_msgs/Float32.h>
#include <controller_manager_msgs/LoadController.h>
#include <controller_manager_msgs/SwitchController.h>
#include <tf/transform_broadcaster.h>

using namespace Eigen;
using namespace std;

#define NUMBER_OF_MOTORS 8
#define SPINDLERADIUS 0.00575
#define msjMeterPerEncoderTick(encoderTicks) (((encoderTicks)/(4096.0)*(2.0*M_PI*SPINDLERADIUS)))
#define msjEncoderTicksPerMeter(meter) ((meter)*(4096.0)/(2.0*M_PI*SPINDLERADIUS))

using namespace std;

class MsjPlatform: public cardsflow::kindyn::Robot{
public:
    /**
     * Constructor
     * @param urdf path to urdf
     * @param cardsflow_xml path to cardsflow xml
     */
    MsjPlatform(string urdf, string cardsflow_xml){
        if (!ros::isInitialized()) {
            int argc = 0;
            char **argv = NULL;
            ros::init(argc, argv, "msj_platform");
        }
        nh = ros::NodeHandlePtr(new ros::NodeHandle);
        motor_command = nh->advertise<roboy_middleware_msgs::MotorCommand>("/roboy/middleware/MotorCommand",1);
        sphere_axis0 = nh->advertise<std_msgs::Float32>("/sphere_axis0/sphere_axis0/target", 1);
        sphere_axis1 = nh->advertise<std_msgs::Float32>("/sphere_axis1/sphere_axis1/target", 1);
        sphere_axis2 = nh->advertise<std_msgs::Float32>("/sphere_axis2/sphere_axis2/target", 1);
        // first we retrieve the active joint names from the parameter server
        vector<string> joint_names;
        nh->getParam("joint_names", joint_names);
        // then we initialize the robot with the cardsflow xml and the active joints
        init(urdf,cardsflow_xml,joint_names);
        // if we do not get the robot state externally, we use the forwardKinematics function to integrate the robot state
        nh->getParam("external_robot_state", external_robot_state);
        update();
        for(int i=0;i<NUMBER_OF_MOTORS;i++)
            l_offset[i] = l[i];

        string path = ros::package::getPath("robots");
        path+="/msj_platform/joint_limits.txt";
        FILE*       file = fopen(path.c_str(),"r");
        if (NULL != file) {
            fscanf(file, "%*[^\n]\n", NULL);
            float qx,qy;
            int i =0;
            while(fscanf(file,"%f %f\n",&qx,&qy)==2){
                limits[0].push_back(qx);
                limits[1].push_back(qy);
                cout << qx << "\t" << qy << endl;
                i++;
            }
            printf("read %d joint limit values\n", i);
        }else{
            cout << "could not open " << path << endl;
        }

        pose_thread.reset(new std::thread(&MsjPlatform::randomPose, this));
        pose_thread->detach();
    };

    /**
     * Updates the robot model and if we do not use gazebo for simulation, we integrate using the forwardKinematics function
     * with a small step length
     */
    void read(){
        update();
        if(!external_robot_state)
            forwardKinematics(0.000005);
    };

    /**
     * Sends motor commands to the real robot
     */
    void write(){
        roboy_middleware_msgs::MotorCommand msg;
        msg.id = 5;
        stringstream str;
        for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
            msg.motors.push_back(i);
            double l_change = l[i]-l_offset[i];
            msg.set_points.push_back(-msjEncoderTicksPerMeter(l_change)); //
            str << l_change << "\t";
        }
        ROS_INFO_STREAM_THROTTLE(1,str.str());
        motor_command.publish(msg);
    };

    int pnpoly(vector<double> limits_x, vector<double> limits_y, double testx, double testy){
        int i, j, c = 0;
        for (i = 0, j = limits_x.size()-1; i < limits_x.size(); j = i++) {
            if ( ((limits_y[i]>testy) != (limits_y[j]>testy)) &&
                 (testx < (limits_x[j]-limits_x[i]) * (testy-limits_y[i]) / (limits_y[j]-limits_y[i]) + limits_x[i]) )
                c = !c;
        }
        return c;
    }

    void randomPose(){
        ros::Time start_time = ros::Time::now();
        double min[3] = {0,0,-1}, max[3] = {0,0,1};
        for(int i=0;i<limits[0].size();i++){
            if(limits[0][i]<min[0])
                min[0] = limits[0][i];
            if(limits[1][i]<min[1])
                min[1] = limits[1][i];
            if(limits[0][i]>max[0])
                max[0] = limits[0][i];
            if(limits[1][i]>max[1])
                max[1] = limits[1][i];
        }

        int sample = 0;
        while(ros::ok()){
            double q0 = rand()/(double)RAND_MAX*(max[0]-min[0])+min[0];
            double q1 = rand()/(double)RAND_MAX*(max[1]-min[1])+min[1];
            double q2 = rand()/(double)RAND_MAX*(max[2]-min[2])+min[2];
            if(pnpoly(limits[0],limits[1],q0,q1)){
                ROS_INFO("new pose");
                std_msgs::Float32 msg;
                msg.data = q0;
                sphere_axis0.publish(msg);
                msg.data = q1;
                sphere_axis1.publish(msg);
                msg.data = q2;
                sphere_axis2.publish(msg);
                ros::Time t0 = ros::Time::now();
                while((ros::Time::now()-t0).toSec()<3){
                    update();
                    forwardKinematics(0.0000001);
                }
                sample++;
            }
            ROS_INFO_STREAM(sample);
        }
    }

    bool external_robot_state; /// indicates if we get the robot state externally
    ros::NodeHandlePtr nh; /// ROS nodehandle
    ros::Publisher motor_command, sphere_axis0, sphere_axis1, sphere_axis2; /// motor command publisher
    double l_offset[NUMBER_OF_MOTORS];
    vector<double> limits[3];
    boost::shared_ptr<std::thread> pose_thread;
};

// Generic functor for Eigen Levenberg-Marquardt minimizer
template<typename _Scalar, int NX = Dynamic, int NY = Dynamic>
struct Functor {
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
    };
    typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

    const int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}

    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; }

    int values() const { return m_values; }
};

struct RobotConfigurationEstimator : Functor<double> {

    RobotConfigurationEstimator(int number_of_samples, int number_of_cables, int number_of_links):
            Functor<double>(3*number_of_cables, 3*number_of_cables), number_of_samples(number_of_samples), number_of_cables(number_of_cables), number_of_links(number_of_links) {
    };

    /**
     * This is the function that is called in each iteration
     * @param x the pose vector (3 rotational 3 translational parameters)
     * @param fvec the error function (the difference between the sensor positions)
     * @return
     */
    int operator()(const VectorXd &x, VectorXd &fvec) const{
        MatrixXd V;
        V.resize(number_of_cables, 6 * number_of_links);
        V.setZero();
        fvec.setZero();
        V.setZero(number_of_cables, 6 * number_of_links);
        for (int cable = 0; cable < number_of_cables; cable++) {
            // V term associated with segment translation
            Vector3d segmentVector;
            Vector3d global_estimate(x[cable*3+0],x[cable*3+1],x[cable*3+2]);
            segmentVector =  robot->segments[cable][0].second->global_coordinates - global_estimate;
            segmentVector.normalize();

            // Total V term in translations
            Vector3d V_ijk_T = robot->world_to_link_transform.back().block(0, 0, 3, 3) * segmentVector;

            Vector3d V_itk_T = robot->segments[cable][0].second->local_coordinates.cross(V_ijk_T);

            V.block(cable, 6 * 6, 1, 3) = V.block(cable, 6 * 6, 1, 3) + V_ijk_T.transpose();
            V.block(cable, 6 * 6 + 3, 1, 3) =
                    V.block(cable, 6 * 6 + 3, 1, 3) + V_itk_T.transpose();
        }
        MatrixXd L = V * robot->W;
//        VectorXd ld_error = L*robot->qd-robot->Ld; // TODO adapt to multiple endeffectors
//        for(int j=0;j<number_of_cables;j++) {
//            fvec[j*3 ] += abs(ld_error[j]);
//            fvec[j*3 + 1] += abs(ld_error[j]);
//            fvec[j*3 + 2] += abs(ld_error[j]);
//        }

        static int counter = 0;
        if((counter++%100)==0){
            ROS_INFO_STREAM_THROTTLE(5,fvec.norm() << endl << x.transpose());
            for(int cable=0;cable<number_of_cables;cable++) {
                char str[100];
                sprintf(str, "cable_estimate_%d", cable);
                geometry_msgs::Pose msg;
                msg.position.x = x[cable*3];
                msg.position.y = x[cable*3+1];
                msg.position.z = x[cable*3+2];
                robot->publishTransform("world",str,msg);
            }
        }
    };

    static cardsflow::kindyn::Robot *robot;

    int number_of_samples, number_of_cables, number_of_links;
};

cardsflow::kindyn::Robot *RobotConfigurationEstimator::robot;

/**
 * controller manager update thread. Here you can define how fast your controllers should run
 * @param cm pointer to the controller manager
 */
void update(controller_manager::ControllerManager *cm) {
    ros::Time prev_time = ros::Time::now();
    ros::Rate rate(100); // changing this value affects the control speed of your running controllers
    while (ros::ok()) {
        const ros::Time time = ros::Time::now();
        const ros::Duration period = time - prev_time;
        cm->update(time, period);
        prev_time = time;
        rate.sleep();
    }
}

int main(){
    if (!ros::isInitialized()) {
        int argc = 0;
        char **argv = NULL;
        ros::init(argc, argv, "cardsflow_example_robot");
    }
    ros::NodeHandle nh;
    string urdf, cardsflow_xml;
    if(nh.hasParam("urdf_file_path") && nh.hasParam("cardsflow_xml")) {
        nh.getParam("urdf_file_path", urdf);
        nh.getParam("cardsflow_xml", cardsflow_xml);
    }else {
        ROS_FATAL("USAGE: rosrun kindyn test_robot path_to_urdf path_to_viapoints_xml");
        return 1;
    }
    ROS_INFO("\nurdf file path: %s\ncardsflow_xml %s", urdf.c_str(), cardsflow_xml.c_str());

    ros::AsyncSpinner spinner(0);
    spinner.start();

    MsjPlatform robot(urdf, cardsflow_xml);

    controller_manager::ControllerManager cm(&robot);

    // we need an additional update thread, otherwise the controllers won't switch
    thread update_thread(update, &cm);
    update_thread.detach();

    ROS_INFO("STARTING ROBOT MAIN LOOP...");

    ros::ServiceClient load_controller = nh.serviceClient<controller_manager_msgs::LoadController>("/controller_manager/load_controller");
    ros::ServiceClient switch_controller = nh.serviceClient<controller_manager_msgs::SwitchController>("/controller_manager/switch_controller");
    controller_manager_msgs::LoadController msg;
    msg.request.name = "sphere_axis0";
    load_controller.call(msg);
    msg.request.name = "sphere_axis1";
    load_controller.call(msg);
    msg.request.name = "sphere_axis2";
    load_controller.call(msg);

    controller_manager_msgs::SwitchController msg2;
    msg2.request.start_controllers.push_back("sphere_axis0");
    msg2.request.start_controllers.push_back("sphere_axis1");
    msg2.request.start_controllers.push_back("sphere_axis2");
    msg2.request.strictness = msg2.request.BEST_EFFORT;
    switch_controller.call(msg2);

    RobotConfigurationEstimator estimator(100,8,7);
    estimator.robot = &robot;

    ros::Time t0 = ros::Time::now();
    while((ros::Time::now()-t0).toSec()<3){
        robot.update();
        robot.forwardKinematics(0.000001);
    }

    NumericalDiff<RobotConfigurationEstimator> *numDiff1;
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<RobotConfigurationEstimator>, double> *lm;
    numDiff1 = new NumericalDiff<RobotConfigurationEstimator>(estimator);
    lm = new LevenbergMarquardt<NumericalDiff<RobotConfigurationEstimator>, double>(*numDiff1);
    lm->parameters.maxfev = 50000;
    lm->parameters.xtol = 1e-10;
    VectorXd x(24);
    x.setZero();
    for(int cable=0;cable<8;cable++){
        x[cable*3] = 0.001*rand()/(double)RAND_MAX;
        x[cable*3+1] = 0.001*rand()/(double)RAND_MAX;
        x[cable*3+2] = 0;
    }
    ROS_INFO_STREAM(x.transpose());
//    x = 0.001*x;
    int ret = lm->minimize(x);
    double error = lm->fnorm;
    robot.number_of_markers_to_publish_at_once = 1;
    for(int cable=0;cable<8;cable++){
        geometry_msgs::Pose msg;
        msg.position.x = x[cable*3];
        msg.position.y = x[cable*3+1];
        msg.position.z = x[cable*3+2];
        msg.orientation.w = 1;
        char str[100];
        sprintf(str,"cable_estimate_%d", cable);
        robot.publishTransform("world",str,msg);
        ros::spinOnce();
    }

    ROS_INFO_STREAM(ret << endl << error << endl << x.transpose());

    update_thread.join();
    return 0;
}


#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <chrono>

#include <unistd.h>
#include <sys/wait.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "../../../core/image.h"
#include "../../../core/helper.h"
#include "../../../core/math_helper.h"
#include "../../../stereo_odometry/base/visual_odometry_base.h"
//#include "../../../stereo_odometry/base/visual_odometry_ransac.h"
#include "../../../tracker/stereo/experiment_factory.h"
#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/refiner/feature_refiner_base.cc"
//#include "../../../tracker/refiner/feature_refiner_klt.h"
#include "../../../tracker/detector/feature_detector_base.h"
#include "../../../tracker/stereo/tracker_helper.h"
#include "../../../optimization/bundle_adjustment/bundle_adjuster_base.h"
//#include "../../../optimization/bundle_adjustment/bundle_adjuster_multiframe.h"
#include "../../math_helper.h"
#include "../../../tracker/base/eval_helper.h"
#include "../../../core/format_helper.h"
#include "../../helper_libviso.h"
//#include "../../cv_plotter.h"

using namespace core;

void run_visual_odometry(const std::string& source_folder,
                         const std::string& imagelistfn,
                         const std::string& experiment_config,
                         const std::string& cparams_file,
                         const std::string& gt_filepath,
                         const std::string& deformation_field_path)
{
  std::deque<cv::Mat> extr_params; // pose mat
  std::deque<cv::Mat> Rt_params;

  double cam_params[5];
  FormatHelper::readCameraParams(cparams_file, cam_params);

  std::vector<std::string> imagelist;
  bool ok = FormatHelper::readStringList(imagelistfn, imagelist);
  if (!ok || imagelist.empty())
    throw "can not open " + imagelistfn + " or the string list is empty\n";

  int start_frame = 0 * 2;
  //int start_frame = 300 * 2;
  //int start_frame = 920 * 2;
  // 01
  //int start_frame = 431 * 2;
  //int start_frame = 482 * 2;
  int end_frame = imagelist.size();
  //int nframes = (imagelist.size() - start_frame) / 2;

  std::cout << source_folder + imagelist[start_frame] << "\n";
  cv::Mat img_left_prev, img_right_prev;
  cv::Mat img_left = cv::imread(source_folder + imagelist[start_frame], CV_LOAD_IMAGE_GRAYSCALE);
  cv::Mat img_right = cv::imread(source_folder + imagelist[start_frame+1], CV_LOAD_IMAGE_GRAYSCALE);
  if (img_left.empty()) {
    std::cout << "Error: no images!\n";
    throw 1;
  }
  int img_rows = img_left.rows;
  int img_cols = img_left.cols;
  bool use_ba; // use bundle adjustment
  std::string output_folder;
  track::FeatureDetectorBase* detector = nullptr;
  track::TrackerBase* mono_tracker = nullptr;
  track::StereoTrackerBase* stereo_tracker = nullptr;
  visodom::VisualOdometryBase* viso = nullptr;
  optim::BundleAdjusterBase* bundle_adjuster = nullptr;
  track::ExperimentFactory::create_experiment(experiment_config, deformation_field_path, cam_params,
                                              img_rows, img_cols, output_folder, &detector, &mono_tracker,
                                              &stereo_tracker, &viso, use_ba, &bundle_adjuster);

  if (use_ba)
    bundle_adjuster->set_camera_params(cam_params);

  //libviso::Matrix pose = libviso::Matrix::eye(4);
  cv::Mat pose = cv::Mat::eye(4, 4, CV_64F);
  //Matrix point_rt = Matrix::eye(4);
  //cv::Mat pose_mat = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat Rt_inv;
  cv::Mat mat_I = cv::Mat::eye(4, 4, CV_64F);
  mat_I.copyTo(Rt_inv);
  cv::Vec<double,7> trans_vec;
  extr_params.push_back(mat_I.clone());
  cv::Mat Rt(4, 4, CV_64F);
  cv::Mat Rt_prev(4, 4, CV_64F);
  cv::Mat Rt_gt(4, 4, CV_64F);
  cv::Mat Rt_gt_prev = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat Rt_gt_curr = cv::Mat::eye(4, 4, CV_64F);
  core::MathHelper::invTrans(Rt_inv, Rt);
  Rt_params.push_back(Rt.clone());

  cv::Mat prev_location_viso = cv::Mat::zeros(4, 1, CV_64F);

  // init the tracker
  stereo_tracker->init(img_left, img_right);

  // matrix transforms points in current camera coordinate system to world coord system
  std::vector<cv::Mat> libviso_Rt_all;
  cv::Mat pose_libviso = cv::Mat::eye(4, 4, CV_64F);
  libviso_Rt_all.push_back(pose_libviso.clone());

  std::string track_name;
  bool have_gt = false;
  std::ifstream gt_file;
  if (!gt_filepath.empty()) {
    have_gt = true;
    gt_file.open(gt_filepath);
  }
  // no need - Tsukuba baseline is in cm
  bool using_kitti = false;
  if (experiment_config.find("tsukuba") != std::string::npos) {
    printf("---------- Using Tsukuba dataset! ------------\n");
    track_name = "00";
  }
  else if (experiment_config.find("bb2") != std::string::npos) {
    printf("---------- Using Bumblebee dataset! ------------\n");
    track_name = "bb";
  }
  // else it is KITTI
  else {
    printf("---------- Using KITTI dataset! ------------\n");
    track_name = imagelistfn.substr(imagelistfn.size()-10,2);
    using_kitti = true;
  }

  std::vector<cv::Mat> gt_motion;
  if (have_gt) {
    // read all GT data
    // skip first (identety) matrix
    FormatHelper::ReadNextRtMatrix(gt_file, Rt_gt_prev);
    core::MathHelper::invTrans(Rt_gt_prev, Rt_gt);
    gt_motion.push_back(Rt_gt.clone());
    int num_motions = ((imagelist.size()/2) - 1);
    for (int i = 0; i < num_motions; i++) {
      FormatHelper::ReadNextRtMatrix(gt_file, Rt_gt_curr);
      cv::Mat Rt_gt_inv = Rt_gt_prev * Rt_gt_curr;
      core::MathHelper::invTrans(Rt_gt_inv, Rt_gt);
      core::MathHelper::invTrans(Rt_gt_curr, Rt_gt_prev);
      gt_motion.push_back(Rt_gt.clone());
    }
    Rt_gt.release();
  }
  FormatHelper::ReadMotionFromFile(gt_file, Rt_gt_prev);

  //std::string bad_folder = "/home/kivan/Projects/datasets/KITTI/tracks/bad_patches/";
  //std::string good_folder = "/home/kivan/Projects/datasets/KITTI/tracks/good_patches/";
  //core::Helper::CleanFolder(bad_folder);
  //core::Helper::CleanFolder(good_folder);

  std::vector<size_t> track_index, track_cnt;
  track_index.assign(stereo_tracker->countFeatures(), 0);
  track_cnt.assign(stereo_tracker->countFeatures(), 0);
  size_t all_tracks_cnt = 0;

  //int h_bins = 15;
  //int v_bins = 5;
  //std::vector<std::vector<double>> left_reproj_errors, right_reproj_errors;
  //left_reproj_errors.resize(h_bins * v_bins);
  //right_reproj_errors.resize(h_bins * v_bins);
  //double scale_factor = 1.2;
  //cv::Mat error_img = cv::Mat::zeros(scale_factor*img_left.rows, scale_factor*img_left.cols, CV_8UC3);

  //int gt_better_cnt = 0;
  int image_num;
  int frame_num = 0;
  auto start = std::chrono::system_clock::now();
  //for (unsigned i = start_frame + 2; i < start_frame+60; i+=2) {
  double max_trans_error = 0.0;
  for (unsigned i = start_frame + 2; i < end_frame; i+=2) {
    stereo_tracker->track(img_left, img_right);

    // draw tracks before ransac
    if (debug)
      track::EvalHelper::DrawStereoTracks(*stereo_tracker, img_left_prev, img_right_prev,
                                          "all_left", "all_right");

    // run odometry optimization
    // Rt is the motion of world points with respect to the camera
    Rt.copyTo(Rt_prev);
    Rt = viso->getMotion(*stereo_tracker);

    // diff check to test for estimation chrashes on KITTI test
    //double trans_diff;
    //core::MathHelper::GetMotionError(Rt_prev, Rt, trans_diff);
    //  std::cout << "Trans diff = " << trans_diff << "\n";
    //if (frame_num > 1 && trans_diff > 0.1) {
    //  std::cout << "[Error]: Big trans diff = " << trans_diff << "\n";
    //  throw 1;
    //}

    // CheckMotionDiff(Rt, Rt_prev);
    //if (viso->process(libviso_tracks)) {
    if (!Rt.empty()) {
      // is it better to kill or not to kill outliers in tracker?
      std::vector<int> inliers = viso->getTrackerInliers();
      std::vector<int> outliers = viso->getTrackerOutliers();
      std::cout << "Tracks before RANSAC: " << stereo_tracker->countActiveTracks() << "\n";
      //FeatureHelper::FilterOutlierTracks(*stereo_tracker, active_tracks, inliers, 30.0, 0.1);
      //FeatureHelper::FilterRansacOutliers(*stereo_tracker, active_tracks, inliers);
      for (size_t j = 0; j < outliers.size(); j++)
        stereo_tracker->removeTrack(outliers[j]);
      std::cout << "Tracks after RANSAC: " << stereo_tracker->countActiveTracks() << "\n";

      //// draw tracks
      if (debug)
        track::EvalHelper::DrawStereoTracks(*stereo_tracker, img_left_prev, img_right_prev,
                                            "ransac_left", "ransac_right");

      //libviso::Matrix Rt_libviso = viso->getMotion();
      //MathHelper::matrixToMat(Rt_libviso, Rt);
      std::cout << "two-frame odometry:\n" << Rt << std::endl;
      if (have_gt) {
        std::cout << "groundtruth:\n" << gt_motion[i/2] << std::endl;
        double trans_error;
        core::MathHelper::GetMotionError(gt_motion[i/2], Rt, trans_error);
        std::cout << "Translation error = " << trans_error << " m\n";
        //if (trans_error > 0.8) throw 1;
        if (trans_error > max_trans_error)
          max_trans_error = trans_error;
      }
      if (debug)
        while(cv::waitKey(0) != 27);

      //track::EvalHelper::DrawTracksAndErrors(*stereo_tracker, cam_params, Rt, img_left_prev, img_right_prev);
      // TODO:
      //Eigen::Vector3d t, r;
      //core::MathHelper::GetEulerAngles(Rt, r);
      //core::MathHelper::GetTranslation(Rt, t);

      // if the motion is to small skip it
      // tracker.undo_state();
      MathHelper::invTrans(Rt, Rt_inv);    // better
      extr_params.push_back(Rt_inv.clone());
      // on success, update current pose
      pose = pose * Rt_inv;
      libviso_Rt_all.push_back(pose.clone());

      if (use_ba) {
        // ----- SBA -----
        bundle_adjuster->update_tracks(*stereo_tracker, Rt);
        //bundle_adjuster_multiframe->update_tracks(*stereo_tracker, Rt);
        // for old BA version
        //bundle_adjuster_multiframe->update_tracks(*stereo_tracker, Rt_inv);
        //sba->updateTracks(img_left, img_right, *stereo_tracker, mat_I, cam_params);
        //if (frame_num >= 2) {
        if (frame_num >= (bundle_adjuster->num_frames() - 1)) {
          double libviso_error = track::EvalHelper::GetStereoReprojError(*stereo_tracker, cam_params, Rt);
          double gt_error;
          if (have_gt)
            gt_error = track::EvalHelper::GetStereoReprojError(*stereo_tracker, cam_params, gt_motion[i/2]);
          bundle_adjuster->optimize();
          cv::Mat Rt_sba = bundle_adjuster->camera_motion(bundle_adjuster->camera_motion_num() - 1);
          cv::Mat Rt_sba_inv;
          core::MathHelper::invTrans(Rt_sba, Rt_sba_inv);
          if (use_ba)
            std::cout << "BA odometry:\n" << Rt_sba_inv << "\n";

          double ba_error = track::EvalHelper::GetStereoReprojError(*stereo_tracker, cam_params, Rt_sba_inv);
          if (have_gt) printf("GT reproj error = %e\n", gt_error);
          printf("Libviso reproj error = %e\n", libviso_error);
          printf("BA reproj error = %e\n", ba_error);
        }
      }

      std::cout << "-----------------------------------------------------------------------\n\n";
    } else {
      std::cout << "Libviso2 failed!\n";
      return;
    }
  }
  auto end = std::chrono::system_clock::now();
  //auto elapsed = end - start;
  std::chrono::duration<double> elapsed = end - start;
  std::cout << "Elapsed time = " << elapsed.count() << " secs\n";
  std::ofstream outfile("times.txt", std::ios_base::app);
  outfile << elapsed.count() << " sec\n";

  if (have_gt)
    std::cout << "Max trans error = " << max_trans_error << " m\n";
  std::cout << "Writing track " << track_name << " to files\n";
  std::ofstream libviso_ofile(output_folder + '/' + track_name + ".txt");

  std::ofstream ba_ofile;
  std::ofstream ba_ofile2;
  //cv::Mat sba_pose;
  if (use_ba) {
    std::string ba_folder = output_folder.substr(0, output_folder.size()-1) + "_ba/";
    //std::string ba_folder2 = output_folder.substr(0, output_folder.size()-1) + "_ba_multiframe/";
    ba_ofile.open(ba_folder + track_name + ".txt");
    //ba_ofile2.open(ba_folder2 + track_name + ".txt");
    //sba_pose = cv::Mat::eye(4, 4, CV_64F);
  }
  for (size_t i = 0; i < libviso_Rt_all.size(); i++) {
    FormatHelper::WriteMatRt(libviso_Rt_all[i], libviso_ofile);
    if (use_ba) {
      FormatHelper::WriteMatRt(bundle_adjuster->camera_motion_acc(i), ba_ofile);
      //FormatHelper::WriteMatRt(bundle_adjuster_multiframe->camera_motion_acc(i), ba_ofile2);
    }

  }

  if (use_ba)
    delete bundle_adjuster;
  delete detector;
  delete mono_tracker;
  delete stereo_tracker;
  delete viso;

  //// run evaluation
  //std::string cmd = "/home/kivan/Projects/cv-stereo/stereo_odometry/evaluation/evaluate_odometry_dense 7 7 " +
  //    std::to_string(start_frame/2) + " " + std::to_string(nframes) + " ./results/ eval_stats";
  //std::cout << cmd << "\n";
  //system(cmd.c_str());
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string experiment_config;
  std::string imagelistfn;
  std::string cam_params_file;
  std::string source_folder;
  std::string gt_filepath;
  std::string deformation_field_path;

  if (argc != 5){
    std::cout << "Usage:\n" << argv[0] << " -c dataset_config -e experiment_config\n";
    return 0;
  }

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help, h", "produce help message")
      ("config,c", po::value<std::string>(&config_file), "config filename")
      ("experiment_config,e", po::value<std::string>(&experiment_config), "experiment config filename");
    po::options_description cmdline_options;
    cmdline_options.add(generic);

    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if (vm.count("help")) {
      std::cout << generic;
      return 0;
    }

    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<std::string>(&cam_params_file)->default_value("camera_params.txt"), "camera params file")
      ("source_folder,s", po::value<std::string>(&source_folder), "folder with source")
      ("imglist,l", po::value<std::string>(&imagelistfn), "file with image list")
      ("groundtruth,g", po::value<std::string>(&gt_filepath), "file with motion GT")
      ("deformation_field,d", po::value<std::string>(&deformation_field_path), "file with deformation field");

    std::ifstream ifs(config_file);
    if (!ifs) {
      throw "can not open config file: " + config_file + "\n";
    }
    else {
      po::store(parse_config_file(ifs, config, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    std::cout << e.what() << "\n";
    return -1;
  }

  std::cout << "Using track config = " << config_file << '\n';
  run_visual_odometry(source_folder, imagelistfn, experiment_config, cam_params_file, gt_filepath, deformation_field_path);

  return 0;
}


/*
 * split_denoise.cpp

 *
 *  Created on: Dec 10, 2015
 *      Author: dch
 */

#include "hoNDArray_fileio.h"
#include "hoCuNDArray_math.h"
#include "vector_td_utilities.h"
#include "hoNDArray_utils.h"
#include "GPUTimer.h"
#include "cuATrousOperator.h"
#include "cuEdgeATrousOperator.h"
#include "cuSbCgSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <boost/program_options.hpp>
#include "subsetAccumulateOperator.h"
#include "cuDCTOperator.h"
#include "osSPSSolver.h"
#include "osMOMSolver.h"
#include "osMOMSolverD.h"
#include "osMOMSolverDual.h"
#include "osMOMSolverD2.h"
#include "hoCuNCGSolver.h"
#include "osMOMSolverD3.h"
#include "osMOMSolverF.h"
#include "osAHZCSolver.h"
#include "ADMMSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/make_shared.hpp>
#include "cuSolverUtils.h"
#include "osPDsolver.h"
#include "osLALMSolver.h"
#include "osLALMSolver2.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include "cuEdgeATrousOperator.h"
#include "cuDCTOperator.h"
#include "cuDCTDerivativeOperator.h"
#include "weightingOperator.h"
#include "hoNDArray_math.h"
#include "cuNCGSolver.h"
#include "accumulateOperator.h"
#include "subselectionOperator.h"
#include "subsetConverter.h"
using namespace std;
using namespace Gadgetron;
using namespace std;

namespace po = boost::program_options;
int main(int argc, char** argv){

	po::options_description desc("Allowed options");
	unsigned int iterations;
	int device;
	float tv_weight,wavelet_weight,huber,dct_weight;
	string outputFile;
	desc.add_options()
    				("help", "produce help message")
    				("input,a", po::value<string>(), "Input filename")
    				("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    				("output,f", po::value<string>(&outputFile)->default_value("reconstruction.real"), "Output filename")
    				("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    				("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    				("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight")
    				("Wavelet,W",po::value<float>(&wavelet_weight)->default_value(0),"Weight of the wavelet operator")
    				("Huber",po::value<float>(&huber)->default_value(0),"Huber weight")
    				("DCT",po::value<float>(&dct_weight)->default_value(0),"DCT regularization")
    				;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}
	std::cout << command_line_string.str();

	cudaSetDevice(device);
	cudaDeviceReset();

	string filename = vm["input"].as<string>();

	auto hoInput = read_nd_array<float>(filename.c_str());

	auto is_dims = *hoInput->get_dimensions();
	auto double_dims = is_dims;
	double_dims.push_back(2);
/*
	cuSbCgSolver<float> solver;
	solver.set_max_outer_iterations(iterations);
	solver.get_inner_solver()->set_max_iterations(10);
	solver.set_output_mode(cuSbCgSolver<float>::OUTPUT_VERBOSE);
*/
	osMOMSolverDual<cuNDArray<float>> solver;
	solver.set_tau(1e0);
	solver.set_max_iterations(iterations);
	solver.set_reg_steps(1);

 if (tv_weight > 0){

  	auto Dx = boost::make_shared<cuPartialDerivativeOperator<float,4>>(0);
  	Dx->set_weight(tv_weight);

  	Dx->set_domain_dimensions(&is_dims);
  	Dx->set_codomain_dimensions(&is_dims);
/*
  	Dx->set_domain_dimensions(&double_dims);
  	Dx->set_codomain_dimensions(&double_dims);
*/
  	auto Dy = boost::make_shared<cuPartialDerivativeOperator<float,3>>(1);
  	Dy->set_weight(tv_weight);
  	Dy->set_domain_dimensions(&is_dims);
  	Dy->set_codomain_dimensions(&is_dims);

	 auto Dz = boost::make_shared<cuPartialDerivativeOperator<float,3>>(2);
	 Dz->set_weight(tv_weight);
	 Dz->set_domain_dimensions(&is_dims);
	 Dz->set_codomain_dimensions(&is_dims);

  	auto Dx1 = boost::make_shared<subselectionOperator<cuNDArray<float>>>(Dx,0);
  	Dx1->set_domain_dimensions(&double_dims);
  	Dx1->set_codomain_dimensions(&is_dims);
  	auto Dy1 = boost::make_shared<subselectionOperator<cuNDArray<float>>>(Dy,0);
  	Dy1->set_domain_dimensions(&double_dims);
  	Dy1->set_codomain_dimensions(&is_dims);

	 auto Dz1 = boost::make_shared<subselectionOperator<cuNDArray<float>>>(Dz,0);
	 Dz1->set_domain_dimensions(&double_dims);
	 Dz1->set_codomain_dimensions(&is_dims);

  	Dx1->set_weight(tv_weight);
  	Dy1->set_weight(tv_weight);
	 Dz1->set_weight(tv_weight);

  	solver.add_regularization_group({Dx1,Dy1});
  	/*
  	solver.add_regularization_group_operator(Dx1);
  	solver.add_regularization_group_operator(Dy1);
  	solver.add_group();
*/
 }

	if (dct_weight > 0){
		//auto dctOp = boost::make_shared<identityOperator<cuNDArray<float>>>();
		auto dctOp = boost::make_shared<cuDCTOperator<float>>();
		dctOp->set_domain_dimensions(&is_dims);
		//dctOp->set_codomain_dimensions(&is_dims);
		dctOp->set_weight(dct_weight);

		auto dctOp1 = boost::make_shared<subselectionOperator<cuNDArray<float>>>(dctOp,1);
		dctOp1->set_domain_dimensions(&double_dims);
		dctOp1->set_codomain_dimensions(dctOp->get_codomain_dimensions().get());
		dctOp1->set_weight(dct_weight);
		solver.add_regularization_operator(dctOp1);

	}

	auto E = boost::make_shared<identityOperator<cuNDArray<float> > >();

	//E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(&is_dims);

	auto B = boost::make_shared<accumulateOperator<cuNDArray<float>>>(E);
	B->set_domain_dimensions(&double_dims);
	B->set_codomain_dimensions(&is_dims);
	B->set_weight(1);

	auto B2 = boost::make_shared<subsetConverter<cuNDArray<float>>>(E);
	B2->set_domain_dimensions(&is_dims);
	solver.set_encoding_operator(B2);

	cuNDArray<float> cuInput (*hoInput);

	auto result = solver.solve(&cuInput);

	write_nd_array(result.get(),outputFile);

}


#include <iostream>6
#include<conio.h>
#include <armadillo>
#include"linearRegression .h"
using namespace std;
using namespace arma;

// Armadillo documentation is available at:
// http://arma.sourceforge.net/docs.html

int
main(int argc, char** argv)
  {
	mat ketquasoxo;
	ketquasoxo.load("w.txt", arma_ascii);
	mat thamSo1 = ketquasoxo.col(1);
	mat thamSo2 = ketquasoxo.cols(1,2);
	mat thamSo3 = ketquasoxo.cols(1, 3);
	mat thamSo4 = ketquasoxo.cols(1, 4);
	mat thamSo5 = ketquasoxo.cols(1, 5);
	//cout << thamSo5;
	//cout << ketquasoxo<<endl;
	//cout << X1;
	mat Y = ketquasoxo.col(2);
	mat heSo1 = LinearRegreeOne(thamSo1, Y);
	mat heSo2 = LinearRegreeOne(thamSo2, ketquasoxo.col(3));
	mat heSo3 = LinearRegreeOne(thamSo3, ketquasoxo.col(4));
	mat heSo4 = LinearRegreeOne(thamSo4, ketquasoxo.col(5));
	mat heso5 = LinearRegreeOne(thamSo5, ketquasoxo.col(6));
	mat conSoMayMan = ones(2,1) ;
	double x;
	ketquasoxo(0, 0);
	cout << "nhap vao con so may man" << endl;
	cin >> x;
	conSoMayMan(1, 0) = x;
	//cout << heSo2;
	mat ketqua =  KetQuaTraVe(heSo1, conSoMayMan);
	conSoMayMan = join_cols(conSoMayMan, ketqua);
	ketqua = KetQuaTraVe(heSo2, conSoMayMan);
	conSoMayMan = join_cols(conSoMayMan, ketqua);
	ketqua = KetQuaTraVe(heSo3, conSoMayMan);
	conSoMayMan = join_cols(conSoMayMan, ketqua);
	ketqua = KetQuaTraVe(heSo4, conSoMayMan);
	conSoMayMan = join_cols(conSoMayMan, ketqua);
	ketqua = KetQuaTraVe(heso5, conSoMayMan);
	conSoMayMan = join_cols(conSoMayMan, ketqua);
	//cout << conSoMayMan;
	cout << "Day so ban nen danh ngay hom nay la:"<<endl;
	cout << (int)conSoMayMan(1, 0)<<" "<<(int) conSoMayMan(2,0)<< " " << (int)conSoMayMan(3, 0)<< " " << (int)conSoMayMan(4, 0)<< " " << (int)conSoMayMan(5, 0)<< " " << (int)conSoMayMan(6, 0)<<endl;
	getch();
	return 0;

  }


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/div.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
/////////////////////////////////////////////////////////////////////////////
// The remnearbyint() function computes the remnearbyint of dividing x by y.  The
// return value is x-n*y, where n is the value x / y, rounded to the nearest
// integer.  If the boost::simd::absolute value of x-n*y is 0.5, n is chosen
// to be even.
/////////////////////////////////////////////////////////////////////////////

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (bd::functor<bs::tag::nearbyint_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if (is_nez(a1))
        return fnms(div(nearbyint, a0, a1), a1, a0);
      else
        return a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (bd::functor<bs::tag::nearbyint_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if (is_nez(a1)&&is_eqz(a0)) return a0;
      return  fnms(div(nearbyint, a0, a1), a1, a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::fast_tag
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &
                                    , bd::functor<bs::tag::nearbyint_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return fnms(div(nearbyint, a0, a1), a1, a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::fast_tag
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &
                                    , bd::functor<bs::tag::nearbyint_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return fnms(div(nearbyint, a0, a1), a1, a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bs::tag::nearbyint_
                          , bd::scalar_< bd::signed_<A0> >
                          , bd::scalar_< bd::signed_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &
                                    , bd::functor<bs::tag::nearbyint_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return std::remainder(a0, a1);
    }
  };
} } }

#endif