//
//  tests.cpp
//  lpm
//
//  Created by Seong-Hwan Jun on 2019-01-28.
//

#include "tests.hpp"

#include <cassert>
#include <cmath>
#include <iostream>
#include <sstream>
#include <string>
#include <unordered_map>

#include <boost/filesystem.hpp>

#include <gsl/gsl_cdf.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics.h>

#include "lpm.hpp"
#include "lpm_likelihood.hpp"

using namespace std;

// 1. test the likelihood calculation
void test_likelihood(string data_path, bool has_passenger)
{
    string rep_path;
    string gold_standard_file_name;
    string data_file_name;
    string pathway_file_name;
    string param_file_name;

    unsigned int n_patients;
    unsigned int n_genes;
    unsigned int n_pathways;

    double gold_log_lik;
    double log_lik;
    double fbp, bgp;

    unsigned int idx;

    // free these objects within the loop
    gsl_matrix *data_matrix = 0;
    gsl_matrix *gold = 0;
    unsigned int *true_pathway = 0;

    for (unsigned int rep = 0; rep < 10; rep++) {
        rep_path = data_path + "/rep" + to_string(rep);
        gold_standard_file_name = rep_path + "/gold_standard.csv";
        data_file_name = rep_path + "/matrix.csv";
        pathway_file_name = rep_path + "/generative_mem_mat.csv";
        param_file_name = rep_path + "/parameters.csv";

        // read the data matrix
        data_matrix = read_csv(data_file_name);
        n_genes = data_matrix->size2;

        // read the gold standard likelihood file
        gold = read_csv(gold_standard_file_name, false);

        // read the true pathway
        true_pathway = new unsigned int[n_genes];
        n_pathways = read_ground_truth_pathway_from_matrix(pathway_file_name, true_pathway);
        if (has_passenger) {
            n_pathways--;
        }

        // read the parameters
        read_error_params(param_file_name, fbp, bgp);

        for (idx = 0; idx < gold->size1; idx++) {
            n_patients = gsl_matrix_get(gold, idx, 0);
            gold_log_lik = gsl_matrix_get(gold, idx, 1);

            gsl_matrix_view sub_matrix = gsl_matrix_submatrix(data_matrix, 0, 0, n_patients, n_genes);
            log_lik = compute_likelihood_from_matrix(&sub_matrix.matrix, true_pathway, n_pathways, n_genes, has_passenger, fbp, bgp);
            assert(abs(log_lik - gold_log_lik) < LIKELIHOOD_ERR_TOL);
            cout << n_patients << ": " << gold_log_lik << ", " << log_lik << endl;
        }

        // free the allocated memory
        delete [] true_pathway;
        delete data_matrix;
        delete gold;
    }

    cout << "Likelihood test passed!" << endl;
}

// run SMC to estimate the log marginal likelihood
//double test_marginal_likelihood_estimate_smc(long seed, gsl_matrix *data_matrix, unsigned int n_pathways, unsigned int n_particles, unsigned int n_smc_iter, unsigned int n_mcmc_iter, bool is_lik_tempered, double fbp, double bgp, unordered_map<string, double> true_probs, double exact_log_marginal_lik)
//{
//    double logZ;
//    double swap_prob = 0;
//    unsigned int n_genes = data_matrix->size2;
//
//    unsigned int *states = new unsigned int[n_particles * n_genes];
//    double *log_weights = new double[n_particles];
//    // run importance sampling with proposal from the prior
//    logZ = run_smc_from_matrix(seed, data_matrix, n_pathways, n_particles, n_smc_iter, n_mcmc_iter, false, swap_prob, fbp, bgp, states, log_weights, is_lik_tempered);
//    cout << "Estimated logZ: " << logZ << endl;
//    //assert(abs(exact_log_marginal_lik - logZ)/abs(exact_log_marginal_lik) < SMC_MARGINAL_ERR_TOL);
//
//    // perform tests using states and log_weights
//    string pathway_str;
//    unsigned int idx = 0;
//    unordered_map<string, double> counts;
//    normalize_destructively(log_weights, n_particles);
//    for (unsigned int i = 0; i < n_particles; i++) {
//        stringstream ss;
//        for (unsigned int j = 0; j < n_genes; j++) {
//            idx = i * n_genes + j;
//            ss << states[idx];
//            if (j < n_genes - 1) {
//                ss << ", ";
//            }
//        }
//        pathway_str = ss.str();
//        if (!counts.count(pathway_str)) {
//            counts[pathway_str] = 0;
//        }
//        counts[pathway_str] += log_weights[i];
//        //cout << pathway_str << ", " << counts[pathway_str] << endl;
//    }
//
//    double prob_est;
//    double prob_truth;
//    for (auto it = true_probs.begin(); it != true_probs.end(); ++it) {
//        pathway_str = it->first;
//        prob_est = counts[pathway_str];
//        prob_truth = true_probs[pathway_str];
//        cout << pathway_str << ", " << prob_est << ", " << prob_truth << endl;
//        assert(abs(prob_est - prob_truth) < SMC_MARGINAL_ERR_TOL);
//    }
//
//    cout << "SMC test passed!" << endl;
//    return logZ;
//
//    delete [] states;
//    delete [] log_weights;
//}

// for small number of genes and pathway, we can compute the exact value of the marginal likelihood by
// enumerating over all possibilities
// then, we can check that the marginal likelihood estimate from IS/SMC approaches this value
//void test_log_marginal_estimates()
//{
//
//    double fbp = 0.127;
//    double bgp = 0.1721;
//    double log_lik;
//
//    unsigned int n_patients = 10;
//    unsigned int n_pathways = 2;
//    unsigned int n_genes = 3;
//
//    unsigned int n_is_iter = 1;
//    unsigned int n_is_particles = 10000;
//
//    unsigned int n_smc_iter = 10;
//    unsigned int n_smc_particles = 20000;
//    double *exact_log_marginal_lik = new double[n_smc_iter];
//    for (unsigned int i = 0; i < n_smc_iter; i++) {
//        exact_log_marginal_lik[i] = DOUBLE_NEG_INF;
//    }
//
//    gsl_rng *random = generate_random_object(123);
//    vector<unsigned int> true_pathway(n_genes);
//    vector<unsigned int> stages(n_patients);
//
//    unsigned int *pathway = new unsigned int[n_genes];
//
//    propose_pathway(random, n_pathways, true_pathway);
//    sample_stages_uniform(random, n_pathways, stages);
//    gsl_matrix *data_matrix = simulate_data(random, n_pathways, true_pathway, stages);
//    add_noise(random, fbp, bgp, data_matrix);
//
//    // enumerate over possible pathway using for loops (one per gene)
//    unordered_map<string, double> state_log_liks;
//    string pathway_str;
//    for (unsigned int i = 0; i < n_pathways; i++) {
//        pathway[0] = i;
//        for (unsigned int j = 0; j < n_pathways; j++) {
//            pathway[1] = j;
//            for (unsigned int k = 0; k < n_pathways; k++) {
//                pathway[2] = k;
//
//                log_lik = compute_likelihood_from_matrix(data_matrix, pathway, n_pathways, n_genes, false, fbp, bgp);
//                if (log_lik != DOUBLE_NEG_INF) {
//                    pathway_str = to_string(pathway[0]) + ", " + to_string(pathway[1]) + ", " + to_string(pathway[2]);
//                    cout << pathway_str << ", " << log_lik << endl;
//                    state_log_liks[pathway_str] = log_lik;
//                    for (unsigned int t = 0; t < n_smc_iter; t++) {
//                        double beta_t = ((double)(t+1) / n_smc_iter);
//                        exact_log_marginal_lik[t] = log_add(exact_log_marginal_lik[t], beta_t * log_lik);
//                    }
//                }
//            }
//        }
//    }
//
//    for (auto it = state_log_liks.begin(); it != state_log_liks.end(); ++it) {
//        state_log_liks[it->first] = exp(state_log_liks[it->first] - exact_log_marginal_lik[n_smc_iter-1]);
//        cout << it->first << ", " << state_log_liks[it->first] << endl;
//    }
//
//    // add the prior factor
//    double log_prior = log(1./6);
//    for (unsigned int i = 0; i < n_smc_iter; i++) {
//        exact_log_marginal_lik[i] += log_prior;
//        if (i > 0) {
//            cout << (i+1) << "/" << n_smc_iter << ", " << exact_log_marginal_lik[i] << ", " << (exact_log_marginal_lik[i] - exact_log_marginal_lik[i-1]) << endl;
//        } else {
//            cout << (i+1) << "/" << n_smc_iter << ", " << exact_log_marginal_lik[i] << ", " << exact_log_marginal_lik[i] << endl;
//        }
//    }
//
//    long seed = gsl_rng_get(random);
//    test_marginal_likelihood_estimate_smc(seed, data_matrix, n_pathways, n_is_particles, n_is_iter, 1, false, fbp, bgp, state_log_liks, exact_log_marginal_lik[n_smc_iter-1]);
//
//    // test SMC algorithm using MH + tempering: n_mcmc_iter > 0 + is_lik_tempered = true
//    unsigned int n_mcmc_iter = 5;
//    unsigned int n_reps = 20;
//    bool is_lik_tempered = true;
//    vector<double> log_marginals(n_reps);
//    for (size_t i = 0; i < n_reps; i++) {
//        seed = gsl_rng_get(random);
//        double logZ = test_marginal_likelihood_estimate_smc(seed, data_matrix, n_pathways, n_smc_particles, n_smc_iter, n_mcmc_iter, is_lik_tempered, fbp, bgp, state_log_liks, exact_log_marginal_lik[n_smc_iter-1]);
//        log_marginals[i] = logZ;
//    }
//
//    double mean = gsl_stats_mean(log_marginals.data(), 1, log_marginals.size());
//    double sd = gsl_stats_sd_m(log_marginals.data(), 1, log_marginals.size(), mean);
//
//    // test that the 95% CI covers the truth
//    double lb = mean - 1.96 * sd;
//    double ub = mean + 1.96 * sd;
//    assert(lb <= exact_log_marginal_lik[n_smc_iter-1] && exact_log_marginal_lik[n_smc_iter-1] <= ub);
//    cout << "Truth: " << exact_log_marginal_lik[n_smc_iter - 1] << endl;
//    cout << "95% CI: " << "[" << lb << ", " << ub << "]" << endl;
//
//    delete [] exact_log_marginal_lik;
//    delete [] pathway;
//    delete random;
//}

