// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the epsilon-insensitive support vector
    regression object from the dlib C++ Library.

    In this example we will draw some points from the sinc() function and do a
    non-linear regression on them.
*/

#include <iostream>
#include <vector>

#include <dlib/svm.h>

using namespace std;
using namespace dlib;

// Here is the sinc function we will be trying to learn with the svr_trainer
// object.
double sinc(double x)
{
    if (x == 0)
        return 1;
    return sin(x)/x;
}

int main()
{
    // Here we declare that our samples will be 1 dimensional column vectors.
    typedef matrix<double,1,1> sample_type;

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    std::vector<sample_type> samples;
    std::vector<double> targets;

    // The first thing we do is pick a few training points from the sinc() function.
    sample_type m;
    for (double x = -10; x <= 4; x += 1)
    {
        m(0) = x;

        samples.push_back(m);
        targets.push_back(sinc(x));
    }

    // Now setup a SVR trainer object.  It has three parameters, the kernel and
    // two parameters specific to SVR.
    svr_trainer<kernel_type> trainer;
    trainer.set_kernel(kernel_type(0.1));

    // This parameter is the usual regularization parameter.  It determines the trade-off
    // between trying to reduce the training error or allowing more errors but hopefully
    // improving the generalization of the resulting function.  Larger values encourage exact
    // fitting while smaller values of C may encourage better generalization.
    trainer.set_c(10);

    // Epsilon-insensitive regression means we do regression but stop trying to fit a data
    // point once it is "close enough" to its target value.  This parameter is the value that
    // controls what we mean by "close enough".  In this case, I'm saying I'm happy if the
    // resulting regression function gets within 0.001 of the target value.
    trainer.set_epsilon_insensitivity(0.001);

    // Now do the training and save the results
    decision_function<kernel_type> df = trainer.train(samples, targets);

    // now we output the value of the sinc function for a few test points as well as the
    // value predicted by SVR.
    m(0) = 2.5; cout << sinc(m(0)) << "   " << df(m) << endl;
    m(0) = 0.1; cout << sinc(m(0)) << "   " << df(m) << endl;
    m(0) = -4;  cout << sinc(m(0)) << "   " << df(m) << endl;
    m(0) = 5.0; cout << sinc(m(0)) << "   " << df(m) << endl;

    // The output is as follows:
    //  0.239389   0.23905
    //  0.998334   0.997331
    // -0.189201   -0.187636
    // -0.191785   -0.218924

    // The first column is the true value of the sinc function and the second
    // column is the output from the SVR estimate.

    // We can also do 5-fold cross-validation and find the mean squared error and R-squared
    // values.  Note that we need to randomly shuffle the samples first.  See the svm_ex.cpp
    // for a discussion of why this is important.
    randomize_samples(samples, targets);
    cout << "MSE and R-Squared: "<< cross_validate_regression_trainer(trainer, samples, targets, 5) << endl;
    // The output is:
    // MSE and R-Squared: 1.65984e-05    0.999901
}


/*
   Copyright (c) Marshall Clow 2014.

   Distributed under the Boost Software License, Version 1.0. (See accompanying
   file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

    For more information, see http://www.boost.org
*/

#include <iostream>
#include <functional>

#include <boost/config.hpp>
#include <boost/algorithm/algorithm.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

namespace ba = boost::algorithm;

void test_power ()
{
    BOOST_CHECK ( ba::power(0, 0) == 1);
    BOOST_CHECK ( ba::power(5, 0) == 1);
    BOOST_CHECK ( ba::power(1, 1) == 1);
    BOOST_CHECK ( ba::power(1, 4) == 1);
    BOOST_CHECK ( ba::power(3, 2) == 9);
    BOOST_CHECK ( ba::power(2, 3) == 8);
    BOOST_CHECK ( ba::power(3, 3) == 27);
    BOOST_CHECK ( ba::power(2, 30) == 0x40000000);
    BOOST_CHECK ( ba::power(5L, 10) == 3125*3125);
    BOOST_CHECK ( ba::power(18, 3) == 18*18*18);

    BOOST_CHECK ( ba::power(3,2) == ba::power(3,2, std::multiplies<int>()));
    BOOST_CHECK ( ba::power(3,2, std::plus<int>()) == 6);
}

void test_power_constexpr ()
{
    BOOST_CXX14_CONSTEXPR bool check_zero_power1 =
        ba::power(0, 0) == 1;
    BOOST_CHECK(check_zero_power1);
    BOOST_CXX14_CONSTEXPR bool check_zero_power2 =
        ba::power(5, 0) == 1;
    BOOST_CHECK(check_zero_power2);
    BOOST_CXX14_CONSTEXPR bool check_one_base1 =
        ba::power(1, 1) == 1;
    BOOST_CHECK(check_one_base1);
    BOOST_CXX14_CONSTEXPR bool check_one_base2 =
        ba::power(1, 4) == 1;
    BOOST_CHECK(check_one_base2);
    BOOST_CXX14_CONSTEXPR bool check_power1 =
        ba::power(3, 2) == 9;
    BOOST_CHECK(check_power1);
    BOOST_CXX14_CONSTEXPR bool check_power2 =
        ba::power(2, 3) == 8;
    BOOST_CHECK(check_power2);
    BOOST_CXX14_CONSTEXPR bool check_power3 =
        ba::power(3, 3) == 27;
    BOOST_CHECK(check_power3);
    BOOST_CXX14_CONSTEXPR bool check_power4 =
        ba::power(2, 30) == 0x40000000;
    BOOST_CHECK(check_power4);
    BOOST_CXX14_CONSTEXPR bool check_power5 =
        ba::power(5L, 10) == 3125*3125;
    BOOST_CHECK(check_power5);
    BOOST_CXX14_CONSTEXPR bool check_power6 =
        ba::power(18, 3) == 18*18*18;
    BOOST_CHECK(check_power6);

    BOOST_CXX14_CONSTEXPR bool check_multiple =
        ba::power(3, 2, std::multiplies<int>()) == ba::power(3, 2);
    BOOST_CHECK(check_multiple);
    BOOST_CXX14_CONSTEXPR bool check_plus =
        ba::power(3, 2, std::plus<int>()) == 6;
    BOOST_CHECK(check_plus);
}

BOOST_AUTO_TEST_CASE( test_main ) {
  test_power ();
  test_power_constexpr ();
}


//
// Created by Federico Vaccaro on 30/11/2018.
#include <iostream>
#include <string>
#include <random>
#include <vector>
#include <fstream>
#include <algorithm>
#include <iterator>

#include <sstream>
#include <cmath>
#include <iomanip>
#include <boost/algorithm/string.hpp>
#include <stdlib.h>

#include "csvio.h"

int main() {
    const int nData = 183*324;  // number of experiments

    std::default_random_engine generator;

    std::vector<std::normal_distribution<float>> centroids;

    std::normal_distribution<float> X0(3.0, 0.2);
    std::normal_distribution<float> Y0(3.0, 0.5);

    std::normal_distribution<float> X1(15.0, 1.0);
    std::normal_distribution<float> Y1(17.0, 0.2);

    std::normal_distribution<float> X2(10.5, 1.5);
    std::normal_distribution<float> Y2(0.0, 1.5);

    centroids.push_back(X0);
    centroids.push_back(Y0);
    centroids.push_back(X1);
    centroids.push_back(Y1);
    centroids.push_back(X2);
    centroids.push_back(Y2);

    std::vector<float> points;

    const int nCentroids = centroids.size()/2;

    points.resize(nData * 2);

    for (int i = 0; i < nData; ++i) {
        int centroid = i % nCentroids;
        std::normal_distribution<float> X_i = centroids[centroid*2];
        std::normal_distribution<float> Y_i = centroids[centroid*2 + 1];
        int idx = i * 2;
        points[idx] = X_i(generator);
        points[idx+1] = Y_i(generator);

    }
    std::string delimiter = ";";
    std::string filename = "../dataset.csv";
    write2VecTo(filename.c_str(), delimiter, points);
    //read2VecFrom(filename.c_str(), delimiter, points);

    return 0;
}


/*
compare the distribution from sampling result
with the original probabilistic distribution
by using Earth Mover's Distance(EMD).

Given two probability density function a and b, the EMD is calculated as follows:

  EMD(a, b) := integral_{x=-inf}^{+inf} { | A(x) - B(x) | }
where A and B is cumulative distribution function of a and b.

*/

#include <map>
#include <string>
#include <iostream>
#include <iomanip>
#include <cmath>
#include <numeric>
#include <algorithm>
#include <tuple>
#include <random>

#include <EigenRand/EigenRand>

template<typename ArrTy, typename PdfFn>
double calc_emd_with_pdf(ArrTy&& arr, const std::tuple<PdfFn, double, double>& cdf_range, size_t step)
{
	double ret = 0;
	auto arr_begin = arr.data();
	auto arr_end = arr_begin + arr.size();
	std::sort(arr_begin, arr_end);
	const auto& pdf = std::get<0>(cdf_range);
	const auto& lb = std::get<1>(cdf_range), &rb = std::get<2>(cdf_range);

	Eigen::ArrayXd cdf{ step };
	double acc = 0;
	for (size_t i = 0; i < step; ++i)
	{
		const double x = lb + (rb - lb) * i / step;
		cdf[i] = acc;
		acc += pdf(x);
	}
	cdf /= acc;

	size_t arr_p = 0;
	for (size_t i = 0; i < step; ++i)
	{
		const double x = lb + (rb - lb) * i / step;
		arr_p = std::find_if(arr_begin + arr_p, arr_end, [&](double a) { return a > x; }) - arr_begin;
		double arr_cum = (double)arr_p / arr.size();
		ret += std::abs(cdf[i] - arr_cum);
	}
	return ret * (rb - lb) / step;
}

template<typename ArrTy, typename CdfFn>
double calc_emd_with_cdf(ArrTy&& arr, const std::tuple<CdfFn, double, double>& cdf_range, size_t step)
{
	double ret = 0;
	auto arr_begin = arr.data();
	auto arr_end = arr_begin + arr.size();
	std::sort(arr_begin, arr_end);
	const auto& cdf = std::get<0>(cdf_range);
	const auto& lb = std::get<1>(cdf_range), &rb = std::get<2>(cdf_range);

	size_t arr_p = 0;
	for (size_t i = 0; i < step; ++i)
	{
		const double x = lb + (rb - lb) * i / step;
		arr_p = std::find_if(arr_begin + arr_p, arr_end, [&](double a) { return a > x; }) - arr_begin;
		double arr_cum = (double)arr_p / arr.size();
		ret += std::abs(cdf(x) - arr_cum);
	}
	return ret * (rb - lb) / step;
}

