// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Point Example - showing geographic (latitude longitude) points

#include <iostream>
#include <iomanip>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/extensions/algorithms/parse.hpp>
#include <boost/geometry/extensions/gis/latlong/latlong.hpp>

// Formula to get the course (direction) between two points.
// This might be a GGL-function in the future.
template <typename P1, typename P2>
inline double get_course(P1 const& p1, P2 const& p2)
{
    double const& lat1 = boost::geometry::get_as_radian<1>(p1);
    double const& lon1 = boost::geometry::get_as_radian<0>(p1);
    double const& lat2 = boost::geometry::get_as_radian<1>(p2);
    double const& lon2 = boost::geometry::get_as_radian<0>(p2);
    // http://williams.best.vwh.net/avform.htm#Crs
    return atan2(sin(lon1-lon2)*cos(lat2),
       cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2));
}

// Formula to calculate the point at a distance/angle from another point
// This might be a GGL-function in the future.
template <typename P1, typename P2>
inline void point_at_distance(P1 const& p1,
        double distance, double tc, double radius,
        P2& p2)
{
    double const two_pi = 2.0 * boost::geometry::math::pi<double>();
    double earth_perimeter = radius * two_pi;
;
    double d = (distance / earth_perimeter) * two_pi;
    double const& lat1 = boost::geometry::get_as_radian<1>(p1);
    double const& lon1 = boost::geometry::get_as_radian<0>(p1);

    // http://williams.best.vwh.net/avform.htm#LL
    double lat = asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc));
    double dlon = atan2(sin(tc)*sin(d)*cos(lat1),cos(d)-sin(lat1)*sin(lat));
    double lon = lon1 - dlon;

    boost::geometry::set_from_radian<1>(p2, lat);
    boost::geometry::set_from_radian<0>(p2, lon);
}

int main()
{
    using namespace boost::geometry;

    typedef model::ll::point<degree> latlon_point;

    latlon_point paris;

    // Assign coordinates to the latlong point, using the methods lat and lon
    // Paris 48 52' 0" N, 2 19' 59" E
    paris.lat(dms<north>(48, 52, 0));
    paris.lon(dms<east>(2, 19, 59));

    std::cout << "Paris: " << boost::geometry::dsv(paris) << std::endl;

    // Constructor using explicit latitude/longitude
    // Lima 12 2' 36" S, 77 1' 42" W
    latlon_point lima(
            latitude<>(dms<south>(12, 2, 36)),
            longitude<>(dms<west>(77, 1, 42)));

    std::cout << "Lima: " << boost::geometry::dsv(lima) << std::endl;

    // Construction with parse utiity
    latlon_point amsterdam = parse<latlon_point>("52 22'23\"N", "4 53'32\"E");
    std::cout << "Amsterdam: " << boost::geometry::dsv(amsterdam) << std::endl;

    // Calculate the distance using the default strategy (Andoyer), and Vincenty
    std::cout << std::setprecision(9);
    std::cout << "Distance Paris-Lima, Andoyer (default) "
        << 0.001 * distance(paris, lima)
        << " km" << std::endl;

    std::cout << "Distance Paris-Lima, Vincenty "
        << 0.001 * distance(paris, lima, strategy::distance::vincenty<double>())
        << " km" << std::endl;

    // Using great circle (=haversine), this is less precise because earth is not a sphere
    double const average_earth_radius = 6372795.0;
    std::cout << "Distance Paris-Lima, great circle "
        << 0.001 * distance(paris, lima, strategy::distance::haversine<double>(average_earth_radius))
        << " km" << std::endl;

    // Convert a latlong point to radians. This might be convenient, although algorithms
    // are transparent on degree/radians
    model::ll::point<radian> paris_rad;
    transform(paris, paris_rad);
    std::cout << "Paris in radians: " << boost::geometry::dsv(paris_rad) << std::endl;

    model::ll::point<radian> amsterdam_rad;
    transform(amsterdam, amsterdam_rad);
    std::cout << "Amsterdam in radians: " << boost::geometry::dsv(amsterdam_rad) << std::endl;

    std::cout << "Distance Paris-Amsterdam, (degree) " << 0.001 * distance(paris, amsterdam) << " km" << std::endl;
    std::cout << "Distance Paris-Amsterdam, (radian) " << 0.001 * distance(paris_rad, amsterdam_rad) << " km" << std::endl;

    std::cout << "Distance Paris-Amsterdam, (mixed) " << 0.001 * distance(paris, amsterdam_rad) << " km" << std::endl;

    // Other way round: have Amsterdam and go 430 km to the south (i.e. first calculate direction)
    double tc = get_course(amsterdam, paris);
    std::cout << "Course: " << (tc * boost::geometry::math::r2d) << std::endl;

    latlon_point paris_calculated;
    point_at_distance(amsterdam, 430 * 1000.0, tc, average_earth_radius, paris_calculated);
    std::cout << "Paris calculated (degree): " << boost::geometry::dsv(paris_calculated) << std::endl;

    return 0;
}


#ifndef MLT_UTILS_OPTIMIZERS_GRADIENT_DESCENT_UPDATES_HPP
#define MLT_UTILS_OPTIMIZERS_GRADIENT_DESCENT_UPDATES_HPP

#include <Eigen/Core>

#include "../../defs.hpp"

namespace mlt {
namespace utils {
namespace optimizers {
	// Implementation of the Vanilla Gradient Descent update rule
	class VanillaGradientDescentUpdate {
	public:
		void restart() {}

		auto step(double learning_rate, MatrixXdRef gradient) {
			return (-learning_rate * gradient).eval();
		}
	};

	// Implementation of the Momentum Gradient Descent update rule
	// Parameters:
	// - double mu: amount of momentum applied on each descent
	class MomentumGradientDescentUpdate {
	public:
		MomentumGradientDescentUpdate(double mu = 0.9) : _mu(mu), _init(false) {}

		void restart() { _init = false; }

		auto step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			_cache = _mu * _cache - learning_rate * gradient;
			_init = true;
			return _cache;
		}

	protected:
		bool _init;
		double _mu;
		MatrixXd _cache;
	};

	// Implementation of the Nesterov's Accelerated Momentum Gradient Descent update rule
	// Parameters:
	// - double mu: amount of momentum applied on each descent
	class NesterovMomentumGradientDescentUpdate {
	public:
		NesterovMomentumGradientDescentUpdate(double mu = 0.9) : _mu(mu), _init(false) {}

		void restart() { _init = false; }

		auto step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			VectorXd velocity_prev = _cache;
			_cache = _mu * _cache - learning_rate * gradient;
			_init = true;
			return (-_mu * velocity_prev + (1 + _mu) * _cache).eval();
		}

	protected:
		bool _init;
		double _mu;
		MatrixXd _cache;
	};

	// Implementation of the Adagrad Gradient Descent update rule
	class AdagradGradientDescentUpdate {
	public:
		AdagradGradientDescentUpdate() : _init(false) {}

		void restart() { _init = false; }

		auto step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			_cache += gradient.array().pow(2).matrix();
			_init = true;
			return (-learning_rate * (gradient.array() / (_cache.array() + 1e-8).sqrt()).matrix()).eval();
		}

	protected:
		bool _init;
		MatrixXd _cache;
	};

	// Implementation of the RMSProp Gradient Descent update rule
	// Parameters:
	// - double decay_rate: the decay rate of the moving average of squared gradients at each step
	class RMSPropGradientDescentUpdate {
	public:
		RMSPropGradientDescentUpdate(double decay_rate = 0.9) : _decay_rate(decay_rate), _init(false) {}

		void restart() { _init = false; }

		MatrixXd step(double learning_rate, MatrixXdRef gradient) {
			if (!_init || _cache.rows() != gradient.rows() || _cache.cols() != gradient.cols()) {
				_cache = MatrixXd::Zero(gradient.rows(), gradient.cols());
			}

			_cache = _decay_rate * _cache + (1 - _decay_rate) * gradient.array().pow(2).matrix();
			_init = true;
			return (-learning_rate * (gradient.array() / (_cache.array() + 1e-8).sqrt()).matrix()).eval();
		}

	protected:
		bool _init;
		double _decay_rate;
		MatrixXd _cache;
	};
}
}
}
#endif


//  Copyright Madhur Chauhan 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/fibonacci.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/test/tools/old/interface.hpp>
#include <cstdint>
#include <exception>
#define BOOST_TEST_MODULE Fibonacci_Test_Module
#include <boost/test/included/unit_test.hpp>

using boost::math::fibonacci;
using boost::math::unchecked_fibonacci;
using namespace boost::multiprecision;
typedef cpp_int BST;
typedef number<backends::cpp_int_backend<2048, 2048, unsigned_magnitude, unchecked>> BST_2048;

// Some sanity checks using OEIS A000045
BOOST_AUTO_TEST_CASE(sanity_checks) {
    BOOST_TEST(fibonacci<char>(0) == 0);    // Base case
    BOOST_TEST(fibonacci<int>(1) == 1);     // Base case
    BOOST_TEST(fibonacci<int16_t>(2) == 1); // Base Case
    BOOST_TEST(fibonacci<int16_t>(3) == 2); // First computation
    BOOST_TEST(fibonacci<int16_t>(10) == 55);
    BOOST_TEST(fibonacci<int16_t>(15) == 610);
    BOOST_TEST(fibonacci<int16_t>(18) == 2584);
    BOOST_TEST(fibonacci<int32_t>(40) == 102334155);
}

// Tests unchecked_fibonacci by computing naively
BOOST_AUTO_TEST_CASE(big_integer_check) {
    // GMP is used as type for fibonacci and cpp_int is for naive computation
    BST val = 0, a = 0, b = 1;
    for (int i = 0; i <= 1e4; ++i) {
        BOOST_TEST(fibonacci<BST>(i) == a);
        val = b, b += a, a = val;
    }
}

// Check for overflow throw using magic constants
BOOST_AUTO_TEST_CASE(overflow_check) {

    // 1. check for unsigned integer overflow
    BOOST_CHECK_NO_THROW(fibonacci<uint64_t>(93));
    BOOST_CHECK_THROW(fibonacci<uint64_t>(94), std::exception);
    BOOST_CHECK_NO_THROW(unchecked_fibonacci<uint64_t>(94));

    // 2. check for signed integer overflow
    BOOST_CHECK_NO_THROW(fibonacci<int64_t>(91));
    // BOOST_CHECK_THROW(fibonacci<int64_t>(92), std::exception); // this should be the correct value but imprecisions
    BOOST_CHECK_THROW(fibonacci<int64_t>(93), std::exception);
    // In UBSAN this will error out, but it is expected
    BOOST_CHECK_NO_THROW(unchecked_fibonacci<int64_t>(93));

    // 3. check for floating point (double)
    BOOST_CHECK_NO_THROW(fibonacci<double>(78));
    BOOST_CHECK_THROW(fibonacci<double>(79), std::exception);
    BOOST_CHECK_NO_THROW(unchecked_fibonacci<double>(79));

    // 4. check using boost's multiprecision unchecked integer
    BOOST_CHECK_NO_THROW(fibonacci<BST_2048>(2950));
    // BOOST_CHECK_THROW(fibonacci<T>(2951), std::exception); // this should be the correct value but imprecisions
    BOOST_CHECK_THROW(fibonacci<BST_2048>(2952), std::exception);
    BOOST_CHECK_NO_THROW(unchecked_fibonacci<BST_2048>(2952));
}

BOOST_AUTO_TEST_CASE(generator_check) {
    // first 5 values
    boost::math::fibonacci_generator<BST_2048> gen;
    for (int i : {0, 1, 1, 2, 3, 5, 8, 13, 21}) {
        BOOST_TEST(gen() == i);
    }

    // test whether the generator is set correctly to the given index --- next
    const int next = 1000; // next <=2950 (checked from test above)
    gen.set(next);
    BST_2048 a = fibonacci<BST_2048>(next), b = fibonacci<BST_2048>(next + 1);
    for (int i = next; i < next + 50; ++i) {
        BOOST_TEST(gen() == a);
        swap(a, b);
        b += a;
    }

    // shift the generator back to next and check again
    a = fibonacci<BST_2048>(next), b = fibonacci<BST_2048>(next + 1);
    gen.set(next);
    for (int i = next; i < next + 50; ++i) {
        BOOST_TEST(gen() == a);
        swap(a, b);
        b += a;
    }
}

BOOST_AUTO_TEST_CASE(constexpr_check) {
    constexpr int x = boost::math::unchecked_fibonacci<int>(32);
    BOOST_TEST(x == 2178309);

    constexpr double y = boost::math::unchecked_fibonacci<double>(40);
    BOOST_TEST(y == 102334155.0);

    // checked fibonacci can't be constexpr because of non-constexpr
    // dependency in detail::log_2, detail::fib_bits_phi, detail::fib_bits_deno
}


//
// Created by peri on 11/17/17.
//