void test_uniform_prior_calculation()
{
    double log_uniform_prior1 = -log_pathway_uniform_prior(2, 3);
    double log_uniform_prior2 = -log_pathway_uniform_prior_log_scale(2, 3);
    assert(abs(log_uniform_prior1 - log_uniform_prior2) < ERR_TOL);
    assert(abs(log_uniform_prior2 - log(1./6)) < ERR_TOL);

    log_uniform_prior1 = -log_pathway_uniform_prior(5, 50);
    log_uniform_prior2 = -log_pathway_uniform_prior_log_scale(5, 50);
    assert(abs(log_uniform_prior1 - log_uniform_prior2) < ERR_TOL);

    unsigned int n_genes = 3;
    unsigned int n_pathways = 2;
    double ret = -log_pathway_uniform_prior(n_pathways, n_genes);
    assert(ret == -log(6));

    n_genes = 8;
    n_pathways = 3;
    ret = -log_pathway_uniform_prior(n_pathways, n_genes);

    // sample pathways from the prior
    gsl_rng *random = generate_random_object(1);
    vector<unsigned int> pathway(n_genes);
    unordered_map<string, unsigned int> counts;
    unordered_map<string, double> probs;
    unsigned int n_mc_samples = 1000000;
    for (unsigned int n = 0; n < n_mc_samples; n++) {
        propose_pathway(random, n_pathways, pathway);
        string s = "";
        for (unsigned int g = 0; g < n_genes; g++) {
            s += to_string(pathway[g]);
            if (g < n_genes - 1) {
                s += ", ";
            }
        }
        if (!counts.count(s)) {
            counts[s] = 0;
            probs[s] = log_pathway_proposal(pathway, n_pathways);
        }
        counts[s] += 1;
    }

    cout << "Size: " << counts.size() << endl;
    assert(abs(ret - log(counts.size())));

    double prob = 0.0;
    for (auto it = counts.begin(); it != counts.end(); ++it) {
        prob = (double)it->second/n_mc_samples;
        ret = exp(probs[it->first]);
        cout << it->first << ", " << ret << ", " << prob << endl;
        assert(abs(prob - ret) < ERR_TOL);
    }

    delete random;
}

void test_run()
{
//    double error = 0.05;
//    const char* input_data = "/Users/seonghwanjun/Dropbox/Research/single-cell-research/repos/linear-progression/data/Experiment2/With_passengers/5/error0.05/rep0/matrix.csv";
//    const char* true_pathway_file = "/Users/seonghwanjun/Dropbox/Research/single-cell-research/repos/linear-progression/data/Experiment2/With_passengers/5/error0.05/rep0/generative_mem_mat.csv";
//    const char* output_path = "/Users/seonghwanjun/Dropbox/Research/single-cell-research/repos/linear-progression/data/Experiment2/With_passengers/5/error0.05/rep0/mcmc/";

    double error = 0.1;
    const char* input_data = "/Users/seonghwanjun/Dropbox/Research/single-cell-research/repos/linear-progression/data/Experiment1/With_passengers/Uniform/error0.1/rep0/matrix.csv";
    const char* true_pathway_file = "/Users/seonghwanjun/Dropbox/Research/single-cell-research/repos/linear-progression/data/Experiment1/With_passengers/Uniform/error0.1/rep0/generative_mem_mat.csv";
    const char* output_path = "/Users/seonghwanjun/Dropbox/Research/single-cell-research/repos/linear-progression/data/Experiment1/With_passengers/Uniform/error0.1/rep0/mcmc/";

    unsigned int *true_pathway = new unsigned int[100];
    read_ground_truth_pathway_from_matrix(true_pathway_file, true_pathway);
    double log_lik_at_truth = compute_likelihood(input_data, true_pathway, 5, 100, true, error, error);
    cout << log_lik_at_truth << endl;

    //double prior_passenger_prob = 0.95;
    //run_mcmc(121, input_data, output_path, 4, 20000, 20, 100, true, 0.1, 0.0, 0.2, 0.05, prior_passenger_prob);
    perform_prediction(output_path, input_data, 5, true, true_pathway);

//    gsl_rng *random = generate_random_object(121);
//    vector<double> log_Zs;
//    for (size_t i = 0; i < 5; i++) {
//        double log_Z = run_smc(gsl_rng_get(random), input_data, 6, 10000, 100, 1, true, 0.1, 0.05, 0.05, 0, 0, true, 6);
//        log_Zs.push_back(log_Z);
//        cout << log_Z << endl;
//    }
//    double log_Z_mean = gsl_stats_mean(log_Zs.data(), 1, log_Zs.size());
//    double log_Z_sd = gsl_stats_sd_m(log_Zs.data(), 1, log_Zs.size(), log_Z_mean);
//    double lb = log_Z_mean - 1.96 * log_Z_sd;
//    double ub = log_Z_mean + 1.96 * log_Z_sd;
//    cout << "95% CI: [" << lb << ", " << ub << "]" <<  endl;
//    gsl_rng_free(random);

    //model_selection(1, input_data, 5, 1, 500, 100, 1, true, 0.1, 0, 0, 1, 6, 0, 0, true);
}

int main(int argc, char *argv[])
{
    bool run_test = false;
    if (run_test) {
        if (argc != 2)
            exit(-1);

        string root_path(argv[1]);
        string lik_test_path1 = root_path + "/data/test/Without_passengers/";
        string lik_test_path2 = root_path + "/data/test/With_passengers/";

        test_uniform_prior_calculation();
        test_likelihood(lik_test_path1, false);
        test_likelihood(lik_test_path2, true);
        //test_log_marginal_estimates();
    } else {
        test_run();
    }

    return 0;
}


/**
 * \ file LMSFilter.cpp
 */

#include <array>
#include <fstream>

#include <ATK/Adaptive/LMSFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Tools/SumFilter.h>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1200;

TEST(LMSFilter, destructor_test)
{
  ASSERT_NO_THROW(std::make_unique<ATK::LMSFilter<double>>(100));
}

TEST(LMSFilter, size_negative_test)
{
  ATK::LMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_size(0), ATK::RuntimeError);
}

TEST(LMSFilter, size_set_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_size(10);
  ASSERT_EQ(filter.get_size(), 10);
}

TEST(LMSFilter, mode_set_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_mode(ATK::LMSFilter<double>::Mode::NORMALIZED);
  ASSERT_EQ(filter.get_mode(), ATK::LMSFilter<double>::Mode::NORMALIZED);
}

TEST(LMSFilter, memory_negative_test)
{
  ATK::LMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_memory(0), ATK::RuntimeError);
}

TEST(LMSFilter, memory_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_memory(0.5);
  ASSERT_EQ(filter.get_memory(), 0.5);
}

TEST(LMSFilter, memory_positive1_test)
{
  ATK::LMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_memory(1), ATK::RuntimeError);
}

TEST(LMSFilter, mu_negative_test)
{
  ATK::LMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_mu(0), ATK::RuntimeError);
}

TEST(LMSFilter, mu_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_mu(0.5);
  ASSERT_EQ(filter.get_mu(), 0.5);
}

TEST(LMSFilter, mu_positive1_test)
{
  ATK::LMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_mu(1), ATK::RuntimeError);
}

TEST(LMSFilter, learning_set_test)
{
  ATK::LMSFilter<double> filter(100);
  ASSERT_EQ(filter.get_learning(), true);
  filter.set_learning(false);
  ASSERT_EQ(filter.get_learning(), false);
}

TEST(LMSFilter, memory_99_test)
{
  std::array<double, PROCESSSIZE> data;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input_lms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(data.data()), PROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  ATK::LMSFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.999);
  filter.set_mu(.01);

  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);

  filter.process(PROCESSSIZE);
  ASSERT_NE(filter.get_w(), nullptr);

  std::array<double, PROCESSSIZE> outdata;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/output_lms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(outdata.data()), PROCESSSIZE * sizeof(double));
  }

  for (unsigned int i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], filter.get_output_array(0)[i], 0.0001);
  }
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_SSE_SSE3_SUM_HPP_INCLUDED
#define NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_SSE_SSE3_SUM_HPP_INCLUDED
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/fusion/include/fold.hpp>
#include <nt2/sdk/meta/strip.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<arithmetic_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::arithmetic_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 base;
      typedef typename std::tr1::result_of<meta::arithmetic(base)>::type  type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      return boost::fusion::fold(a0,0,functor<tag::plus_>());
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<double_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::double_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type                 type;
    };

    NT2_FUNCTOR_CALL(1) {
      A0 b1 = {_mm_hadd_pd (a0, Zero<A0>())};
      double r;
      _mm_store_sd(&r, b1);
      return r;
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::sum_, tag::cpu_,
                      (A0),
                      ((simd_<float_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::sum_(tag::simd_<tag::float_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::scalar_of<A0>::type   type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      A0 b1 = {_mm_hadd_ps (a0, Zero<A0>())};
      b1 = _mm_hadd_ps (b1, Zero<A0>());
      float r;
      _mm_store_ss(&r, b1);
      return r;
    }
  };
} }

#endif
// modified by jt the 05/01/2011


//============================================================================
// Name        : test2.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
#include <Eigen/Dense>
//#include <unsupported/Eigen/AutoDiff>
#include <unsupported/Eigen/AdolcForward>
//#include <unsupported/Eigen/AdolcSupport>

//#include <chrono>
//#include <iomanip>
#include <adolc.h>
#include <adolc/adouble.h>
#include <vector>
#include <time.h>
#include <math.h>
#include <fstream>
#include <thread>
#include <future>
#include <algorithm>
#include <iterator>
#include "cost.h"
#include "dynamics.h"
#include "iLQR.h"
#include "utils.h"
#include<unistd.h>
#include "DataStreamClient.h"

int main()
{

//
//	Eigen::VectorXd X0(4,1),inputs(2,1);
//	X0<<0,0,0,0;
//	inputs<<0,0;
//
//	constexpr size_t state_size=4;
//	constexpr size_t input_size=2;
	constexpr size_t horizon=5;
	constexpr int total_steps=300;
	srand((unsigned int) time(0));
////	int horizon=1000;
	unsigned int tag1(1),tag2(2),tag3(3),tag4(4),tag5(5),tag6(6)
	,tag7(7),tag8(8),tag9(9);

//	double time_step=0.2;
	double time_step=0.1;

//

//	/* This part is unicycle simulation*/
//
//	cost<4,2> running_cost_uni(Unicycle_Cost::running_cost,tag1);
//	cost<4,2> terminal_cost_uni(Unicycle_Cost::terminal_cost,tag2);
//	dynamics<4,2> dynamics_uni(Unicycle_Dynamics::dynamics,tag3,time_step);
//
//	iLQR<4,2,horizon> uni_solver(running_cost_uni,terminal_cost_uni,dynamics_uni);
//	Eigen::Matrix<double,2,1> u_uni=Eigen::Matrix<double,2,1>::Zero();
//	Eigen::Matrix<double,4,1> x0_uni=Eigen::Matrix<double,4,1>::Zero();
//	x0_uni<<-5,-5,0,0;
//////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//	Eigen::Matrix<double,4,1> x_goal_uni;
//
//
////
//	iLQR<4,2,horizon>::input_trajectory u_init_uni;
//	std::fill(std::begin(u_init_uni), std::end(u_init_uni), u_uni);
//	iLQR<4,2,horizon>::state_input_trajectory soln_uni;
//	iLQR<4,2,total_steps>::state_input_trajectory soln_uni_rhc;
//
//
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	uni_solver.set_MPC(u_init_uni);
//	int execution_steps=1;
//	soln_uni_rhc.first[0]=x0_uni;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//
//	x_goal_uni<<5,5,0,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
////		X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_uni=uni_solver.run_MPC(soln_uni_rhc.first[i], x_goal_uni, 100, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
////		std::cout<<soln_uni.first[39]<<std::endl;
//		soln_uni_rhc.first[i+1]=soln_uni.first[1];
//		soln_uni_rhc.second[i]=soln_uni.second[0];
//	}
//	t_end = clock();
//////
//////
////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<4,2,total_steps>(state_path,input_path, soln_uni_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);
//
//
//	/* End of unicycle simulation*/

/* This part is 2-unicycle simulation*/

//	cost<8,4> running_cost_uni2(Unicycle_Cost::running_cost2,tag1);
//	cost<8,4> terminal_cost_uni2(Unicycle_Cost::terminal_cost2,tag2);
//	dynamics<8,4> dynamics_uni2(Unicycle_Dynamics::dynamics_2,tag3,time_step);
//
//	iLQR<8,4,horizon> uni2_solver(running_cost_uni2,terminal_cost_uni2,dynamics_uni2);
//	Eigen::Matrix<double,4,1> u_uni2=Eigen::Matrix<double,4,1>::Random()*0.1;
//	Eigen::Matrix<double,8,1> x0_uni2=Eigen::Matrix<double,8,1>::Zero();
//	x0_uni2<<0,0.1,0,0, 10,0,M_PI,0;
//
//	Eigen::Matrix<double,8,1> x_goal_uni2;
//
//
////
//	iLQR<8,4,horizon>::input_trajectory u_init_uni2;
//	for(auto &item : u_init_uni2)
//		item=Eigen::Matrix<double,4,1>::Random()*0.1;
//	std::fill(std::begin(u_init_uni2), std::end(u_init_uni2), u_uni2);
//	iLQR<8,4,horizon>::state_input_trajectory soln_uni2;
//	iLQR<8,4,total_steps>::state_input_trajectory soln_uni2_rhc;
//
//
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	uni2_solver.set_MPC(u_init_uni2);
//	int execution_steps=1;
//	soln_uni2_rhc.first[0]=x0_uni2;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//
//	x_goal_uni2<<10,0,0,0, 0,0,M_PI,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
////		X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_uni2=uni2_solver.run_MPC(soln_uni2_rhc.first[i], x_goal_uni2, 5, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
////		std::cout<<soln_uni.first[39]<<std::endl;
//		soln_uni2_rhc.first[i+1]=soln_uni2.first[1];
//		soln_uni2_rhc.second[i]=soln_uni2.second[0];
//	}
//	t_end = clock();
////
////
//////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<8,4,total_steps>(state_path,input_path, soln_uni2_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);

/* End of 2-unicycle simulation*/

/* This part is single drone simulation*/

//	cost<12,4> running_cost_dr(Drone_Cost::running_cost,tag1);
//	cost<12,4> terminal_cost_dr(Drone_Cost::terminal_cost,tag2);
//	dynamics<12,4> dynamics_dr(Drone_Dynamics::dynamics,tag3,time_step);
//
//	iLQR<12,4,horizon> drone_solver(running_cost_dr,terminal_cost_dr,dynamics_dr);
//	Eigen::Matrix<double,4,1> u_drone=Eigen::Matrix<double,4,1>::Ones();
//	Eigen::Matrix<double,12,1> X0_drone=Eigen::Matrix<double,12,1>::Zero();
//////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//	Eigen::Matrix<double,12,1> X_goal_drone;
//
//	u_drone=u_drone*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
////
//	iLQR<12,4,horizon>::input_trajectory u_init_drone;
//	std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
//	iLQR<12,4,horizon>::state_input_trajectory soln_drone;
//	iLQR<12,4,total_steps>::state_input_trajectory soln_drone_rhc;
//
//
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	drone_solver.set_MPC(u_init_drone);
//	int execution_steps=1;
//	soln_drone_rhc.first[0]=X0_drone;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//	X_goal_drone<<1,1,1.5, 0,0,0, 0,0,0, 0,0,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
////		X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_drone=drone_solver.run_MPC(soln_drone_rhc.first[i], X_goal_drone, 100, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		std::cout<<soln_drone.first[39]<<std::endl;
//		soln_drone_rhc.first[i+1]=soln_drone.first[1];
//		soln_drone_rhc.second[i]=soln_drone.second[0];
//	}
//	t_end = clock();
//////
//////
////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<12,4,total_steps>(state_path,input_path, soln_drone_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);

/* End of single drone simulation*/

/* This part is 2-drone simulation*/

//	cost<12*2,4*2> running_cost_dr2(Drone_Cost::running_cost2,tag4);
//	cost<12*2,4*2> terminal_cost_dr2(Drone_Cost::terminal_cost2,tag5);
//	dynamics<12*2,4*2> dynamics_dr2(Drone_Dynamics::dynamics_2,tag6,time_step);
//	iLQR<12*2,4*2,horizon>::input_trajectory u_init_drone2;
//
//	iLQR<12*2,4*2,horizon> drone2_solver(running_cost_dr2,terminal_cost_dr2,dynamics_dr2);
//	Eigen::Matrix<double,4*2,1> u_drone2=Eigen::Matrix<double,4*2,1>::Ones();
//	Eigen::Matrix<double,12*2,1> X0_drone2=Eigen::Matrix<double,12*2,1>::Zero();
////	X0_drone2<<-2.5,-2.5,-2.5, 0,0,0, 0,0,0, 0,0,0,
////			2.5,-2.5,-2.5, 0,0,0, 0,0,0, 0,0,0;
//	X0_drone2<<-1,0,2, 0,0,0, 0,0,0, 0,0,0,
//			1,0,2, 0,0,0, 0,0,0, 0,0,0;
//	Eigen::Matrix<double,12*2,1> X_goal_drone2;
//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	drone2_solver.set_MPC(u_init_drone2);
//	int execution_steps=1;
//	soln_drone2_rhc.first[0]=X0_drone2;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
////	X_goal_drone2<<2.5,2.5,2.5, 0,0,0, 0,0,0, 0,0,0,
////			-2.5,2.5,2.5, 0,0,0, 0,0,0, 0,0,0;
//	X_goal_drone2<<1,0,2, 0,0,0, 0,0,0, 0,0,0,
//			-1,0,2, 0,0,0, 0,0,0, 0,0,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
//
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_drone2=drone2_solver.run_MPC(soln_drone2_rhc.first[i], X_goal_drone2, 20, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		soln_drone2_rhc.first[i+1]=soln_drone2.first[1];
//		soln_drone2_rhc.second[i]=soln_drone2.second[0];
//	}
//	t_end = clock();
//////
//////
////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<12*2,4*2,total_steps>(state_path,input_path, soln_drone2_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);
//
/* End of 2-drone simulation*/

//
//

//
////	diff = difftime( stop, start );
////	std::cout << "Time: " << diff << " seconds";
////	seconds = (double)(t_end - t_start) / CLOCKS_PER_SEC;
////	printf("Time: %f s\n", seconds);
//		drone_solver.set_MPC(u_init_drone);
////		t_start = clock();
//		std::string state_write_path="/Users/talhakavuncu/Desktop/cflib_stream.txt";
//		std::string state_read_path="/Users/talhakavuncu/Desktop/stream.txt";
//		std::ifstream state_read_file(state_read_path);
//		std::ofstream state_write_file(state_write_path);
//		Eigen::IOFormat CommaInitFmt(Eigen::FullPrecision, Eigen::DontAlignCols, ",", ", ", "", "", "", "");
//		double dt,x,y,z,roll,pitch,yaw;
//		state_read_file>>dt>>x>>y >> z>>roll >> pitch>>yaw;
//		X0_drone<<x,y,z,roll,pitch,yaw,x/dt,y/dt,z/dt,roll/dt,pitch/dt,yaw/dt;
//		X_goal_drone<<0,0,1, 0,0,0, 0,0,0, 0,0,0;
//		while(true)
//		{
////			X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////			if (i>total_steps/3)
////				X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
////			std::cout<<"iteration "<<i<<std::endl;
//	//
//
//			soln_drone=drone_solver.run_MPC(X0_drone, X_goal_drone, 50, 1);
//			state_write_file.open(state_write_path,std::ios::out | std::ios::trunc);
//			state_write_file<<soln_drone.first[1].transpose().format(CommaInitFmt);
////			i.transpose().format(CommaInitFmt)
//			state_write_file.close();
//			state_read_file>>dt>>x>>y >> z>>roll >> pitch>>yaw;
//			X0_drone<<x,y,z,roll,pitch,yaw,x/dt,y/dt,z/dt,roll/dt,pitch/dt,yaw/dt;
//	////		unsigned int microsecond = 1000000;
//	////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
////			soln_drone_rhc.first[i+1]=soln_drone.first[1];
//
////			soln_drone_rhc.second[i]=soln_drone.second[0];
//		}
////		t_end = clock();

/* This part is integrator simulation */

//
//cost<3,3> integrator_running_cost(Single_Integrator_Cost::running_cost,tag7);
//cost<3,3> integrator_terminal_cost(Single_Integrator_Cost::terminal_cost,tag8);
//dynamics<3,3> integrator_dynamics(Single_Integrator_3D::dynamics,tag9,time_step);
//iLQR<3,3,horizon>::input_trajectory u_init_integrator;
//iLQR<3,3,horizon> integrator_solver(integrator_running_cost,integrator_terminal_cost,integrator_dynamics);
//Eigen::Matrix<double,3,1> u_integrator=Eigen::Matrix<double,3,1>::Zero();
//Eigen::Matrix<double,3,1> x0_integrator=Eigen::Matrix<double,3,1>::Zero();
//Eigen::Matrix<double,3,1> x_goal_integrator;
//iLQR<3,3,horizon>::state_input_trajectory soln_integrator;
//iLQR<3,3,total_steps>::state_input_trajectory soln_integrator_rhc;
//
////	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//	std::fill(std::begin(u_init_integrator), std::end(u_init_integrator), u_integrator);
////	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
////	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//	clock_t t_start, t_end;
////
//////
//	x0_integrator<<0,0,0.5;
////	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
//	double seconds;
//	integrator_solver.set_MPC(u_init_integrator);
//	int execution_steps=1;
//	soln_integrator_rhc.first[0]=x0_integrator;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
//		x_goal_integrator<<2,2,2;
////		if (i>total_steps/3)
////			x_goal_integrator<<0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
////////
////		auto term1=soln_integrator_rhc.first[i]+0.2*Eigen::Matrix<double,3,1>::Random();
////		auto term2=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term3=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term4=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term5=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term6=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term7=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term8=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term9=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term10=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//		soln_integrator=integrator_solver.run_MPC(soln_integrator_rhc.first[i],
//				x_goal_integrator, 100, execution_steps);
//////
//////////		unsigned int microsecond = 1000000;
//////////		usleep(2 * microsecond);
//////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		soln_integrator_rhc.first[i+1]=soln_integrator.first[1];
//		soln_integrator_rhc.second[i]=soln_integrator.second[0];
//	}
//	t_end = clock();
//////
////////
//////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<3,3,total_steps>(state_path,input_path, soln_integrator_rhc);
//////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);

/*single integrator simulation ends */

/*Single integrator 2 agent simulation */

//	cost<6,6> integrator2_running_cost(Single_Integrator_Cost::running_cost2,tag7);
//	cost<6,6> integrator2_terminal_cost(Single_Integrator_Cost::terminal_cost2,tag8);
//	dynamics<6,6> integrator2_dynamics(Single_Integrator_3D::dynamics2,tag9,time_step);
//	iLQR<6,6,horizon>::input_trajectory u_init_integrator2;
//	iLQR<6,6,horizon> integrator2_solver(integrator2_running_cost,integrator2_terminal_cost,integrator2_dynamics);
//	Eigen::Matrix<double,6,1> u_integrator2=Eigen::Matrix<double,6,1>::Zero();
//	Eigen::Matrix<double,6,1> x0_integrator2=Eigen::Matrix<double,6,1>::Zero();
//	x0_integrator2<<-1,0,2.001, 1,0,2;
////	std::cout<<x0_integrator2<<std::endl;
//	Eigen::Matrix<double,6,1> x_goal_integrator2;
//	x_goal_integrator2<<1,0,2, -1,0,2;
//	iLQR<6,6,horizon>::state_input_trajectory soln_integrator2;
//	iLQR<6,6,total_steps>::state_input_trajectory soln_integrator2_rhc;
//
//	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//	//	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//		clock_t t_start, t_end;
//	//
//	////
//		double seconds;
//		integrator2_solver.set_MPC(u_init_integrator2);
//		int execution_steps=1;
//		soln_integrator2_rhc.first[0]=x0_integrator2;
//		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//		t_start = clock();
//		for(int i=0;i<total_steps;++i)
//		{
////			x_goal_integrator2<<2,2,2;
//	//		if (i>total_steps/3)
//	//			x_goal_integrator<<0,0,0;
//			std::cout<<"iteration "<<i<<std::endl;
//	//////
//			soln_integrator2=integrator2_solver.run_MPC(soln_integrator2_rhc.first[i],
//					x_goal_integrator2, 5, execution_steps);
//	////
//	////////		unsigned int microsecond = 1000000;
//	////////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//			soln_integrator2_rhc.first[i+1]=soln_integrator2.first[1];
//			soln_integrator2_rhc.second[i]=soln_integrator2.second[0];
//		}
//		t_end = clock();
//	////
//	//////
//	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		write_file<6,6,total_steps>(state_path,input_path, soln_integrator2_rhc);
//	////
//		std::cout<<"finished"<<std::endl;
//		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//		printf("Time: %f s\n", seconds);

/*Single integrator 2 agent simulation ends*/

/*Double Integrator Simulation*/

//
//
//	cost<6,3> integrator_running_cost(Double_Integrator_Cost::running_cost,tag7);
//	cost<6,3> integrator_terminal_cost(Double_Integrator_Cost::terminal_cost,tag8);
//	dynamics<6,3> integrator_dynamics(Double_Integrator_3D::dynamics,tag9,time_step);
//	iLQR<6,3,horizon>::input_trajectory u_init_integrator;
//	iLQR<6,3,horizon> integrator_solver(integrator_running_cost,integrator_terminal_cost,integrator_dynamics);
//	Eigen::Matrix<double,3,1> u_integrator=Eigen::Matrix<double,3,1>::Zero();
//	Eigen::Matrix<double,6,1> x0_integrator=Eigen::Matrix<double,6,1>::Zero();
//	Eigen::Matrix<double,6,1> x_goal_integrator;
//	iLQR<6,3,horizon>::state_input_trajectory soln_integrator;
//	iLQR<6,3,total_steps>::state_input_trajectory soln_integrator_rhc;
//
//	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//		std::fill(std::begin(u_init_integrator), std::end(u_init_integrator), u_integrator);
//	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//		clock_t t_start, t_end;
//	//
//	////
//		x0_integrator<<0,0,0.5, 0,0,0;
//	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
//		double seconds;
//		integrator_solver.set_MPC(u_init_integrator);
//		int execution_steps=1;
//		soln_integrator_rhc.first[0]=x0_integrator;
//		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//		t_start = clock();
//		for(int i=0;i<total_steps;++i)
//		{
//			x_goal_integrator<<2,2,2, 0,0,0;
//	//		if (i>total_steps/3)
//	//			x_goal_integrator<<0,0,0;
//			std::cout<<"iteration "<<i<<std::endl;
//	//////
//	//		auto term1=soln_integrator_rhc.first[i]+0.2*Eigen::Matrix<double,3,1>::Random();
//	//		auto term2=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term3=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term4=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term5=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term6=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term7=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term8=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term9=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term10=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//			soln_integrator=integrator_solver.run_MPC(soln_integrator_rhc.first[i],
//					x_goal_integrator, 5, execution_steps);
//	////
//	////////		unsigned int microsecond = 1000000;
//	////////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//			soln_integrator_rhc.first[i+1]=soln_integrator.first[1];
//			soln_integrator_rhc.second[i]=soln_integrator.second[0];
//		}
//		t_end = clock();
//	////
//	//////
//	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		write_file<6,3,total_steps>(state_path,input_path, soln_integrator_rhc);
//	////
//		std::cout<<"finished"<<std::endl;
//		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//		printf("Time: %f s\n", seconds);

/*Double Integrator Simulation Ends*/

/*First Order Drone Simulation */

	cost<6,6> drone_running_cost(Drone_First_Order_Cost::running_cost,tag7);
	cost<6,6> drone_terminal_cost(Drone_First_Order_Cost::terminal_cost,tag8);
	dynamics<6,6> drone_dynamics(Drone_First_Order_Dynamics::dynamics,tag9,time_step);
	iLQR<6,6,horizon>::input_trajectory u_init_drone;
	iLQR<6,6,horizon> drone_solver(drone_running_cost,drone_terminal_cost,drone_dynamics);
	Eigen::Matrix<double,6,1> u_drone=Eigen::Matrix<double,6,1>::Zero();
	Eigen::Matrix<double,6,1> x0_drone=Eigen::Matrix<double,6,1>::Zero();
	Eigen::Matrix<double,6,1> x_goal_drone;
	iLQR<6,6,horizon>::state_input_trajectory soln_drone;
	iLQR<6,6,total_steps>::state_input_trajectory soln_drone_rhc;

	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
		std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;

		clock_t t_start, t_end;
	//
	////
		x0_drone<<0,0,0.5, 0,0,0;
	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
		double seconds;
		drone_solver.set_MPC(u_init_drone);
		int execution_steps=1;
		soln_drone_rhc.first[0]=x0_drone;
		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
		t_start = clock();
		for(int i=0;i<total_steps;++i)
		{
			x_goal_drone<<2,2,2, 0,0,0;
	//		if (i>total_steps/3)
	//			x_goal_integrator<<0,0,0;
			std::cout<<"iteration "<<i<<std::endl;
			double scale=0.01;
			auto term1=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term2=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term3=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term4=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term5=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term6=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term7=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term8=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term9=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term10=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
			soln_drone=drone_solver.run_MPC(term1,
					x_goal_drone, 5, execution_steps);
	////
	////////		unsigned int microsecond = 1000000;
	////////		usleep(2 * microsecond);
	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
			soln_drone_rhc.first[i+1]=soln_drone.first[1];
			soln_drone_rhc.second[i]=soln_drone.second[0];
		}
		t_end = clock();
	////
	//////
	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
		write_file<6,6,total_steps>(state_path,input_path, soln_drone_rhc);
	////
		std::cout<<"finished"<<std::endl;
		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
		printf("Time: %f s\n", seconds);

/*First Order Drone Simulation Ends*/

/*First Order 2 Drone Simulation*/

//	cost<12,12> drone2_running_cost(Drone_First_Order_Cost::running_cost2,tag7);
//	cost<12,12> drone2_terminal_cost(Drone_First_Order_Cost::terminal_cost2,tag8);
//	dynamics<12,12> drone2_dynamics(Drone_First_Order_Dynamics::dynamics_2,tag9,time_step);
//	iLQR<12,12,horizon>::input_trajectory u_init_drone2;
//	iLQR<12,12,horizon> drone2_solver(drone2_running_cost,drone2_terminal_cost,drone2_dynamics);
//	Eigen::Matrix<double,12,1> u_drone2=Eigen::Matrix<double,12,1>::Zero();
//	Eigen::Matrix<double,12,1> x0_drone2=Eigen::Matrix<double,12,1>::Zero();
//	Eigen::Matrix<double,12,1> x_goal_drone2;
//	iLQR<12,12,horizon>::state_input_trajectory soln_drone2;
//	iLQR<12,12,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//		std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//		clock_t t_start, t_end;
//	//
//	////
////		x0_drone2<<0,0,0.5, 0,0,1;
//		x0_drone2<<-3,0,2.1, 0,0,0, 3,0,2, 0,0,0;
//		x_goal_drone2<<3,0,2, 0,0,0, -3,0,2, 0,0,0;
//
//	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
//		double seconds;
//		drone2_solver.set_MPC(u_init_drone2);
//		int execution_steps=1;
//		soln_drone2_rhc.first[0]=x0_drone2;
//		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//		t_start = clock();
//		for(int i=0;i<total_steps;++i)
//		{
////			x_goal_drone2<<2,2,2, 0,0,0;
//	//		if (i>total_steps/3)
//	//			x_goal_integrator<<0,0,0;
//			std::cout<<"iteration "<<i<<std::endl;
//	//////
//	//		auto term1=soln_integrator_rhc.first[i]+0.2*Eigen::Matrix<double,3,1>::Random();
//	//		auto term2=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term3=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term4=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term5=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term6=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term7=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term8=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term9=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term10=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//			soln_drone2=drone2_solver.run_MPC(soln_drone2_rhc.first[i],
//					x_goal_drone2, 5, execution_steps);
//	////
//	////////		unsigned int microsecond = 1000000;
//	////////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//			soln_drone2_rhc.first[i+1]=soln_drone2.first[1];
//			soln_drone2_rhc.second[i]=soln_drone2.second[0];
//		}
//		t_end = clock();
//	////
//	//////
//	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		write_file<12,12,total_steps>(state_path,input_path, soln_drone2_rhc);
//	////
//		std::cout<<"finished"<<std::endl;
//		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//		printf("Time: %f s\n", seconds);

/*First Order 2 Drone Simulation Ends*/
};


#pragma once

#include <scomplex/simplicial_complex.hpp>
#include <scomplex/types.hpp>

#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>
#include <cmath>
#include <exception>
#include <tuple>
#include <vector>

#include <memory>
#include <fstream>

namespace gsimp {

class non_zero_chain : public std::exception {};

class bounding_chain {
    std::shared_ptr<simplicial_complex> s_comp;
    std::vector<std::unique_ptr<matrix_t>> boundary_matrices;
    void populate_matrices();

   public:
    bounding_chain(simplicial_complex& sc);
    bounding_chain(std::shared_ptr<simplicial_complex> sc);
    bounding_chain(std::vector<point_t>& points, std::vector<cell_t>& tris);
    ~bounding_chain();
    chain_t get_bounding_chain(chain_t&);
};

//---------------------------------------
// implementation

bounding_chain::~bounding_chain() {}

bounding_chain::bounding_chain(std::shared_ptr<simplicial_complex> sc) {
    s_comp = sc;
    populate_matrices();
}

bounding_chain::bounding_chain(simplicial_complex& sc) {
    s_comp = std::make_shared<simplicial_complex>(sc);
    populate_matrices();
}

bounding_chain::bounding_chain(std::vector<point_t>& points,
                               std::vector<cell_t>& tris) {
    s_comp = std::make_shared<simplicial_complex>(points,tris);
    populate_matrices();
}

void bounding_chain::populate_matrices() {
    for (int d = 0; d < s_comp->dimension(); ++d) {
        auto level_matrix = s_comp->get_boundary_matrix(d);
        boundary_matrices.push_back(
            std::unique_ptr<matrix_t>(new matrix_t(level_matrix)));
    }
}

// round vectors for comparison
vector_t round_vec(vector_t vec) {
    vector_t rounded_vec(vec.rows());
    for (int i = 0; i < vec.rows(); ++i) {
        int coef = round(vec.coeffRef(i));
        if (coef != 0) rounded_vec.coeffRef(i) = coef;
    }
    return rounded_vec;
}

bool equals(vector_t vec1, vector_t vec2) {
    for (int i = 0; i < vec1.rows(); ++i) {
        if (vec1.coeffRef(i) != vec2.coeffRef(i)) {
            std::cout << i << ": " << vec1.coeffRef(i) << " "
                      << vec2.coeffRef(i) << '\n';
            return false;
        }
    }
    return true;
}

chain_t bounding_chain::get_bounding_chain(chain_t& chain) {
    int chain_d;
    vector_t chain_v;
    std::tie<int, vector_t>(chain_d, chain_v) = chain;

    if (chain_d >= s_comp->dimension()) throw non_zero_chain();

    Eigen::LeastSquaresConjugateGradient<matrix_t> lscg;
    lscg.compute(*(boundary_matrices.at(chain_d)));

    vector_t bound_chain(round_vec(lscg.solve(chain_v)));
    vector_t result(round_vec(*(boundary_matrices.at(chain_d)) * bound_chain));

    if (equals(result, chain_v))
        return chain_t(chain_d + 1, bound_chain);
    else
        throw non_zero_chain();
}

};


// SPDX-License-Identifier: Apache-2.0
//
// Copyright 2015 Conrad Sanderson (http://conradsanderson.id.au)
// Copyright 2015 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("init_fill_1")
  {
  mat Z( 5,  6, fill::zeros);
  mat O( 5,  6, fill::ones);
  mat I( 5,  6, fill::eye);
  mat U(50, 60, fill::randu);
  mat N(50, 60, fill::randn);

  REQUIRE( accu(Z != 0) == 0   );
  REQUIRE( accu(O != 0) == 5*6 );
  REQUIRE( accu(I != 0) == 5   );

  REQUIRE(   mean(vectorise(U)) == Approx(0.500).margin(0.05) );
  REQUIRE( stddev(vectorise(U)) == Approx(0.288).margin(0.05) );

  REQUIRE(   mean(vectorise(N)) == Approx(0.0).margin(0.05) );
  REQUIRE( stddev(vectorise(N)) == Approx(1.0).margin(0.05) );

  mat X(5, 6, fill::none);   // only to test instantiation
  }

TEST_CASE("init_fill_2")
  {
  cube Z( 5,  6, 2, fill::zeros);
  cube O( 5,  6, 2, fill::ones);
  cube U(50, 60, 2, fill::randu);
  cube N(50, 60, 2, fill::randn);

  REQUIRE( accu(Z != 0) == 0     );
  REQUIRE( accu(O != 0) == 5*6*2 );

  REQUIRE(   mean(vectorise(U)) == Approx(0.500).margin(0.05) );
  REQUIRE( stddev(vectorise(U)) == Approx(0.288).margin(0.05) );

  REQUIRE(   mean(vectorise(N)) == Approx(0.0).margin(0.05) );
  REQUIRE( stddev(vectorise(N)) == Approx(1.0).margin(0.05) );

  cube X(5, 6, 2, fill::none);   // only to test instantiation

  // cube I;  REQUIRE_THROWS( I = cube(5, 6, 2, fill::eye) );
  }


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/SIS.h>
#include <BayesFilters/utils.h>

#include <fstream>
#include <iostream>
#include <utility>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

SIS::SIS
(
    unsigned int num_particle,
    std::size_t state_size_linear,
    std::size_t state_size_circular,
    std::unique_ptr<ParticleSetInitialization> initialization,
    std::unique_ptr<PFPrediction> prediction,
    std::unique_ptr<PFCorrection> correction,
    std::unique_ptr<Resampling> resampling
) noexcept :
    ParticleFilter(std::move(initialization), std::move(prediction), std::move(correction), std::move(resampling)),
    num_particle_(num_particle),
    state_size_(state_size_linear + state_size_circular),
    pred_particle_(num_particle_, state_size_linear, state_size_circular),
    cor_particle_(num_particle_, state_size_linear, state_size_circular)
{ }

SIS::SIS
(
    unsigned int num_particle,
    std::size_t state_size_linear,
    std::unique_ptr<ParticleSetInitialization> initialization,
    std::unique_ptr<PFPrediction> prediction,
    std::unique_ptr<PFCorrection> correction,
    std::unique_ptr<Resampling> resampling
) noexcept :
    SIS(num_particle, state_size_linear, 0, std::move(initialization), std::move(prediction), std::move(correction), std::move(resampling))
{ }

SIS::~SIS() noexcept
{ }

SIS::SIS(SIS&& sir_pf) noexcept :
    ParticleFilter(std::move(sir_pf)),
    pred_particle_(std::move(sir_pf.pred_particle_)),
    cor_particle_(std::move(sir_pf.cor_particle_)),
    num_particle_(sir_pf.num_particle_),
    state_size_(sir_pf.state_size_)
{ }

SIS& SIS::operator=(SIS&& sir_pf) noexcept
{
    ParticleFilter::operator=(std::move(sir_pf));

    num_particle_ = sir_pf.num_particle_;

    state_size_ = sir_pf.state_size_;

    pred_particle_ = std::move(sir_pf.pred_particle_);

    cor_particle_ = std::move(sir_pf.cor_particle_);

    return *this;
}

bool SIS::initialization()
{
    return initialization_->initialize(pred_particle_);
}

void SIS::filteringStep()
{
    if (getFilteringStep() != 0)
        prediction_->predict(cor_particle_, pred_particle_);

    correction_->correct(pred_particle_, cor_particle_);

    /* Normalize weights using LogSumExp. */
    cor_particle_.weight().array() -= utils::log_sum_exp(cor_particle_.weight());

    log();

    if (resampling_->neff(cor_particle_.weight()) < static_cast<double>(num_particle_)/3.0)
    {
        ParticleSet res_particle(num_particle_, state_size_);
        VectorXi res_parent(num_particle_, 1);

        resampling_->resample(cor_particle_, res_particle, res_parent);

        cor_particle_ = res_particle;
    }
}

bool SIS::runCondition()
{
    return true;
}

void SIS::log()
{
    logger(pred_particle_.state().transpose(), pred_particle_.weight().transpose(),
           cor_particle_.state().transpose(), cor_particle_.weight().transpose());
}


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

  @copyright 2015 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_OPERATOR_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-operator Operators

    Those functions provides scalar and SIMD version of the native C and C++ operators.
  **/
} }

#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_not.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/comma.hpp>
#include <boost/simd/function/complement.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_less_equal.hpp>
#include <boost/simd/function/is_not_equal.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_not.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/modulo.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/unary_plus.hpp>

#endif


#include <boost/config.hpp>
#include <iostream>
#include <fstream> //file output
#include <cfloat>
#include <omp.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graph_traits.hpp>

void printUsageAndExit()
{
  printf("%s", "Usage:./rmatg x y\n");
  printf("%s", "x is the size of the graph, x>32 (Boost generator hang if x<32)\n");
  printf("%s", "y is the source of sssp\n");
  exit(0);
}

int main(int argc, char *argv[])
{
  // read size
  if (argc < 3) printUsageAndExit();
  int size = atoi (argv[1]);
  if (size<32) printUsageAndExit();
  int source_sssp =atoi (argv[2]);
  assert (size > 1 && size < INT_MAX);
  assert (source_sssp >= 0 && source_sssp < size);
  const unsigned num_edges = 15 * size;

  // Some boost types
  typedef boost::no_property VertexProperty;
  typedef boost::property<boost::edge_weight_t, float> EdgeProperty;
  typedef boost::adjacency_list<boost::mapS, boost::vecS, boost::directedS, VertexProperty, EdgeProperty> Graph;
  typedef boost::unique_rmat_iterator<boost::minstd_rand, Graph> RMATGen;
  typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  boost::minstd_rand gen;
  boost::graph_traits<Graph>::edge_iterator edge, edge_end;

  /************************
   * Random weights
   ************************/
  // !!! WARNING !!!
  // watch the stack
  float* weight = new float[num_edges];
  int count = 0;
  for( int i = 0; i < num_edges;  ++i)
    weight[i] = (rand()%10)+(rand()%100)*(1.2e-2f);

  /************************
   * RMAT Gen
   ************************/
  Graph g(RMATGen(gen, size, num_edges, 0.57, 0.19, 0.19, 0.05,true),RMATGen(),weight, size);
  std::cout << "Generator : done. Edges = "<<boost::num_edges(g)<<std::endl;
  assert (num_edges == boost::num_edges(g));
  // debug print after gen
  //for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
  //  std::cout << boost::source(*edge, g) << ' ' << boost::target(*edge, g)<< ' '<<  boost::get(boost::get(boost::edge_weight, g),*edge) << '\n';

  /************************
   * Dijkstra
   ************************/
  std::vector<vertex_descriptor> p(num_vertices(g));
  std::vector<float> d(num_vertices(g));
  vertex_descriptor s = vertex(source_sssp, g); //define soruce node

  double start = omp_get_wtime();
  dijkstra_shortest_paths(g, s,
                          predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
                          distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))));

  double stop = omp_get_wtime();
  std::cout << "Time = " << stop-start << "s"<< std::endl;

  /************************
   * Print
   ************************/
  /*
  boost::graph_traits<Graph>::vertex_iterator vi, vend;
  std::cout << "SOURCE = "<< source_sssp << std::endl;
  for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi)
  {
    if (d[*vi] != FLT_MAX)
    {
      std::cout << "d(" << *vi << ") = " << d[*vi] << ", ";
      std::cout << "parent = " << p[*vi] << std::endl;
    }
    else
      std::cout << "d(" << *vi << ") = INF"<< std::endl;
  }
  */
  return 0;

}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [hi]
#include <boost/simd/bitwise.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs = boost::simd;
using iT = std::int32_t;
using pack_it = bs::pack<iT, 4>;

int main() {
  pack_it pi = {1, 2, -1, 32769};

  std::cout << "---- simd" << '\n'
            << "<- pi =           " << pi << '\n'
            << "-> bs::hi(pi) =   " << bs::hi(pi) << '\n';

  iT xi = 2;

  std::cout << "---- scalar" << '\n'
            << "<- xi =           " << xi << '\n'
            << "-> bs::hi(xi) =   " << bs::hi(xi) << '\n';
  return 0;
}
//! [hi]


//////////////////////////////////////////////////////////////////////////////
// monomials_horner::multi_indexes.hpp                                      //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_MONOMIALS_HORNER_MULTI_INDEXES_HPP_ER_2009
#define BOOST_MONOMIALS_HORNER_MULTI_INDEXES_HPP_ER_2009
#include <vector>
#include <functional>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/range.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
#include <boost/assert.hpp>
#include <boost/view/rows_iterator.hpp>
#include <boost/monomials_horner/monomials_properties.hpp>

namespace boost{
namespace monomials_horner{

    // Example of multi indexes for N = 3, degree =2
    // >(0,0,0)
    // >(1,0,0) >(0,1,0) >(0,0,1)
    // >(2,0,0) (1,1,0) (1,0,1) >(0,2,0) (0,1,1) >(0,0,2)
    // Note that
    // {(2,0,0) (1,1,0) (1,0,1)} = {(1,0,0) (0,1,0) (0,0,1)} + (1,0,0)
    // {(0,2,0) (0,1,1)} = {(0,1,0) (0,0,1)} + (0,1,0) etc.

    // N is the dimension of the multivariate monomial
    template<unsigned N>
    class multi_indexes{
      public:
        typedef std::vector<unsigned>                   storage_type;
        typedef typename storage_type::iterator         base_iter_type;
        typedef typename storage_type::size_type        size_type;
        typedef storage_type::value_type                base_value_type;

//        typedef matrix_view::row_iterator<base_iter_type>  iter_type;
        typedef view::detail::rows_iterator<base_iter_type>  iter_type;
        typedef std::vector<iter_type>                  iters_type;
        typedef typename iter_type::value_type          value_type;
        typedef iterator_range<iter_type>               iter_range_type;
        typedef typename iters_type::iterator           iters_iter_type;
        typedef typename iter_type::difference_type     iter_diff_type;
        typedef std::vector<iter_diff_type>             iters_diff_type;
        typedef typename iters_diff_type::iterator      iters_diff_iter_type;

        static iter_range_type get(unsigned int degree){
            static unsigned max_degree = 0;
            static storage_type storage(N,(base_value_type)(0));
            static iters_diff_type read_bs_dists(N,0);

// // keeping read_bs rather than read_bs_dists causes
// // error 'gliblc detected' at runtime
//            static iters_type   read_bs(
//                N,
//                matrix_view::make_row_iterator(
//                    begin(storage),
//                    N
//            ));

            //e.g. N=3: (1,0,0, 0,1,0, 0,0,1)
            static storage_type variables = variables_init();

            if(degree>max_degree){
                unsigned i = max_degree+1;
                unsigned n = degree+1;
                size_type sz_old = size(storage)/N;
                size_type sz_new
                    = properties::number_degree_less_than(degree,N);
                storage.resize(sz_new*N);
//                iter_type write_b = matrix_view::make_row_iterator(
//                    begin(storage),
//                    N
//                );
                iter_type write_b = view::detail::make_rows_iterator(
                    begin(storage),
                    end(storage),
                    N
                );
                std::advance(write_b,sz_old);

                iters_type read_bs;

//old implem, do not uncomment
//                iters_diff_iter_type b = begin(read_bs_dists);
//                iters_diff_iter_type e = end(read_bs_dists);
//                for(iters_diff_iter_type i = b; i<e; i++){
//                    iter_type it(begin(storage),N);
//                    std::advance(it,*i);
//                    read_bs.push_back(it);
//                }

                transform(
                    begin(read_bs_dists),
                    end(read_bs_dists),
                    back_inserter(read_bs),
                    unary_op(
//                        matrix_view::make_row_iterator(
//                            begin(storage),
//                            N
//                        )
                        view::detail::make_rows_iterator(
                            begin(storage),
                            end(storage),
                            N
                        )
                    )
                );

                while(i<n){
                    BOOST_ASSERT(size(read_bs_dists)==N);
                    BOOST_ASSERT(std::distance(
//                        matrix_view::make_row_iterator(
//                            begin(variables),
//                            N
//                        ),
//                        matrix_view::make_end_row_iterator(
//                                begin(variables),
//                                end(variables),
//                                N
//                        )

                        view::detail::make_rows_iterator(
                            begin(variables),
                            end(variables),
                            N
                        ),
                        view::detail::make_end_rows_iterator(
                                begin(variables),
                                end(variables),
                                N
                        )
                    )==N);

                    //(***) bug
                    write_b = for_each(
                        make_zip_iterator(
                            make_tuple(
                                begin(read_bs),
//                                matrix_view::make_row_iterator(
//                                    begin(variables),
//                                    N
//                                )
                                view::detail::make_rows_iterator(
                                    begin(variables),
                                    end(variables),
                                    N
                                )
                            )
                        ),
                        make_zip_iterator(
                            make_tuple(
                                end(read_bs),
//                                matrix_view::make_end_row_iterator(
//                                    begin(variables),
//                                    end(variables),
//                                    N
//                                )
                                view::detail::make_end_rows_iterator(
                                    begin(variables),
                                    end(variables),
                                    N
                                )
                            )
                        ),
                        zip_func(write_b)
                    ).write_b;
                    ++i;
                }
                read_bs_dists.clear();
                std::transform(
                    begin(read_bs),
                    end(read_bs),
                    back_inserter(read_bs_dists),
                    bind(
                        std::distance<iter_type>,
//                        matrix_view::make_row_iterator(
//                            begin(storage),
//                            N
//                        ),
                        view::detail::make_rows_iterator(
                            begin(storage),
                            end(storage),
                            N
                        ),
                        _1
                    )
                );
                max_degree = degree;
            }//if(degree>max_degree)

            size_type dist
                = properties::number_degree_less_than(degree,N);
            iter_type b(begin(storage),end(storage),N);
            iter_type e = b;
            std::advance(e,dist);
            iter_range_type(b,e);
            return iter_range_type(b,e);
        }//get(unsigned degree)