template<typename ArrTy, typename PmfFn>
double calc_kldiv(ArrTy&& arr, const std::tuple<PmfFn, int, int>& pmf_range)
{
	double ret = 0;
	const auto& pmf = std::get<0>(pmf_range);
	const auto& b = std::get<1>(pmf_range), e = std::get<2>(pmf_range);
	Eigen::ArrayXd p{ e - b + 1 }, q{ e - b + 1};
	p.setZero();
	for (int i = 0; i < arr.size(); ++i)
	{
		if (arr[i] < b || arr[i] > e) continue;
		p[arr[i] - b] += 1;
	}
	p /= arr.size();

	for (int i = b; i <= e; ++i) q[i - b] = pmf(i);
	q /= q.sum();

	return (((p / q) + 1e-12).log() * p).sum();
}

double gamma_dist(double x, double alpha, double beta)
{
	if (x <= 0) return 0;
	return std::exp(-x / beta)
		/ (std::tgamma(alpha) * std::pow(beta, alpha))
		* std::pow(x, alpha - 1);
}

double chisquared_dist(double x, double n)
{
	if (x <= 0) return 0;
	return std::pow(x, n / 2 - 1) * std::exp(-x / 2)
		/ std::pow(2, n / 2) / std::tgamma(n / 2);
}

double student_t_dist(double x, double n)
{
	return std::pow(1 + x * x /n, -(n + 1) / 2);
}

double fisher_f_dist(double x, double m, double n)
{
	if (x <= 0) return 0;
	return std::sqrt((std::pow(m * x, m) * std::pow(n, n)) / std::pow(m * x + n, m + n)) / x;
}

double combination(int n, int c)
{
	if (n == 0 || n == 1 || c == 0 || c == n) return 1;
	c = std::min(c, n - c);

	double ret = 1;
	for (int i = 0; i < c; ++i)
	{
		ret *= (n - i);
		ret /= i + 1;
	}
	return ret;
}

double binomial_dist(int x, int n, double p)
{
	return combination(n, x) * std::pow(p, x) * std::pow(1 - p, n - x);
}

double negative_binomial_dist(int x, int n, double p)
{
	return combination(n + x - 1, x) * std::pow(1 - p, x);
}

auto balanced_cdf = std::make_tuple([](double x) { return (x + 1) / 2; }, -1., 1.);
auto ur_cdf = std::make_tuple([](double x) { return x; }, 0., 1.);
auto normal_cdf = std::make_tuple([](double x) { return (1 + std::erf(x / std::sqrt(2))) / 2; }, -8., 8.);
auto lognormal_cdf = std::make_tuple([](double x) { return (1 + std::erf(std::log(x) / std::sqrt(2))) / 2; }, 0., 4.);
auto exp_cdf = std::make_tuple([](double x) { return 1 - std::exp(-x); }, 0., 32.);
auto weibull_cdf = std::make_tuple([](double x) { return 1 - std::exp(-std::pow(x, 2)); }, 0., 32.);
auto extreme_value_cdf = std::make_tuple([](double x) { return std::exp(-std::exp(1 - x)); }, -8., 24.);
auto gamma11_pdf = std::make_tuple([](double x) { return gamma_dist(x, 1, 1); }, 0., 16.);
auto gamma51_pdf = std::make_tuple([](double x) { return gamma_dist(x, 5, 1); }, 0., 16.);
auto gamma21_pdf = std::make_tuple([](double x) { return gamma_dist(x, 0.2, 1); }, 0., 16.);
auto chisquared_pdf = std::make_tuple([](double x) { return chisquared_dist(x, 7); }, 0., 64.);
auto cauchy_cdf = std::make_tuple([](double x) { return std::atan(x) / Eigen::Rand::constant::pi + 0.5; }, -16., 16.);
auto student5_pdf = std::make_tuple([](double x) { return student_t_dist(x, 5); }, -8., 8.);
auto student20_pdf = std::make_tuple([](double x) { return student_t_dist(x, 20); }, -8., 8.);
auto fisher11_cdf = std::make_tuple([](double x) { return 2 * std::atan(std::sqrt(x)) / Eigen::Rand::constant::pi; }, 0., 256.);
auto fisher55_pdf = std::make_tuple([](double x) { return fisher_f_dist(x, 5, 5); }, 0., 16.);

auto uniform10_pmf = std::make_tuple([](int x) { return 1; }, 0, 9);
auto discrete_pmf = std::make_tuple([](int x) { return x + 1; }, 0, 5);
auto poisson1_pmf = std::make_tuple([](int x) { return std::pow(1, x) / std::tgamma(x + 1); }, 0, 127);
auto poisson16_pmf = std::make_tuple([](int x) { return std::pow(16, x) / std::tgamma(x + 1); }, 0, 127);
auto binomial1_pmf = std::make_tuple([](int x) { return binomial_dist(x, 10, 0.5); }, 0, 10);
auto binomial2_pmf = std::make_tuple([](int x) { return binomial_dist(x, 30, 0.75); }, 0, 30);
auto binomial3_pmf = std::make_tuple([](int x) { return binomial_dist(x, 50, 0.25); }, 0, 50);
auto negbinomial1_pmf = std::make_tuple([](int x) { return negative_binomial_dist(x, 10, 0.5); }, 0, 127);
auto negbinomial2_pmf = std::make_tuple([](int x) { return negative_binomial_dist(x, 20, 0.25); }, 0, 127);
auto negbinomial3_pmf = std::make_tuple([](int x) { return negative_binomial_dist(x, 30, 0.75); }, 0, 127);
auto geometric25_pmf = std::make_tuple([](int x) { return std::pow(1 - 0.25, x); }, 0, 127);
auto geometric75_pmf = std::make_tuple([](int x) { return std::pow(1 - 0.75, x); }, 0, 127);