#ifndef VANILLA_ACTIVATION_FUNCTIONS_HPP
#define VANILLA_ACTIVATION_FUNCTIONS_HPP

#include <cmath>
#include <iostream>
#include <boost/multi_array.hpp>
#include "boost/array.hpp"
#include <boost/next_prior.hpp>
#include "boost/cstdlib.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

class activation_functions{

public:
    template <typename Array>
    void ReLu(Array& A)
    {
        const auto n = A.data() + A.num_elements();
        for (auto i = A.data(); i != n; ++i){
            if(*i < 0){
                *i = 0.0;
            }
        }
    }

public:
    template <typename Array>
    void sigmoid(Array& A)
    {
        const auto n = A.data() + A.num_elements();
        for (auto i = A.data(); i != n; ++i){
            auto x = *i;
            if(x < 0){
                *i = expl(x) / (1.0 + expl(x));
            }
            else{
                *i = 1.0 / (1.0 + expl(-1.0*x));
            }

        }
    }

public:
    template <typename Array>
    void tanh(Array& A)
    {
        const auto n = A.data() + A.num_elements();
        for (auto i = A.data(); i != n; ++i){
            *i = (2*expl(-1 *i) - 1)/(2*expl(-1 *i) + 1);
        }
    }

public:
    template <typename T>
    void ReLu(boost::numeric::ublas::matrix<T>& M)
    {
        for(unsigned i = 0; i < M.size1(); ++ i){
            for(unsigned j = 0; j < M.size2(); ++ j){
                if(M(i, j) < 0){
                    M(i, j) = 0;
                }
            }
        }
    }

public:
    template <typename T>
    void sigmoid(boost::numeric::ublas::matrix<T>& M)
    {
        for(unsigned i = 0; i < M.size1(); ++ i){
            for (unsigned j = 0; j < M.size2(); ++j) {
                auto x = M(i, j);
                if(x < 0){
                    M(i, j) = expl(x) / (1.0 + expl(x));
                }
                else {
                    M(i, j) = 1.0 / (1.0 + expl(-1.0*x));
                }
            }
        }

    }

public:
    template <typename T>
    void tanh(boost::numeric::ublas::matrix<T>& M)
    {
        for(unsigned i = 0; i < M.size1(); ++ i){
            for(unsigned j = 0; j < M.size2(); ++ j) {
                M(i, j) = (2 * expl(-1 * M(i, j)) - 1) / (2 * expl(-1 * M(i, j)) + 1);
            }
        }
    }

};
#endif //ACTIVATION_FUNCTIONS_HPP


#pragma once

#include <Eigen/Dense>

#include <cmath>
#include <iostream>
#include <type_traits>

namespace pcv
{

template<class NumericType0, class NumericType1>
double getEuclideanDistance(
    const Eigen::Matrix<NumericType0, 2, 1> &point0,
    const Eigen::Matrix<NumericType1, 2, 1> &point1)
{
    return std::sqrt(
            std::pow(point0.x()-point1.x(), 2)
          + std::pow(point0.y()-point1.y(), 2));
}

template<class NumericType>
std::optional<Eigen::Vector2d> getSegmentIntersectionPoint2(
    const Eigen::Matrix<NumericType, 2, 1> &segmentOnePoint0,
    const Eigen::Matrix<NumericType, 2, 1> &segmentOnePoint1,
    const Eigen::Matrix<NumericType, 2, 1> &segmentTwoPoint0,
    const Eigen::Matrix<NumericType, 2, 1> &segmentTwoPoint1,
    double error = 1e-10)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    Eigen::Matrix<double, 2, 2> linearEquationsLhs;
    linearEquationsLhs <<
        segmentOnePoint0.y() - segmentOnePoint1.y(),
        segmentOnePoint1.x() - segmentOnePoint0.x(),
        segmentTwoPoint0.y() - segmentTwoPoint1.y(),
        segmentTwoPoint1.x() - segmentTwoPoint0.x();

    Eigen::Vector2d linearEquationsRhs;
    linearEquationsRhs <<
          segmentOnePoint0.y() * (segmentOnePoint1.x() - segmentOnePoint0.x())
        - segmentOnePoint0.x() * (segmentOnePoint1.y() - segmentOnePoint0.y()),
          segmentTwoPoint0.y() * (segmentTwoPoint1.x() - segmentTwoPoint0.x())
        - segmentTwoPoint0.x() * (segmentTwoPoint1.y() - segmentTwoPoint0.y());

    Eigen::Vector2d intersection =
        linearEquationsLhs.colPivHouseholderQr().solve(linearEquationsRhs);

    if (intersection.x() >= std::min(segmentOnePoint0.x(), segmentOnePoint1.x())-error
        && intersection.x() <= std::max(segmentOnePoint0.x(), segmentOnePoint1.x())+error
        && intersection.y() >= std::min(segmentOnePoint0.y(), segmentOnePoint1.y())-error
        && intersection.y() <= std::max(segmentOnePoint0.y(), segmentOnePoint1.y())+error
        && intersection.x() >= std::min(segmentTwoPoint0.x(), segmentTwoPoint1.x())-error
        && intersection.x() <= std::max(segmentTwoPoint0.x(), segmentTwoPoint1.x())+error
        && intersection.y() >= std::min(segmentTwoPoint0.y(), segmentTwoPoint1.y())-error
        && intersection.y() <= std::max(segmentTwoPoint0.y(), segmentTwoPoint1.y())+error)
    {
        return intersection;
    }

    return {};
}

}


#include <cmath>
#include <catch2/catch.hpp>
#include <Eigen/Dense>
#include "configure.h"
#include "factory.h"
#include "normal_multivar.h"
#include "numeric_utils.h"

TEST_CASE("Test generation of random numbers", "[RandomNumbers]") {
  // Initialize the factories
  config::initialize();
  // Seed value for repeatability
  int seed = 100;
  auto random_generator = Factory<numeric_utils::RandomGenerator, int>::instance()
    ->create("MultivariateNormal", std::move(seed));
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> random_numbers;

  SECTION("Generate normally distributed random numbers for single random "
          "variable") {
    Eigen::VectorXd means(1);
    Eigen::MatrixXd cov(1,1);
    means(0) = 1.789;
    cov(0, 0) = 0.0123;

    // Single random variable
    random_generator->generate(random_numbers, means, cov, 100);

    double average = 0.0;
    for (unsigned int i = 0; i < random_numbers.size(); ++i) {
      average = average + random_numbers(i);
    }
    average = average / random_numbers.size();
    REQUIRE(average == Approx(means(0)).epsilon(0.01));
  }

  SECTION("Generate normally distributed random numbers for multiple "
          "uncorrelated random variables", "[RandomNumbers]") {
    Eigen::VectorXd means(4);
    Eigen::MatrixXd cov = Eigen::MatrixXd::Zero(4,4);
    means << 1.789, 0.01, -10012.7, 702;
    cov(0, 0) = 0.0123 * 0.0123;
    cov(1, 1) = 0.00005;
    cov(2, 2) = 50.0;
    cov(3, 3) = 25;

    // Random variable vector
    random_generator->generate(random_numbers, means, cov, 1000);

    std::vector<double> averages(means.size());
    for (unsigned int i = 0; i < random_numbers.cols(); ++i) {
      for (unsigned int j = 0; j < means.size(); ++j) {
	averages[j] = averages[j] + random_numbers(j, i);
      }
    }

    REQUIRE(averages[0] / random_numbers.cols() == Approx(means(0)).epsilon(0.01));
    REQUIRE(averages[1] / random_numbers.cols() == Approx(means(1)).epsilon(0.02));
    REQUIRE(averages[2] / random_numbers.cols() == Approx(means(2)).epsilon(0.01));
    REQUIRE(averages[3] / random_numbers.cols() == Approx(means(3)).epsilon(0.01));
  }

  SECTION("Generate normally distributed random numbers for correlated random "
          "variable", "[RandomNumbers]") {
    Eigen::VectorXd means(3);
    Eigen::MatrixXd cov = Eigen::MatrixXd::Zero(3, 3);
    means << 64.0, 300.0, 60.0;

    // Try bad COV matrix
    // clang-format off
    cov << 1.0, 1.0, 0.0,
           1.0, 1.0, 1.0,
           0.0, 1.0, 1.0;
    // clang-format on

    bool success = random_generator->generate(random_numbers, means, cov, 250000);
    REQUIRE(success == false);

    // Try good COV matrix
    // clang-format off
    cov << 504.0, 360.0, 180.0,
           360.0, 360.0, 0.0,
           180.0, 0.0, 720.0;
    // clang-format on
    success = random_generator->generate(random_numbers, means, cov, 250000);
    REQUIRE(success == true);

    Eigen::VectorXd averages = Eigen::VectorXd::Zero(means.size());
    for (unsigned int i = 0; i < means.size(); ++i) {
      for (unsigned int j = 0; j < random_numbers.cols(); ++j) {
	averages(i) = averages(i) + random_numbers(i, j);
      }
    }

    averages = averages / random_numbers.cols();

    REQUIRE(averages(0) == Approx(means(0)).epsilon(0.01));
    REQUIRE(averages(1) == Approx(means(1)).epsilon(0.01));
    REQUIRE(averages(2) == Approx(means(2)).epsilon(0.01));

    // Compute covariance matrix from random values
    Eigen::MatrixXd deviation_scores =
        Eigen::MatrixXd::Zero(random_numbers.rows(), random_numbers.cols());

    for (unsigned int i = 0; i < random_numbers.cols(); ++i) {
      deviation_scores.col(i) = random_numbers.col(i) - averages;
    }

    Eigen::MatrixXd calculated_cov =
        (deviation_scores * deviation_scores.transpose()) / random_numbers.cols();

    REQUIRE(cov.lpNorm<2>() == Approx(calculated_cov.lpNorm<2>()).epsilon(0.01));
  }

  SECTION("Check that number generated using the same seed match", "[RandomNumbers]") {
    int seed = 500;
    auto random_generator1 =
        Factory<numeric_utils::RandomGenerator, int>::instance()->create(
            "MultivariateNormal", std::move(seed));
    auto random_generator2 =
        Factory<numeric_utils::RandomGenerator, int>::instance()->create(
            "MultivariateNormal", std::move(seed));

    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> random_numbers1;
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> random_numbers2;
    Eigen::VectorXd means(1);
    Eigen::MatrixXd cov(1,1);
    means(0) = 1.789;
    cov(0, 0) = 0.0123;

    random_generator1->generate(random_numbers1, means, cov, 100);
    random_generator2->generate(random_numbers2, means, cov, 100);
    REQUIRE(random_numbers1 == random_numbers2);
  }
}


#include <Eigen/Core>
#include <iostream>

using namespace Eigen;

int main()
{
  for (int size=1; size<=4; ++size)
  {
    MatrixXi m(size,size+1);         // a (size)x(size+1)-matrix of int's
    for (int j=0; j<m.cols(); ++j)   // loop over columns
      for (int i=0; i<m.rows(); ++i) // loop over rows
        m(i,j) = i+j*m.rows();       // to access matrix coefficients,
                                     // use operator()(int,int)
    std::cout << m << "\n\n";
  }

  VectorXf v(4); // a vector of 4 float's
  // to access vector coefficients, use either operator () or operator []
  v[0] = 1; v[1] = 2; v(2) = 3; v(3) = 4;
  std::cout << "\nv:\n" << v << std::endl;
}


#include <armadillo>
#include <cassert>

namespace mona
{
    auto meshgrid(const arma::fvec& x, const arma::fvec& y) -> std::tuple<arma::fmat, arma::fmat>
    {
        arma::fmat x_mat(y.size(), x.size());
        arma::fmat y_mat(y.size(), x.size());

        x_mat.each_row() = x.t();
        y_mat.each_col() = y;

        return {x_mat, y_mat};
    }

    template <typename Callable>
    auto apply(Callable f, const arma::fmat& x, const arma::fmat& y) -> arma::fmat
    {
        assert(x.n_rows == y.n_rows);
        assert(x.n_cols == y.n_cols);

        arma::fmat z(x.n_rows, x.n_cols);

        for(auto j = 0ul; j < x.n_cols; j++)
        {
            for(auto i = 0ul; i < x.n_rows; i++)
            {
                z(i , j) = f(x(i, j), y(i, j));
            }
        }
        return z;
    }

    auto linspace(float a, float b, size_t n)
    {
        assert(b > a);
        assert(n > 1);

        arma::fvec res(n);
        const auto step = (b - a) / (n - 1);
        auto val = a;
        for(auto& e: res)
        {
            e = val;
            val += step;
        }
        // make the last value match b exactly
        res[n - 1] = b;
        return res;
    }
}


#include <ros/ros.h>
#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include <stdlib.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <Eigen/LU>
#include <math.h>
#include <stdio.h>
//#include <turtlesim/Pose.h>
#include <Eigen/Geometry>
#include "gazebo_msgs/ApplyJointEffort.h"
#include <math.h>

using namespace Eigen;

//For geometry_msgs::Twist using:
// 		dummy.linear.x
// 		dummy.linear.y
// 		dummy.angular.z
geometry_msgs::Twist robot_position;
geometry_msgs::Twist velocity_msg;

//rate_hz assignment
double rate_hz = 1;

// Transform robot velocities (x,y,w) to motor velocities (m1,m2,m3,m4)
double* getMotorValue(int x_velocity, int y_velocity, int w_velocity){
	double deg1 = 3 * M_PI / 10;
	double deg2 = 3 * M_PI / 10;
	double deg3 = 7 * M_PI / 30;
	double deg4 = 7 * M_PI / 30;
	double s1 = sin(deg1), c1 = cos(deg1);
	double s2 = sin(deg2), c2 = cos(deg2);
	double s3 = sin(deg3), c3 = cos(deg3);
	double s4 = sin(deg4), c4 = cos(deg4);
	double R = 8.5;
	double r = 3.4;

	double velXMod = x_velocity * 3.5 / ( 2 * M_PI * r );
    double velYMod = y_velocity * 3.5 / ( 2 * M_PI * r );
    double velWMod = w_velocity * 3.5 / ( 2 * M_PI * r );

    velWMod = R * velWMod;
    double velMots[4];

    velMots[0] = (double)( s1 * velXMod + c1 * velYMod + velWMod);
    velMots[1] = (double)(-s2 * velXMod + c2 * velYMod + velWMod);
    velMots[2] = (double)(-s3 * velXMod - c3 * velYMod + velWMod);
    velMots[3] = (double)( s4 * velXMod - c4 * velYMod + velWMod);

    return velMots;
}

void get_vel_vec(const geometry_msgs::Twist& msg) {
	velocity_msg.linear.x = msg.linear.x;
	velocity_msg.linear.y = msg.linear.y;
    velocity_msg.angular.z = msg.linear.z;
}

int main(int argc, char **argv){
	ros::init(argc,argv,"robot_velocity_node");
	ros::NodeHandle nh;
	ROS_INFO_STREAM("robot_velocity_node initialized");
	ROS_INFO_STREAM(ros::this_node::getName());

	// Suscribe to Gazebo service ApplyJointEffor
	ros::ServiceClient client = nh.serviceClient<gazebo_msgs::ApplyJointEffort>("/gazebo/apply_joint_effort");
	gazebo_msgs::ApplyJointEffort eff_msg[4];

	ros::Subscriber sub_vel = nh.subscribe("/target_vel_topic", 1000, &get_vel_vec);

	double tiempo = 0;

    //define the max speed
	double cruise_speed = 50;

    //define the rate
	ros::Rate rate(rate_hz);
	ros::Time start_time ;
	ros::Duration duration ;

	double effort[4];

	while (ros::ok())
	{
	double* velMots = getMotorValue(velocity_msg.linear.x,velocity_msg.linear.y,velocity_msg.angular.z);

	effort [0] = velMots[0];
	effort [1] = velMots[1];
	effort [2] = velMots[2];
	effort [3] = velMots[3];

		if(client.exists()){

			start_time.sec = 0;
			start_time.nsec = 0;
			duration.sec = 1/rate_hz;
			duration.nsec = 0;

			// Wheel-Joint 1
			eff_msg[0].request.joint_name = "chassis_JOINT_1";
			eff_msg[0].request.duration = duration;
			eff_msg[0].request.effort = effort[0];
			eff_msg[0].request.start_time = start_time;

			// Wheel-Joint 2
			eff_msg[1].request.joint_name = "chassis_JOINT_2";
			eff_msg[1].request.duration = duration;
			eff_msg[1].request.effort = effort[1];
			eff_msg[1].request.start_time = start_time;

			// Wheel-Joint 3
			eff_msg[2].request.joint_name = "chassis_JOINT_3";
			eff_msg[2].request.duration = duration;
			eff_msg[2].request.effort = effort[2];
			eff_msg[2].request.start_time = start_time;

			// Wheel-Joint 4
			eff_msg[3].request.joint_name = "chassis_JOINT_4";
			eff_msg[3].request.duration = duration;
			eff_msg[3].request.effort = effort[03];
			eff_msg[3].request.start_time = start_time;

			client.call(eff_msg[0]);
			client.call(eff_msg[1]);
			client.call(eff_msg[2]);
			client.call(eff_msg[3]);
			ROS_INFO_STREAM("Joints ==> 1: " << ((eff_msg[0].response.success == 1) ? "TRUE" : "FALSE") <<
			" 2: " << ((eff_msg[1].response.success == 1) ? "TRUE" : "FALSE") <<
			" 3: " << ((eff_msg[2].response.success == 1) ? "TRUE" : "FALSE") <<
			" 4: " << ((eff_msg[3].response.success == 1) ? "TRUE" : "FALSE"));
		}

		ros::spinOnce();
		rate.sleep();
    }
    return 0;
}


#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main() {
  Matrix3f A;
  Vector3f b;
  A << 1, 2, 3, 4, 5, 6, 7, 8, 10;
  b << 3, 3, 4;
  cout << "Here is the matrix A:\n" << A << endl;
  cout << "Here is the vector b:\n" << b << endl;
  Vector3f x = A.colPivHouseholderQr().solve(b);
  cout << "The solution is:\n" << x << endl;
}


#include "stdafx.h"
#include "CppUnitTest.h"
#include "splines.h"
#include <iostream>
#include <iomanip>
#include <limits>
#include <vector>
 #include <fstream>
#include <array>
#include <Eigen/Dense>
#include <Eigen/StdVector>

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

namespace UnitTest1
{
	TEST_CLASS(UnitTest1)

	{
	public:
		double max_double = std::numeric_limits<double>::max();

		TEST_METHOD(Cubic2dCoefficient)
		{
			std::ifstream in_file("cubic2d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("cubic2dcoeff.points");
			Eigen::Vector2d control_point;
			std::vector<Eigen::Vector2d> control_points;
			while (in_file >> control_point.x() >> control_point.y()) {
				control_points.push_back(control_point);
			}
			std::vector<double> coefficients;
			for (size_t i = 0; i < control_points.size() / 4; ++i) {
				coefficients = bezier::GetCoefficients<double>(control_points, i, 3, bezier::k2d);
				Assert::IsTrue(coefficients.size() == 8);
				out_file << std::fixed << std::setprecision(14);
				for (auto j : coefficients) {
					out_file << j << '\n';
				}
			}
		}

		TEST_METHOD(Cubic3dCoefficient)
		{
			std::ifstream in_file("cubic3d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("cubic3dcoeff.points");
			Eigen::Vector3d control_point;
			std::vector<Eigen::Vector3d> control_points;
			while (in_file >> control_point.x() >> control_point.y() >> control_point.z()) {
				control_points.push_back(control_point);
			}
			std::vector<double> coefficients;
			for (size_t i = 0; i < control_points.size() / 4; ++i) {
				coefficients = bezier::GetCoefficients<double>(control_points, i);
				Assert::IsTrue(coefficients.size() == 12);
				out_file << std::fixed << std::setprecision(14);
				for (auto j : coefficients) {
					out_file << j << '\n';
				}
			}
		}

		TEST_METHOD(Quad2dCoefficient)
		{
			std::ifstream in_file("quad2d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("quad2dcoeff.points");
			Eigen::Vector2d control_point;
			std::vector<Eigen::Vector2d> control_points;
			while (in_file >> control_point.x() >> control_point.y()) {
				control_points.push_back(control_point);
			}
			std::vector<double> coefficients;
			for (size_t i = 0; i < control_points.size() / 3; ++i) {
				coefficients = bezier::GetCoefficients<double>(control_points, i, 2, bezier::k2d);
				Assert::IsTrue(coefficients.size() == 6);
				out_file << std::fixed << std::setprecision(14);
				for (auto j : coefficients) {
					out_file << j << '\n';
				}
			}
		}

		TEST_METHOD(Quad3dCoefficient)
		{
			std::ifstream in_file("quad3d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("quad3dcoeff.points");
			Eigen::Vector3d control_point;
			std::vector<Eigen::Vector3d> control_points;
			while (in_file >> control_point.x() >> control_point.y() >> control_point.z()) {
				control_points.push_back(control_point);
			}
			std::vector<double> coefficients;
			for (size_t i = 0; i < control_points.size() / 3; ++i) {
				coefficients = bezier::GetCoefficients<double>(control_points, i, 2, bezier::k3d);
				Assert::IsTrue(coefficients.size() == 9);
				out_file << std::fixed << std::setprecision(14);
				for (auto j : coefficients) {
					out_file << j << '\n';
				}
			}
		}

		TEST_METHOD(Cubic2dCoordinate)
		{
			std::ifstream in_file("cubic2d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("cubic2dcoord.points");
			Eigen::Vector2d control_point;
			std::vector<Eigen::Vector2d> control_points;
			while (in_file >> control_point.x() >> control_point.y()) {
				control_points.push_back(control_point);
			}

			Eigen::Vector2d coordinate;
			Eigen::Vector2d empty;
			empty << max_double, max_double;
			coordinate = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 4; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .25, i, 3, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .5, i, 3, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .75, i, 3, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
			}

		}

		TEST_METHOD(Cubic3dCoordinate)
		{
			std::ifstream in_file("cubic3d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("cubic3dcoord.points");
			Eigen::Vector3d control_point;
			std::vector<Eigen::Vector3d> control_points;
			while (in_file >> control_point.x() >> control_point.y() >> control_point.z()) {
				control_points.push_back(control_point);
			}

			Eigen::Vector3d coordinate;
			Eigen::Vector3d empty;
			empty << max_double, max_double, max_double;
			coordinate = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 4; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .25, i);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .5, i);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .75, i);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
			}

		}

		TEST_METHOD(Quad2dCoordinate)
		{
			std::ifstream in_file("quad2d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("quad2dcoord.points");
			Eigen::Vector2d control_point;
			std::vector<Eigen::Vector2d> control_points;
			while (in_file >> control_point.x() >> control_point.y()) {
				control_points.push_back(control_point);
			}
			Eigen::Vector2d coordinate;
			Eigen::Vector2d empty;
			empty << max_double, max_double;
			coordinate = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 3; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .25, i, 2, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .5, i, 2, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .75, i, 2, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
			}

		}

		TEST_METHOD(Quad3dCoordinate)
		{
			std::ifstream in_file("quad3d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("quad3dcoord.points");
			Eigen::Vector3d control_point;
			std::vector<Eigen::Vector3d> control_points;
			while (in_file >> control_point.x() >> control_point.y() >> control_point.z()) {
				control_points.push_back(control_point);
			}
			Eigen::Vector3d coordinate;
			Eigen::Vector3d empty;
			empty << max_double, max_double, max_double;
			coordinate = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 3; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .25, i, 2);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .5, i, 2);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
				coordinate = bezier::GetPosition<double>(control_points, .75, i, 2);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				coordinate = empty;
			}

		}

		TEST_METHOD(Cubic2dTangent)
		{
			std::ifstream in_file("cubic2d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("cubic2dtan.points");
			Eigen::Vector2d control_point;
			std::vector<Eigen::Vector2d> control_points;
			while (in_file >> control_point.x() >> control_point.y()) {
				control_points.push_back(control_point);
			}

			Eigen::Vector2d coordinate;
			Eigen::Vector2d tangent;
			Eigen::Vector2d empty;
			empty << max_double, max_double;
			coordinate = empty;
			tangent = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 4; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .5, i, 3, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				tangent = bezier::GetFirstDerivative<double>(control_points, .5, i, 3, bezier::k2d);
				Assert::IsFalse(tangent == empty);
				tangent.normalize();
				tangent += coordinate;
				out_file << tangent.transpose() << '\n';
				coordinate = empty;
				tangent = empty;
			}
		}

		TEST_METHOD(Cubic3dTangent)
		{
			//using Point = Eigen::Vector3d;
			using Point = std::array<double, 3>;
			std::ifstream in_file("cubic.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("cubic.points");
			Point control_point;
			std::vector<Point> control_points;
			//while (in_file >> control_point.x() >> control_point.y() >> control_point.z()) {
			//	control_points.push_back(control_point);
			//}
			while (in_file >> control_point[0] >> control_point[1] >> control_point[2]) {
				control_points.push_back(control_point);
			}

			Point coordinate;
			Point tangent;
			Point normal;
			Point empty;
			//empty << max_double, max_double, max_double;
			for (size_t i = 0; i < 3; ++i) {
				empty[i] = max_double;
			}
			coordinate = empty;
			normal = empty;
			tangent = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 4; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .25, i);
				Assert::IsFalse(coordinate == empty);
				Eigen::Vector3d eigen_coordinate;
				eigen_coordinate << coordinate[0], coordinate[1], coordinate[2];
				out_file << eigen_coordinate.transpose() << '\n';
				//out_file << coordinate[0] << ' ' << coordinate[1] << ' ' << coordinate[2] << '\n';
				tangent = bezier::GetFirstDerivative<double>(control_points, .25, i);
				Assert::IsFalse(tangent == empty);
				Eigen::Vector3d normal_tangent;
				normal_tangent << tangent[0], tangent[1], tangent[2];
				normal_tangent.normalize();
				normal_tangent += eigen_coordinate;
				out_file << normal_tangent.transpose() << '\n';
				//out_file << tangent.transpose() << '\n';
				normal = bezier::GetNormal<double>(control_points, .25, i);
				Assert::IsFalse(normal == empty);
				Eigen::Vector3d eigen_normal;
				eigen_normal << normal[0], normal[1], normal[2];
				eigen_normal.normalize();
				eigen_normal += eigen_coordinate;
				out_file << eigen_normal.transpose() << '\n';
				coordinate = empty;
				tangent = empty;

				coordinate = bezier::GetPosition<double>(control_points, .5, i);
				Assert::IsFalse(coordinate == empty);
				eigen_coordinate << coordinate[0], coordinate[1], coordinate[2];
				out_file << eigen_coordinate.transpose() << '\n';
				//out_file << coordinate[0] << ' ' << coordinate[1] << ' ' << coordinate[2] << '\n';
				tangent = bezier::GetFirstDerivative<double>(control_points, .5, i);
				Assert::IsFalse(tangent == empty);
				normal_tangent << tangent[0], tangent[1], tangent[2];
				normal_tangent.normalize();
				normal_tangent += eigen_coordinate;
				out_file << normal_tangent.transpose() << '\n';
				//out_file << tangent.transpose() << '\n';
				normal = bezier::GetNormal<double>(control_points, .5, i);
				Assert::IsFalse(normal == empty);
				eigen_normal << normal[0], normal[1], normal[2];
				eigen_normal.normalize();
				eigen_normal += eigen_coordinate;
				out_file << eigen_normal.transpose() << '\n';
				coordinate = empty;
				tangent = empty;

				coordinate = bezier::GetPosition<double>(control_points, .75, i);
				Assert::IsFalse(coordinate == empty);
				eigen_coordinate << coordinate[0], coordinate[1], coordinate[2];
				out_file << eigen_coordinate.transpose() << '\n';
				//out_file << coordinate[0] << ' ' << coordinate[1] << ' ' << coordinate[2] << '\n';
				tangent = bezier::GetFirstDerivative<double>(control_points, .75, i);
				Assert::IsFalse(tangent == empty);
				normal_tangent << tangent[0], tangent[1], tangent[2];
				normal_tangent.normalize();
				normal_tangent += eigen_coordinate;
				out_file << normal_tangent.transpose() << '\n';
				//out_file << tangent.transpose() << '\n';
				normal = bezier::GetNormal<double>(control_points, .75, i);
				Assert::IsFalse(normal == empty);
				eigen_normal << normal[0], normal[1], normal[2];
				eigen_normal.normalize();
				eigen_normal += eigen_coordinate;
				out_file << eigen_normal.transpose() << '\n';
				coordinate = empty;
				tangent = empty;
			}
		}

		TEST_METHOD(Quad2dTangent)
		{
			std::ifstream in_file("quad2d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("quad2dtan.points");
			Eigen::Vector2d control_point;
			std::vector<Eigen::Vector2d> control_points;
			while (in_file >> control_point.x() >> control_point.y()) {
				control_points.push_back(control_point);
			}

			Eigen::Vector2d coordinate;
			Eigen::Vector2d tangent;
			Eigen::Vector2d normal;
			Eigen::Vector2d empty;
			empty << max_double, max_double;
			coordinate = empty;
			tangent = empty;
			normal = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 3; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .5, i, 2, bezier::k2d);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				tangent = bezier::GetFirstDerivative<double>(control_points, .5, i, 2, bezier::k2d);
				Assert::IsFalse(tangent == empty);
				tangent.normalize();
				tangent += coordinate;
				out_file << tangent.transpose() << '\n';
				//normal = bezier::GetNormal<double, bezier::k2d, 2>(control_points, i, .5);
				//Assert::IsFalse(normal == empty);
				//normal.normalize();
				//normal += coordinate;
				//out_file << normal.transpose() << '\n';
				coordinate = empty;
				tangent = empty;
			}
		}

		TEST_METHOD(Quad3dTangent)
		{
			using Point = Eigen::Vector3d;
			std::ifstream in_file("quad3d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("quad3dtan.points");
			Point control_point;
			std::vector<Point> control_points;
			while (in_file >> control_point.x() >> control_point.y() >> control_point.z()) {
				control_points.push_back(control_point);
			}

			Point coordinate;
			Point tangent;
			Point normal;
			Point empty;
			empty << max_double, max_double, max_double;
			coordinate = empty;
			tangent = empty;
			normal = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 3; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .75, i, 2);
				Assert::IsFalse(coordinate == empty);
				out_file << coordinate.transpose() << '\n';
				tangent = bezier::GetFirstDerivative<double>(control_points, .75, i, 2);
				Assert::IsFalse(tangent == empty);
				tangent.normalize();
				tangent += coordinate;
				out_file << tangent.transpose() << '\n';
				normal = bezier::GetNormal<double>(control_points, .75, i, 2);
				Assert::IsFalse(normal == empty);
				normal.normalize();
				normal += coordinate;
				out_file << normal.transpose() << '\n';
				coordinate = empty;
				tangent = empty;
			}
		}

		TEST_METHOD(Quartic3dTangent)
		{
			using Point = Eigen::Vector3d;
			//using Point = std::array<double, 3>;
			std::ifstream in_file("quartic3d.dat");
			Assert::IsTrue(in_file.is_open());
			std::ofstream out_file("quartic3dtan.points");
			Point control_point;
			std::vector<Point> control_points;
			//while (in_file >> control_point.x() >> control_point.y() >> control_point.z()) {
			//	control_points.push_back(control_point);
			//}
			while (in_file >> control_point[0] >> control_point[1] >> control_point[2]) {
				control_points.push_back(control_point);
			}

			Point coordinate;
			Point tangent;
			Point normal;
			Point empty;
			//empty << max_double, max_double, max_double;
			for (size_t i = 0; i < 3; ++i) {
				empty[i] = max_double;
			}
			coordinate = empty;
			normal = empty;
			tangent = empty;
			out_file << std::fixed << std::setprecision(14);
			for (size_t i = 0; i < control_points.size() / 5; ++i) {
				coordinate = bezier::GetPosition<double>(control_points, .5, i, 4);
				Assert::IsFalse(coordinate == empty);
				Eigen::Vector3d eigen_coordinate;
				eigen_coordinate << coordinate[0], coordinate[1], coordinate[2];
				out_file << eigen_coordinate.transpose() << '\n';
				//out_file << coordinate[0] << ' ' << coordinate[1] << ' ' << coordinate[2] << '\n';
				tangent = bezier::GetFirstDerivative<double>(control_points, .5, i, 4, bezier::k3d);
				Assert::IsFalse(tangent == empty);
				Eigen::Vector3d normal_tangent;
				normal_tangent << tangent[0], tangent[1], tangent[2];
				normal_tangent.normalize();
				normal_tangent += eigen_coordinate;
				out_file << normal_tangent.transpose() << '\n';
				normal = bezier::GetNormal<double>(control_points, .5, i, 4);
				Assert::IsFalse(normal == empty);
				Eigen::Vector3d eigen_normal;
				eigen_normal << normal[0], normal[1], normal[2];
				eigen_normal.normalize();
				eigen_normal += eigen_coordinate;
				out_file << eigen_normal.transpose() << '\n';
				coordinate = empty;
				tangent = empty;
			}
		}

		TEST_METHOD(PascalRow) {
			std::ofstream out_file("pascalRow.dat");
			out_file << std::fixed << std::setprecision(0);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> pascal_row = bezier::GetBinomialCoefficients<double>(10);
			//out_file << pascal_row.transpose() << '\n';
		}

	};
}


#include <vector>
#include <chrono>
#include <iostream>
#include <Eigen/Dense>

template<typename vectype>
double Clenshaw1D(const vectype &c, double ind){
    int N = static_cast<int>(c.size()) - 1;
    double u_k = 0, u_kp1 = 0, u_kp2 = 0;
    for (int k = N; k >= 0; --k){
        // Do the recurrent calculation
        u_k = 2.0*ind*u_kp1 - u_kp2 + c[k];
        if (k > 0){
            // Update the values
            u_kp2 = u_kp1; u_kp1 = u_k;
        }
    }
    return (u_k - u_kp2)/2;
}

/// With STL datatypes
template<typename Mat, typename vectype>
double Clenshaw2D(const Mat& a, double x, double y, vectype& b) {
    std::size_t m = a.size() - 1;
    std::size_t n = a[0].size() - 1;
    for (auto i = 0; i < b.size(); ++i) {
        b[i] = Clenshaw1D(a[i], y);
    }
    return Clenshaw1D(b, x);
}

template<typename MatType, int Cols = MatType::ColsAtCompileTime>
auto Clenshaw1DByRow(const MatType& c, double ind) {
    int N = static_cast<int>(c.rows()) - 1;
    static Eigen::Array<double, 1, Cols> u_k, u_kp1, u_kp2;
    // Not statically sized
    if constexpr (Cols < 0) {
        int M = c.rows();
        u_k.resize(M);
        u_kp1.resize(M);
        u_kp2.resize(M);
    }
    u_k.setZero(); u_kp1.setZero(); u_kp2.setZero();

    for (int k = N; k >= 0; --k) {
        // Do the recurrent calculation
        u_k = 2.0 * ind * u_kp1 - u_kp2 + c.row(k);
        if (k > 0) {
            // Update the values
            u_kp2 = u_kp1; u_kp1 = u_k;
        }
    }
    return (u_k - u_kp2) / 2;
}

/// With Eigen datatypes
template<typename MatType>
double Clenshaw2DEigen(const MatType& a, double x, double y) {
    auto b = Clenshaw1DByRow(a, y);
    return Clenshaw1D(b.matrix(), x);
}

template<int Rows, int Cols>
void test_Eigen(int M){
    using MatType = Eigen::Array<double, Rows, Cols>;
    MatType aa;
    if constexpr ((Rows < 0) || (Cols < 0)) {
        aa.resize(M + 1, M + 1);
    }
    else{
        aa.resize(M + 1, M + 1);
    }
    aa.fill(0.0);
    for (auto i = 0; i < M + 1; ++i) {
        for (auto j = 0; j < M + 1; ++j) {
            aa(i, j) = i + j;
        }
    }
    int N = 1000 * 1000;
    volatile auto r = 0.0, x = 0.1, y = 0.7;
    auto startTime = std::chrono::system_clock::now();
    for (int i = 0; i < N; ++i) {
        auto v = Clenshaw2DEigen(aa, x, y);
        r += v;
    }
    auto endTime = std::chrono::system_clock::now();
    auto elap_us = std::chrono::duration<double>(endTime - startTime).count() / N * 1e6;
    std::cout << elap_us << " us/call. (Eigen-powered) value:" << (r / N) << std::endl;
}

int main(){
    std::vector<std::vector<double>> a;
    const int M = 8;
    for (auto i = 0; i <= M; ++i){
        a.push_back(std::vector<double>(M+1, 0.0)); // One would normally use Eigen here, but the challenge is to use only standard library elements...
    }
    {
        for (auto i = 0; i < M + 1; ++i) {
            for (auto j = 0; j < M + 1; ++j) {
                a[i][j] = i + j;
            }
        }
        std::vector<double> b(M+1, 0.0);
        int N = 1000 * 1000;
        volatile auto r = 0.0, x = 0.1, y = 0.7;
        auto startTime = std::chrono::system_clock::now();
        for (int i = 0; i < N; ++i) {
            auto v = Clenshaw2D(a, x, y, b);
            r += v;
        }
        auto endTime = std::chrono::system_clock::now();
        auto elap_us = std::chrono::duration<double>(endTime - startTime).count() / N * 1e6;
        std::cout << elap_us << " us/call. value:" << (r / N) << std::endl;
    }
    std::cout  << "Dynamic:" << std::endl;
    test_Eigen<Eigen::Dynamic, Eigen::Dynamic>(M);

    std::cout  << "Static:" << std::endl;
    test_Eigen<M+1, M+1>(M);
}


/*************************************************
 * @Description: file content
 * @Author: yuanquan
 * @Email: yuanquan2011@qq.com
 * @Date: 2021-05-17 21:20:13
 * @LastEditors: yuanquan
 * @LastEditTime: 2021-07-24 12:20:47
 * @copyright: Copyright (c) yuanquan
 *************************************************/
#include <iostream>
#include <vector>
#include <set>
#include <string>
#include <gtest/gtest.h>
#include <boost/array.hpp>
#include <tuple>

using namespace std;
using namespace boost;
using namespace testing;

// 10: b1010, 3: b0011
int divide(int dividend, int divisor) {
	if(dividend == 0) return 0;
	if(divisor == INT32_MIN)
	{
		if(dividend == INT32_MIN)return 1;
		else return 0;
	}
	if(dividend == INT32_MIN && divisor == -1) return INT32_MAX;
	if(dividend == INT32_MIN && divisor == 1) return INT32_MIN;

	bool isPositive = (dividend > 0 && divisor > 0) || (dividend < 0 && divisor < 0);

	int result = 0;
	if(divisor < 0) divisor = -divisor;
	if(dividend == INT32_MIN)
	{
		++result;
		dividend += divisor;
	}
	if(dividend < 0) dividend = -dividend;
	for(int i=31; i >= 0; --i)
	{
		if((dividend >> i) >= divisor)
		{
			dividend -= (divisor << i);
			result += (1 << i);
		}
	}
	if(isPositive) return result;
	else return -result;
}

TEST(divideTest, all)
{
	using TestData = tuple<int, int, int>;
	vector<TestData> datas{
		make_tuple(10, 3, 3),
		make_tuple(7, -3, -2),
		make_tuple(0, 1, 0),
		make_tuple(1, 1, 1),
		make_tuple(INT32_MIN, -1, INT32_MAX),
		make_tuple(INT32_MIN, INT32_MIN, 1),
	};

	for(auto& dat : datas)
	{
		auto [dividend, divisor, result] = dat;
		EXPECT_EQ(result, divide(dividend, divisor)) << dividend << "/" << divisor;
	}
}


// https://projecteuler.net/problem=57
/*
Square root convergents

It is possible to show that the square root of two can be expressed as
an infinite continued fraction.

2^1/2 = 1 + 1/(2 + 1/(2 + 1/(2 + ... ))) = 1.414213...

By expanding this for the first four iterations, we get:

1 + 1/2 = 3/2 = 1.5
1 + 1/(2 + 1/2) = 7/5 = 1.4
1 + 1/(2 + 1/(2 + 1/2)) = 17/12 = 1.41666...
1 + 1/(2 + 1/(2 + 1/(2 + 1/2))) = 41/29 = 1.41379...

The next three expansions are 99/70, 239/169, and 577/408, but the eighth expansion,
1393/985, is the first example where the number of digits in the numerator exceeds
the number of digits in the denominator.

In the first one-thousand expansions, how many fractions contain a numerator
with more digits than denominator?

Solution:
*/

#include <iostream>
#include <cstdint>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>
#include <chrono>

auto compute() {
	using namespace boost::multiprecision;
	constexpr uint32_t limit = 1000;

	cpp_int natural = 3, den = 2;
	uint32_t count = 0;
	for (uint32_t t = 2; t <= limit; ++t) {
		natural += den << 1;
		den = natural - den;
		if (natural.str().length() > den.str().length()) {
			++count;
			//std::cout << t << ',';
		}
	}

	return count;
}

#ifdef _MSC_VER
	template <class T>
	inline void DoNotOptimize(const T &value) {
		__asm { lea ebx, value }
	}
#else
	template <class T>
	__attribute__((always_inline)) inline void DoNotOptimize(const T &value) {
		asm volatile("" : "+m"(const_cast<T &>(value)));
	}
#endif

int main() {
	using namespace std;
	using namespace chrono;
	auto start = high_resolution_clock::now();
	auto result = compute();
	DoNotOptimize(result);
	cout << "Done in "
		<< duration_cast<nanoseconds>(high_resolution_clock::now() - start).count() / 1e6
		<< " miliseconds." << endl;
	cout << result << endl;
}


// Ax=b
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
   Matrix3f A;
   Vector3f b;
   A << 1,2,3,  4,5,6,  7,8,10;
   b << 3, 3, 4;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the vector b:\n" << b << endl;
   Vector3f x = A.colPivHouseholderQr().solve(b); // QR decompsition
   // ColPivHouseholderQR<Matrix3f> dec(A);
   // Vector3f x = dec.solve(b);
   cout << "The solution is:\n" << x << endl;
}

PartialPivLU            partialPivLu()          Invertible              ++      ++      +
FullPivLU               fullPivLu()             None                    -       - -     +++
HouseholderQR           householderQr()         None                    ++      ++      +
ColPivHouseholderQR     colPivHouseholderQr()   None                    ++      -       +++
FullPivHouseholderQR    fullPivHouseholderQr()  None                    -       - -     +++
LLT                     llt()                   Positive definite       +++     +++     +
LDLT                    ldlt()                  Positive or
                                                negative semidefinite   +++     +       ++
JacobiSVD               jacobiSvd()             None                    - -     - - -   +++


/**
	@file rotation.cpp
	Utilities for expressing and converting rotations of reference frames

	@brief All of the rotation parameterizations here represent rotations
	of an initial fixed frame to a new frame. Therefore using these
	parameterizations to operate on a vector is the same as transforming
	the new frame coordinates into the original fixed frame.

	Examples if rotation describes relation of body w.r.t. inertial
	(how to rotate inertial to body):
	R: coverts body frame coordinates into inertial frame coordinates
	euler: usual roll-pitch-yaw of body frame w.r.t inertial frame

*/

#include <utils/rotation.hpp>

#include <Eigen/Dense>
#include <cmath>

/**
    @brief Converts the rotation matrix R to the Euler angles (ZYX)
    (Yaw-Pitch-Roll) (psi, theta, phi) used for aircraft conventions.
    Note to compute the Euler angles for the aircraft this should be
    the R matrix that converts a vector from body frame coordinates
    to a vector in inertial frame coordinates.

    @param[in] R  rotation matrix
    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
*/
void Rot::R_to_euler(const Eigen::Matrix3d& R, Eigen::Vector3d& e) {
    // Check for singularity that occurs when pitch = 90 deg
    if (sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0)) >= 1e-6) {
    	e(0) = atan2(R(2,1), R(2,2));
        e(1) = asin(-R(2,0));
        e(2) = atan2(R(1,0), R(0,0));
    }
    else {
    	e(0) = atan2(-R(1,2), R(1,1));
        e(1) = asin(-R(2,0));
        e(2) = 0.0;
    }
}

/**
    @brief Converts Euler angles (ZYX) (Yaw-Pitch-Roll) (psi, theta, phi)
    into the inertial to body rotation matrix

    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
    @param[in] R  rotation matrix
*/
void Rot::euler_to_R(const Eigen::Vector3d& e, Eigen::Matrix3d& R) {
    R(0,0) = cos(e(1))*cos(e(2));
    R(0,1) = sin(e(0))*sin(e(1))*cos(e(2)) - cos(e(0))*sin(e(2));
    R(0,2) = sin(e(0))*sin(e(2)) + cos(e(0))*sin(e(1))*cos(e(2));
    R(1,0) = cos(e(1))*sin(e(2));
    R(1,1) = cos(e(0))*cos(e(2)) + sin(e(0))*sin(e(1))*sin(e(2));
    R(1,2) = cos(e(0))*sin(e(1))*sin(e(2)) - sin(e(0))*cos(e(2));
    R(2,0) = -sin(e(1));
    R(2,1) = sin(e(0))*cos(e(1));
    R(2,2) = cos(e(0))*cos(e(1));
}

/**
    @brief Converts a unit quaternion into a rotation matrix

    @param[in] q  unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
    @param[in] R  rotation matrix
*/
void Rot::quat_to_R(const Eigen::Vector4d& q, Eigen::Matrix3d& R) {
    R(0,0) = 1.0 - 2.0*q(2)*q(2) - 2.0*q(3)*q(3);
    R(0,1) = 2.0*q(1)*q(2) - 2.0*q(3)*q(0);
    R(0,2) = 2.0*q(1)*q(3) + 2.0*q(2)*q(0);
    R(1,0) = 2.0*q(1)*q(2) + 2.0*q(3)*q(0);
    R(1,1) = 1.0 - 2.0*q(1)*q(1) - 2.0*q(3)*q(3);
    R(1,2) = 2.0*q(2)*q(3) - 2.0*q(1)*q(0);
    R(2,0) = 2.0*q(1)*q(3) - 2.0*q(2)*q(0);
    R(2,1) = 2.0*q(2)*q(3) + 2.0*q(1)*q(0);
    R(2,2) = 1.0 - 2.0*q(1)*q(1) - 2.0*q(2)*q(2);
}

/**
    @brief Converts a unit quaternion into the aircraft Euler angles
    (ZYX) (Yaw-Pitch-Roll) (psi, theta, phi)

    @param[in] q  unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
*/
void Rot::quat_to_euler(const Eigen::Vector4d& q, Eigen::Vector3d& e) {
    double R00 = 1.0 - 2.0*q(2)*q(2) - 2.0*q(3)*q(3);
    double R10 = 2.0*q(1)*q(2) + 2*q(3)*q(0);
    if (sqrt(R00*R00 + R10*R10 >= 1e-6)) {
        e(0) = atan2(2.0*q(2)*q(3) + 2.0*q(1)*q(0), 1.0 - 2.0*q(1)*q(1) - 2.0*q(2)*q(2));
        e(1) = asin(-2.0*q(1)*q(3) + 2.0*q(2)*q(0));
        e(2) = atan2(R10, R00);
    }
    else {
        e(0) = atan2(-2.0*q(2)*q(3) - 2.0*q(1)*q(0), 1.0 - 2.0*q(1)*q(1) - 2.0*q(3)*q(3));
        e(1) = asin(-2.0*q(1)*q(3) + 2.0*q(2)*q(0));
        e(2) = 0.0;
    }
}

/**
    @brief Converts aircraft Euler angles (ZYX) (Yaw-Pitch-Roll) (psi, theta, phi)
    into unit quaternion

    @param[in] e  euler angles e = (phi, theta, psi) = (roll, pitch, yaw)
    @param[in] q  unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
*/
void Rot::euler_to_quat(const Eigen::Vector3d& e, Eigen::Vector4d& q) {
    double phi = 0.5 * e(0);
    double th = 0.5 * e(1);
    double psi = 0.5 * e(2);
    q(0) = cos(phi) * cos(th) * cos(psi) + sin(phi) * sin(th) * sin(psi);
    q(1) = sin(phi) * cos(th) * cos(psi) - cos(phi) * sin(th) * sin(psi);
    q(2) = cos(phi) * sin(th) * cos(psi) + sin(phi) * cos(th) * sin(psi);
    q(3) = cos(phi) * cos(th) * sin(psi) - sin(phi) * sin(th) * cos(psi);
}

/**
    @brief Converts an axis and angle into a unit quaternion

    @param[in] aa  axis/angle aa = (x, y, z), th = ||aa||, e = aa/th
    @param[in] q   unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
*/
void Rot::axis_to_quat(const Eigen::Vector3d& aa, Eigen::Vector4d& q) {
    double th = aa.norm();
    if (th < 0.0000001) {
        q(0) = 1.0;
        q(1) = 0.0;
        q(2) = 0.0;
        q(3) = 0.0;
    }
    else {
        Eigen::Vector3d e = aa.normalized();
        q(0) = cos(th/2.0);
        q(1) = e(0)*sin(th/2.0);
        q(2) = e(1)*sin(th/2.0);
        q(3) = e(2)*sin(th/2.0);
    }
}

/**
    @brief Converts a unit quaternion into a normalized axis and angle.

    @param[in] q   unit quaternion q = (w, x, y, z) = w + (x i, y j, z k)
    @param[in] aa  axis/angle aa = (x, y, z), th = ||aa||, e = aa/th
*/
void Rot::quat_to_axis(const Eigen::Vector4d& q, Eigen::Vector3d& aa) {
    if (q(0) > 0.9999999) { // no rotation
        aa(0) = 0.0;
        aa(1) = 0.0;
        aa(2) = 0.0;
	}
    else {
        double m = 2.0*acos(q(0))/sqrt(1.0 - q(0)*q(0));
        aa(0) = m*q(1);
        aa(1) = m*q(2);
        aa(2) = m*q(3);

        // Scale to keep magnitude less than pi
        double th = aa.norm();
        if (th > M_PI) {
            double th_new = 2 * M_PI - th;
            aa = -(th_new/th)*aa;
        }
    }
}

/**
    @brief Composes two rotations parameterized by unit quaternions
    and outputs a single quaternion representing the composed rotation.
    This performs rotation p first and then rotation q after.

    @param[in] p   unit quaternion p = (pw, px, py, pz) = pw + (px i, py j, pz k)
    @param[in] q   unit quaternion q = (qw, qx, qy, qz) = qw + (qx i, qy j, qz k)
    @param[in] o   composed unit quaternion o = (ow, ox, oy, oz) = ow + (ox i, oy j, oz k)
*/
void Rot::compose_quats(const Eigen::Vector4d& p, const Eigen::Vector4d q, Eigen::Vector4d& o) {
    o(0) = p(0)*q(0) - (p(1)*q(1) + p(2)*q(2) + p(3)*q(3));
    o(1) = p(0)*q(1) + p(1)*q(0) + p(2)*q(3) - p(3)*q(2);
    o(2) = p(0)*q(2) - p(1)*q(3) + p(2)*q(0) + p(3)*q(1);
    o(3) = p(0)*q(3) + p(1)*q(2) - p(2)*q(1) + p(3)*q(0);
}

/**
    @brief Inverts a unit quaternion, gives the opposite rotation

    @param[in] q   unit quaternion q = (qw, qx, qy, qz) = qw + (qx i, qy j, qz k)
*/
void Rot::invert_quat(Eigen::Vector4d& q) {
    q(1) *= -1.0;
    q(2) *= -1.0;
    q(3) *= -1.0;
}

double Rot::rad_to_deg(double th) {
    return th*(180.0/M_PI);
}

double Rot::deg_to_rad(double th) {
    return th*(M_PI/180.0);
}

/**
    @brief Wraps angle th [rad] to range [0, 2pi]
*/
double Rot::wrap_to_2pi(double th) {
    th = fmod(th, 2.0*M_PI);
    if (th < 0) return th + 2.0 * M_PI;
    else return th;
}

/**
    @brief Wraps angle th [rad] to range [-pi, pi]
*/
double Rot::wrap_to_pi(double th) {
    return Rot::wrap_to_2pi(th + M_PI) - M_PI;
}


// Computer the winding number of a polygon at a point
// Author: Shayan Hoshyari

#ifndef polyvec_winding_number_
#define polyvec_winding_number_

#include <Eigen/Core>

namespace polyvec {
    namespace WindingNumber {
        // Input:
        // polygon: polygon 2 x numpoints matrix
        // point: the point to compute the winding number at
        // Returns:
        // winding_number: value of winding number
        // is_trustable: is the number trustable, or are we too close to the boundary?
        // NOTE: assumes that the polygon points are sorted in CCW order.
        // multiply the answer by -1 if the order is CW.
        void compute_winding ( const Eigen::Matrix2Xd& polygon, const Eigen::Vector2d& point,  double& winding_number, bool& is_trustable );

        // Input:
        // polygon: polygon 2 x numpoints matrix
        // Returns:
        // winding_number: is ccw
       void compute_orientation ( const Eigen::Matrix2Xd& polygon,  bool &is_ccw, double &area );
       void compute_orientation ( const Eigen::Matrix2Xd& polygon,  bool &is_ccw );

    }
} // end of polyvec

#endif


#include "SGFitLeastSquares.h"
#include <Eigen/Eigen>
#include <Eigen/nnls.h>

namespace Probulator
{
	SgBasis sgFitLeastSquares(const SgBasis& basis, const std::vector<RadianceSample>& samples)
	{
		using namespace Eigen;
		SgBasis result = basis;

		MatrixXf A;
		A.resize(samples.size(), basis.size());
		for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
		{
			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				A(sampleIt, lobeIt) = sgEvaluate(basis[lobeIt].p, basis[lobeIt].lambda, samples[sampleIt].direction);
			}
		}

		for (u32 channelIt = 0; channelIt < 3; ++channelIt)
		{
			VectorXf b;
			b.resize(samples.size());
			for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
			{
				b[sampleIt] = samples[sampleIt].value[channelIt];
			}

			VectorXf x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				result[lobeIt].mu[channelIt] = x[lobeIt];
			}
		}

		return result;
	}

	// Non-negative version of least squares
	SgBasis sgFitNNLeastSquares(const SgBasis& basis, const std::vector<RadianceSample>& samples)
	{
		using namespace Eigen;
		SgBasis result = basis;

		MatrixXf A;
		A.resize(samples.size(), basis.size());
		for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
		{
			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				A(sampleIt, lobeIt) = sgEvaluate(basis[lobeIt].p, basis[lobeIt].lambda, samples[sampleIt].direction);
			}
		}

		NNLS<MatrixXf> nnlssolver(A);
		for (u32 channelIt = 0; channelIt < 3; ++channelIt)
		{
			VectorXf b;
			b.resize(samples.size());
			for (u64 sampleIt = 0; sampleIt < samples.size(); ++sampleIt)
			{
				b[sampleIt] = samples[sampleIt].value[channelIt];
			}

			// -- run the solver
			nnlssolver.solve(b);
			VectorXf x = nnlssolver.x();

			for (u64 lobeIt = 0; lobeIt < basis.size(); ++lobeIt)
			{
				result[lobeIt].mu[channelIt] = x[lobeIt];
			}
		}

		return result;
	}
}


/**
 * Copyright 2014-2017 Steven T Sell (ssell@vertexfragment.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "Math/MathCommon.hpp"

#ifdef _DEBUG

#include "gtest/gtest.h"

#include <boost/utility/binary.hpp>

//------------------------------------------------------------------------------------------

TEST(MathCommon, Normalise)
{
    double dValue = 240.0f;
    dValue = Ocular::Math::Normalize<double>(dValue, 0.0, 180.0);

    EXPECT_NEAR(dValue, 60.0, Ocular::Math::EPSILON_DOUBLE);

    int iValue = -20;
    iValue = Ocular::Math::Normalize<int>(iValue, -18, 18);

    EXPECT_EQ(iValue, 16);
}

TEST(MathCommon, DegreesToRadians)
{
    double degrees = 180.0;
    double result  = Ocular::Math::DegreesToRadians<double>(degrees);

    EXPECT_NEAR(result, Ocular::Math::PI, Ocular::Math::EPSILON_DOUBLE);

    degrees = -90.0;
    result  = Ocular::Math::DegreesToRadians<double>(degrees);

    EXPECT_NEAR(result, -Ocular::Math::PI_OVER_TWO, Ocular::Math::EPSILON_DOUBLE);

    degrees = 0.0;
    result  = Ocular::Math::DegreesToRadians<double>(degrees);

    EXPECT_NEAR(result, 0.0, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, RadiansToDegrees)
{
    double radians = Ocular::Math::PI;
    double result  = Ocular::Math::RadiansToDegrees<double>(radians);

    EXPECT_NEAR(result, 180.0, Ocular::Math::EPSILON_DOUBLE);

    radians = -Ocular::Math::PI_OVER_TWO;
    result  = Ocular::Math::RadiansToDegrees<double>(radians);

    EXPECT_NEAR(result, -90.0, Ocular::Math::EPSILON_DOUBLE);

    radians = 0.0;
    result  = Ocular::Math::RadiansToDegrees<double>(radians);

    EXPECT_NEAR(result, 0.0, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, Clamp)
{
    double valA = 35.0;

    double expectA = 20.0;
    double expectB = 50.0;
    double expectC = 35.0;

    double resultA = Ocular::Math::Clamp<double>(valA, 0.0, 20.0);
    double resultB = Ocular::Math::Clamp<double>(valA, 50.0, 100.0);
    double resultC = Ocular::Math::Clamp<double>(valA, 0.0, 100.0);

    EXPECT_NEAR(expectA, resultA, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectB, resultB, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectC, resultC, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, RoundUpDecimal)
{
    double valA = 0.2749999;
    double valB = 0.975;
    double valC = 0.86736;

    double expectA = 0.275;
    double expectB = 0.98;
    double expectC = 0.868;

    double resultA = Ocular::Math::RoundUpDecimal<double>(valA, 3);
    double resultB = Ocular::Math::RoundUpDecimal<double>(valB, 2);
    double resultC = Ocular::Math::RoundUpDecimal<double>(valC, 3);

    EXPECT_NEAR(expectA, resultA, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectB, resultB, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectC, resultC, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, RoundUpPowTen)
{
    double valA = 1250.0;
    double valB = 18.0;

    double expectA = 1300.0;
    double expectB = 100.0;

    double resultA = Ocular::Math::RoundUpPowTen<double>(valA, 2);
    double resultB = Ocular::Math::RoundUpPowTen<double>(valB, 2);

    EXPECT_NEAR(expectA, resultA, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectB, resultB, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, RoundDownDecimal)
{
    double valA = 0.2749999;
    double valB = 0.975;
    double valC = 0.86736;

    double expectA = 0.274;
    double expectB = 0.97;
    double expectC = 0.867;

    double resultA = Ocular::Math::RoundDownDecimal<double>(valA, 3);
    double resultB = Ocular::Math::RoundDownDecimal<double>(valB, 2);
    double resultC = Ocular::Math::RoundDownDecimal<double>(valC, 3);

    EXPECT_NEAR(expectA, resultA, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectB, resultB, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectC, resultC, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, RoundDownPowTen)
{
    double valA = 1250.0;
    double valB = 18.0;

    double expectA = 1200.0;
    double expectB = 0.0;

    double resultA = Ocular::Math::RoundDownPowTen<double>(valA, 2);
    double resultB = Ocular::Math::RoundDownPowTen<double>(valB, 2);

    EXPECT_NEAR(expectA, resultA, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectB, resultB, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, RoundDecimal)
{
    double valA = 0.127;
    double valB = 0.123;

    double expectA = 0.13;
    double expectB = 0.12;

    double resultA = Ocular::Math::RoundDecimal(valA, 2);
    double resultB = Ocular::Math::RoundDecimal(valB, 2);

    EXPECT_NEAR(expectA, resultA, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectB, resultB, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, RoundPowTen)
{
    double valA = 1270.0;
    double valB = 1230.0;

    double expectA = 1300.0;
    double expectB = 1200.0;

    double resultA = Ocular::Math::RoundPowTen<double>(valA, 2);
    double resultB = Ocular::Math::RoundPowTen<double>(valB, 2);

    EXPECT_NEAR(expectA, resultA, Ocular::Math::EPSILON_DOUBLE);
    EXPECT_NEAR(expectB, resultB, Ocular::Math::EPSILON_DOUBLE);
}

TEST(MathCommon, Clz32)
{
	const uint32_t valueA    = 100;
	const uint32_t expectedA = 25;
	const uint32_t resultA   = Ocular::Math::Clz(valueA);

	EXPECT_EQ(expectedA, resultA);

	const uint32_t valueB    = 4294967295;
	const uint32_t expectedB = 0;
	const uint32_t resultB   = Ocular::Math::Clz(valueB);

	EXPECT_EQ(expectedB, resultB);

	const uint32_t valueC    = 1;
	const uint32_t expectedC = 31;
	const uint32_t resultC   = Ocular::Math::Clz(valueC);

	EXPECT_EQ(expectedC, resultC);
}

TEST(MathCommon, Clz64)
{
	const uint64_t valueA    = 100;
	const uint64_t expectedA = 57;
	const uint64_t resultA   = Ocular::Math::Clz(valueA);

	EXPECT_EQ(expectedA, resultA);

	const uint64_t valueB    = 4294967295;
	const uint64_t expectedB = 32;
	const uint64_t resultB   = Ocular::Math::Clz(valueB);

	EXPECT_EQ(expectedB, resultB);

	const uint64_t valueC    = 1;
	const uint64_t expectedC = 63;
	const uint64_t resultC   = Ocular::Math::Clz(valueC);

	EXPECT_EQ(expectedC, resultC);
}

#endif


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;

using namespace std;

int furthest_dist(const weighted_graph &G, int s) {
  int n = boost::num_vertices(G);
  std::vector<int> dist_map(n);

  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));

  int max_dist = 0;
  for(int i = 1; i < n; i++) {
    if(dist_map[i] > max_dist)
      max_dist = dist_map[i];
  }
  return max_dist;
}

int span_tree_weight(const weighted_graph &G, const weight_map &weights) {
  std::vector<edge_desc> mst;    // vector to store MST edges (not a property map!)

  boost::kruskal_minimum_spanning_tree(G, std::back_inserter(mst));
  int total_weight = 0;
  for (std::vector<edge_desc>::iterator it = mst.begin(); it != mst.end(); ++it) {
    total_weight += weights[*it];
  }
  return total_weight;
}

void testcase() {
  int n; cin >> n;
  int m; cin >> m;
  weighted_graph G(n);
  weight_map weights = boost::get(boost::edge_weight, G);
  edge_desc e;
  for(int i = 0; i < m; i++) {
    int u,v; cin >> u; cin >> v;
    int w; cin >> w;
    e = boost::add_edge(u, v, G).first; weights[e] = w;
  }

  cout << span_tree_weight(G, weights) << " ";
  cout << furthest_dist(G, 0) << endl;

}
int main()
{
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  for(int i = 0; i < t; i++)
    testcase();
  return 0;
}


#include <cmath>
#include <boost/test/unit_test.hpp>

#include "../test_utils.hh"
#include "../fixture.hh"
#include "../vector_utils.hh"

BOOST_FIXTURE_TEST_CASE(vector_dot_float, vector_fixture)
{
    // Given two vector of five values.
    std::vector<float> values1 = {4.234f, 3214.4243f, 290342.0f, 0.0f, -1.0f};
    std::vector<float> values2 = {3.0f, 392.9001f, 0.005f, 5.0f, 29844.05325811f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);
    auto vector2 = getScyllaVectorOf(test_const::float_vector_2_id, values2);

    // When performing dot product of these two vectors.
    float res = scheduler->sdot(*vector1, *vector2);

    float sum = 0;
    for (int i = 0; i < values1.size(); i++) {
        sum += values1[i] * values2[i];
    }

    print_vector(*vector1);
    print_vector(*vector2);
    std::cout << std::setprecision(20) << sum << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then the dot product is correctly calculated and equal to sum.
    BOOST_CHECK(std::abs(sum - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_dot_float_same_obj, vector_fixture)
{
    // Given one vector of five values.
    std::vector<float> values1 = {4.234f, 214.4243f, 342.0f, 0.0f, -1.0f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);

    // When performing dot product of this vector x this vector.
    float res = scheduler->sdot(*vector1, *vector1);

    float sum = 0;
    for (float v : values1) {
        sum += v * v;
    }
    std::cout << std::setprecision(20) << sum << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then the dot product is correctly calculated and equal to sum.
    BOOST_CHECK(std::abs(sum - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_sdsdot_float, vector_fixture)
{
    // Given two vector of five values.
    std::vector<float> values1 = {4.234f, 3214.4243f, 290342.0f, 0.0f, -1.0f};
    std::vector<float> values2 = {3.0f, 392.9001f, 0.005f, 5.0f, 29844.05325811f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);
    auto vector2 = getScyllaVectorOf(test_const::float_vector_2_id, values2);

    // When performing dot product of double precision plus value
    // of these two vectors.
    float res = scheduler->sdsdot(0.5f, *vector1, *vector2);

    double sum = 0.5f;
    for (int i = 0; i < values1.size(); i++) {
        sum += (double)values1[i] * (double)values2[i];
    }
    std::cout << std::setprecision(20) << sum << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then the dot product is correctly calculated and equal to sum + value.
    BOOST_CHECK(std::abs((float)sum - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_dsdot_float, vector_fixture)
{
    // Given two vector of five values.
    std::vector<float> values1 = {4.234f, 3214.4243f, 290342.0f, 0.0f, -1.0f};
    std::vector<float> values2 = {3.0f, 392.9001f, 0.005f, 5.0f, 29844.05325811f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);
    auto vector2 = getScyllaVectorOf(test_const::float_vector_2_id, values2);

    // When performing dot product with double precision of these two vectors.
    double res = scheduler->dsdot(*vector1, *vector2);

    double sum = 0.0f;
    for (int i = 0; i < values1.size(); i++) {
        sum += (double)values1[i] * (double)values2[i];
    }
    std::cout << std::setprecision(20) << sum << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then the dot product is correctly calculated and equal to sum.
    BOOST_CHECK(std::abs(sum - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_dot_double, vector_fixture)
{
    // Given two vector of five values.
    std::vector<double> values1 = {4.234, 3214.4243, 290342.0, 0.0, -1.0};
    std::vector<double> values2 = {3.0, 392.9001, 0.005, 5.0, 29844.05325811};
    auto vector1 = getScyllaVectorOf(test_const::double_vector_1_id, values1);
    auto vector2 = getScyllaVectorOf(test_const::double_vector_2_id, values2);

    // When performing dot product of these two vectors.
    double res = scheduler->ddot(*vector1, *vector2);

    double sum = 0;
    for (int i = 0; i < values1.size(); i++) {
        sum += values1[i] * values2[i];
    }
    std::cout << std::setprecision(20) << sum << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then the dot product is correctly calculated and equal to sum.
    BOOST_CHECK(std::abs(sum - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_norm_float, vector_fixture)
{
    // Given vector of some values.
    std::vector<float> values1 = {0.00494931f, 0.119193f, 0.927604f, 0.354004f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);

    // When performing vector euclidean norm.
    float res = scheduler->snrm2(*vector1);

    float nrm = 0;
    for (float v : values1) {
        nrm += v * v;
    }
    nrm = std::sqrt(nrm);
    std::cout << std::setprecision(20) << nrm << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then the norm is correctly calculated and equal to nrm.
    BOOST_CHECK(std::abs(nrm - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_sasum_float, vector_fixture)
{
    // Given vector of some values.
    std::vector<float> values1 = {0.00494931f, 0.119193f, -0.927604f, 0.354004f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);

    // When performing sum of absolute values in this vector.
    float res = scheduler->sasum(*vector1);

    float abs_sum = 0;
    for (float v : values1) {
        abs_sum += std::abs(v);
    }
    std::cout << std::setprecision(20) << abs_sum << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then it is correctly calculated and equal to sum.
    BOOST_CHECK(std::abs(abs_sum - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_isamax_float, vector_fixture)
{
    // Given vector of some values.
    std::vector<float> values1 = {0.00494931f, 0.119193f, -0.927604f, 0.354004f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);

    // When performing max fetch on the abs values in this vector.
    scylla_blas::index_t res = scheduler->isamax(*vector1);

    scylla_blas::index_t max_index = 0;
    for (scylla_blas::index_t i = 1; i < values1.size(); i++) {
        if (std::abs(values1[i]) > std::abs(values1[max_index])) {
            max_index = i;
        }
    }
    std::cout << std::setprecision(20) << values1[max_index] << "=sum\n";
    std::cout << res << "=res index\n";

    // Then the found index corresponds to the index of largest absolute value's index.
    BOOST_CHECK(max_index + 1 == res);
}

BOOST_FIXTURE_TEST_CASE(vector_norm_double, vector_fixture)
{
    // Given vector of some values.
    std::vector<double> values1 = {0.00494931, 0.119193, 0.927604, 0.354004};
    auto vector1 = getScyllaVectorOf(test_const::double_vector_1_id, values1);

    // When performing vector euclidean norm.
    double res = scheduler->dnrm2(*vector1);

    double nrm = 0;
    for (double i : values1) {
        nrm += i * i;
    }
    nrm = sqrt(nrm);
    std::cout << std::setprecision(20) << nrm << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then the norm is correctly calculated and equal to nrm.
    BOOST_CHECK(std::abs(nrm - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_dasum_double, vector_fixture)
{
    // Given vector of some values.
    std::vector<double> values1 = {0.00494931, 0.119193, -0.927604, 0.354004};
    auto vector1 = getScyllaVectorOf(test_const::double_vector_1_id, values1);

    // When performing sum of absolute values in this vector.
    double res = scheduler->dasum(*vector1);

    double abs_sum = 0;
    for (double v : values1) {
        abs_sum += std::abs(v);
    }
    std::cout << std::setprecision(20) << abs_sum << "=sum\n";
    std::cout << std::setprecision(20) << res << "=res\n";

    // Then it is correctly calculated and equal to sum.
    BOOST_CHECK(std::abs(abs_sum - res) < scylla_blas::epsilon);
}

BOOST_FIXTURE_TEST_CASE(vector_idamax_double, vector_fixture)
{
    // Given vector of some values.
    std::vector<double> values1 = {0.00494931, 0.119193, -0.927604, 0.354004};
    auto vector1 = getScyllaVectorOf(test_const::double_vector_1_id, values1);

    // When performing max fetch on the abs values in this vector.
    scylla_blas::index_t res = scheduler->idamax(*vector1);

    scylla_blas::index_t max_index = 0;
    for (scylla_blas::index_t i = 1; i < values1.size(); i++) {
        if (std::abs(values1[i]) > std::abs(values1[max_index])) {
            max_index = i;
        }
    }
    std::cout << std::setprecision(20) << values1[max_index] << "=sum\n";
    std::cout << max_index << "=max_index\n";
    std::cout << res << "=res index\n";

    // Then the found index corresponds to the index of largest absolute value's index.
    BOOST_CHECK(max_index + 1 == res);
}


/**	\file 	test_main.cpp
*	\brief	Testing Kernels in MLearnKernels.h
*/
// Test framework
#define CATCH_CONFIG_MAIN
#include <catch.hpp>
#include <test_common.h>

// MLearn
#include <MLearn/Core>
#include <MLearn/Sampling/GaussianSampling.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

// Boost includes
#include <boost/random/taus88.hpp>

TEST_CASE("Test utility functions for multivariate gaussian sampling."){
	typedef double FT;
	using namespace MLearn;
	using namespace Eigen;
	using namespace Sampling::Gaussian;
	using namespace SamplingImpl;
	uint dim = 3;
	uint N_samples = 10;
	MLMatrix<FT> A(dim, dim);

	A << 4, 1 ,-1,
	     1, 2 , 1,
	    -1, 1 , 2;

	SECTION("Test transformation extraction"){
		// Preallocation
		MLMatrix<FT> transform(dim, dim);

		SECTION("Test using pure cholesky!"){
			LLT<MLMatrix<FT>> cholesky(A);
			transform = MLMatrix<FT>::Random(dim, dim);

			REQUIRE_FALSE(
				TestUtils::diff_norm(transform*transform.transpose(), A) ==
				Approx(0).margin(TEST_FLOAT_TOLERANCE));

			transform_from_decomposition(transform, cholesky);

			REQUIRE(
				TestUtils::diff_norm(transform*transform.transpose(), A) ==
				Approx(0).margin(TEST_FLOAT_TOLERANCE));
		}

		SECTION("Test using eigensolver!"){
			SelfAdjointEigenSolver<MLMatrix<FT>> eigensolver(A);
			transform = MLMatrix<FT>::Random(dim, dim);

			REQUIRE_FALSE(
				TestUtils::diff_norm(transform*transform.transpose(), A) ==
				Approx(0).margin(TEST_FLOAT_TOLERANCE));

			transform_from_decomposition(transform, eigensolver);

			REQUIRE(
				TestUtils::diff_norm(transform*transform.transpose(), A) ==
				Approx(0).margin(TEST_FLOAT_TOLERANCE));

		}

	}

	SECTION("Test samples transformation"){
		MLMatrix<FT> orig_samples = MLMatrix<FT>::Random(dim, N_samples);
		MLMatrix<FT> samples = orig_samples;
		MLVector<FT> mean = MLVector<FT>::Random(dim);

		MLMatrix<FT> transform(dim, dim);
		transform_from_covariance<TransformMethod::CHOL>(transform, A);

		SECTION("Test transformation using transform"){
			transform_gaussian_samples_with_transform(mean, transform, samples);

			MLMatrix<FT> reverted_samples = samples;
			reverted_samples.colwise() -= mean;
			reverted_samples = transform.inverse()*reverted_samples;

			REQUIRE(
				TestUtils::diff_norm(orig_samples, reverted_samples) ==
				Approx(0).margin(TEST_FLOAT_TOLERANCE));
		}

		SECTION("Test transformation using covariance"){
			transform_gaussian_samples_with_covariance<TransformMethod::CHOL>
				(mean, A, samples);

			MLMatrix<FT> reverted_samples = samples;
			reverted_samples.colwise() -= mean;
			reverted_samples = transform.inverse()*reverted_samples;

			REQUIRE(
				TestUtils::diff_norm(orig_samples, reverted_samples) ==
				Approx(0).margin(TEST_FLOAT_TOLERANCE));
		}
	}

	SECTION("Test samples generation ..."){
		uint N_samples = 34;
		uint dim = 5;
		SECTION("... with custom random number generator."){
			boost::random::taus88 rng(seed_from_time());

			MLMatrix<FT> samples =
				sample_standard_gaussian<FT>(dim, N_samples, rng);

			REQUIRE(samples.rows() == dim);
			REQUIRE(samples.cols() == N_samples);
			REQUIRE(samples.norm() > 0.0);
		}

		SECTION("... without random number generator."){
			MLMatrix<FT> samples =
				sample_standard_gaussian<FT>(dim, N_samples);

			REQUIRE(samples.rows() == dim);
			REQUIRE(samples.cols() == N_samples);
			REQUIRE(samples.norm() > 0.0);
		}
	}

}

TEST_CASE("Multivariate gaussian class test"){
	typedef double FT;
	using namespace MLearn;
	using namespace Eigen;
	using namespace Sampling::Gaussian;

	uint N_samples = 17;
	uint dim = 3;

	MLVector<FT> mean = MLVector<FT>::Random(dim);
	MLMatrix<FT> covariance(dim, dim);

	covariance << 4, 1 ,-1,
	              1, 2 , 1,
	             -1, 1 , 2;

	SECTION("Test static sampling function"){
		MLMatrix<FT> samples = MultivariateGaussian<FT>::sample(
			mean, covariance, N_samples);

		REQUIRE(samples.cols() == N_samples);
		REQUIRE(samples.rows() == dim);
		REQUIRE(samples.norm() > 0.0);
	}

	SECTION("Test sampling using instantiated class"){
		MultivariateGaussian<FT> mg(mean, covariance);
		MLMatrix<FT> samples = mg.sample(N_samples);

		REQUIRE(samples.cols() == N_samples);
		REQUIRE(samples.rows() == dim);
		REQUIRE(samples.norm() > 0.0);
	}

	SECTION("Test class"){
		MultivariateGaussian<FT> ref_mg(mean, covariance);

		REQUIRE(TestUtils::diff_norm(mean, ref_mg.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(TestUtils::diff_norm(covariance, ref_mg.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		MultivariateGaussian<FT> copy_mg(ref_mg);

		REQUIRE(TestUtils::diff_norm(mean, copy_mg.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(TestUtils::diff_norm(covariance, copy_mg.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		MultivariateGaussian<FT>
			move_mg(std::move(MultivariateGaussian<FT>(ref_mg)));

		REQUIRE(TestUtils::diff_norm(mean, move_mg.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(TestUtils::diff_norm(covariance, move_mg.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		MultivariateGaussian<FT> copy_assign_mg;
		copy_assign_mg = ref_mg;
		REQUIRE(TestUtils::diff_norm(mean, copy_assign_mg.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(TestUtils::diff_norm(covariance, copy_assign_mg.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		MultivariateGaussian<FT> move_assign_mg;
		move_assign_mg = std::move(MultivariateGaussian<FT>(ref_mg));
		REQUIRE(TestUtils::diff_norm(mean, move_assign_mg.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(TestUtils::diff_norm(covariance, move_assign_mg.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));

		MLVector<FT> new_mean = MLVector<FT>::Random(dim);
		MLMatrix<FT> new_covariance = 5.0*covariance;

		ref_mg.set_mean(new_mean);
		REQUIRE(TestUtils::diff_norm(new_mean, ref_mg.mean()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		ref_mg.set_covariance(new_covariance);
		REQUIRE(TestUtils::diff_norm(new_covariance, ref_mg.covariance()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
		REQUIRE(TestUtils::diff_norm(new_covariance,
						ref_mg.transform()*ref_mg.transform().transpose()) ==
			Approx(0).margin(TEST_FLOAT_TOLERANCE));
	}

}


#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <map>
#include <vector>
#include <iostream>
#include <iomanip>

static void boost_uniform( const int trials , unsigned seed )
{
  // Generate numbers from a uniforma distribution using boost
  // Roll dice and keep track of the some of the dice
  // Print the resulting distribuiton for the trial

  boost::mt19937 random_num_gen( seed );
  boost::uniform_int<> urange(1,6);
  boost::variate_generator< boost::mt19937 , boost::uniform_int<> > dice( random_num_gen , urange );

  // create a map to store the count of the number of occurrences of the sum on the dist
  // first is the dice sum the second is the count;
  std::map<int,int> throw_cnt;
  for( int i = 2 ; i <=12 ; i++ ) throw_cnt[i]=0;

  for( int i = 0; i < trials ; i++ )
    {
      int d1 = dice() , d2 = dice() , s = d1 + d2;
      std::map<int,int>::iterator itr = throw_cnt.find(s);
      (*itr).second++;
    }

  double n = static_cast<double>(trials);
  std::cout << "Dice Throw : Trials = " << std::setprecision(1) << std::scientific << n << std::endl;
  for( std::map<int,int>::iterator itr = throw_cnt.begin() ; itr != throw_cnt.end() ; ++itr )
    {
      double p = static_cast<double>((*itr).second) / n;
      std::cout << std::setw(3) << (*itr).first << "," << std::setw(10) << std::setprecision(8) << std::fixed << p << std::endl;
    }
  std::cout << std::endl;
}

static void boost_normal( const int trials , unsigned seed , double mean , double variance )
{
  // Generate Normal Random Variables using boost libraties
  // Then assess the density at +/- 2 sigma and +/- 1 sigma

  boost::mt19937 random_num_gen( seed );
  boost::normal_distribution<double> n_dist(mean,variance);
  boost::variate_generator< boost::mt19937 , boost::normal_distribution<double> > normal_rv( random_num_gen , n_dist );

  std::vector<double> dist;
  dist.reserve(trials);
  for( int i = 0 ; i < trials ; i++ )
    {
      dist.push_back( normal_rv() );
    }

  std::sort( dist.begin(), dist.end() );

  double s = sqrt(variance);

  if( dist.empty() ) return;
  std::vector<double>::iterator itr_2slb = std::lower_bound( dist.begin() , dist.end() , -2.0*s );
  std::vector<double>::iterator itr_2sub = std::lower_bound( dist.begin() , dist.end() , 2.0*s );

  std::vector<double>::iterator itr_1slb = std::lower_bound( dist.begin() , dist.end() , -1.0*s );
  std::vector<double>::iterator itr_1sub = std::lower_bound( dist.begin() , dist.end() , 1.0*s );

  double nobs_2sd = itr_2sub - itr_2slb;
  double nobs_1sd = itr_1sub - itr_1slb;
  double n = static_cast<double>( trials );
  std::cout << "Normal RV : ";
  std::cout << std::setw(8) << std::setprecision(1) << std::scientific << static_cast<double>(trials);
  std::cout << " : mean +/- 2sigma % = " << std::setw(10) << std::setprecision(6) << std::fixed << nobs_2sd/n;
  std::cout << " : mean +/- 1sigma % = " << std::setw(10) << std::setprecision(6) << std::fixed << nobs_1sd/n;
  std::cout << std::endl;
}

static void boost_number_gen()
{
  const unsigned  seed = 5489u;;
  boost_uniform(1e3,seed);
  boost_uniform(1e6,seed);
  boost_uniform(1e7,seed);

  const double mean= 0.0 , variance = 1.0 ;
  boost_normal( 1e2 , seed , mean , variance );
  boost_normal( 1e3 , seed , mean , variance );
  boost_normal( 1e6 , seed , mean , variance );
  boost_normal( 1e7 , seed , mean , variance );
}


// Copyright (c) 2017 Evan S Weinberg
// A reference piece of code which computes matrix elements
// of a reference sparse Laplace operator to fill a dense
// Eigen matrix, then computes the spectrum and prints
// the Eigenvalues.

// This code is for real, symmetric matrices.
// This code lives on github at github.com/weinbe2/eigens-with-eigen/

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// This is just for convenience. By default, all matrices
// in Eigen are column major. You can replace "Dynamic"
// with a specific number to template just one size.
// You can also ust use "MatrixXd".
typedef Matrix<double, Dynamic, Dynamic, ColMajor> dMatrix;

// Reference 1-D Laplace function.
void laplace_1d(double* out, double* in, const int L, const double m2);

int main(int argc, char** argv)
{
  double *in_real;
  double *out_real;

  // Set output precision to be long.
  cout << setprecision(10);

  // Basic information about the lattice.
  const int length = 8;
  const double m_sq = 0.001;

  // Print the basic info.
  std::cout << "1D Laplace operator, length " << length << ", mass squared " << m_sq << ", zero boundary conditions.\n";
  std::cout << "Change the length and mass by modifying the source.\n";

  // Allocate.
  in_real = new double[length];
  out_real = new double[length];

  // Zero out.
  for (int i = 0; i < length; i++)
  {
    in_real[i] = out_real[i] = 0.0;
  }

  //////////////////////////
  // REAL, SYMMETRIC CASE //
  //////////////////////////

  std::cout << "Real, Symmetric case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real = dMatrix::Zero(length, length);

  // Form matrix elements. This is where it's important that
  // dMatrix is column major.
  for (int i = 0; i < length; i++)
  {
    // Set a point on the rhs for a matrix element.
    // If appropriate, zero out the previous point.
    if (i > 0)
    {
      in_real[i-1] = 0.0;
    }
    in_real[i] = 1.0;

    // Zero out the "out" vector. I defined "laplace_1d" to
    // not require this, but I put this here for generality.
    for (int j = 0; j < length; j++)
      out_real[j] = 0.0;

    // PUT YOUR MAT-VEC HERE.
    laplace_1d(out_real, in_real, length, m_sq);

    // Copy your output into the right memory location.
    // If your data layout supports it, you can also pass
    // "mptr" directly as your "output vector" when you call
    // your mat-vec.
    double* mptr = &(mat_real(i*length));

    for (int j = 0; j < length; j++)
    {
      mptr[j] = out_real[j];
    }
  }

  // We've now formed the dense matrix. We print it here
  // as a sanity check if it's small enough.
  if (length <= 16)
  {
    std::cout << mat_real << "\n";
  }

  // Get the eigenvalues and eigenvectors.
  SelfAdjointEigenSolver< dMatrix > eigsolve_real(length);
  eigsolve_real.compute(mat_real);

  // Remark: if you only want the eigenvalues, you can call
  // eigsolve_real.compute(mat_real, EigenvaluesOnly);

  // Print the eigenvalues.
  dMatrix evals = eigsolve_real.eigenvalues();
  std::cout << "The eigenvalues are:\n" << evals << "\n\n";
  // You can also index individual eigenvalues as "evals(i)", where
  // "i" zero-indexes the eigenvalues.

  // Print the eigenvectors if the matrix is small enough.
  // As a remark, this also shows you how to access the eigenvectors.
  if (length <= 16)
  {
    for (int i = 0; i < length; i++)
    {
      // You can use "VectorXd" as the type instead.
      dMatrix evec = eigsolve_real.eigenvectors().col(i);

      // Print the eigenvector.
      std::cout << "Eigenvector " << i << " equals:\n" << evec << "\n\n";

      // You can also copy the eigenvector into another array as such:
      for (int j = 0; j < length; j++)
      {
        out_real[j] = evec(j);
      }
    }
  }

  // Clean up.
  delete[] in_real;
  delete[] out_real;

  return 0;
}

// Reference 1-D Laplace function.
void laplace_1d(double* out, double* in, const int L, const double m2)
{
  // Zero boundary conditions. Set first and last element explicitly.
  out[0] = (2+m2)*in[0] - in[1];
  out[L-1] = (2+m2)*in[L-1] - in[L-2];

  // The rest.
  for (int i = 1; i < L-1; i++)
  {
    out[i] = (2+m2)*in[i] - in[i-1] - in[i+1];
  }

  // Done.
  return;
}


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

using namespace std;

int main(int argc, char** argv)
{
    // 机器人B在坐标系O中的坐标：
    Eigen::Vector3d B(3, 4, M_PI);

    // 坐标系B到坐标O的转换矩阵：
    Eigen::Matrix3d TOB;
    TOB << cos(B(2)), -sin(B(2)), B(0),
           sin(B(2)),  cos(B(2)), B(1),
              0,          0,        1;

    // 坐标系O到坐标B的转换矩阵:
    Eigen::Matrix3d TBO = TOB.inverse();

    // 机器人A在坐标系O中的坐标：
    Eigen::Vector3d A(1, 3, -M_PI / 2);

    // 求机器人A在机器人B中的坐标：
    Eigen::Vector3d BA;
    // TODO 参照第一课PPT
    // start your code here (5~10 lines)
    Eigen::Matrix3d TOA;

    // end your code here

    cout << "The right answer is BA: 2 1 1.5708" << endl;
    cout << "Your answer is BA: " << BA.transpose() << endl;

    return 0;
}


// A majority of this code is from Rosetta Code: https://rosettacode.org/wiki/Ackermann_function#Efficient_version
// This adaptation allows for user input
#include <iostream>
#include <sstream>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/lexical_cast.hpp>

using big_int = boost::multiprecision::cpp_int;

big_int ipow(big_int base, big_int exp);
big_int ackermann(unsigned m, unsigned n);

int main(){
	std::string mStr, nStr;

	std::cout << "Ack(m,n)\nm = ";
	std::getline(std::cin, mStr);

	std::cout << "n = ";
	std::getline(std::cin, nStr);

	std::cout << "Ack(" + mStr + ',' + nStr + ") = "
		<< ackermann(boost::lexical_cast<unsigned>(mStr),boost::lexical_cast<unsigned>(nStr)) << '\n';

	return 0;
}

big_int ipow(big_int base, big_int exp){
	big_int result(1);
	while(exp){
		if(exp & 1){
			result *= base;
		}
		exp >>= 1;
		base *= base;
	}
	return result;
}

big_int ackermann(unsigned m, unsigned n){
	static big_int (*ack)(unsigned, big_int) =
		[](unsigned m, big_int n)->big_int {
			switch(m){
				case 0:
					return n+1;
				case 1:
					return n+2;
				case 2:
					return 3+2*n;
				case 3:
					return 5 + 8 * (ipow(big_int(2), n) - 1);
				default:
					return n == 0 ? ack(m - 1, big_int(1)) : ack(m - 1, ack(m, n - 1));
			}
		};
	return ack(m, big_int(n));
}