      private:
        typedef monomials_properties<> properties;
        multi_indexes(); //do not implement

        static storage_type variables_init(){
            storage_type variables(N*N,0);
            for(unsigned i=0; i<N; i++){
                variables[i*N+i]=1;
            }
            return variables;
        }

        struct unary_op{
            iter_type b;
            unary_op(iter_type b_):b(b_){}
            unary_op(const unary_op& that):b(that.b){}
            unary_op& operator=(const unary_op& that){
                if(&that!=this){b=that.b;}
                return *this;
            }
            typedef iter_type                   result_type;
            typedef const iter_diff_type&       argument_type;
            result_type operator()(argument_type diff)const{
                result_type it = b;
                std::advance(it,diff);
                return it;
            }
        };

        struct zip_func
        {
            typedef const tuple<
                iter_type&,const value_type&>&                  argument_type;
            typedef void                                        result_type;

            iter_type write_b;
            iter_type read_e;
            unsigned cnt;//TODO remove

            zip_func(iter_type write_b_)
            :write_b(write_b_),read_e(write_b),cnt(0){}

            zip_func(const zip_func& that)
            :write_b(that.write_b),
            read_e(that.read_e),
            cnt(that.cnt){}

            zip_func& operator=(const zip_func& that){
                if(&that!=this){
                    write_b = that.write_b;
                    read_e = that.read_e;
                    cnt = that.cnt;
                }
                return *this;
            }

            result_type operator()(argument_type t){
                value_type variable_ir = (t.template get<1>());
                iter_type read_b = (t.template get<0>());
                t.template get<0>() = write_b;
                write_b =
                    for_each(
                        read_b,
                        read_e,
                        unary_op(write_b,variable_ir)
                    ).write_b;
            }

            struct unary_op{
                unary_op(iter_type write_b_,value_type variable_ir)
                :write_b(write_b_),variable(variable_ir){}

                unary_op(const unary_op& that)
                :write_b(that.write_b),variable(that.variable){}

                unary_op& operator=(const unary_op& that)
                {
                    if(&that!=this){
                        write_b = that.write_b;
                        variable = that.variable;
                    }
                    return *this;
                }

                typedef typename iter_type::value_type     argument_type;
                typedef void                               result_type;
                result_type operator()(argument_type ir){

                    BOOST_ASSERT(size(*write_b)==size(ir));
                    BOOST_ASSERT(size(ir)==size(variable));
                    std::transform(
                        begin(ir),
                        end(ir),
                        begin(variable),
                        begin(*write_b),
                        bind(
                            std::plus<base_value_type>(),
                            _1,
                            _2
                        )
                    );
                    ++write_b;
                }
                iter_type   write_b;
                value_type  variable;
            };//unary_op
        };//zip_func
    };//multi_indexes

}// monomials_horner
}// boost

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2010 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "sparse.h"
#include <Eigen/SparseExtra>

#ifdef EIGEN_TAUCS_SUPPORT
#include <Eigen/TaucsSupport>
#endif

template<typename Scalar> void sparse_ldlt(int rows, int cols)
{
  double density = std::max(8./(rows*cols), 0.01);
  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
  typedef Matrix<Scalar,Dynamic,1> DenseVector;

  SparseMatrix<Scalar> m2(rows, cols);
  DenseMatrix refMat2(rows, cols);

  DenseVector b = DenseVector::Random(cols);
  DenseVector refX(cols), x(cols);

  initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, 0, 0);
  for(int i=0; i<rows; ++i)
    m2.coeffRef(i,i) = refMat2(i,i) = ei_abs(ei_real(refMat2(i,i)));

  refX = refMat2.template selfadjointView<Upper>().ldlt().solve(b);
  typedef SparseMatrix<Scalar,Upper|SelfAdjoint> SparseSelfAdjointMatrix;
  x = b;
  SparseLDLT<SparseSelfAdjointMatrix> ldlt(m2);
  if (ldlt.succeeded())
    ldlt.solveInPlace(x);
  else
    std::cerr << "warning LDLT failed\n";

  VERIFY_IS_APPROX(refMat2.template selfadjointView<Upper>() * x, b);
  VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: default");
}

void test_sparse_ldlt()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(sparse_ldlt<double>(8, 8) );
    int s = ei_random<int>(1,300);
    CALL_SUBTEST_2(sparse_ldlt<std::complex<double> >(s,s) );
    CALL_SUBTEST_1(sparse_ldlt<double>(s,s) );
  }
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;
typedef MT::value_traits       VT;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(make_union_test)
{

  V                  const min_coord1 = V::make( -VT::two(), -VT::two(), -VT::two() );
  V                  const max_coord1 = V::make( -VT::one(), -VT::one(), -VT::one() );
  geometry::AABB<V>  const aabb1      = geometry::make_aabb(min_coord1, max_coord1);
  geometry::DOP<T,6> const left       = geometry::convert<6,V>(aabb1);

  V                  const min_coord2 = V::make(  VT::one(),  VT::one(),  VT::one() );
  V                  const max_coord2 = V::make(  VT::two(),  VT::two(),  VT::two() );
  geometry::AABB<V>  const aabb2      = geometry::make_aabb(min_coord2, max_coord2);
  geometry::DOP<T,6> const right       = geometry::convert<6,V>(aabb2);

  geometry::DOP<T,6> const all = geometry::make_union(left, right);

  BOOST_CHECK( geometry::is_valid(all) );

  BOOST_CHECK_EQUAL(all.size(), 6u);

  BOOST_CHECK_EQUAL(  all(0).lower(), -2.0 );
  BOOST_CHECK_EQUAL(  all(1).lower(), -2.0 );
  BOOST_CHECK_EQUAL(  all(2).lower(), -2.0 );
  BOOST_CHECK_EQUAL(  all(0).upper(),  2.0 );
  BOOST_CHECK_EQUAL(  all(1).upper(),  2.0 );
  BOOST_CHECK_EQUAL(  all(2).upper(),  2.0 );

}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Open Source Movement Analysis Library
 * Copyright (C) 2016, Moveck Solution Inc., all rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer.
 *     * Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials
 *       provided with the distribution.
 *     * Neither the name(s) of the copyright holders nor the names
 *       of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written
 *       permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "openma/body/unitquaternionposeestimator.h"

#include "openma/body/landmarksregistrar.h"
#include "openma/body/landmarkstranslator.h"
#include "openma/body/model.h"
#include "openma/body/point.h"
#include "openma/body/referenceframe.h"
#include "openma/body/segment.h"
#include "openma/body/skeletonhelper.h"
#include "openma/body/utils.h"
#include "openma/base/trial.h"
#include "openma/base/logger.h"
#include "openma/math.h"

#include <Eigen/Eigenvalues> // Eigen::SelfAdjointEigenSolver

#include <iostream>

// -------------------------------------------------------------------------- //
//                                 PUBLIC API                                 //
// -------------------------------------------------------------------------- //

OPENMA_INSTANCE_STATIC_TYPEID(ma::body::UnitQuaternionPoseEstimator);

namespace ma
{
namespace body
{
  /**
   * @class UnitQuaternionPoseEstimator openma/body/unitquaternionposeestimator.h
   * @brief Least square pose estimator based on unit quaternions
   *
   * This estimator is based on the paper of Horn [1] that use unit quaternions to find the rigid body transformation betwen two group of markers.
   *
   * @par References
   *  1. Horn B.K.P., Closed-form solution of absolute orientation using unit quaternions, Journal of the Optical Society of America, 1987, 4
   *
   * @ingroup openma_body
   */

  /**
   * Constructor
   */
  UnitQuaternionPoseEstimator::UnitQuaternionPoseEstimator(const std::string& name, Node* parent)
  : PoseEstimator(name,parent)
  {};

  /**
   * Destructor (default)
   */
  UnitQuaternionPoseEstimator::~UnitQuaternionPoseEstimator() _OPENMA_NOEXCEPT = default;

  /**
   * Internaly call the method UnitQuaternion::reconstruct().
   */
  bool UnitQuaternionPoseEstimator::run(Model* output, SkeletonHelper* helper, Trial* trial)
  {
    // 0. Check
    if (output == nullptr)
    {
      error("UnitQuaternionPoseEstimator - Null output passed. Pose estimator aborted.");
      return false;
    }
    if (helper == nullptr)
    {
      error("UnitQuaternionPoseEstimator - Null helper passed. Pose estimator aborted.");
      return false;
    }
    if (trial == nullptr)
    {
      error("UnitQuaternionPoseEstimator - Null trial passed. Pose estimator aborted.");
      return false;
    }
    // 1. Look for the child node MarkerClusterRegistration
    auto mcr = helper->findChild("MarkerClusterRegistration",{},false);
    if (mcr == nullptr)
    {
      error("UnitQuaternionPoseEstimator - No marker cluster registration found. Pose estimator aborted.");
      return false;
    }
    const auto& lt = helper->findChild<LandmarksTranslator*>({},{},false);
    const auto& segments = output->segments()->findChildren<Segment*>({},{},false);
    double startTime = 0.0, sampleRate = 0.0;
    bool ok = false;
    for (auto segment : segments)
    {
      const auto& lr = segment->findChild<LandmarksRegistrar*>({},{},false);
      if (lr == nullptr)
        continue;
      // Look for the markers in the trial and the marker cluster registration
      auto globalMarkers = extract_landmark_positions(nullptr, lr->retrieveLandmarks(lt, trial->timeSequences()), &sampleRate, &startTime, &ok);
      if (!ok)
      {
        error("UnitQuaternionPoseEstimator - The sampling information is not consistent between required landmarks (sampling rates or start times are not the same). Calibration aborted.");
        return false;
      }
      std::vector<std::pair<Eigen::Map<Eigen::Matrix<double,3,1>>,const math::Map<math::Position>&>> mappedMarkers;
      mappedMarkers.reserve(globalMarkers.size());
      auto it = globalMarkers.begin();
      while (it != globalMarkers.end())
      {
        Point* localMarker = nullptr;
        if (!it->second.isValid() || ((localMarker = mcr->findChild<Point*>(segment->name()+"."+it->first,{},false)) == nullptr))
          it = globalMarkers.erase(it);
        else
        {
          mappedMarkers.push_back({localMarker->data(),it->second});
          ++it;
        }
      }
      assert(globalMarkers.size() == mappedMarkers.size());
      if (mappedMarkers.size() < 3)
      {
        error("Less than 3 valid markers was found for the segment '%s'. Impossible to compute the TCS. Pose estimator aborted.", segment->name().c_str());
        return false;
      }
      // Reconstruct for each sample
      unsigned numSamples = std::numeric_limits<unsigned>::max();
      for (const auto& marker : globalMarkers)
      {
        numSamples = std::min<unsigned>(numSamples, marker.second.rows());
        if (marker.second.rows() != numSamples)
        {
          error("The number of samples for the markers used by the cluster '%s.Cluster' is not the same. Impossible to compute the TCS. Pose estimator aborted.", segment->name().c_str());
          return false;
        }
      }
      ma::math::Pose tcs(numSamples);
      Eigen::Matrix<double,3,3> M;
      Eigen::Matrix<double,4,4> N;
      Eigen::Matrix<double,3,Eigen::Dynamic> ps1, ps2;
      for (unsigned i = 0 ; i < numSamples ; ++i)
      {
        int inc = 0;
        ps1.setZero(3,static_cast<int>(globalMarkers.size()));
        ps2.setZero(3,static_cast<int>(globalMarkers.size()));
        for (const auto& m : mappedMarkers)
        {
          if (m.second.residuals().coeff(i) >= 0.0)
          {
            ps1.col(inc) = m.first; // Local
            ps2.col(inc) = m.second.values().row(i); // Global
            ++inc;
          }
        }
        if (inc < 3) // Not enough landmark to create the least square fitting.
        {
          tcs.residuals().coeffRef(i) = -1.0;
          continue;
        }
        ps1.resize(Eigen::NoChange,inc);
        ps2.resize(Eigen::NoChange,inc);
        // Express the point sets regarding to their respective center
        Eigen::Matrix<double,3,1> p1 = (ps1.rowwise().sum() / static_cast<double>(inc));
        ps1 -= p1.replicate(1,inc);
        // ps1.row(0).array() -= p1.x();
        // ps1.row(1).array() -= p1.y();
        // ps1.row(2).array() -= p1.z();
        Eigen::Matrix<double,3,1> p2 = ps2.rowwise().sum() / static_cast<double>(inc);
        ps2 -= p2.replicate(1,inc);
        // ps2.row(0).array() -= p2.x();
        // ps2.row(1).array() -= p2.y();
        // ps2.row(2).array() -= p2.z();
        // Build the matrice N
        M.setZero();
        for (int j = 0 ; j < inc ; ++j)
          M += ps1.col(j) * ps2.col(j).transpose();
        N.setZero();
        N.coeffRef(0,1) = N.coeffRef(1,0) = M.coeff(1,2) - M.coeff(2,1);
        N.coeffRef(0,2) = N.coeffRef(2,0) = M.coeff(2,0) - M.coeff(0,2);
        N.coeffRef(0,3) = N.coeffRef(3,0) = M.coeff(0,1) - M.coeff(1,0);
        N.coeffRef(1,2) = N.coeffRef(2,1) = M.coeff(0,1) + M.coeff(1,0);
        N.coeffRef(1,3) = N.coeffRef(3,1) = M.coeff(2,0) + M.coeff(0,2);
        N.coeffRef(2,3) = N.coeffRef(3,2) = M.coeff(1,2) + M.coeff(2,1);
        N.coeffRef(0,0) =  M.coeff(0,0) + M.coeff(1,1) + M.coeff(2,2);
        N.coeffRef(1,1) =  M.coeff(0,0) - M.coeff(1,1) - M.coeff(2,2);
        N.coeffRef(2,2) = -M.coeff(0,0) + M.coeff(1,1) - M.coeff(2,2);
        N.coeffRef(3,3) = -M.coeff(0,0) - M.coeff(1,1) + M.coeff(2,2);
        // Extract the eigen vector associated with the most positive eigen value and compute the rotation matrix (the extracted eigen vector is a quaternion)
        Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double,4,4> > eig(N);
        int idx; eig.eigenvalues().maxCoeff(&idx);
        // Q2R need to be done in 2 steps as the eigen vector is formatted as WXYZ and the internal storage in Eigen is XYZW
        Eigen::Matrix<double,4,1> q = eig.eigenvectors().col(idx);
        Eigen::Matrix<double,3,3> R = Eigen::Quaternion<double>(q.coeff(0),q.coeff(1),q.coeff(2),q.coeff(3)).toRotationMatrix();
        // Set the pose
        auto row = tcs.values().row(i);
        row.segment<3>(0) = R.col(0);    // u
        row.segment<3>(3) = R.col(1);    // v
        row.segment<3>(6) = R.col(2);    // w
        row.segment<3>(9) = p2 - R * p1; // o
        tcs.residuals().coeffRef(i) = 0.0;
      }
      // Reconstruction of the SCS
      // Look for Reference frame in the node MarkerClusterRegistration
      auto relframe = mcr->findChild<ReferenceFrame*>(segment->name() + ".SCS", {}, false);
      if (relframe != nullptr)
      {
        relframe->addParent(segment);
        math::to_timesequence(transform_relative_frame(relframe, segment, tcs), segment->name() + ".SCS", sampleRate, startTime, TimeSequence::Pose, "", segment);
      }
      math::to_timesequence(tcs, segment->name() + ".TCS", sampleRate, startTime, TimeSequence::Pose, "", segment);
    }
    return true;
  }
};
};


#ifndef STAN_MATH_PRIM_SCAL_FUN_LGAMMA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LGAMMA_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace stan {
namespace math {

/**
 * Return the natural logarithm of the gamma function applied to
 * the specified argument.
 *
   \f[
   \mbox{lgamma}(x) =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \ln\Gamma(x) & \mbox{if } x\not\in \{\dots, -3, -2, -1, 0\}\\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{lgamma}(x)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \Psi(x) & \mbox{if } x\not\in \{\dots, -3, -2, -1, 0\}\\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
\f]
*
* @param x argument
* @return natural logarithm of the gamma function applied to
* argument
*/
inline double lgamma(double x) {
  return boost::math::lgamma(x, boost_policy_t());
}

/**
 * Return the natural logarithm of the gamma function applied
 * to the specified argument.
 *
 * @param x argument
 * @return natural logarithm of the gamma function applied to
 * argument
 */
inline double lgamma(int x) { return boost::math::lgamma(x, boost_policy_t()); }

}  // namespace math
}  // namespace stan
#endif


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <math/prim/mat/prob/VectorIntRNGTestRig.hpp>
#include <limits>
#include <vector>

class NegativeBinomial2LogTestRig : public VectorIntRNGTestRig {
 public:
  NegativeBinomial2LogTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6},
                            {-0.5, 0.0, 0.1, 1.7}, {-2, 0, 1, 2}, {}, {},
                            {0.1, 1.1, 4.99}, {1, 2, 3}, {-3.0, -2.0, 0.0},
                            {-3, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& eta, const T2& phi, const T3&,
                        T_rng& rng) const {
    return stan::math::neg_binomial_2_log_rng(eta, phi, rng);
  }

  template <typename T1>
  double pmf(int y, T1 eta, double phi, double) const {
    return std::exp(stan::math::neg_binomial_2_log_lpmf(y, eta, phi));
  }
};

TEST(ProbDistributionsNegativeBinomial2Log, errorCheck) {
  check_dist_throws_all_types(NegativeBinomial2LogTestRig());
}

TEST(ProbDistributionsNegativeBinomial2Log, distributionCheck) {
  check_counts_real_real(NegativeBinomial2LogTestRig());
}