template<typename Rng>
std::map<std::string, double> test_eigenrand_cont(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXf arr{ size };
	Rng urng{ seed };

	arr = Eigen::Rand::balancedLike(arr, urng);
	ret["balanced"] = calc_emd_with_cdf(arr, balanced_cdf, step);

	arr = Eigen::Rand::uniformRealLike(arr, urng);
	ret["uniformReal"] = calc_emd_with_cdf(arr, ur_cdf, step);

	arr = Eigen::Rand::normalLike(arr, urng);
	ret["normal"] = calc_emd_with_cdf(arr, normal_cdf, step);

	arr = Eigen::Rand::lognormalLike(arr, urng);
	ret["lognormal"] = calc_emd_with_cdf(arr, lognormal_cdf, step);

	arr = Eigen::Rand::gammaLike(arr, urng, 1, 1);
	ret["gamma(1,1)"] = calc_emd_with_pdf(arr, gamma11_pdf, step);

	arr = Eigen::Rand::gammaLike(arr, urng, 5, 1);
	ret["gamma(5,1)"] = calc_emd_with_pdf(arr, gamma51_pdf, step);

	arr = Eigen::Rand::gammaLike(arr, urng, 0.2, 1);
	ret["gamma(0.2,1)"] = calc_emd_with_pdf(arr, gamma21_pdf, step);

	arr = Eigen::Rand::exponentialLike(arr, urng);
	ret["exponential"] = calc_emd_with_cdf(arr, exp_cdf, step);

	arr = Eigen::Rand::weibullLike(arr, urng, 2);
	ret["weibull(2,1)"] = calc_emd_with_cdf(arr, weibull_cdf, step);

	arr = Eigen::Rand::extremeValueLike(arr, urng, 1, 1);
	ret["extremeValue(1,1)"] = calc_emd_with_cdf(arr, extreme_value_cdf, step);

	arr = Eigen::Rand::chiSquaredLike(arr, urng, 7);
	ret["chiSquared(7)"] = calc_emd_with_pdf(arr, chisquared_pdf, step);

	arr = Eigen::Rand::cauchyLike(arr, urng);
	ret["cauchy"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	arr = Eigen::Rand::studentTLike(arr, urng, 1);
	ret["studentT(1)"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	arr = Eigen::Rand::studentTLike(arr, urng, 5);
	ret["studentT(5)"] = calc_emd_with_pdf(arr, student5_pdf, step);

	arr = Eigen::Rand::studentTLike(arr, urng, 20);
	ret["studentT(20)"] = calc_emd_with_pdf(arr, student20_pdf, step);

	arr = Eigen::Rand::fisherFLike(arr, urng, 1, 1);
	ret["fisherF(1,1)"] = calc_emd_with_cdf(arr, fisher11_cdf, step);

	arr = Eigen::Rand::fisherFLike(arr, urng, 5, 5);
	ret["fisherF(5,5)"] = calc_emd_with_pdf(arr, fisher55_pdf, step);

#ifdef TEST_DOUBLE
	Eigen::ArrayXd arrd{ size };
	arrd = Eigen::Rand::uniformRealLike(arrd, urng);
	ret["uniformReal/double"] = calc_emd_with_cdf(arrd, ur_cdf, step);

	arrd = Eigen::Rand::normalLike(arrd, urng);
	ret["normal/double"] = calc_emd_with_cdf(arrd, normal_cdf, step);

	arrd = Eigen::Rand::lognormalLike(arrd, urng);
	ret["lognormal/double"] = calc_emd_with_cdf(arrd, lognormal_cdf, step);

	arrd = Eigen::Rand::gammaLike(arrd, urng, 1, 1);
	ret["gamma(1,1)/double"] = calc_emd_with_pdf(arrd, gamma11_pdf, step);

	arrd = Eigen::Rand::gammaLike(arrd, urng, 5, 1);
	ret["gamma(5,1)/double"] = calc_emd_with_pdf(arrd, gamma51_pdf, step);

	arrd = Eigen::Rand::gammaLike(arrd, urng, 0.2, 1);
	ret["gamma(0.2,1)/double"] = calc_emd_with_pdf(arrd, gamma21_pdf, step);

	arrd = Eigen::Rand::exponentialLike(arrd, urng);
	ret["exponential/double"] = calc_emd_with_cdf(arrd, exp_cdf, step);

	arrd = Eigen::Rand::weibullLike(arrd, urng, 2);
	ret["weibull(2,1)/double"] = calc_emd_with_cdf(arrd, weibull_cdf, step);

	arrd = Eigen::Rand::extremeValueLike(arrd, urng, 1, 1);
	ret["extremeValue(1,1)/double"] = calc_emd_with_cdf(arrd, extreme_value_cdf, step);

	arrd = Eigen::Rand::chiSquaredLike(arrd, urng, 7);
	ret["chiSquared(7)/double"] = calc_emd_with_pdf(arrd, chisquared_pdf, step);

	arrd = Eigen::Rand::cauchyLike(arrd, urng);
	ret["cauchy/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	arrd = Eigen::Rand::studentTLike(arrd, urng, 1);
	ret["studentT(1)/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	arrd = Eigen::Rand::studentTLike(arrd, urng, 5);
	ret["studentT(5)/double"] = calc_emd_with_pdf(arrd, student5_pdf, step);

	arrd = Eigen::Rand::studentTLike(arrd, urng, 20);
	ret["studentT(20)/double"] = calc_emd_with_pdf(arrd, student20_pdf, step);

	arrd = Eigen::Rand::fisherFLike(arrd, urng, 1, 1);
	ret["fisherF(1,1)/double"] = calc_emd_with_cdf(arrd, fisher11_cdf, step);

	arrd = Eigen::Rand::fisherFLike(arrd, urng, 5, 5);
	ret["fisherF(5,5)/double"] = calc_emd_with_pdf(arrd, fisher55_pdf, step);
#endif
	return ret;
}

template<typename Rng>
std::map<std::string, double> test_eigenrand_disc(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXi arri{ size };
	Rng urng{ seed };

	arri = Eigen::Rand::uniformIntLike(arri, urng, 0, 9);
	ret["uniformInt(0,9)"] = calc_kldiv(arri, uniform10_pmf);

	arri = Eigen::Rand::discreteLike(arri, urng, { 1, 2, 3, 4, 5, 6 });
	ret["discrete(1,2,3,4,5,6)"] = calc_kldiv(arri, discrete_pmf);

	arri = Eigen::Rand::poissonLike(arri, urng, 1);
	ret["poisson(1)"] = calc_kldiv(arri, poisson1_pmf);

	arri = Eigen::Rand::poissonLike(arri, urng, 16);
	ret["poisson(16)"] = calc_kldiv(arri, poisson16_pmf);

	arri = Eigen::Rand::binomialLike(arri, urng, 10, 0.5);
	ret["binomial(10,0.5)"] = calc_kldiv(arri, binomial1_pmf);

	arri = Eigen::Rand::binomialLike(arri, urng, 30, 0.75);
	ret["binomial(30,0.75)"] = calc_kldiv(arri, binomial2_pmf);

	arri = Eigen::Rand::binomialLike(arri, urng, 50, 0.25);
	ret["binomial(50,0.25)"] = calc_kldiv(arri, binomial3_pmf);

	arri = Eigen::Rand::negativeBinomialLike(arri, urng, 10, 0.5);
	ret["negativeBinomial(10,0.5)"] = calc_kldiv(arri, negbinomial1_pmf);

	arri = Eigen::Rand::negativeBinomialLike(arri, urng, 20, 0.25);
	ret["negativeBinomial(20,0.25)"] = calc_kldiv(arri, negbinomial2_pmf);

	arri = Eigen::Rand::negativeBinomialLike(arri, urng, 30, 0.75);
	ret["negativeBinomial(30,0.75)"] = calc_kldiv(arri, negbinomial3_pmf);

	arri = Eigen::Rand::geometricLike(arri, urng, 0.25);
	ret["geometric(0.25)"] = calc_kldiv(arri, geometric25_pmf);

	arri = Eigen::Rand::geometricLike(arri, urng, 0.75);
	ret["geometric(0.75)"] = calc_kldiv(arri, geometric75_pmf);
	return ret;
}

std::map<std::string, double> test_old(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXf arr{ size };

	arr = Eigen::ArrayXf::Random(size);
	ret["balanced"] = calc_emd_with_cdf(arr, balanced_cdf, step);
	return ret;
}

std::map<std::string, double> test_cpp11_cont(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXf arr{ size };
	Eigen::ArrayXd arrd{ size };
	std::mt19937_64 urng{ seed };

	{
		std::uniform_real_distribution<float> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["uniformReal"] = calc_emd_with_cdf(arr, ur_cdf, step);

	{
		std::uniform_real_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["uniformReal/double"] = calc_emd_with_cdf(arrd, ur_cdf, step);

	{
		std::normal_distribution<> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["normal"] = calc_emd_with_cdf(arr, normal_cdf, step);

	{
		std::normal_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["normal/double"] = calc_emd_with_cdf(arrd, normal_cdf, step);

	{
		std::lognormal_distribution<> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["lognormal"] = calc_emd_with_cdf(arr, lognormal_cdf, step);

	{
		std::lognormal_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["lognormal/double"] = calc_emd_with_cdf(arrd, lognormal_cdf, step);

	{
		std::gamma_distribution<> dist{ 1, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(1,1)"] = calc_emd_with_pdf(arr, gamma11_pdf, step);

	{
		std::gamma_distribution<double> dist{ 1, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(1,1)/double"] = calc_emd_with_pdf(arrd, gamma11_pdf, step);

	{
		std::gamma_distribution<> dist{ 5, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(5,1)"] = calc_emd_with_pdf(arr, gamma51_pdf, step);

	{
		std::gamma_distribution<double> dist{ 5, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(5,1)/double"] = calc_emd_with_pdf(arrd, gamma51_pdf, step);

	{
		std::gamma_distribution<> dist{ 0.2, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(0.2,1)"] = calc_emd_with_pdf(arr, gamma21_pdf, step);

	{
		std::gamma_distribution<double> dist{ 0.2, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(0.2,1)/double"] = calc_emd_with_pdf(arrd, gamma21_pdf, step);

	{
		std::exponential_distribution<> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["exponential"] = calc_emd_with_cdf(arr, exp_cdf, step);

	{
		std::exponential_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["exponential/double"] = calc_emd_with_cdf(arrd, exp_cdf, step);

	{
		std::weibull_distribution<> dist{ 2, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["weibull(2,1)"] = calc_emd_with_cdf(arr, weibull_cdf, step);

	{
		std::weibull_distribution<double> dist{ 2, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["weibull(2,1)/double"] = calc_emd_with_cdf(arrd, weibull_cdf, step);

	{
		std::extreme_value_distribution<> dist{ 1, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["extremeValue(1,1)"] = calc_emd_with_cdf(arr, extreme_value_cdf, step);

	{
		std::extreme_value_distribution<double> dist{ 1, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["extremeValue(1,1)/double"] = calc_emd_with_cdf(arrd, extreme_value_cdf, step);

	{
		std::chi_squared_distribution<> dist{ 7 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["chiSquared(7)"] = calc_emd_with_pdf(arr, chisquared_pdf, step);

	{
		std::chi_squared_distribution<double> dist{ 7 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["chiSquared(7)/double"] = calc_emd_with_pdf(arrd, chisquared_pdf, step);

	{
		std::cauchy_distribution<> dist{ 0, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["cauchy"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	{
		std::cauchy_distribution<double> dist{ 0, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["cauchy/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	{
		std::student_t_distribution<> dist{ 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(1)"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	{
		std::student_t_distribution<double> dist{ 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(1)/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	{
		std::student_t_distribution<> dist{ 5 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(5)"] = calc_emd_with_pdf(arr, student5_pdf, step);

	{
		std::student_t_distribution<double> dist{ 5 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(5)/double"] = calc_emd_with_pdf(arrd, student5_pdf, step);

	{
		std::student_t_distribution<> dist{ 20 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(20)"] = calc_emd_with_pdf(arr, student20_pdf, step);

	{
		std::student_t_distribution<double> dist{ 20 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(20)/double"] = calc_emd_with_pdf(arrd, student20_pdf, step);

	{
		std::fisher_f_distribution<> dist{ 1, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(1,1)"] = calc_emd_with_cdf(arr, fisher11_cdf, step);

	{
		std::fisher_f_distribution<double> dist{ 1, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(1,1)/double"] = calc_emd_with_cdf(arrd, fisher11_cdf, step);

	{
		std::fisher_f_distribution<> dist{ 5, 5 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(5,5)"] = calc_emd_with_pdf(arr, fisher55_pdf, step);

	{
		std::fisher_f_distribution<double> dist{ 5, 5 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(5,5)/double"] = calc_emd_with_pdf(arrd, fisher55_pdf, step);
	return ret;
}

std::map<std::string, double> test_cpp11_disc(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXi arri{ size };
	std::mt19937_64 urng{ seed };

	{
		std::uniform_int_distribution<> dist{ 0, 9 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["uniformInt(0,9)"] = calc_kldiv(arri, uniform10_pmf);

	{
		std::discrete_distribution<> dist{ 1, 2, 3, 4, 5, 6 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["discrete(1,2,3,4,5,6)"] = calc_kldiv(arri, discrete_pmf);

	{
		std::poisson_distribution<> dist{ 1 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["poisson(1)"] = calc_kldiv(arri, poisson1_pmf);

	{
		std::poisson_distribution<> dist{ 16 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["poisson(16)"] = calc_kldiv(arri, poisson16_pmf);

	{
		std::binomial_distribution<> dist{ 10, 0.5 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["binomial(10,0.5)"] = calc_kldiv(arri, binomial1_pmf);

	{
		std::binomial_distribution<> dist{ 30, 0.75 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["binomial(30,0.75)"] = calc_kldiv(arri, binomial2_pmf);

	{
		std::binomial_distribution<> dist{ 50, 0.25 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["binomial(50,0.25)"] = calc_kldiv(arri, binomial3_pmf);

	{
		std::negative_binomial_distribution<> dist{ 10, 0.5 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["negativeBinomial(10,0.5)"] = calc_kldiv(arri, negbinomial1_pmf);

	{
		std::negative_binomial_distribution<> dist{ 20, 0.25 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["negativeBinomial(20,0.25)"] = calc_kldiv(arri, negbinomial2_pmf);

	{
		std::negative_binomial_distribution<> dist{ 30, 0.75 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["negativeBinomial(30,0.75)"] = calc_kldiv(arri, negbinomial3_pmf);

	{
		std::geometric_distribution<> dist{ 0.25 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["geometric(0.25)"] = calc_kldiv(arri, geometric25_pmf);

	{
		std::geometric_distribution<> dist{ 0.75 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["geometric(0.75)"] = calc_kldiv(arri, geometric75_pmf);
	return ret;
}

int main(int argc, char** argv)
{
	size_t size = 32768, step = size * 8, repeat = 50;

	if (argc > 1) size = std::stoi(argv[1]);
	if (argc > 2) step = std::stoi(argv[2]);

	std::map<std::string, double> err, errSq, kl, klSq;

	for (size_t i = 0; i < repeat; ++i)
	{
		std::cout << "Repeat " << i << " ..." << std::endl;
		for (auto& p : test_eigenrand_cont<std::mt19937_64>(size, step, 42 * i))
		{
			err[p.first + "\t:EigenRand"] += p.second;
			errSq[p.first + "\t:EigenRand"] += p.second * p.second;
		}

		for (auto& p : test_eigenrand_disc<std::mt19937_64>(size, step, 42 * i))
		{
			kl[p.first + "\t:EigenRand"] += p.second;
			klSq[p.first + "\t:EigenRand"] += p.second * p.second;
		}

#if defined(EIGEN_VECTORIZE_SSE2) || defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_NEON)
		for (auto& p : test_eigenrand_cont<Eigen::Rand::Vmt19937_64>(size, step, 42 * i))
		{
			err[p.first + "\t:ERand+Vrng"] += p.second;
			errSq[p.first + "\t:ERand+Vrng"] += p.second * p.second;
		}

		for (auto& p : test_eigenrand_disc<Eigen::Rand::Vmt19937_64>(size, step, 42 * i))
		{
			kl[p.first + "\t:ERand+Vrng"] += p.second;
			klSq[p.first + "\t:ERand+Vrng"] += p.second * p.second;
		}
#endif
#ifndef SKIP_REFERENCE
		for (auto& p : test_cpp11_cont(size, step, 42 * i))
		{
			err[p.first + "\t:C++11"] += p.second;
			errSq[p.first + "\t:C++11"] += p.second * p.second;
		}

		for (auto& p : test_cpp11_disc(size, step, 42 * i))
		{
			kl[p.first + "\t:C++11"] += p.second;
			klSq[p.first + "\t:C++11"] += p.second * p.second;
		}

		for (auto& p : test_old(size, step, 42 * i))
		{
			err[p.first + "\t:Old"] += p.second;
			errSq[p.first + "\t:Old"] += p.second * p.second;
		}
#endif
	}

	std::cout << "[Earth Mover's Distance] Mean (Stdev)" << std::endl;
	for (auto& p : err)
	{
		double mean = p.second / repeat;
		double var = (errSq[p.first] / repeat) - mean * mean;
		size_t sp = p.first.find('\t');
		std::cout << std::left << std::setw(28) << p.first.substr(0, sp);
		std::cout << std::setw(14) << p.first.substr(sp + 1);
		std::cout << ": " << mean << " (" << std::sqrt(var) << ")" << std::endl;
	}
	std::cout << std::endl;
	std::cout << "[KL Divergence] Mean (Stdev)" << std::endl;
	for (auto& p : kl)
	{
		double mean = p.second / repeat;
		double var = (klSq[p.first] / repeat) - mean * mean;
		size_t sp = p.first.find('\t');
		std::cout << std::left << std::setw(28) << p.first.substr(0, sp);
		std::cout << std::setw(14) << p.first.substr(sp + 1);
		std::cout << ": " << mean << " (" << std::sqrt(var) << ")" << std::endl;
	}
	std::cout << std::endl;
	return 0;
}


#ifndef _ECO_UTIL_HPP_
#define _ECO_UTIL_HPP_

#include <iostream>
#include <string>
#include <math.h>
#include <algorithm>
#include <complex>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "ffttools.hpp"
#include "matrix_operator.hpp"

namespace eco_tracker {

typedef std::vector<std::vector<Eigen::MatrixXcf> > EcoFeats;

EcoFeats runFFt(const EcoFeats &xlw);

void genGaussianYf(
    const float& sigma_y,
    const float& T_size,
    const std::vector<Eigen::MatrixXcf>& k_x,
    const std::vector<Eigen::MatrixXcf>& k_y,
    std::vector<Eigen::MatrixXcf>& y_f);

void genCosWindow(
    const int& row,
    const int& col,
    Eigen::MatrixXcf& cos_window);

EcoFeats initInterpolateFFt(
    const EcoFeats& xlf,
    const std::vector<Eigen::MatrixXcf>& interp1_fs,
    const std::vector<Eigen::MatrixXcf>& interp2_fs);

EcoFeats interpolateDFT(
    const EcoFeats& xlf,
    const std::vector<Eigen::MatrixXcf>& interp1_fs,
    const std::vector<Eigen::MatrixXcf>& interp2_fs);

EcoFeats computeFeautrePower(const EcoFeats &feats);

EcoFeats compactFourierCoeff(const EcoFeats &xf);

EcoFeats fullFourierCoeff(const EcoFeats &xf);

std::vector<Eigen::MatrixXcf> vectorFeature(const EcoFeats &x);

void initProjectionMatrix(
    const EcoFeats& init_sample,
	const std::vector<int>& compressed_dim,
    std::vector<Eigen::MatrixXcf>& project_matrix);

EcoFeats projectFeature(
    const EcoFeats& x,
    const std::vector<Eigen::MatrixXcf>& project_matrix);

EcoFeats projectFeatureMultScale(
    const EcoFeats& x,
    const std::vector<Eigen::MatrixXcf>& project_matrix);

EcoFeats EcoFeatureDotDivide(
    const EcoFeats &a,
    const EcoFeats &b);

std::complex<float> EcoFeatInnerProduct(
    const EcoFeats& f1,
    const EcoFeats& f2);

EcoFeats shiftSample(EcoFeats &xf,
                     float x,
                     float y,
                     std::vector<Eigen::MatrixXcf> kx,
                     std::vector<Eigen::MatrixXcf> ky);

std::vector<Eigen::MatrixXcf> getProjectMatEnergy(
    const std::vector<Eigen::MatrixXcf> project_mat,
    const std::vector<int>& feature_dim,
	const std::vector<Eigen::MatrixXcf>& yf);

} // namespace eco_tracker
#endif


#include <Eigen/Dense>
#include <matplotlibcpp.h>
#include <vector>

namespace plt = matplotlibcpp;
using namespace Eigen;

int main(){
    using std::vector;

    int n = 5000;
    Eigen::VectorXd x(n), y(n), z(n), w = Eigen::VectorXd::Ones(n);
    for (int i = 0; i < n; ++i)
    {
        double value = (1.0 + i) / n;
        x(i) = value;
        y(i) = value * value;
        z(i) = value * value * value;
    }

    vector<double> xs(n), ys(n), zs(n), ws(n);

    Map<VectorXd>(&xs[0], n) = x;
    Map<VectorXd>(&ys[0], n) = y;
    Map<VectorXd>(&zs[0], n) = z;
    Map<VectorXd>(&ws[0], n) = w;

    plt::loglog(xs, ys);                             // f(x) = x^2
    plt::loglog(xs, ws, "r--");                      // f(x) = 1, red dashed line
    plt::loglog(xs, zs, "g:"); // f(x) = x^3, green dots + label

    plt::title("Some functions of $x$"); // add a title

    plt::save("sample_eigen2.png");

    return 0;
}


#include <armadillo>
#include <iostream>

using namespace arma;

int main(int argc, char *argv[]) {
    if (argc < 2) {
        std::cerr << "# error: no file specified" << std::endl;
        return 1;
    }
    mat A;
    A.load(argv[1], raw_ascii);
    A.print("A:");
    std::cout << "det(A) = " << det(A) << std::endl;
    return 0;
}


#include <Eigen/Core>

extern "C" double my_norm( double x, double y, double z )
{
  return ::Eigen::Vector3d{ x, y, z }.norm();
}


// solving A * X = B
// using driver function gesv()

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::size_t;
using std::cout;
using std::endl;

typedef std::complex<double> cmpx;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmpx, ublas::column_major> cm_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
typedef ublas::matrix<cmpx, ublas::row_major> cm_t;
#endif

int main() {

  cout << endl;
  cout << "real system:" << endl << endl;

  size_t n = 5;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs);
  // b -- right-hand side matrix:
  // .. see leading comments for `gesv()' in clapack.hpp
#ifndef F_ROW_MAJOR
  m_t b (n, nrhs);
#else
  m_t b (nrhs, n);
#endif

  std::vector<int> ipiv (n);  // pivot vector

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t aa (a);  // copy of a, because a is `lost' after gesv()

  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  atlas::set (1., xc0);  // x[.,0] = 1
  atlas::set (2., xc1);  // x[.,1] = 2
#ifndef F_ROW_MAJOR
  atlas::gemm (a, x, b);  // b = a x, so we know the result ;o)
#else
  // see leading comments for `gesv()' in clapack.hpp
  ublas::matrix_row<m_t> br0 (b, 0), br1 (b, 1);
  atlas::gemv (a, xc0, br0);  // b[0,.] = a x[.,0]
  atlas::gemv (a, xc1, br1);  // b[1,.] = a x[.,1]  =>  b^T = a x
#endif

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

  atlas::gesv (a, ipiv, b);   // solving the system, b contains x
  print_m (b, "X");
  cout << endl;

#ifndef F_ROW_MAJOR
  atlas::gemm (aa, b, x);     // check the solution
#else
  atlas::gemv (aa, br0, xc0);
  atlas::gemv (aa, br1, xc1);
#endif
  print_m (x, "B = A X");
  cout << endl;

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

  cout << endl;
  cout << "complex system:" << endl << endl;
  cm_t ca (3, 3);
  cm_t cx (3, 1);
#ifndef F_ROW_MAJOR
  cm_t cb (3, 1);
#else
  cm_t cb (1, 3);
#endif

  ca (0, 0) = cmpx (3, 0);
  ca (0, 1) = cmpx (4, 2);
  ca (0, 2) = cmpx (-7, 5);
  ca (1, 0) = cmpx (4, -2);
  ca (1, 1) = cmpx (-5, 0);
  ca (1, 2) = cmpx (0, -3);
  ca (2, 0) = cmpx (-7, -5);
  ca (2, 1) = cmpx (0, 3);
  ca (2, 2) = cmpx (2, 0);
  print_m (ca, "CA");
  cout << endl;
  cm_t caa (ca);

  ublas::matrix_column<cm_t> cx0 (cx, 0);
  atlas::set (cmpx (1, -1), cx0);
#ifndef F_ROW_MAJOR
  ublas::matrix_column<cm_t> cb0 (cb, 0);
#else
  ublas::matrix_row<cm_t> cb0 (cb, 0);
#endif
  atlas::gemv (ca, cx0, cb0);
  print_m (cb, "CB");
  cout << endl;

  int ierr = atlas::gesv (ca, cb); // with `internal' pivot vector
  if (ierr == 0) {
    print_m (cb, "CX");
    cout << endl;
    atlas::gemv (caa, cb0, cx0);
    print_m (cx, "CB");
  }
  else
    cout << "matrix is singular" << endl;

  cout << endl;
}


#include "eigen_gpu.h"
#include "args.h"

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

//----( randomization )-------------------------------------------------------

inline float random_real () { return tanf(3 * (random_std() - 0.5f)); }

void randomize (Vector<float> & x)
{
  for (size_t i = 0; i < x.size; ++i) {
    x[i] = random_real();
  }
}

void randomize (VectorXf & x)
{
  for (int i = 0; i < x.size(); ++i) {
    x[i] = random_real();
  }
}

void randomize (MatrixXf & x)
{
  for (int j = 0; j < x.cols(); ++j) {
  for (int i = 0; i < x.rows(); ++i) {
    x(i,j) = random_real();
  }}
}

void randomize (MatrixSf & mat, float density)
{
  ASSERT_LT(0, density);
  ASSERT_LT(density, 1);

  Eigen::DynamicSparseMatrix<float> temp(mat.rows(), mat.cols());

  const size_t X = mat.rows();
  const size_t Y = mat.cols();

  size_t num_entries = ceil(density * mat.rows() * mat.cols());
  for (size_t e = 0; e < num_entries; ++e) {

    int x,y;
    do {
      int xy = random_choice(X*Y);
      x = xy % X;
      y = xy / X;
    } while (temp.coeff(x,y) > 0);

    temp.coeffRef(x,y) = random_real();
  }

  mat = temp;
}

//----( tests )---------------------------------------------------------------

void test_dense_matrix_matrix (
    int rows,
    int cols,
    int width,
    size_t iters,
    bool trans_A,
    bool trans_B)
{
  LOG("\nTesting dense matrix-matrix product"
      << " (op(A) = " << (trans_A ? "A" : "A'")
      << ", op(B) = " << (trans_B ? "B" : "B'")
      << ")");

  MatrixXf lhs(rows, cols);   randomize(lhs);
  MatrixXf rhs(cols, width);  randomize(rhs);

  if (trans_A) lhs.transposeInPlace();
  if (trans_B) rhs.transposeInPlace();

  //DEBUG("")
  //PRINT4(rows, cols, lhs.rows(), lhs.cols());

  MatrixXf cpu_prod(rows, width);
  MatrixXf gpu_prod(rows, width);

  float speedup = 1;
  {
    Timer timer;

    for (size_t i = 0; i < iters; ++i) {
      if (trans_A) {
        if (trans_B) cpu_prod = lhs.transpose() * rhs.transpose();
        else         cpu_prod = lhs.transpose() * rhs;
      } else {
        if (trans_B) cpu_prod = lhs * rhs.transpose();
        else         cpu_prod = lhs * rhs;
      }
    }

    float rate = iters / timer.elapsed();
    float gflops = rate * rows * cols * width * 1e-9f;
    LOG("Cpu performed " << rate << " mul/sec = " << gflops << " gflops");
    speedup /= rate;
  }

  {
    Timer timer;

    for (size_t i = 0; i < iters; ++i) {
      Gpu::matrix_multiply(lhs, trans_A, rhs, trans_B, gpu_prod);
    }

    float rate = iters / timer.elapsed();
    float gflops = rate * rows * cols * width * 1e-9f;
    LOG("Gpu performed " << rate << " mul/sec = " << gflops << " gflops");
    speedup *= rate;
    PRINT(speedup);
  }

  //PRINT(as_vector(lhs));
  //PRINT(as_vector(rhs));
  //PRINT(as_vector(cpu_prod));
  //PRINT(as_vector(gpu_prod));

  float rms_lhs = lhs.norm() / sqrtf(lhs.size());
  float rms_rhs = rhs.norm() / sqrtf(rhs.size());
  float rms_error = (cpu_prod - gpu_prod).norm() / sqrtf(cpu_prod.size());
  PRINT(rms_error);
  ASSERTW_LT(rms_error, 1e-6f * rms_lhs * rms_rhs);
}

void test_sparse_matrix_vector (
    int rows,
    int cols,
    float density,
    size_t iters,
    bool trans)
{
  LOG("\nTesting sparse matrix-vector product"
      << (trans ? " (transposed)" : ""));

  MatrixSf sparse(rows, cols);
  randomize(sparse, density);

  Gpu::SparseMultiplier multiply(sparse, 1);

  VectorXf rhs(trans ? rows : cols);
  randomize(rhs);

  VectorXf cpu_prod(trans ? cols : rows);
  VectorXf gpu_prod(trans ? cols : rows);

  float speedup = 1;
  {
    Timer timer;

    for (size_t i = 0; i < iters; ++i) {
      if (trans) cpu_prod = sparse.transpose() * rhs;
      else cpu_prod = sparse * rhs;
    }

    float rate = iters / timer.elapsed();
    float gflops = rate * rows * cols * density * 1e-9f;
    LOG("Cpu performed " << rate << " mul/sec = " << gflops << " gflops");
    speedup /= rate;
  }

  {
    Timer timer;

    for (size_t i = 0; i < iters; ++i) {
      multiply.left_mul(rhs, gpu_prod, trans);
    }

    float rate = iters / timer.elapsed();
    float gflops = rate * rows * cols * density * 1e-9f;
    LOG("Gpu performed " << rate << " mul/sec = " << gflops << " gflops");
    speedup *= rate;
    PRINT(speedup);
  }

  float rms_sparse = sparse.norm() / sqrtf(sparse.size());
  float rms_rhs = rhs.norm() / sqrtf(rhs.size());
  float rms_error = (cpu_prod - gpu_prod).norm() / sqrtf(cpu_prod.size());
  PRINT(rms_error);
  ASSERTW_LT(rms_error, 1e-6f * rms_sparse * rms_rhs);
}

void test_sparse_matrix_matrix (
    int rows,
    int cols,
    int width,
    float density,
    size_t iters,
    bool trans)
{
  LOG("\nTesting sparse matrix-matrix product"
      << (trans ? " (transposed)" : ""));

  MatrixSf sparse(rows, cols);
  randomize(sparse, density);

  Gpu::SparseMultiplier multiply(sparse, 64);

  MatrixXf rhs(trans ? rows : cols, width);
  randomize(rhs);

  MatrixXf cpu_prod(trans ? cols : rows, width);
  MatrixXf gpu_prod(trans ? cols : rows, width);

  float speedup = 1;
  {
    Timer timer;

    for (size_t i = 0; i < iters; ++i) {
      if (trans) cpu_prod = sparse.transpose() * rhs;
      else cpu_prod = sparse * rhs;
    }

    float rate = iters / timer.elapsed();
    float gflops = rate * rows * cols * width * density * 1e-9f;
    LOG("Cpu performed " << rate << " mul/sec = " << gflops << " gflops");
    speedup /= rate;
  }

  {
    Timer timer;

    for (size_t i = 0; i < iters; ++i) {
      multiply.left_mul(rhs, gpu_prod, trans);
    }

    float rate = iters / timer.elapsed();
    float gflops = rate * rows * cols * width * density * 1e-9f;
    LOG("Gpu performed " << rate << " mul/sec = " << gflops << " gflops");
    speedup *= rate;
    PRINT(speedup);
  }

  float rms_sparse = sparse.norm() / sqrtf(sparse.size());
  float rms_rhs = rhs.norm() / sqrtf(rhs.size());
  float rms_error = (cpu_prod - gpu_prod).norm() / sqrtf(cpu_prod.size());
  PRINT(rms_error);
  ASSERTW_LT(rms_error, 1e-6f * rms_sparse * rms_rhs);
}

//----( main )----------------------------------------------------------------

const char * help_message =
"Usage: eigen_gpu_test [ROWS] [COLS] [WIDTH] [DENSITY] [ITERS]"
;

int main (int argc, char ** argv)
{
  Args args(argc, argv, help_message);
  LOG(help_message);

  int rows = args.pop(2048);
  int cols = args.pop(4096);
  int width = args.pop(1024);
  float density = args.pop(0.01f);
  size_t iters = args.pop(1);
  PRINT5(rows, cols, width, density, iters);
  // Test results below were generated with
  // rows = 2048, cols = 4096, density = 0.01, iters = 1

  test_sparse_matrix_vector(rows, cols, density, iters, false);
  // Cpu performed 4830.92 mul/sec = 0.405247 gflops
  // Gpu performed 242.131 mul/sec = 0.0203114 gflops
  // rms_error = 0.0110587

  test_sparse_matrix_vector(rows, cols, density, iters, true);
  // Cpu performed 5780.35 mul/sec = 0.484891 gflops
  // Gpu performed 14705.9 mul/sec = 1.23362 gflops
  // rms_error = 0.00113682

  test_sparse_matrix_matrix(rows, cols, width, density, iters, false);
  // Cpu performed 0.700048 mul/sec = 0.0601337 gflops
  // Gpu performed 1.75981 mul/sec = 0.151167 gflops
  // rms_error = 0.0506754

  test_sparse_matrix_matrix(rows, cols, width, density, iters, true);
  // Cpu performed 0.643097 mul/sec = 0.0552416 gflops
  // Gpu performed 34.4816 mul/sec = 2.96194 gflops
  // rms_error = 0.00686571

  test_dense_matrix_matrix(rows, cols, width, iters, false, false);
  // Cpu performed 3.5864 mul/sec = 30.807 gflops
  // Gpu performed 27.9431 mul/sec = 240.029 gflops
  // rms_error = 21.362

  // TODO get these working
  //test_dense_matrix_matrix(rows, cols, width, iters, true, false);
  //test_dense_matrix_matrix(rows, cols, width, iters, false, true);
  //test_dense_matrix_matrix(rows, cols, width, iters, true, true);

  LOG("");
  return 0;
}


#include <iostream>
#include <cmath>

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

#include <sophus/so3.h>
#include <sophus/se3.h>

int main(int argc, char** argv) {
  Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();

  Sophus::SO3 SO3_R(R);

  Sophus::SO3 SO3_V(0, 0, M_PI / 2);

  Eigen::Quaterniond Q(R);
  Sophus::SO3 SO3_Q(Q);

  std::cout << "SO3 from matrix: " << SO3_R << std::endl;
  std::cout << "SO3 from vector: " << SO3_V << std::endl;
  std::cout << "SO3 from quaternion: " << SO3_Q << std::endl;

  Eigen::Vector3d so3 = SO3_R.log();
  std::cout << "so3 = " << so3.transpose() << std::endl;

  std::cout << "so3 hat = \n" << Sophus::SO3::hat(so3) << std::endl;

  std::cout << "so3 hat vee = " << Sophus::SO3::vee(Sophus::SO3::hat(so3)) << std::endl;

  Eigen::Vector3d update_so3(1e-4, 0, 0);
  Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R;
  std::cout << "SO3 updated = " << SO3_updated << std::endl;

  std::cout << "op SE3" << std::endl;

  Eigen::Vector3d t(1, 0, 0);
  Sophus::SE3 SE3_Rt(R, t);
  Sophus::SE3 SE3_Qt(Q, t);

  std::cout << "SE3 from R, t = \n" << SE3_Rt << std::endl;
  std::cout << "SE3 from Q, t = \n" << SE3_Qt << std::endl;

  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();

  std::cout << "se3 = \n" << se3.transpose() << std::endl;

  std::cout << "se3 hat = \n" << Sophus::SE3::hat(se3) << std::endl;
  std::cout << "se3 hat vee = \n" <<
            Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << std::endl;

  Vector6d update_se3;
  update_se3.setZero();
  update_se3(0, 0) = 1e-4d;
  Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt;

  std::cout << "SE3 updated = \n" << SE3_updated.matrix() << std::endl;

  return 0;
}


// Copyright 2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

//[ guide_custom_accumulators_builtin

#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <cassert>
#include <iostream>
#include <sstream>

int main() {
  using namespace boost::histogram;
  using mean = accumulators::mean<>;

  // Create a 1D-profile, which computes the mean of samples in each bin.
  auto h = make_histogram_with(dense_storage<mean>(), axis::integer<>(0, 2));
  // The factory function `make_profile` is provided as a shorthand for this, so this is
  // equivalent to the previous line: auto h = make_profile(axis::integer<>(0, 2));

  // An argument marked as `sample` is passed to the accumulator.
  h(0, sample(1)); // sample goes to first cell
  h(0, sample(2)); // sample goes to first cell
  h(1, sample(3)); // sample goes to second cell
  h(1, sample(4)); // sample goes to second cell

  std::ostringstream os;
  for (auto&& x : indexed(h)) {
    // Accumulators usually have methods to access their state. Use the arrow
    // operator to access them. Here, `count()` gives the number of samples,
    // `value()` the mean, and `variance()` the variance estimate of the mean.
    os << boost::format("index %i count %i mean %.1f variance %.1f\n") % x.index() %
              x->count() % x->value() % x->variance();
  }
  std::cout << os.str() << std::flush;
  assert(os.str() == "index 0 count 2 mean 1.5 variance 0.5\n"
                     "index 1 count 2 mean 3.5 variance 0.5\n");
}

//]


///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2020 - 2021.                 //
//  Distributed under the Boost Software License,                //
//  Version 1.0. (See accompanying file LICENSE_1_0.txt          //
//  or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////

#include <cmath>

#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/bindings/decwide_t.hpp>

#include <math/wide_decimal/decwide_t_examples.h>

namespace
{
  constexpr std::uint32_t wide_decimal_digits10 = UINT32_C(1001);

  using dec1001_t = math::wide_decimal::decwide_t<wide_decimal_digits10>;
}

bool math::wide_decimal::example009_boost_math_standalone()
{
  const dec1001_t x = dec1001_t(UINT32_C(123456789)) / 100U;

  using std::cbrt;
  using std::fabs;

  // Compare wide-decimal's cube root function with that of Boost.Math.
  // Also exercise several different interpretations of the constant pi.

  const dec1001_t c       = cbrt(x / math::wide_decimal::pi<wide_decimal_digits10, typename dec1001_t::limb_type, std::allocator<void>, double>());

  const dec1001_t control = boost::math::cbrt(x / boost::math::constants::pi<dec1001_t>());

  const dec1001_t closeness = fabs(1 - (c / control));

  const bool result_is_ok = closeness < (std::numeric_limits<dec1001_t>::epsilon() * 10);

  return result_is_ok;
}

// Enable this if you would like to activate this main() as a standalone example.
#if 0

#include <iomanip>
#include <iostream>

int main()
{
  const bool result_is_ok = math::wide_decimal::example009_boost_math_standalone();

  std::cout << "result_is_ok: " << std::boolalpha << result_is_ok << std::endl;
}

#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic pop
#endif

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#endif


#include <iostream>
#include <Eigen/Dense>
#include "lsq.h"

int main() {
    Eigen::MatrixXf A = Eigen::MatrixXf::Random(3,3);
    Eigen::VectorXf b = Eigen::VectorXf::Random(3);
    LSQ* lsq = new LSQ(A, b);
    Eigen::VectorXf x = lsq->solve();
    std::cout << x << std::endl;
    delete lsq;
}


// File       : gemm_eigen.cpp
// Created    : Tue 23 Oct 2018 02:57:27 PM CEST
// Description: GEMM Eigen implementation
// Copyright 2018 ETH Zurich. All Rights Reserved.

#include <Eigen/Core>
#include "common.h"

// Wrapper to map pointer to Eigen matrix type (needed in benchmark)
template <typename T>
using EMat = Eigen::Map< Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> >;

/**
 * @brief General matrix-matrix multiplication kernel (GEMM). Computes C = AB.
 * Eigen library implementation
 *
 * @param A Eigen matrix dimension p x r
 * @param B Eigen matrix dimension r x q
 * @param C Eigen matrix dimension p x q
 */
void gemm_eigen(const EMat<Real>& A, const EMat<Real>& B, EMat<Real>& C)
{
    C.noalias() += A * B;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Mathworks. gravitysphericalharmonic, implement spherical harmonic representation of
 *        planetary gravity. Help documentation of MATLAB R2012a, 2012.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "tudat/basics/testMacros.h"
#include "tudat/math/basic/mathematicalConstants.h"

#include "tudat/math/basic/coordinateConversions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_coordinate_conversions )

//! Test if spherical-to-Cartesian conversion is working correctly.
BOOST_AUTO_TEST_CASE( testSphericalToCartesianConversion )
{
    using std::sqrt;
    using mathematical_constants::PI;
    using coordinate_conversions::convertSphericalToCartesian;

    // Test 1: test conversion of: ( 0.0, 0.0, 0.0 ).
    {
        Eigen::Vector3d sphericalCoordinates( 0.0, 0.0, 0.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates( 0.0, 0.0, 0.0 );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_BASE( cartesianCoordinates, expectedCartesianCoordinates )
                BOOST_CHECK_SMALL( cartesianCoordinates.coeff( row, col ),
                                   std::numeric_limits< double >::min( ) );
    }

    // Test 2: test conversion of: ( 1.0, pi/6, pi/6 ), from Stewart (2003), Exercise 12.7.15.
    {
        Eigen::Vector3d sphericalCoordinates( 1.0, PI / 6.0, PI / 6.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates(
                    sqrt( 3.0 ) / 4.0, 1.0 / 4.0, sqrt( 3.0 ) / 2.0 );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    cartesianCoordinates, expectedCartesianCoordinates, 1.0e-15 );
    }

    // Test 3: test conversion of: ( 2.0, pi/4, pi/3 ), from Stewart (2003), Exercise 12.7.17.
    {
        Eigen::Vector3d sphericalCoordinates( 2.0, PI / 4.0, PI / 3.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates(
                    0.5 * sqrt( 2.0 ), 0.5 * sqrt( 6.0 ), sqrt( 2.0 ) );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( cartesianCoordinates, expectedCartesianCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: test conversion of: ( 2.0, pi/3, pi/4 , from Stewart (2003), Section 12.7,
    //         Example 4.
    {
        Eigen::Vector3d sphericalCoordinates( 2.0, PI / 3.0, PI / 4.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates( sqrt( 1.5 ), sqrt( 1.5 ), 1.0 );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( cartesianCoordinates, expectedCartesianCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

//! Test if Cartesian-to-spherical conversion is working correctly.
BOOST_AUTO_TEST_CASE( testCartesianToSphericalConversion )
{
    using std::acos;
    using std::atan2;
    using std::pow;
    using std::sqrt;
    using coordinate_conversions::convertCartesianToSpherical;
    using mathematical_constants::PI;

    // Test 1: Test conversion of: ( 0.0, 0.0, 0.0 ).
    {
        Eigen::Vector3d cartesianCoordinates = Eigen::Vector3d::Zero( );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates = Eigen::Vector3d::Zero( );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: Test conversion of: ( 1.0, sqrt( 3 ), 2 * sqrt(3) ), from Stewart (2003),
    // Exercise 12.7.19.
    {
        Eigen::Vector3d cartesianCoordinates( 1.0, sqrt( 3. ), 2.0 * sqrt( 3. ) );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates( 4.0, PI / 6.0, PI / 3.0 );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: Test conversion of: ( 0.0, -1.0, -1.0 ), from Stewart (2003), Exercise 12.7.21.
    {
        Eigen::Vector3d cartesianCoordinates( 0.0, -1.0, -1.0 );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates( sqrt( 2.0 ), 3.0 * PI / 4.0, -PI / 2.0 );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = Eigen::Vector3d::Zero( );

        // Compute conversions.
        sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: Test conversion of: ( 0.0, 2 sqrt( 3 ), -2.0 ), from Stewart (2003), Section 12.7,
    // Example 5.
    {
        Eigen::Vector3d cartesianCoordinates( 0.0, 2.0 * sqrt( 3. ), -2.0 );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates( 4.0, 2.0 * PI / 3.0, PI / 2.0 );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from cylindrical (r, theta, z) to Cartesian (x, y, z) coordinates.
BOOST_AUTO_TEST_CASE( testCylindricalToCartesianPositionCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::xCartesianCoordinateIndex;
    using coordinate_conversions::yCartesianCoordinateIndex;
    using coordinate_conversions::zCartesianCoordinateIndex;

    // Test 1: test conversion of ( 0.0, pi, 1.2 ).
    {
        // Set cylindrical coordinates.
        const Eigen::Vector3d cylindricalCoordinates( 0.0, PI, 1.2 );

        // Set expected Cartesian coordinates.
        const Eigen::Vector3d expectedCartesianCoordinates( 0.0, 0.0, 1.2 );

        // Convert cylindrical to Cartesian coordinates.
        const Eigen::Vector3d computedCartesianCoordinates
                = coordinate_conversions::
                convertCylindricalToCartesian( cylindricalCoordinates );

        // Check if computed Cartesian coordinates match expected values.
        BOOST_CHECK_SMALL( computedCartesianCoordinates( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( computedCartesianCoordinates( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianCoordinates( zCartesianCoordinateIndex ),
                                    computedCartesianCoordinates( zCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.3, -pi/2, -3.5 ).
    {
        // Set cylindrical coordinates.
        const Eigen::Vector3d cylindricalCoordinates( 2.3, -PI/2, -3.5 );

        // Set expected Cartesian coordinates.
        const Eigen::Vector3d expectedCartesianCoordinates( 2.3 * std::cos( -PI / 2.0 ),
                                                            2.3 * std::sin( -PI / 2.0 ),
                                                            -3.5 );

        // Convert cylindrical to Cartesian coordinates.
        const Eigen::Vector3d convertedCartesianCoordinates
                = coordinate_conversions::
                convertCylindricalToCartesian( cylindricalCoordinates );

        // Check if converted Cartesian coordinates match expected values.
        BOOST_CHECK_SMALL( convertedCartesianCoordinates ( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCartesianCoordinates.segment( yCartesianCoordinateIndex, 2 ),
                    convertedCartesianCoordinates.segment( yCartesianCoordinateIndex, 2 ),
                    std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from cylindrical (r, theta, z, Vr, Vtheta, Vz) to Cartesian
// (x, y, z, xdot, ydot, zdot) state.
BOOST_AUTO_TEST_CASE( testCylindricalToCartesianPositionAndVelocityCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::xCartesianCoordinateIndex;
    using coordinate_conversions::yCartesianCoordinateIndex;
    using coordinate_conversions::zCartesianCoordinateIndex;

    // Test 1: test conversion of (2.1, pi/2.0, 1.2, 5.4, 4.5, -3.9).
    {
        // Set Cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d cylindricalState =
                ( Eigen::Vector6d( ) << 2.1, PI / 2.0, 1.2, 5.4, 4.5, -3.9 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d expectedCartesianState =
                ( Eigen::Vector6d( )
                  << 2.1 * std::cos( PI / 2.0 ),
                  2.1 * std::sin( PI / 2.0 ),
                  1.2,
                  5.4 * std::cos( PI / 2.0 ) - 4.5 * std::sin( PI / 2.0 ),
                  5.4 * std::sin( PI / 2.0 ) + 4.5 * std::cos( PI / 2.0 ),
                  -3.9 ).finished( );

        // Convert cylindrical to Cartesian state (x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d convertedCartesianState
                = coordinate_conversions::
                convertCylindricalToCartesianState( cylindricalState );

        // Check if converted Cartesian state match expected state.
        BOOST_CHECK_SMALL( convertedCartesianState ( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCartesianState.segment( yCartesianCoordinateIndex, 5 ),
                    convertedCartesianState.segment( yCartesianCoordinateIndex, 5 ),
                    std::numeric_limits< double >::epsilon( ) );

    }

    // Test 2: test conversion of (0.0, 8.2*pi/3.0, -2.5, -5.8, 0.0, 1.7).
    {
        // Set Cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d cylindricalState = ( Eigen::Vector6d( )
                                                   << 0.0, 8.2 * PI / 3.0,
                                                   -2.5, -5.8, 0.0, 1.7 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d expectedCartesianState = ( Eigen::Vector6d( )
                                                         << 0.0,
                                                         0.0,
                                                         -2.5,
                                                         -5.8 * std::cos( 8.2 * PI / 3.0 ),
                                                         -5.8 * std::sin( 8.2 * PI / 3.0 ),
                                                         1.7 ).finished( );

        // Convert cylindrical to Cartesian state (x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d convertedCartesianState
                = coordinate_conversions::
                convertCylindricalToCartesianState( cylindricalState );

        // Check if converted Cartesian state match expected state.
        BOOST_CHECK_SMALL( convertedCartesianState( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCartesianState.segment( zCartesianCoordinateIndex, 4 ),
                    convertedCartesianState.segment( zCartesianCoordinateIndex, 4 ),
                    std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from Cartesian (x, y, z) to cylindrical (r, theta, z) coordinates.
BOOST_AUTO_TEST_CASE( testCartesianToCylindricalPositionCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::rCylindricalCoordinateIndex;
    using coordinate_conversions::thetaCylindricalCoordinateIndex;
    using coordinate_conversions::zCylindricalCoordinateIndex;

    // Test 1: test conversion of ( 0.0, 0.0, 1.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( 0.0, 0.0, 1.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( 0.0, 0.0, 1.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        BOOST_CHECK_SMALL( convertedCylindricalCoordinates( rCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalCoordinates( thetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalCoordinates( zCylindricalCoordinateIndex ),
                                    convertedCylindricalCoordinates( zCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 0.0, 2.0, 1.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( 0.0, 2.0, 1.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( 2.0, PI / 2.0, 1.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalCoordinates,
                                           convertedCylindricalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( 0.0, -2.0, -1.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( 0.0, -2.0, -1.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( 2.0, 3.0 * PI / 2.0, -1.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalCoordinates,
                                           convertedCylindricalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: test conversion of ( -5.0, -8.0, 5.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( -5.0, -8.0, 5.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( std::sqrt( 25.0 + 64.0 ),
                                                              std::atan2( -8.0,-5.0 ) + 2.0 * PI,
                                                              5.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalCoordinates,
                                           convertedCylindricalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

}

// Test conversion from Cartesian (x, y, z, xdot, ydot, zdot) to cylindrical
// (r, theta, z, Vr, Vtheta, Vz) state.
BOOST_AUTO_TEST_CASE( testCartesianToCylindricalPositionAndVelocityCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::rCylindricalCoordinateIndex;
    using coordinate_conversions::thetaCylindricalCoordinateIndex;
    using coordinate_conversions::zCylindricalCoordinateIndex;
    using coordinate_conversions::rDotCylindricalCoordinateIndex;
    using coordinate_conversions::vThetaCylindricalCoordinateIndex;
    using coordinate_conversions::zDotCylindricalCoordinateIndex;

    // Test 1: test conversion of ( 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).
    {
        // Set Cartesian state (x,y,z,xdot,ydot,zdot).
        const Eigen::Vector6d cartesianState = ( Eigen::Vector6d( )
                                                 << 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).finished( );

        // Set expected cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d expectedCylindricalState =
                ( Eigen::Vector6d( )
                                                           << 0.0, 0.0, 1.0,
                                                           std::sqrt( 25.0 + 36.0 ),
                                                           0.0, -9.0 ).finished( );

        // Convert Cartesian to cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d convertedCylindricalState
                = coordinate_conversions::
                convertCartesianToCylindricalState( cartesianState );

        // Check that converted cylindrical state matches expected state.
        BOOST_CHECK_SMALL( convertedCylindricalState( rCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalState( thetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( zCylindricalCoordinateIndex ),
                                    convertedCylindricalState( zCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( rDotCylindricalCoordinateIndex ),
                                    convertedCylindricalState( rDotCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalState( vThetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( zDotCylindricalCoordinateIndex ),
                                    convertedCylindricalState( zDotCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.0, 0.0, -5.0, -4.0, 6.0, -6.0 ).
    {
        // Set Cartesian state (x,y,z,xdot,ydot,zdot).
        const Eigen::Vector6d cartesianState = ( Eigen::Vector6d( )
                                                 << 2.0, 0.0, -5.0, -4.0, 6.0, -6.0 ).finished( );

        // Set expected cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d expectedCylindricalState = ( Eigen::Vector6d( )
                                                           << 2.0, 0.0, -5.0,
                                                           -4.0, 6.0, -6.0 ).finished( );

        // Convert Cartesian to cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d convertedCylindricalState
                = coordinate_conversions::
                convertCartesianToCylindricalState( cartesianState );

        // Check that converted cylindrical state matches expected state.
        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( rCylindricalCoordinateIndex ),
                                    convertedCylindricalState( rCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalState( thetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCylindricalState.segment( zCylindricalCoordinateIndex, 4 ),
                    convertedCylindricalState.segment( zCylindricalCoordinateIndex, 4 ),
                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( -7.0, -4.0, 3.0, 5.0, -3.0, 7.0 ).
    {
        // Set Cartesian state (x,y,z,xdot,ydot,zdot).
        const Eigen::Vector6d cartesianState = ( Eigen::Vector6d( )
                                                 << -7.0, -4.0, 3.0, 5.0, -3.0, 7.0 ).finished( );

        // Set expected cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d expectedCylindricalState = ( Eigen::Vector6d( )
                                                           << std::sqrt( 49.0 + 16.0 ),
                                                           std::atan2( -4.0, -7.0 ) + 2.0 * PI,
                                                           3.0,
                                                           ( -7.0 * 5.0 + ( -4.0 ) * -3.0 )
                                                           / std::sqrt( 49.0 + 16.0 ),
                                                           ( -7.0 * -3.0 - ( -4.0 ) * 5.0 )
                                                           / sqrt( 49.0 + 16.0 ), 7.0 ).finished( );

        // Convert Cartesian to cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const Eigen::Vector6d convertedCylindricalState
                = coordinate_conversions::
                convertCartesianToCylindricalState( cartesianState );

        // Check that converted cylindrical state matches expected state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalState, convertedCylindricalState,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from spherical to Cartesian gradient.
BOOST_AUTO_TEST_CASE( test_SphericalToCartesianGradientConversion )
{
    // Define an arbitrary spherical gradient.
    const Eigen::Vector3d sphericalGradient( -2.438146967844150, 1.103964186650749e4,
                                             5.932496087870976e1 );

    // Define an arbitrary spherical position vector.
    const Eigen::Vector3d cartesianCoordinates( -7.0e6, 8.5e6, -6.5e6 );

    // Compute Cartesian gradient.
    const Eigen::Vector3d cartesianGradient = coordinate_conversions::
            convertSphericalToCartesianGradient( sphericalGradient, cartesianCoordinates );

    // Define expected Cartesian gradient. These values are obtained from the computations of
    // 'gx', 'gy' and 'gz' in the MATLAB function 'gravitysphericalharmonics'. This function is
    // described by Mathworks [2012].
    const Eigen::Vector3d expectedCartesianGradient( 1.334464111786447, -1.620429182163669,
                                                     1.240151677089496 );

    // Check if the computed Cartesian gradient matches the expected value.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCartesianGradient, cartesianGradient, 1e-15 );
}

// Test derivative of Cartesian gradient, keeping spherical gradient constant.
BOOST_AUTO_TEST_CASE( test_SphericalToCartesianGradientPartialDerivatives )
{
    // Define an arbitrary spherical gradient.
    const Eigen::Vector3d sphericalGradient( -2.438146967844150, 1.103964186650749e4,
                                             5.932496087870976e1 );

    // Define an arbitrary spherical position vector.
    const Eigen::Vector3d cartesianCoordinates( -7.0e6, 8.5e6, -6.5e6 );

    // Define perturbation for numerical computation of partial
    const Eigen::Vector3d cartesianCoordinatePerturbation( 10.0, 10.0, 10.0 );
    Eigen::Vector3d perturbedCartesianCoordinates;

    std::vector< Eigen::Matrix3d > matrixPartials;
    matrixPartials.resize( 3 );

    std::vector< Eigen::Matrix3d > subMatrices;
    subMatrices.resize( 3 );
    // Compute analytical partial (with sub-components)
    coordinate_conversions::getDerivativeOfSphericalToCartesianGradient( sphericalGradient, cartesianCoordinates, subMatrices );

    Eigen::Matrix3d uppPerturbedMatrix;
    for( unsigned int i = 0; i < 3; i++ )
    {
        // Compute partial numerically.
        perturbedCartesianCoordinates = cartesianCoordinates;
        perturbedCartesianCoordinates( i ) += cartesianCoordinatePerturbation( i );
        matrixPartials[ i ] = coordinate_conversions::getSphericalToCartesianGradientMatrix(
                    perturbedCartesianCoordinates );

        perturbedCartesianCoordinates = cartesianCoordinates;
        perturbedCartesianCoordinates( i ) -= cartesianCoordinatePerturbation( i );
        matrixPartials[ i ] -= coordinate_conversions::getSphericalToCartesianGradientMatrix(
                    perturbedCartesianCoordinates );

        matrixPartials[ i ] /= ( 2.0 * cartesianCoordinatePerturbation( i ) );

        // Normalize components for uniform tolerance.
        matrixPartials[ i ].block( 0, 0, 3, 1 ) = matrixPartials[ i ].block( 0, 0, 3, 1 ) / cartesianCoordinates.norm( );
        subMatrices[ i ].block( 0, 0, 3, 1 ) = subMatrices[ i ].block( 0, 0, 3, 1 ) / cartesianCoordinates.norm( );

        for( unsigned k = 0; k < 3; k++ )
        {
            for( unsigned l = 0; l < 3; l++ )
            {
                BOOST_CHECK_SMALL( std::fabs( matrixPartials[ i ]( k, l ) - subMatrices[ i ]( k, l ) ), 1.0E-23 );
            }

        }
    }
}

// Test conversion from Cartesian (x, y, z, xdot, ydot, zdot) to Spherical (radius, azimuth,
// elevation, radial velocity, azimuthal velocity, elevational velocity) state.
BOOST_AUTO_TEST_CASE( testCartesianToSphericalStateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::radiusSphericalCoordinateIndex;
    using coordinate_conversions::azimuthSphericalCoordinateIndex;
    using coordinate_conversions::elevationSphericalCoordinateIndex;
    using coordinate_conversions::radialVelocitySphericalCoordinateIndex;
    using coordinate_conversions::azimuthVelocitySphericalCoordinateIndex;
    using coordinate_conversions::elevationVelocitySphericalCoordinateIndex;
    using coordinate_conversions::convertCartesianToSphericalState;
    using std::sqrt;

    // Test 1: test conversion of ( 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).
    {
        // Set Cartesian state (x, y , z, xdot, ydot, zdot).
        const Eigen::Vector6d cartesianState
                = ( Eigen::Vector6d( ) << 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).finished( );

        // Set expected spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d expectedSphericalState
                = ( Eigen::Vector6d( ) << 1.0, 0.0, PI / 2.0, -9.0, 6.0, -5.0 ).finished( );

        // Convert Cartesian to spherical state.
        const Eigen::Vector6d convertedSphericalState
                = convertCartesianToSphericalState( cartesianState );

        // Check that converted spherical state matches expected spherical state.
        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState( radiusSphericalCoordinateIndex ),
                                    convertedSphericalState( radiusSphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedSphericalState( azimuthSphericalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState( elevationSphericalCoordinateIndex ),
                                    convertedSphericalState( elevationSphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).
    {
        // Set Cartesian state (x, y , z, xdot, ydot, zdot).
        const Eigen::Vector6d cartesianState
                = ( Eigen::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Set expected spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d expectedSphericalState
                = ( Eigen::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Convert Cartesian to spherical state ( r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d convertedSphericalState
                = convertCartesianToSphericalState( cartesianState );

        // Check that converted spherical state matches expected spherical state.
        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState( radiusSphericalCoordinateIndex ),
                                    convertedSphericalState( radiusSphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedSphericalState( azimuthSphericalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedSphericalState( elevationSphericalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( -1.0, -1.0, sqrt( 2.0 ), -1.0, 1.0, sqrt( 2.0 ) ).
    {
        // Set Cartesian state ( x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d cartesianState
                = ( Eigen::Vector6d( )
                    << -1.0, -1.0, sqrt( 2.0 ), -1.0, 1.0, sqrt( 2.0 ) ).finished( );

        // Set expected spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d expectedSphericalState
                = ( Eigen::Vector6d( )
                    << 2.0, -PI * 3.0 / 4.0, PI / 4.0, 1.0, -sqrt( 2.0 ), 1.0 ).finished( );

        // Convert Cartesian to spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d convertedSphericalState
                = convertCartesianToSphericalState( cartesianState );

        // Check that converted spherical state matches the expected spherical state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedSphericalState, convertedSphericalState,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from spherical (radius, azimuth, elevation, radial velocity, azimuthal velocity,
// elevational velocity) to Cartesian state (x, y, z, xdot, ydot, zdot).
BOOST_AUTO_TEST_CASE( testSphericalToCartesianStateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::xCartesianCoordinateIndex;
    using coordinate_conversions::yCartesianCoordinateIndex;
    using coordinate_conversions::zCartesianCoordinateIndex;
    using coordinate_conversions::xDotCartesianCoordinateIndex;
    using coordinate_conversions::yDotCartesianCoordinateIndex;
    using coordinate_conversions::zDotCartesianCoordinateIndex;
    using coordinate_conversions::convertSphericalToCartesianState;
    using std::sqrt;

    // Test 1: test conversion of ( 1.0, 0.0, pi/2, -9.0, 6.0, -5.0 ).
    {
        // Set Spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d sphericalState
                = ( Eigen::Vector6d( ) << 1.0, 0.0, PI / 2.0, -9.0, 6.0, -5.0 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d expectedCartesianState
                = ( Eigen::Vector6d( ) << 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).finished( );

        // Convert spherical to Cartesian state.
        const Eigen::Vector6d convertedCartesianState
                = convertSphericalToCartesianState( sphericalState );

        // Check that converted Cartesian state matches expected state.
        BOOST_CHECK_SMALL( convertedCartesianState( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( zCartesianCoordinateIndex ),
                                    convertedCartesianState( zCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( xDotCartesianCoordinateIndex ),
                                    convertedCartesianState( xDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( yDotCartesianCoordinateIndex ),
                                    convertedCartesianState( yDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( zDotCartesianCoordinateIndex ),
                                    convertedCartesianState( zDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).
    {
        // Set spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d sphericalState
                = ( Eigen::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Set expected Cartesian state ( x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d expectedCartesianState
                = ( Eigen::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Convert spherical to Cartesian state.
        const Eigen::Vector6d convertedCartesianState
                = convertSphericalToCartesianState( sphericalState );

        // Check that converted Cartesian state matches expected state.
        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( xCartesianCoordinateIndex ),
                                    convertedCartesianState( xCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( zCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( xDotCartesianCoordinateIndex ),
                                    convertedCartesianState( xDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( yDotCartesianCoordinateIndex ),
                                    convertedCartesianState( yDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( zDotCartesianCoordinateIndex ),
                                    convertedCartesianState( zDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( 2.0, -3*pi/4, pi/4, 1.0, -sqrt( 2.0 ), 1.0 ).
    {
        // Set spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const Eigen::Vector6d sphericalState
                = ( Eigen::Vector6d( )
                    << 2.0, -PI * 3.0 / 4.0, PI / 4.0, 1.0, -sqrt( 2.0 ), 1.0 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const Eigen::Vector6d expectedCartesianState
                = ( Eigen::Vector6d( )
                    << -1.0, -1.0, sqrt( 2.0 ), -1.0, 1.0, sqrt( 2.0 ) ).finished( );

        // Convert spherical to Cartesian state.
        const Eigen::Vector6d convertedCartesianState
                = convertSphericalToCartesianState( sphericalState );

        // Check that converted Cartesian state matches the expected state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCartesianState, convertedCartesianState,
                                           1.0e-15 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat