/*!
 * @file intmath_test.cpp
 *
 * @author Andrzej Ciarkowski <mailto:andrzej.ciarkowski@gmail.com>
 */
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <climits>
#include <cstdint>
#include <dsp++/intmath.h>

using namespace dsp;

BOOST_AUTO_TEST_SUITE(intmath)

BOOST_AUTO_TEST_CASE(test_signum)
{
	BOOST_CHECK(dsp::signum(-10) == -1);
	BOOST_CHECK(dsp::signum(INT_MIN) == -1);
	BOOST_CHECK(dsp::signum(0) == 0);
	BOOST_CHECK(dsp::signum(100) == 1);
	BOOST_CHECK(dsp::signum(INT_MAX) == 1);
	BOOST_CHECK(dsp::signum(0u) == 0u);
	BOOST_CHECK(dsp::signum(123u) == 1u);
}

BOOST_AUTO_TEST_CASE(test_add)
{
	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(1, 1) == 2);
	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(10, -10) == 0);
	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(-100, -100) == -200);
	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(-1000, 1000) == 0);
	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(INT_MAX, 1000) < 0);
	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(INT_MIN, -1000) > 0);

	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(10000u, 1u) == 10001u);
	BOOST_CHECK(dsp::add<dsp::overflow::fastest>(0u, 0u) == 0u);

	BOOST_CHECK(dsp::add<dsp::overflow::saturate>(1, 1) == 2);
	BOOST_CHECK(dsp::add<dsp::overflow::saturate>(1, INT_MAX) == INT_MAX);
	BOOST_CHECK(dsp::add<dsp::overflow::saturate>(-1, INT_MIN) == INT_MIN);
	BOOST_CHECK(dsp::add<dsp::overflow::saturate>(INT_MAX - 100, 110) == INT_MAX);
	BOOST_CHECK(dsp::add<dsp::overflow::saturate>(INT_MIN + 1000, -1001) == INT_MIN);
	BOOST_CHECK(dsp::add<dsp::overflow::saturate>(1u, UINT_MAX) == UINT_MAX);

	BOOST_CHECK_NO_THROW(dsp::add<dsp::overflow::exception>(INT_MAX - 10, 10));
	BOOST_CHECK_NO_THROW(dsp::add<dsp::overflow::exception>(INT_MIN + 100, 100));
	BOOST_CHECK_THROW(dsp::add<dsp::overflow::exception>(INT_MAX - 10, 11), std::overflow_error);
	BOOST_CHECK_THROW(dsp::add<dsp::overflow::exception>(INT_MIN + 11, -15), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(test_sub)
{
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(1, 1) == 0);
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(10, -10) == 20);
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(-100, -100) == 0);
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(-1000, 1000) == -2000);
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(INT_MIN, 1000) > 0);
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(INT_MAX, -1000) < 0);

	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(10000u, 1u) == 9999u);
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(0u, 0u) == 0u);
	BOOST_CHECK(dsp::sub<dsp::overflow::fastest>(0u, 100u) > 100u);

	BOOST_CHECK(dsp::sub<dsp::overflow::saturate>(1, 1) == 0);
	BOOST_CHECK(dsp::sub<dsp::overflow::saturate>(INT_MIN, 100) == INT_MIN);
	BOOST_CHECK(dsp::sub<dsp::overflow::saturate>(INT_MAX, -110) == INT_MAX);
	BOOST_CHECK(dsp::sub<dsp::overflow::saturate>(1u, UINT_MAX) == 0);

	BOOST_CHECK_NO_THROW(dsp::sub<dsp::overflow::exception>(INT_MIN + 10, 10));
	BOOST_CHECK_NO_THROW(dsp::sub<dsp::overflow::exception>(INT_MAX - 100, -100));
	BOOST_CHECK_THROW(dsp::sub<dsp::overflow::exception>(INT_MAX - 10, -11), std::overflow_error);
	BOOST_CHECK_THROW(dsp::sub<dsp::overflow::exception>(INT_MIN + 11, 15), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(test_mul)
{
	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(1, 1) == 1);
	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(10, -10) == -100);
	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(-100, -100) == 10000);
	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(-1000, 1000) == -1000000);
	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(INT_MAX, 2) < 0);

	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(10000u, 1u) == 10000u);
	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(0u, 0u) == 0u);
	BOOST_CHECK(dsp::mul<dsp::overflow::fastest>(0u, 100u) == 0u);

	BOOST_CHECK(dsp::mul<dsp::overflow::saturate>(1, 1) == 1);
	BOOST_CHECK(dsp::mul<dsp::overflow::saturate>(INT_MIN, 100) == INT_MIN);
	BOOST_CHECK(dsp::mul<dsp::overflow::saturate>(INT_MIN, -100) == INT_MAX);
	BOOST_CHECK(dsp::mul<dsp::overflow::saturate>(INT_MAX, 110) == INT_MAX);
	BOOST_CHECK(dsp::mul<dsp::overflow::saturate>(INT_MAX, -110) == INT_MIN);
	BOOST_CHECK(dsp::mul<dsp::overflow::saturate>(INT_MIN, -1) == INT_MAX);
	BOOST_CHECK(dsp::mul<dsp::overflow::saturate>(2u, UINT_MAX) == UINT_MAX);

	BOOST_CHECK_NO_THROW(dsp::mul<dsp::overflow::exception>(INT_MIN, 1));
	BOOST_CHECK_NO_THROW(dsp::mul<dsp::overflow::exception>(INT_MAX, -1));
	BOOST_CHECK_THROW(dsp::mul<dsp::overflow::exception>(INT_MIN, -1), std::overflow_error);
	BOOST_CHECK_THROW(dsp::mul<dsp::overflow::exception>(INT_MAX, 2), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(test_div)
{
	BOOST_CHECK(dsp::div<dsp::overflow::fastest>(1, 1) == 1);
	BOOST_CHECK(dsp::div<dsp::overflow::fastest>(10, -10) == -1);
	BOOST_CHECK(dsp::div<dsp::overflow::fastest>(-100, -100) == 1);
	BOOST_CHECK(dsp::div<dsp::overflow::fastest>(-1000, 1000) == -1);
	BOOST_CHECK(dsp::div<dsp::overflow::fastest>(INT_MAX, 2) > 0);

	BOOST_CHECK(dsp::div<dsp::overflow::fastest>(10000u, 2u) == 5000u);
	BOOST_CHECK(dsp::div<dsp::overflow::fastest>(0u, 100u) == 0u);

	BOOST_CHECK(dsp::div<dsp::overflow::saturate>(INT_MIN, -1) == INT_MAX);
	BOOST_CHECK_NO_THROW(dsp::div<dsp::overflow::exception>(INT_MIN, 1));
	BOOST_CHECK_NO_THROW(dsp::div<dsp::overflow::exception>(INT_MAX, -1));
	BOOST_CHECK_THROW(dsp::div<dsp::overflow::exception>(INT_MIN, -1), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(test_mod)
{
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(1, 1) == 0);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(10, -10) == 0);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(-100, -100) == 0);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(-1000, 1000) == 0);

	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(11, 10) == 1);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(-11, 10) == -1);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(11, -10) == 1);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(-11, -10) == -1);

	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(INT_MAX, 2) == 1);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(INT_MIN, 2) == 0);

	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(10000u, 2u) == 0u);
	BOOST_CHECK(dsp::mod<dsp::overflow::fastest>(0u, 100u) == 0u);
}

BOOST_AUTO_TEST_CASE(test_neg)
{
	BOOST_CHECK(dsp::neg<dsp::overflow::fastest>(1) == -1);
	BOOST_CHECK(dsp::neg<dsp::overflow::fastest>(10) == -10);
	BOOST_CHECK(dsp::neg<dsp::overflow::fastest>(-100) == 100);
	BOOST_CHECK(dsp::neg<dsp::overflow::fastest>(INT_MAX) == -INT_MAX);

	BOOST_CHECK(dsp::neg<dsp::overflow::saturate>(10000u) == 0u);
	BOOST_CHECK(dsp::neg<dsp::overflow::fastest>(0u) == 0u);

	BOOST_CHECK(dsp::neg<dsp::overflow::saturate>(INT_MIN) == INT_MAX);
	BOOST_CHECK_THROW(dsp::neg<dsp::overflow::exception>(INT_MIN), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(test_round)
{
	BOOST_CHECK((dsp::round<rounding::truncated>(19234, 0) == 19234));
	BOOST_CHECK((dsp::round<rounding::nearest>(19234, 0) == 19234));
	BOOST_CHECK((dsp::round<rounding::negative>(19234, -1) == 19234));
	BOOST_CHECK((dsp::round<rounding::positive>(19234, -15) == 19234));

	BOOST_CHECK((dsp::round<rounding::truncated>(10, 3) == 8));
	BOOST_CHECK((dsp::round<rounding::nearest>(10, 3) == 8));
	BOOST_CHECK((dsp::round<rounding::negative>(10, 3) == 8));
	BOOST_CHECK((dsp::round<rounding::positive>(10, 3) == 16));

	BOOST_CHECK((dsp::round<rounding::truncated>(12, 3) == 8));
	BOOST_CHECK((dsp::round<rounding::nearest>(12, 3) == 16));
	BOOST_CHECK((dsp::round<rounding::negative>(12, 3) == 8));
	BOOST_CHECK((dsp::round<rounding::positive>(12, 3) == 16));

	BOOST_CHECK((dsp::round<rounding::truncated>(-10, 3) == -8));
	BOOST_CHECK((dsp::round<rounding::nearest>(-10, 3) == -8));
	BOOST_CHECK((dsp::round<rounding::negative>(-10, 3) == -16));
	BOOST_CHECK((dsp::round<rounding::positive>(-10, 3) == -8));

	BOOST_CHECK((dsp::round<rounding::truncated>(-12, 3) == -8));
	BOOST_CHECK((dsp::round<rounding::nearest>(-12, 3) == -16));
	BOOST_CHECK((dsp::round<rounding::negative>(-12, 3) == -16));
	BOOST_CHECK((dsp::round<rounding::positive>(-12, 3) == -8));

	BOOST_CHECK((dsp::round<rounding::truncated>(std::int16_t(32767), 15) == 0));
	BOOST_CHECK((dsp::round<rounding::nearest>(std::int16_t(32767), 15) == 0));
	BOOST_CHECK((dsp::round<rounding::truncated>(std::int16_t(-32768), 15) == 0));
	BOOST_CHECK((dsp::round<rounding::nearest>(std::int16_t(-32768), 15) == 0));

	BOOST_CHECK_NO_THROW((dsp::round<rounding::truncated, overflow::exception>(std::int16_t(32767), 14)));
	BOOST_CHECK((dsp::round<rounding::truncated, overflow::exception>(std::int16_t(32767), 14) == 16384));
	BOOST_CHECK_THROW((dsp::round<rounding::nearest, overflow::exception>(std::int16_t(32767), 14)), std::overflow_error);
	BOOST_CHECK_NO_THROW((dsp::round<rounding::nearest, overflow::exception>(std::int16_t(-32767), 14)));
	BOOST_CHECK((dsp::round<rounding::nearest, overflow::exception>(std::int16_t(-32767), 14) == std::int16_t(-32768)));
}

BOOST_AUTO_TEST_CASE(test_check_overflow)
{
	std::int16_t val = 16383;
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 14));
	++val;
	BOOST_CHECK_THROW(dsp::overflow_check_handle<overflow::exception>(val, 14), std::overflow_error);
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 15));
	val = 32767;
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 15));
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 16));
	val = -16384;
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 14));
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 15));
	--val;
	BOOST_CHECK_THROW(dsp::overflow_check_handle<overflow::exception>(val, 14), std::overflow_error);
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 15));
	val = 0;
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(val, 0));
	++val;
	BOOST_CHECK_THROW(dsp::overflow_check_handle<overflow::exception>(val, 0), std::overflow_error);

	std::uint16_t uval = 16383;
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 14));
	++uval;
	BOOST_CHECK_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 14), std::overflow_error);
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 15));
	uval = 32767;
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 15));
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 16));
	uval = 32768;
	BOOST_CHECK_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 15), std::overflow_error);
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 16));
	uval = 0;
	BOOST_CHECK_NO_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 0));
	++uval;
	BOOST_CHECK_THROW(dsp::overflow_check_handle<overflow::exception>(uval, 0), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(test_rint)
{
	BOOST_CHECK((dsp::rint<std::int16_t>(0.5f, rounding::truncated, overflow::fastest) == 0));
	BOOST_CHECK((dsp::rint<std::int16_t>(0.9f, rounding::truncated, overflow::fastest) == 0));
	BOOST_CHECK((dsp::rint<std::int16_t>(-0.9f, rounding::truncated, overflow::fastest) == 0));
	BOOST_CHECK((dsp::rint<std::int16_t>(-11.f, rounding::truncated, overflow::fastest) == -11));
	BOOST_CHECK((dsp::rint<std::int16_t>(100.f, rounding::truncated, overflow::fastest) == 100));

	BOOST_CHECK((dsp::rint<std::int16_t>(0.1, rounding::nearest, overflow::fastest) == 0));
	BOOST_CHECK((dsp::rint<std::int16_t>(-0.1, rounding::nearest, overflow::fastest) == 0));
	BOOST_CHECK((dsp::rint<std::int32_t>(100.7, rounding::nearest, overflow::fastest) == 101));
	BOOST_CHECK((dsp::rint<std::int32_t>(-100.7, rounding::nearest, overflow::fastest) == -101));
	BOOST_CHECK((dsp::rint<std::int16_t>(32767.6, rounding::positive, overflow::saturate) == 32767));
	BOOST_CHECK((dsp::rint<std::int16_t>(-32769.6, rounding::positive, overflow::saturate) == -32768));
	BOOST_CHECK((dsp::rint<std::int16_t>(-32767.6, rounding::negative, overflow::saturate) == -32768));
	BOOST_CHECK((dsp::rint<std::int16_t>(-32767.6, rounding::positive, overflow::saturate) == -32767));

	BOOST_CHECK_THROW((dsp::rint<std::int16_t>(-32768.6, rounding::nearest, overflow::exception)), std::overflow_error);
	BOOST_CHECK_THROW((dsp::rint<std::int16_t>(32767.6, rounding::nearest, overflow::exception)), std::overflow_error);
	BOOST_CHECK_THROW((dsp::rint<std::uint16_t>(-1., rounding::nearest, overflow::exception)), std::overflow_error);
	BOOST_CHECK_THROW((dsp::rint<std::uint16_t>(-1., rounding::nearest, overflow::exception)), std::overflow_error);

	BOOST_CHECK_THROW((dsp::rint<int>(-2147483648.5, rounding::nearest, overflow::exception)), std::overflow_error);
	BOOST_CHECK_THROW((dsp::rint<int>(2147483647.5, rounding::nearest, overflow::exception)), std::overflow_error);
}

BOOST_AUTO_TEST_CASE(test_gcd)
{
	BOOST_CHECK(dsp::gcd(1, 1) == 1);
	BOOST_CHECK(dsp::gcd(5, 3) == 1);
	BOOST_CHECK(dsp::gcd(3, 5) == 1);
	BOOST_CHECK(dsp::gcd(6, 4) == 2);
	BOOST_CHECK(dsp::gcd(8, 4) == 4);
	BOOST_CHECK(dsp::gcd(44100, 48000) == 300);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef MLT_UTILS_EIGEN_HPP
#define MLT_UTILS_EIGEN_HPP

#include <random>

#include <Eigen/Core>

#include "../defs.hpp"

namespace mlt {
namespace utils {
namespace eigen {
	inline Map<const MatrixXd> ravel(MatrixXdRef x) {
		return Map<const MatrixXd>(x.data(), x.size(), 1);
	}

	inline Map<const MatrixXd> unravel(MatrixXdRef x, size_t rows, size_t cols) {
		return Map<const MatrixXd>(x.data(), rows, cols);
	}

	template <class MatrixA, class MatrixB, typename Rng = default_random_engine >
	auto tied_random_cols_subset(MatrixA&& a, MatrixB&& b, size_t subset_size, Rng&& rng = Rng()) {
		assert(subset_size > 0);
		uniform_int_distribution<size_t> distribution(0, a.cols() - 1);

		auto a_batch = a.leftCols(subset_size).eval();
		auto b_batch = b.leftCols(subset_size).eval();

		for (auto i = 0; i < subset_size; i++) {
			auto cidx = distribution(rng);
			a_batch.col(i) = a.col(cidx);
			b_batch.col(i) = b.col(cidx);
		}

		return make_tuple(a_batch, b_batch);
	}

	auto classes_vector_to_classes_matrix(VectorXiRef classes) {
		auto classes_matrix = MatrixXi{ MatrixXi::Zero(classes.maxCoeff() + 1, classes.size()) };

		for (unsigned int i = 0; i < classes.size(); i++) {
			classes_matrix(classes(i), i) = 1;
		}

		return classes_matrix;
	}

	auto classes_matrix_to_classes_vector(MatrixXiRef classes) {
		assert((classes.colwise().sum().array() == 1).all());

		auto classes_vector = VectorXi(classes.cols());

		for (size_t col = 0; col < classes.cols(); col++) {
			for (size_t row = 0; row < classes.rows(); row++) {
				if (classes(row, col) == 1) {
					classes_vector(col) = (row);
					break;
				}
			}
		}

		return classes_vector;
	}

	inline auto max_row(VectorXdRef x) {
		int max_row;
		x.maxCoeff(&max_row);
		return max_row;
	}
}
}
}
#endif


#pragma once

#include <Eigen/Core>
#include <Eigen/StdVector>

namespace km
{
namespace types
{
template <typename Scalar_t, int M, int N>
using Mat = Eigen::Matrix<Scalar_t, M, N>;

template <typename Scalar_t, int N>
using Vec = Mat<Scalar_t, N, 1>;

template <typename Scalar_t>
using MatX = Mat<Scalar_t, Eigen::Dynamic, Eigen::Dynamic>;

template <typename Scalar_t>
using VecX = Vec<Scalar_t, Eigen::Dynamic>;

template <typename Scalar_t, int N>
using SquareMat = Mat<Scalar_t, N, N>;

template <typename T>
using vector_aligned = std::vector<T, Eigen::aligned_allocator<T>>;
} // namespace types
} // namespace km


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

#include <cassert>
#include <cstddef>
#include <cstdlib>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/typeof/typeof.hpp>

struct edge_t
{
  unsigned long first;
  unsigned long second;
};

// A graphic of the min-cut is available at <http://www.boost.org/doc/libs/release/libs/graph/doc/stoer_wagner_imgs/stoer_wagner.cpp.gif>
int main()
{
  using namespace std;

  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
    boost::no_property, boost::property<boost::edge_weight_t, int> > undirected_graph;
  typedef boost::property_map<undirected_graph, boost::edge_weight_t>::type weight_map_type;
  typedef boost::property_traits<weight_map_type>::value_type weight_type;

  // define the 16 edges of the graph. {3, 4} means an undirected edge between vertices 3 and 4.
  edge_t edges[] = {{3, 4}, {3, 6}, {3, 5}, {0, 4}, {0, 1}, {0, 6}, {0, 7},
    {0, 5}, {0, 2}, {4, 1}, {1, 6}, {1, 5}, {6, 7}, {7, 5}, {5, 2}, {3, 4}};

  // for each of the 16 edges, define the associated edge weight. ws[i] is the weight for the edge
  // that is described by edges[i].
  weight_type ws[] = {0, 3, 1, 3, 1, 2, 6, 1, 8, 1, 1, 80, 2, 1, 1, 4};

  // construct the graph object. 8 is the number of vertices, which are numbered from 0
  // through 7, and 16 is the number of edges.
  undirected_graph g(edges, edges + 16, ws, 8, 16);

  // define a property map, `parities`, that will store a boolean value for each vertex.
  // Vertices that have the same parity after `stoer_wagner_min_cut` runs are on the same side of the min-cut.
  BOOST_AUTO(parities, boost::make_one_bit_color_map(num_vertices(g), get(boost::vertex_index, g)));

  // run the Stoer-Wagner algorithm to obtain the min-cut weight. `parities` is also filled in.
  int w = boost::stoer_wagner_min_cut(g, get(boost::edge_weight, g), boost::parity_map(parities));

  cout << "The min-cut weight of G is " << w << ".\n" << endl;
  assert(w == 7);

  cout << "One set of vertices consists of:" << endl;
  size_t i;
  for (i = 0; i < num_vertices(g); ++i) {
    if (get(parities, i))
      cout << i << endl;
  }
  cout << endl;

  cout << "The other set of vertices consists of:" << endl;
  for (i = 0; i < num_vertices(g); ++i) {
    if (!get(parities, i))
      cout << i << endl;
  }
  cout << endl;

  return EXIT_SUCCESS;
}


//------------------------------------------------------------------------------
/// \file StateMonadExamples_tests.cpp
/// \author Ernest Yeung
/// \email  ernestyalumni@gmail.com
/// \ref Edward Ashford Lee, Sanjit Arunkumar Seshia.
/// Introduction to Embedded Systems: A Cyber-Physical Systems Approach
/// (The MIT Press) Second Edition. The MIT Press; Second edition
/// (December 30, 2016). ISBN-10: 0262533812. ISBN-13: 978-0262533812
//------------------------------------------------------------------------------
#include "Categories/Monads/StateMonad.h"
#include "Categories/Monads/StateMonadExamples.h"

#include <boost/test/unit_test.hpp>

using namespace Categories::Monads::StateMonad::Examples;
using Categories::Monads::StateMonad::Compose;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Monads)
BOOST_AUTO_TEST_SUITE(StateMonadExamples_tests)

BOOST_AUTO_TEST_SUITE(GarageCounter_tests)

const GarageCounter::InputPorts up_not_down {
	GarageCounter::SignalPresence::present,
	GarageCounter::SignalPresence::absent};

const GarageCounter::InputPorts not_up_down {
	GarageCounter::SignalPresence::absent,
	GarageCounter::SignalPresence::present};

// Example 3.4, Fig. 3.4, garage counter Finite-State Machine (FSM), Lee and
// Seshia (2016), pp. 50

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(GarageCounterIncrements)
{
	GarageCounter::GarageCounterMorphism1 reaction {up_not_down};

	BOOST_TEST(reaction(5).first == 6);
	BOOST_TEST(reaction(6).first == 7);
	BOOST_TEST(reaction(7).first == 8);

	{
		GarageCounter::GarageCounterMorphism garage_counter {};
		auto reaction = garage_counter(up_not_down);

		BOOST_TEST(reaction(5).first == 6);
		BOOST_TEST(reaction(6).first == 7);
		BOOST_TEST(reaction(7).first == 8);
	}
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(GarageCounterDecrements)
{
	GarageCounter::GarageCounterMorphism1 reaction {not_up_down};

	BOOST_TEST(reaction(5).first == 4);
	BOOST_TEST(reaction(6).first == 5);
	BOOST_TEST(reaction(7).first == 6);

	{
		GarageCounter::GarageCounterMorphism garage_counter {};
		auto reaction = garage_counter(not_up_down);

		BOOST_TEST(reaction(5).first == 4);
		BOOST_TEST(reaction(6).first == 5);
		BOOST_TEST(reaction(7).first == 6);
	}
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ComposeGarageCounterMorphism1s)
{
	auto composed_reactions = Compose(
		GarageCounter::GarageCounterMorphism{},
		GarageCounter::GarageCounterMorphism{});

}

BOOST_AUTO_TEST_SUITE_END() // GarageCounter_tests

BOOST_AUTO_TEST_SUITE(ModestThermostatMorphism_tests)

//------------------------------------------------------------------------------
/// \ref pp. 51, Lee and Seshia, Introduction to Embedded Systems, Figure 3.5:
/// A model of a thermostat with hysteresis.
//------------------------------------------------------------------------------
ModestThermostat::ModestThermostatMorphism thermostat {18, 22};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(NoChangeForTemperaturesInHysteresis)
{
	{
		auto transition_function = thermostat(18.1);
		auto result = transition_function(ModestThermostat::State::cooling);
		BOOST_TEST((result.first == ModestThermostat::State::cooling));
		BOOST_TEST(!result.second);
	}
	{
		auto transition_function = thermostat(21.9);
		auto result = transition_function(ModestThermostat::State::heating);
		BOOST_TEST((result.first == ModestThermostat::State::heating));
		BOOST_TEST(!result.second);
	}
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ContinuesCoolingIfTemperatureIsAboveALimit)
{
	auto transition_function = thermostat(22.1);
	auto result = transition_function(ModestThermostat::State::cooling);
	BOOST_TEST((result.first == ModestThermostat::State::cooling));
	BOOST_TEST(!result.second);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ContinuesCoolingIfTemperatureIsAtALimit)
{
	auto transition_function = thermostat(22.0);
	auto result = transition_function(ModestThermostat::State::cooling);
	BOOST_TEST((result.first == ModestThermostat::State::cooling));
	BOOST_TEST(!result.second);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ContinuesHeatingIfTemperatureIsBelowALimit)
{
	auto transition_function = thermostat(17.9);
	auto result = transition_function(ModestThermostat::State::heating);
	BOOST_TEST((result.first == ModestThermostat::State::heating));
	BOOST_TEST(!result.second);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ContinuesHeatingIfTemperatureIsBelowAtALimit)
{
	auto transition_function = thermostat(18.0);
	auto result = transition_function(ModestThermostat::State::heating);
	BOOST_TEST((result.first == ModestThermostat::State::heating));
	BOOST_TEST(!result.second);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BeginsHeatingIfTemperatureIsBelowALimitAndHadBeenCooling)
{
	auto transition_function = thermostat(17.9);
	auto result = transition_function(ModestThermostat::State::cooling);
	BOOST_TEST((result.first == ModestThermostat::State::heating));
	BOOST_TEST(static_cast<bool>(result.second));
	BOOST_TEST((*result.second == ModestThermostat::Output::heatOn));
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BeginsCoolingIfTemperatureIsAboveALimitAndHadBeenHeating)
{
	auto transition_function = thermostat(22.1);
	auto result = transition_function(ModestThermostat::State::heating);
	BOOST_TEST((result.first == ModestThermostat::State::cooling));
	BOOST_TEST(static_cast<bool>(result.second));
	BOOST_TEST((*result.second == ModestThermostat::Output::heatOff));
}

BOOST_AUTO_TEST_SUITE_END() // ModestThermostatMorphism_tests

BOOST_AUTO_TEST_SUITE(TrafficLight_tests)

//------------------------------------------------------------------------------
/// \ref pp. 62, Lee and Seshia, Introduction to Embedded Systems, Figure 3.10:
/// State machine model of traffic light controller that keeps track of passage,
/// assuming it reacts at regular intervals.
//------------------------------------------------------------------------------
constexpr unsigned int count_time {60};
constexpr unsigned int yellow_to_red_time {5};
TrafficLight::TrafficLightMorphism traffic_light {
	count_time,
	yellow_to_red_time};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(
	TransitionsFromRedToGreenOnlyWhenCountIsGreaterThanOrEqualToCountTime)
{
	{
		auto transition_function =
			traffic_light(59, TrafficLight::Pedestrian::present);
		auto result = transition_function(TrafficLight::State::red);
		BOOST_TEST((result.first == TrafficLight::State::red));
		BOOST_TEST(!result.second.signal_);
		BOOST_TEST(!result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(59, TrafficLight::Pedestrian::absent);
		auto result = transition_function(TrafficLight::State::red);
		BOOST_TEST((result.first == TrafficLight::State::red));
		BOOST_TEST(!result.second.signal_);
		BOOST_TEST(!result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(60, TrafficLight::Pedestrian::present);
		auto result = transition_function(TrafficLight::State::red);
		BOOST_TEST((result.first == TrafficLight::State::green));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_green));
		BOOST_TEST(result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(61, TrafficLight::Pedestrian::present);
		auto result = transition_function(TrafficLight::State::red);
		BOOST_TEST((result.first == TrafficLight::State::green));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_green));
		BOOST_TEST(result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(60, TrafficLight::Pedestrian::absent);
		auto result = transition_function(TrafficLight::State::red);
		BOOST_TEST((result.first == TrafficLight::State::green));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_green));
		BOOST_TEST(result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(61, TrafficLight::Pedestrian::absent);
		auto result = transition_function(TrafficLight::State::red);
		BOOST_TEST((result.first == TrafficLight::State::green));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_green));
		BOOST_TEST(result.second.reset_count_);
	}
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(GreenToYellowWhenCountIsGreaterThanOrEqualToCountTime)
{
	{
		auto transition_function =
			traffic_light(59, TrafficLight::Pedestrian::absent);
		auto result = transition_function(TrafficLight::State::green);
		BOOST_TEST((result.first == TrafficLight::State::green));
		BOOST_TEST(!result.second.signal_);
		BOOST_TEST(!result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(60, TrafficLight::Pedestrian::present);
		auto result = transition_function(TrafficLight::State::green);
		BOOST_TEST((result.first == TrafficLight::State::yellow));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_yellow));
		BOOST_TEST(result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(61, TrafficLight::Pedestrian::present);
		auto result = transition_function(TrafficLight::State::green);
		BOOST_TEST((result.first == TrafficLight::State::yellow));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_yellow));
		BOOST_TEST(result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(60, TrafficLight::Pedestrian::absent);
		auto result = transition_function(TrafficLight::State::green);
		BOOST_TEST((result.first == TrafficLight::State::yellow));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_yellow));
		BOOST_TEST(result.second.reset_count_);
	}
	{
		auto transition_function =
			traffic_light(61, TrafficLight::Pedestrian::absent);
		auto result = transition_function(TrafficLight::State::green);
		BOOST_TEST((result.first == TrafficLight::State::yellow));
		BOOST_TEST((
			result.second.signal_.value() == TrafficLight::Signal::signal_yellow));
		BOOST_TEST(result.second.reset_count_);
	}
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(
	YellowToPendingWhenCountIsLessThanCountTimeAndPedestrianPresent)
{
	{
		auto transition_function =
			traffic_light(59, TrafficLight::Pedestrian::present);
		auto result = transition_function(TrafficLight::State::green);
		BOOST_TEST((result.first == TrafficLight::State::pending));
		BOOST_TEST(!result.second.signal_);
		BOOST_TEST(!result.second.reset_count_);
	}
}

BOOST_AUTO_TEST_SUITE_END() // TrafficLight_tests

BOOST_AUTO_TEST_SUITE_END() // StateMonadExamples_tests
BOOST_AUTO_TEST_SUITE_END() // Monads
BOOST_AUTO_TEST_SUITE_END() // Categories


/* boost random/uniform_on_sphere.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_on_sphere.hpp,v 1.1.1.1 2007/10/29 07:32:44 cvsadmin Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP
#define BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP

#include <vector>
#include <algorithm>     // std::transform
#include <functional>    // std::bind2nd, std::divides
#include <boost/random/normal_distribution.hpp>

namespace boost {

template<class RealType = double, class Cont = std::vector<RealType> >
class uniform_on_sphere
{
public:
  typedef RealType input_type;
  typedef Cont result_type;

  explicit uniform_on_sphere(int dim = 2) : _container(dim), _dim(dim) { }

  // compiler-generated copy ctor and assignment operator are fine

  void reset() { _normal.reset(); }

  template<class Engine>
  const result_type & operator()(Engine& eng)
  {
    RealType sqsum = 0;
    for(typename Cont::iterator it = _container.begin();
        it != _container.end();
        ++it) {
      RealType val = _normal(eng);
      *it = val;
      sqsum += val * val;
    }
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    // for all i: result[i] /= sqrt(sqsum)
    std::transform(_container.begin(), _container.end(), _container.begin(),
                   std::bind2nd(std::divides<RealType>(), sqrt(sqsum)));
    return _container;
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_on_sphere& sd)
  {
    os << sd._dim;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_on_sphere& sd)
  {
    is >> std::ws >> sd._dim;
    sd._container.resize(sd._dim);
    return is;
  }
#endif

private:
  normal_distribution<RealType> _normal;
  result_type _container;
  int _dim;
};

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP


#include "pose_utils.h"

#include <fstream>
#include <sstream>
#include <stdexcept>

#include <Eigen/Geometry>

PoseList loadPosesKittiFormat(std::string fn)
{
  std::ifstream ifs(fn);
  if(!ifs.is_open()) {
    throw std::runtime_error("failed to open pose file");
  }

  PoseList ret;
  while(!ifs.eof()) {
    std::string line;
    std::getline(ifs, line);

    std::stringstream ss(line);
    double vals[12];
    if(!line.empty()) {
      for(int i = 0; i < 12; ++i) {
        ss >> vals[i];
      }

      Mat44 T(Mat44::Identity());
      for(int i = 0, c=0; i < 3; ++i) {
        for(int j = 0; j < 4; ++j) {
          T(i,j) = vals[c++];
        }
      }

      ret.push_back( T );
    }
  }

  return ret;
}

bool writePosesKittiFormat(std::string fn, const PoseList& T)
{
  std::ofstream ofs(fn);
  if(!ofs.is_open())
    return false;

  for(size_t i = 0; i < T.size(); ++i) {
    for(int r = 0; r < 3; ++r) {
      for(int c = 0; c < 4; ++c) {
        ofs << (T[i](r,c)) << " ";
      }
    }
    ofs << "\n";
  }

  return true;
}

PoseList convertPoseToLocal(const PoseList& T_w)
{
  if(T_w.empty())
    throw std::runtime_error("no poses");

  PoseList T_i(T_w.size());
  T_i[0] = Eigen::Isometry3d( T_w[0] ).inverse().matrix();
  for(size_t i = 1; i < T_w.size(); ++i) {
    T_i[i] = Eigen::Isometry3d(T_w[i]).inverse().matrix() * T_w[i-1];
  }

  return T_i;
}


#include <iostream>
#include <cstdlib>

#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>

//#define TVMTL_MANIFOLD_DEBUG
//#define TVMTL_MANIFOLD_DEBUG_GRASSMANN
#include <mtvmtl/core/manifold.hpp>

using namespace tvmtl;

const int N=3;
const int P=2;

typedef Manifold<GRASSMANN, N, P> mf_t;

typedef typename mf_t::value_type mat;

template <class T>
void test(T& vec1, T& vec2){

	std::cout << mf_t::MyType << std::endl;

	std::cout << "Vector 1:\n" << vec1 << std::endl;
	std::cout << "\nVector 2:\n" << vec2 << std::endl;

	std::cout<< "\n\n==========DISTANCES TEST==========" << std::endl;
	std::cout << "Geodesic distance: " << mf_t::distGeod_squared(vec1, vec2) << std::endl;
	std::cout << "Projection F distance: " << mf_t::distPF_squared(vec1, vec2) << std::endl;

	std::cout<< "\n\n==========PERMUTATION MATRIX TEST==========" << std::endl;

	auto vec1t = vec1.transpose();
	Eigen::VectorXd vecvec1 = Eigen::Map<Eigen::VectorXd>(vec1.data(), vec1.size());
	Eigen::VectorXd vecvec1t = Eigen::Map<Eigen::VectorXd>(vec1t.data(), vec1t.size());

	std::cout << "\n\nTest of the Permutation Matrix Knp:" <<std::endl;
	std::cout << "s1:\n" << vec1 << std::endl;
	std::cout << "s1^t:\n" << vec1t << std::endl;
	std::cout << "\nVectorized s1:\n" << vecvec1 << std::endl;
	std::cout << "\nVectorized s1^t:\n" << vecvec1t << std::endl;
	std::cout << "\nPermutation Matrix:\n" << mf_t::permutation_matrix.toDenseMatrix() << std::endl;
	std::cout << "\nPermutation Matrix Indices:\n" << mf_t::permutation_matrix.indices() << std::endl;
	std::cout << "\nP * Vectorized s1:\n" << mf_t::permutation_matrix * vecvec1 << std::endl;

	std::cout<< "\n\n==========DERIVATIVE COMPUTATION TEST==========" << std::endl;
	typedef typename mf_t::deriv2_type mat9x9;

	mat d1x,d1y;
	mf_t::deriv1x_dist_squared(vec1, vec2, d1x);
	mf_t::deriv1y_dist_squared(vec1, vec2, d1y);
	mat9x9 d2xx, d2xy, d2yy;
	mf_t::deriv2xx_dist_squared(vec1, vec2, d2xx);
	mf_t::deriv2xy_dist_squared(vec1, vec2, d2xy);
	mf_t::deriv2yy_dist_squared(vec1, vec2, d2yy);

	std::cout << "\nFirst Derivative:" << std::endl;
	std::cout << d1x << std::endl;
	std::cout << std::endl;
	std::cout << d1y << std::endl;

	std::cout << "\nSecond Derivative:" << std::endl;
	std::cout << d2xx << std::endl;
	std::cout << std::endl;
	std::cout << d2xy << std::endl;
	std::cout << std::endl;
	std::cout << d2yy << std::endl;

	std::cout<< "\n\n==========TANGENT SPACE BASIS TEST==========" << std::endl;
	mf_t::tm_base_type t, t2;
	mf_t::tangent_plane_base(vec1, t);
	std::cout << "\nTangent Base Restriction:" << std::endl;
	std::cout << t << std::endl;
	std::cout << "\nTangent Base Restriction alternative calculatiopn: " << std::endl;
	Eigen::HouseholderQR<mf_t::value_type> qr(vec1);
	Eigen::Matrix<mf_t::scalar_type, N, N> Q = qr.householderQ();
	Eigen::Matrix<mf_t::scalar_type, N, N - P> vec1orth = Q.rightCols(N-P);
	t2 = Eigen::kroneckerProduct(Eigen::Matrix<mf_t::scalar_type, P, P>::Identity(), vec1orth.transpose());
	std::cout << t2 << std::endl;

	std::cout<< "\n\n==========RESTRICTED DERIVIATIVES COMPUTATION TEST==========" << std::endl;
	mf_t::restricted_deriv2_type rd2xx, rd2xy, rd2yy;
	rd2xx = t.transpose() * d2xx * t;
	rd2xy = t.transpose() * d2xy * t;
	rd2yy = t.transpose() * d2yy * t;
	std::cout << "\nRestricted second Derivatives:" << std::endl;
	std::cout << rd2xx << std::endl;
	std::cout << std::endl;
	std::cout << rd2xy << std::endl;
	std::cout << std::endl;
	std::cout << rd2yy << std::endl;

	std::cout<< "\n\n==========EXPONENTIAL LOGARITHM CONSISTENCY CHECK==========" << std::endl;
	mf_t::value_type u, z, v;
	std::cout << "\n\nExponential map test:" << std::endl;
	mf_t::exp(vec1, vec2, z);
	std::cout << "Exp(s1,s2) = \n" << z << std::endl;

	mf_t::log(vec1, vec2, u);
	mf_t::exp(vec1, u, z);
	mf_t::log(vec1, z, v);
	std::cout << "\nLogarithm map test:" << std::endl;
	std::cout << "U = Log(s1, s2) = \n" << u << std::endl;
	std::cout << "\nThe next two expression should be the same:" << std::endl;
	std::cout << "distGeod_squared(s1, s2) = " << mf_t::distGeod_squared(vec1,vec2) << std::endl;
	std::cout << "tr U^TU = " << (u.transpose()*u).trace() << std::endl;

	std::cout << "\nZ = Exp(s1, U) = \n"  << z << "\n and s2  =\n" << vec2 << "\nshould have distance close to zero for geodesic and projection F-norm :\n";
	std::cout << "\ndistGeod_squared(Z, s2) = " << mf_t::distGeod_squared(z,vec2) << std::endl;
	std::cout << "distPF_squared(Z, s2) = " << mf_t::distPF_squared(z,vec2) << std::endl;
//	std::cout << "\nV = Log(s1, Z) = \n" << v << std::endl;

	std::cout<< "\n\n==========KARCHER MEAN CONSISTENCY TEST==========" << std::endl;
	mf_t::value_type kmean, d1, d2, sum;
	mf_t::karcher_mean(kmean, vec1, vec2);

	std::cout << "\nK = K(s1, s2) =\n " << kmean << std::endl;
	mf_t::log(kmean, vec1, d1);
	mf_t::log(kmean, vec2, d2);
	sum = d1 + d2;
	std::cout << "\n||Sum_i Log(K, si)|| should be close to zero:" << (sum.transpose()*sum).trace() << std::endl;
	std::cout << "\nThe following four distances should all be approximately equal, the upper pair and the lower pair should be exactly equal:\n";
	std::cout << "Geodesic squared distance between s1 and K: " << mf_t::distGeod_squared(vec1, kmean) << std::endl;
	std::cout << "Geodesic squared distance between s2 and K: " << mf_t::distGeod_squared(vec2, kmean) << std::endl;
	std::cout << "Projection F squared distance between s1 and K: " << mf_t::distPF_squared(vec1, kmean) << std::endl;
	std::cout << "Projection F squared distance between s2 and K: " << mf_t::distPF_squared(vec2, kmean) << std::endl;

	std::cout<< "\n\n==========KARCHER MEAN NEWTON TEST==========" << std::endl;
	mf_t::value_type grad, X, Y, s;
	mat9x9 H;
	Eigen::VectorXd G, S;

	typedef Eigen::Matrix<mf_t::scalar_type, N, N> matn;
	typedef Eigen::Matrix<mf_t::scalar_type, P, P> matp;
	typedef Eigen::Matrix<mf_t::scalar_type, P, N> matpn;

	matn I = Eigen::Matrix<mf_t::scalar_type, N, N>::Identity();
	matp Ip = Eigen::Matrix<mf_t::scalar_type, P, P>::Identity();

	matn XorthP, H1;
	matp H2;

	X=vec1;

	for (int i=0; i<8; ++i){
	    XorthP = I - X * X.transpose();
	    grad = -XorthP * vec1 * vec1.transpose() * X - XorthP * vec2 * vec2.transpose() * X;
	    H1 = XorthP * vec1 * vec1.transpose() + XorthP * vec2 * vec2.transpose();
	    H2 = X.transpose() * vec1 * vec1.transpose() * X +  X.transpose() * vec2 * vec2.transpose() * X;
	    H = kroneckerProduct(Ip, H1) - kroneckerProduct(H2.transpose(), I);
	    G = Eigen::Map<Eigen::VectorXd>(grad.data(), grad.size());
	    S = H.fullPivLu().solve(G);
	    s = Eigen::Map<mf_t::value_type>(S.data());
	    mf_t::exp(X,s,X);
	}
	std::cout << "\nK = K(s1, s2) =\n " << X << std::endl;

	std::cout << "\n||grad|| should be close to zero:" << (grad.transpose()*grad).trace() << std::endl;
	std::cout << "\nThe following four distances should all be approximately equal, the upper pair and the lower pair should be exactly equal:\n";
	std::cout << "Geodesic squared distance between s1 and K: " << mf_t::distGeod_squared(vec1, X) << std::endl;
	std::cout << "Geodesic squared distance between s2 and K: " << mf_t::distGeod_squared(vec2, X) << std::endl;
	std::cout << "Projection F squared distance between s1 and K: " << mf_t::distPF_squared(vec1, X) << std::endl;
	std::cout << "Projection F squared distance between s2 and K: " << mf_t::distPF_squared(vec2, X) << std::endl;

	std::cout << "\n\n------WITH TANGENT SPACE RESTRICTION-----"    << std::endl;
	X=vec1;

	mf_t::restricted_deriv2_type HR;
	Eigen::VectorXd GR, SR;
	mf_t::tm_base_type tb;

	for (int i=0; i<8; ++i){
	    XorthP = I - X * X.transpose();
	    grad = -XorthP * vec1 * vec1.transpose() * X - XorthP * vec2 * vec2.transpose() * X;
	    H1 = XorthP * vec1 * vec1.transpose() + XorthP * vec2 * vec2.transpose();
	    H2 = X.transpose() * vec1 * vec1.transpose() * X +  X.transpose() * vec2 * vec2.transpose() * X;
	    H = kroneckerProduct(Ip, H1) - kroneckerProduct(H2.transpose(), I);
	    G = Eigen::Map<Eigen::VectorXd>(grad.data(), grad.size());
	    mf_t::tangent_plane_base(X, tb);
	    HR = tb.transpose() * H * tb;
	    GR = tb.transpose() * G;
	    SR = HR.fullPivLu().solve(GR);
	    S = tb * SR;
	    s = Eigen::Map<mf_t::value_type>(S.data());
	    mf_t::exp(X,s,X);
	}

	std::cout << "\nK = K(s1, s2) =\n " << X << std::endl;

	std::cout << "\n||grad|| should be close to zero:" << (grad.transpose()*grad).trace() << std::endl;
	std::cout << "\nThe following four distances should all be approximately equal, the upper pair and the lower pair should be exactly equal:\n";
	std::cout << "Geodesic squared distance between s1 and K: " << mf_t::distGeod_squared(vec1, X) << std::endl;
	std::cout << "Geodesic squared distance between s2 and K: " << mf_t::distGeod_squared(vec2, X) << std::endl;
	std::cout << "Projection F squared distance between s1 and K: " << mf_t::distPF_squared(vec1, X) << std::endl;
	std::cout << "Projection F squared distance between s2 and K: " << mf_t::distPF_squared(vec2, X) << std::endl;

	std::cout<< "\n\n==========2ND ORDER TAYLOR DERIVATIVE CONSISTENCY CHECK==========" << std::endl;
	double h = 1e-4;
	std::cout << "\n\nTaylor expansion Derivative Tests with perturbation O(h) = O(" << h << ")" <<std::endl;
	mf_t::value_type dx, dy;

	matn HXproj = I - vec1 * vec1.transpose();
	matn HYproj = I - vec2 * vec2.transpose();

	std::cout << "\n------Tangent vector tests-----"    << std::endl;
	mat Xpdx, Ypdy;
	dx = mat::Random(); mf_t::projector(dx); dx = h * HXproj * dx;
	mf_t::exp(vec1, dx, Xpdx);
	Eigen::VectorXd vecdx = Eigen::Map<Eigen::VectorXd>(dx.data(), dx.size());
	std::cout << "\ndx should be in the horizontal space at X, i.e. X^Tdx=0:\n "<< (vec1.transpose()*dx).norm() << std::endl;

	dy = mat::Random(); mf_t::projector(dy); dy = h * HYproj * dy;
	mf_t::exp(vec2, dy, Ypdy);
	Eigen::VectorXd vecdy = Eigen::Map<Eigen::VectorXd>(dy.data(), dy.size());
	std::cout << "\ndy should be in the horizontal space at Y, i.e. Y^Tdy=0:\n "<< (vec2.transpose()*dy).norm() << std::endl;

	double exact = mf_t::dist_squared(vec1 + dx, vec2 + dy);
	double exact2 = mf_t::dist_squared(Xpdx, Ypdy);
	double taylor_order1 = mf_t::dist_squared(vec1, vec2) + d1x.cwiseProduct(dx).sum() + d1y.cwiseProduct(dy).sum();
	double taylor_order2 = taylor_order1 + 0.5 * d2xx.cwiseProduct(vecdx * vecdx.transpose()).sum() + 0.5 * d2yy.cwiseProduct(vecdy * vecdy.transpose()).sum() + d2xy.cwiseProduct(vecdx * vecdy.transpose()).sum();

	std::cout << "\n\nError of first order Taylor " << std::abs(taylor_order1 - exact) << " = O(h^"<< std::log10(std::abs(taylor_order1 - exact))/std::log10(h)  << ") " <<std::endl;
	std::cout << "Error of first order Taylor " << std::abs(taylor_order1 - exact2) << " = O(h^"<< std::log10(std::abs(taylor_order1 - exact2))/std::log10(h)  << ") " <<std::endl;
	std::cout << "\nError of second order Taylor " << std::abs(taylor_order2 - exact) << " = O(h^"<< std::log10(std::abs(taylor_order2 - exact))/std::log10(h)  << ") " <<std::endl;
	std::cout << "Error of second order Taylor " << std::abs(taylor_order2 - exact2) << " = O(h^"<< std::log10(std::abs(taylor_order2 - exact2))/std::log10(h)  << ") " <<std::endl;

	std::cout<< "\n\n==========2ND ORDER TAYLOR DERIVATIVE ALTERNATIVE CHECK==========" << std::endl;
	h = 1e-4;
	std::cout << "\n\nTaylor expansion Derivative Tests with perturbation O(h) = O(" << h << ")" <<std::endl;
	std::cout << "\n------Single variable, fixed Y-----"    << std::endl;

	X=vec1;
	Y=vec2;

	XorthP = I - X * X.transpose();
	grad = -XorthP * Y * Y.transpose() * X;

	H1 = XorthP * Y * Y.transpose();
	H2 = X.transpose() * Y * Y.transpose() * X;
	H = kroneckerProduct(Ip, H1) - kroneckerProduct(H2.transpose(), I);

	double exact1, exact3, exact4;
	double taylor_firstorder = mf_t::dist_squared(X, Y) + grad.cwiseProduct(dx).sum();

	exact1 = mf_t::distGeod_squared(Xpdx,Y);
	exact2 = mf_t::distGeod_squared(X+dx,Y);
	exact3 = mf_t::dist_squared(Xpdx,Y);
	exact4 = mf_t::dist_squared(X+dx,Y);

	std::cout << "\nGeodesic distances at (X,Y): " << mf_t::distGeod_squared(X,Y) << std::endl;
	std::cout << "1)Geodesic distances at (exp_X(dx),Y): " << mf_t::distGeod_squared(Xpdx,Y) << std::endl;
	std::cout << "2)Geodesic distances at (X+dx,Y): " << mf_t::distGeod_squared(X+dx,Y) << std::endl;
	std::cout << "\nProjection F distance at (X,Y): " << mf_t::dist_squared(X,Y) << std::endl;
	std::cout << "3)Projection F distance at (exp_X(dx),Y): " << mf_t::dist_squared(Xpdx,Y) << std::endl;
	std::cout << "4)Projection F distance at (X+dx,Y): " << mf_t::dist_squared(X+dx,Y) << std::endl;

	std::cout << "\n1)Error of first order Taylor " << std::abs(taylor_firstorder - exact1) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact1))/std::log10(h)  << ") " <<std::endl;
	std::cout << "2)Error of first order Taylor " << std::abs(taylor_firstorder - exact2) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact2))/std::log10(h)  << ") " <<std::endl;
	std::cout << "\n3)Error of first order Taylor " << std::abs(taylor_firstorder - exact3) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact3))/std::log10(h)  << ") " <<std::endl;
	std::cout << "4)Error of first order Taylor " << std::abs(taylor_firstorder - exact4) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact4))/std::log10(h)  << ") " <<std::endl;

}

int main(int argc, const char *argv[])
{
	srand(42);

	mat s1, s2;
	s1 = mat::Random();
	mf_t::projector(s1);

	s2 = mat::Random();
	mf_t::projector(s2);

	std::cout << "s1=\n" << s1 << std::endl;

	std::cout << "\n\nRANDOM Matrices" << std::endl;
	test(s1,s2);

	return 0;
}


#include <glm/models/links/identity.hpp>

#include <armadillo>

using namespace arma;
identity_link::identity_link()
    : glm_link::glm_link( "identity" )
{
}

vec
identity_link::init_beta(const mat &X, const vec &y) const
{
    vec mu = (y + 0.5) / 2.0;
    vec eta = mu;

    return pinv( X ) * eta;
}

vec
identity_link::mu(const arma::vec &eta) const
{
    return eta;
}

vec
identity_link::eta(const arma::vec &mu) const
{
    return mu;
}

vec
identity_link::mu_eta(const arma::vec &mu) const
{
    return arma::ones<arma::vec>( mu.n_elem );
}


//  (C) Copyright Gennadiy Rozental 2001-2014.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

// Boost.Test
#include <boost/test/unit_test.hpp>
#include <boost/test/utils/algorithm.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/parameterized_test.hpp>
using namespace boost::unit_test;

// BOOST
#include <boost/functional.hpp>
#include <boost/static_assert.hpp>
#include <boost/mem_fn.hpp>
#include <boost/bind/bind.hpp>

// STL
#include <string>
#include <stdexcept>
#include <algorithm>
#include <functional>
#include <iostream>
#include <memory>
#include <list>

//____________________________________________________________________________//

template<int n>
struct power_of_10 {
    BOOST_STATIC_CONSTANT( unsigned long, value = 10*power_of_10<n-1>::value );
};

template<>
struct power_of_10<0> {
    BOOST_STATIC_CONSTANT( unsigned long, value = 1 );
};

//____________________________________________________________________________//

template<int AlphabetSize>
class hash_function {
public:
    BOOST_STATIC_ASSERT( AlphabetSize <= 5 );

    explicit        hash_function( std::string const& alphabet )
    : m_alphabet( alphabet )
    {
        if( m_alphabet.size() != AlphabetSize )
            throw std::runtime_error( "Wrong alphabet size" );

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

        if( std::adjacent_find( m_alphabet.begin(), m_alphabet.end() ) != m_alphabet.end() )
            throw std::logic_error( "Duplicate characters in alphabet" );
    }

    unsigned long   operator()( std::string const& arg )
    {
        m_result = 0;

        if( arg.length() > 8 )
            throw std::runtime_error( "Wrong argument size" );

        std::string::const_iterator it = std::find_if( arg.begin(), arg.end(),
            BOOST_TEST_BIND1ST( boost::mem_fun( &hash_function::helper_ ), this ) );

        if( it != arg.end() )
            throw std::out_of_range( std::string( "Invalid character " ) + *it );

        return m_result;
    }

private:
    bool            helper_( char c )
    {
        std::string::const_iterator it = std::find( m_alphabet.begin(), m_alphabet.end(), c );

        if( it == m_alphabet.end() )
            return true;

        m_result += power_of_10_( it - m_alphabet.begin() );

        return false;
    }

    unsigned long   power_of_10_( int i ) {
        switch( i ) {
        case 0: return power_of_10<0>::value;
        case 1: return power_of_10<1>::value;
        case 2: return power_of_10<2>::value;
        case 3: return power_of_10<3>::value;
        case 4: return power_of_10<4>::value;
        default: return 0;
        }
    }

    // Data members
    std::string     m_alphabet;
    unsigned long   m_result;
};

//____________________________________________________________________________//

struct hash_function_test_data {
    std::string     orig_string;
    unsigned long   exp_value;

    friend std::istream& operator>>( std::istream& istr, hash_function_test_data& test_data )
    {
        std::istream& tmp = istr >> test_data.orig_string;
        return  !tmp ? tmp : istr >> test_data.exp_value;
    }
};

//____________________________________________________________________________//

class hash_function_tester {
public:
    explicit        hash_function_tester( std::string const& alphabet )
    : m_function_under_test( alphabet ) {}

    void            test( hash_function_test_data const& test_data )
    {
        if( test_data.exp_value == (unsigned long)-1 )
            BOOST_CHECK_THROW( m_function_under_test( test_data.orig_string ), std::runtime_error );
        else if( test_data.exp_value == (unsigned long)-2 )
            BOOST_CHECK_THROW( m_function_under_test( test_data.orig_string ), std::out_of_range );
        else {
            BOOST_TEST_MESSAGE( "Testing: " << test_data.orig_string );
            BOOST_CHECK_EQUAL( m_function_under_test( test_data.orig_string ), test_data.exp_value );
        }
    }

private:
    hash_function<4> m_function_under_test;
};

//____________________________________________________________________________//

struct massive_hash_function_test : test_suite {
    massive_hash_function_test() : test_suite( "massive_hash_function_test" ) {
        std::string alphabet;
        std::cout << "Enter alphabet (4 characters without delimeters)\n";
        std::cin >> alphabet;

        boost::shared_ptr<hash_function_tester> instance( new hash_function_tester( alphabet ) );

        std::cout << "\nEnter test data in a format [string] [value] to check correct calculation\n";
        std::cout << "Enter test data in a format [string] -1 to check long string validation\n";
        std::cout << "Enter test data in a format [string] -2 to check invalid argument string validation\n";

        std::list<hash_function_test_data> test_data_store;

        while( !std::cin.eof() ) {
            hash_function_test_data test_data;

            if( !(std::cin >> test_data) )
                break;

            test_data_store.push_back( test_data );
        }

        add( make_test_case( &hash_function_tester::test,
                             "hash_function_tester",
                             __FILE__,
                             __LINE__,
                             instance,
                             test_data_store.begin(),
                             test_data_store.end() ) );
    }
};

//____________________________________________________________________________//

test_suite*
init_unit_test_suite( int, char* [] ) {
    framework::master_test_suite().p_name.value = "Unit test example 12";

    framework::master_test_suite().add( new massive_hash_function_test );

    return 0;
}

//____________________________________________________________________________//

// EOF


// An interval object.
#pragma once

#include <array>

#include <Eigen/Core>

#include <tight_inclusion/types.hpp>

namespace ticcd {
    // calculate a*(2^b)
    uint64_t power(const uint64_t a, const uint8_t b);

    // calculate 2^exponent
    inline uint64_t pow2(const uint8_t exponent) { return power(1l, exponent); }

    // return power t. n=result*2^t
    uint8_t reduction(const uint64_t n, uint64_t &result);

    //<k,n> pair present a number k/pow(2,n)
    struct NumCCD {
        uint64_t numerator;
        uint8_t denom_power;

        NumCCD() {}

        NumCCD(uint64_t p_numerator, uint8_t p_denom_power)
            : numerator(p_numerator), denom_power(p_denom_power)
        {
        }

        NumCCD(Scalar x);

        ~NumCCD() {}

        uint64_t denominator() const { return pow2(denom_power); }

        // convert NumCCD to double number
        Scalar value() const { return Scalar(numerator) / denominator(); }

        operator double() const { return value(); }

        NumCCD operator+(const NumCCD &other) const;

        bool operator==(const NumCCD &other) const
        {
            return numerator == other.numerator
                   && denom_power == other.denom_power;
        }
        bool operator!=(const NumCCD &other) const { return !(*this == other); }
        bool operator<(const NumCCD &other) const;
        bool operator<=(const NumCCD &other) const
        {
            return (*this == other) || (*this < other);
        }
        bool operator>=(const NumCCD &other) const { return !(*this < other); }
        bool operator>(const NumCCD &other) const { return !(*this <= other); }

        bool operator<(const Scalar other) const { return value() < other; }
        bool operator>(const Scalar other) const { return value() > other; }
        bool operator==(const Scalar other) const { return value() == other; }

        static bool is_sum_leq_1(const NumCCD &num1, const NumCCD &num2);
    };

    // an interval represented by two double numbers
    struct Interval {
        NumCCD lower;
        NumCCD upper;

        Interval() {}

        Interval(const NumCCD &p_lower, const NumCCD &p_upper)
            : lower(p_lower), upper(p_upper)
        {
        }

        ~Interval() {}

        std::pair<Interval, Interval> bisect() const;

        bool overlaps(const Scalar r1, const Scalar r2) const;
    };

    typedef std::array<Interval, 3> Interval3;
    Array3 width(const Interval3 &x);

} // namespace ticcd


/*
 * Common.hpp
 *
 *  Created on: Feb 9, 2014
 *      Author: Bloeschm
 */

#ifndef LWF_COMMON_HPP_
#define LWF_COMMON_HPP_

#include <map>
#include <type_traits>
#include <tuple>
#include <Eigen/Dense>
#include <iostream>
#include "lightweight_filtering/PropertyHandler.hpp"

typedef Eigen::Quaterniond QPD;
typedef Eigen::Matrix3d MPD;
typedef Eigen::Vector3d V3D;
typedef Eigen::Matrix3d M3D;
typedef Eigen::VectorXd VXD;
typedef Eigen::MatrixXd MXD;

inline M3D gSM(const V3D& vec){
  M3D mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

static void enforceSymmetry(MXD& mat){
  mat = 0.5*(mat+mat.transpose()).eval();
}

inline M3D Lmat (const V3D& a) {
  const double norm = a.norm();
  const M3D skewMatrix = gSM(a);
  if (norm < 1.0e-4) {
    return M3D::Identity() + 0.5*skewMatrix;
  }
  return M3D::Identity() + (double(1.0) - cos(norm))/(norm*norm)*skewMatrix + (norm - sin(norm))/(norm*norm*norm)*(skewMatrix*skewMatrix);
}

inline V3D log_map(const QPD& q) {
    using std::acos;
    using std::sqrt;

    // define these compile time constants to avoid std::abs:
    static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
    NearlyNegativeOne = -1.0 + 1e-10;

    V3D omega;

    const double qw = q.w();
    // See Quaternion-Logmap.nb in doc for Taylor expansions
    if (qw > NearlyOne) {
      // Taylor expansion of (angle / s) at 1
      // (2 + 2 * (1-qw) / 3) * q.vec();
      omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
    } else if (qw < NearlyNegativeOne) {
      // Taylor expansion of (angle / s) at -1
      // (-2 - 2 * (1 + qw) / 3) * q.vec();
      omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
    } else {
      // Normal, away from zero case
      double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
      // Important:  convert to [-pi,pi] to keep error continuous
      if (angle > M_PI)
      angle -= twoPi;
      else if (angle < -M_PI)
      angle += twoPi;
      omega = (angle / s) * q.vec();
    }

    return omega;
}

inline QPD exp_map(const V3D& omega) {
    using std::cos;
    using std::sin;

    double theta2 = omega.dot(omega);
    if (theta2 > std::numeric_limits<double>::epsilon()) {
      double theta = std::sqrt(theta2);
      double ha = 0.5 * theta;
      V3D vec = (sin(ha) / theta) * omega;
      return QPD(cos(ha), vec.x(), vec.y(), vec.z());
    } else {
      // first order approximation sin(theta/2)/theta = 0.5
      V3D vec = 0.5 * omega;
      return QPD(1.0, vec.x(), vec.y(), vec.z());
    }
}

inline QPD box_plus(const QPD& q, const V3D& v) {
  return exp_map(v) * q;
}

inline V3D box_minus(const QPD& q1, const QPD& q2) {
  return log_map(q1 * q2.inverse());
}

namespace LWF{
  enum FilteringMode{
    ModeEKF,
    ModeUKF,
    ModeIEKF
  };
}

#endif /* LWF_COMMON_HPP_ */


#ifndef CANNON_ML_PATTERN_SEARCH_H
#define CANNON_ML_PATTERN_SEARCH_H

/*!
 * \file cannon/ml/pattern_search.hpp
 * \brief File containing PatternSearchOptimizer class definition.
 */

#include <functional>

#include <Eigen/Dense>

#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

using RealFunc = std::function<double(const VectorXd&)>;

namespace cannon {
  namespace ml {

    CANNON_CLASS_FORWARD(OptimizationResult);

    /*!
     * \brief Class representing a pattern search optimizer. Pattern Search is
     * a derivative-free optimization method, similar to the Nelder-Mead method
     * but with better convergence guarantees. See
     * https://en.wikipedia.org/wiki/Pattern_search_(optimization).
     */
    class PatternSearchOptimizer {
      public:
        PatternSearchOptimizer() = delete;

        /*!
         * \brief Constructor taking a function to optimize, the dimension of
         * input space, whether to reorient the search pattern, and tuning
         * parameters.
         */
        PatternSearchOptimizer(RealFunc f, unsigned int dim, bool
            reorient=false, double forcing_func_param=1.0, double reorient_param=0.1) : f_(f), dim_(dim),
        reorient_(reorient), forcing_func_param_(forcing_func_param), reorient_param_(reorient_param) {}

        /*!
         * \brief Minimize the unconstrained, derivative-free problem represented by this object.
         *
         * \param start Start state for iterative optimization
         * \param iterations Maximum number of iterations to perform
         * \param eps Small number used for testing convergence
         * \param contraction_param Parameter used to shrink search step size
         * \param expansion_param Parameter used to expand search step size
         * \param initial_step Initial step size
         */
        OptimizationResult optimize(const VectorXd& start, unsigned int iterations=1000, double eps=1e-10,
            double contraction_param=0.9, double expansion_param=1.5, double
            initial_step=1.0);

      private:

        /*!
         * \brief Create a positive spanning simplex set from the input vector.
         * This ensures that the input direction is a search direction.
         *
         * \param first The vector to build a simplex set around.
         *
         * \returns The simplex direction set.
         */
        std::vector<VectorXd> create_simplex_set_(const VectorXd& first);

        /*!
         * \brief Forcing function used to ensure that function evaluations
         * decrease sufficiently.
         *
         * \param t Input used to compute forcing term.
         *
         * \returns Forcing term.
         */
        double forcing_func_(double t);

        /*!
         * \brief Update leading direction by exponential averaging with the
         * input difference vector.
         *
         * \param leading_dir The leading search direction to update.
         * \param diff Difference vector to use for updating.
         */
        void update_leading_dir_(VectorXd& leading_dir, const VectorXd& diff);

        RealFunc f_; //!< Function to be optimized
        unsigned int dim_; //!< Input dimension
        bool reorient_; //!< Whether to reorient leading direction

        double forcing_func_param_; //!< Forcing function parameter
        double reorient_param_; //!< Reorientation speed parameter

    };

  } // namespace ml
} // namespace cannon

#endif /* ifndef CANNON_ML_PATTERN_SEARCH_H */


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <complex>
#include <tuple>

#include <chrono>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/splitting.h"
#include "vlasovpp/lagrange5.h"
#include "vlasovpp/config.h"
#include "vlasovpp/signal_handler.h"

struct iter_s {
  std::size_t iter;
  double dt;
  double current_time;
  double Lhfh;
  double LE;
};
/*
struct time_stages {
  std::chrono::duration<double> step;
  std::chrono::duration<double> stage_1;
  std::chrono::duration<double> stage_2;
  std::chrono::duration<double> stage_3;
  std::chrono::duration<double> stage_4;
  std::chrono::duration<double> stage_5;
};
*/
#define save(data,dir,suffix,x_y) {\
  std::stringstream filename; filename << #data << "_" << suffix << ".dat"; \
  std::ofstream of( dir / filename.str() );\
  std::transform( data.begin() , data.end() , std::ostream_iterator<std::string>(of,"\n") , x_y );\
  of.close();\
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

#define ping(X) std::cerr << __LINE__ << " " << #X << ":" << X << std::endl
int debug = 0;

auto
maxwellian ( double rho , double u , double T ) {
  //std::cout << rho << " " << u << " " << T << std::endl;
  //std::cout << rho/(std::sqrt(2.*math::pi<double>()*T)) << std::endl;
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int
main(int argc, char const *argv[])
{

  std::string p("config.init");
  if ( argc > 1 ) {
    p = argv[1];
  }
  auto c = config(p);
  c.name = "vhls";

  c.create_output_directory();
  std::cout << c << std::endl;
  std::ofstream oconfig( c.output_dir / "config.init" );
  oconfig << c << std::endl;
  oconfig.close();

/* --------------------------------------------------------------- */
  std::size_t Nx = c.Nx, Nv = c.Nv;

  // $(u_c,E,\hat{f}_h)$ and $f_h$
  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);
  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);
  ublas::vector<double> uc1(Nx) , uc2(Nx) , uc3(Nx) , uc4(Nx) , ucn(Nx),
                        E1 (Nx) , E2 (Nx) , E3 (Nx) , E4 (Nx) , En (Nx);
  complex_field<double,1> hfh1(boost::extents[Nv][Nx]) , hfh2(boost::extents[Nv][Nx]) ,
                          hfh3(boost::extents[Nv][Nx]) , hfh4(boost::extents[Nv][Nx]) ,
                          hfhn(boost::extents[Nv][Nx]) ;

  const double Kx = 0.5;
  // phase-space domain
  fh.range.v_min = -12.; fh.range.v_max = 12.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();

  // compute dx, dv
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/Nv;
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/Nx;

  double dt = 0.04; //0.5*fh.step.dv;

  // velocity and frequency
  ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  const double l = fh.range.x_max-fh.range.x_min;
  ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  // initial condition
  auto tb_M1 = maxwellian(0.5*c.alpha,c.ui,1.) , tb_M2 = maxwellian(0.5*c.alpha,-c.ui,1.);
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      //fh[k][i] = ( 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-c.ui)) + 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+c.ui)) )*(1.+0.04*std::cos(0.3*Xi(i)));
      //fh[k][i] = ( 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-c.ui)) + 0.5*c.alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+c.ui)) )*(1.+0.04*std::cos(Kx*Xi(i)));

      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(&(fh[k][0]),&(fh[k][Nx-1])+1,&(hfh[k][0]));
  }
  fh.write( c.output_dir / "init_vhls.dat" );
/*
  std::cout << "Nx: " << Nx << "\n";
  std::cout << "Nv: " << Nv << "\n";
  std::cout << "v_min: " << fh.range.v_min << "\n";
  std::cout << "v_max: " << fh.range.v_max << "\n";
  std::cout << "x_min: " << fh.range.x_min << "\n";
  std::cout << "x_max: " << fh.range.x_max << "\n";
  std::cout << "dt: " << dt << "\n";
  std::cout << "dx: " << fh.step.dx << "\n";
  std::cout << "dv: " << fh.step.dv << "\n";
  std::cout << "Tf: " << c.Tf << "\n";
  std::cout << "f_0: " << "\"tb\"" << "\n";
  std::cout << std::endl;
*/
  const double rho_c = 1.-c.alpha;
  // init E (electric field) with Poisson
  {
    poisson<double> poisson_solver(Nx,l);
    ublas::vector<double> rho(Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += rho_c; } // add (1-alpha) for cold particules
    E = poisson_solver(rho);
  }

  // monitoring data
  std::vector<iter_s> iterations; iterations.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<iter_s> success_iterations; success_iterations.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> ee;
  std::vector<double> Emax;
  std::vector<double> H;
  std::vector<double> times;

  //std::vector<time_stages> durations_s; durations_s.reserve(int(std::ceil(c.Tf/dt))+1);

  auto save_data = [&] ( std::string suffix ) {
    save(iterations,c.output_dir,suffix,[](auto const& it) { std::stringstream ss; ss << it.iter << " " << it.dt << " " << it.current_time << " " << it.Lhfh << " " << it.LE; return ss.str(); })
    save(success_iterations,c.output_dir,suffix,[](auto const& it) { std::stringstream ss; ss << it.iter << " " << it.dt << " " << it.current_time << " " << it.Lhfh << " " << it.LE; return ss.str(); })

    auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };
    save(ee,c.output_dir,suffix,dt_y);
    save(Emax,c.output_dir,suffix,dt_y);
    save(H,c.output_dir,suffix,dt_y);

    /*
    auto writer_times = [&,count=0] (auto const & t ) mutable {
      std::stringstream ss;
      ss<<times[count++]<<" "<<t.step.count()<<" "<<t.stage_1.count()<<" "<<t.stage_2.count()<<" "<<t.stage_3.count()<<" "<<t.stage_4.count()<<" "<<t.stage_5.count();
      return ss.str();
    };
    c << monitoring::data( "times_"+suffix+".dat" , durations_s , writer_times );
    */
  };

  signal_handler::signal_handler<SIGINT,SIGILL>::handler( [&]( int signal ) -> void {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[36msave data...\033[0m\n";

    save_data("vhls_SIGINT");
    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
    fh.write( c.output_dir / "vp_vhls_SIGINT.dat" );
  });

  /*
  signal_handler::signal_handler<SIGINT>::function_handler = [&](int signal) {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[36msave data...\033[0m\n" ;

    save_data("vhls_SIGINT");
    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][0])+c.Nx,&(fh[k][0])); }
    fh.write( c.output_dir / "vp_vhls_SIGINT.dat" );
  };
  signal_handler::signal_handler<SIGINT>::signal();
  */

  times.push_back(0);
  {
    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
  }
  Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

  //U_type<double,1> U({uc,E,hfh};
  splitting<double,1> Lie( fh , l , rho_c );

  std::size_t i_t = 0;
  double current_time = 0.;
  const double alpha1=1./(4.-std::cbrt(4.)), alpha2=alpha1, alpha3=1./(1.-SQ(std::cbrt(4.)));

  const double g1 = alpha1, g2 = alpha1+alpha2;
  const double w1 = (g2*(1.-g2))/(g1*(g1-1.)-g2*(g2-1.)) , w2 = 1.-w1 , w3 = w2 , w4 = w1;

  while ( current_time < c.Tf ) {
    std::cout<<"\r ["<<std::setw(6)<<i_t<<"] "<< std::setw(8) << current_time << " (" << std::setw(9) << dt << ")"<<std::flush;
    /**
    // Strang classique
    Lie.phi_a(0.5*dt,uc,E,hfh);
    Lie.phi_b(0.5*dt,uc,E,hfh);
    Lie.phi_c(dt,uc,E,hfh);
    Lie.phi_b(0.5*dt,uc,E,hfh);
    Lie.phi_a(0.5*dt,uc,E,hfh);
    /**/

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfhn.origin() );
    std::copy(  E.begin() ,  E.end() ,  En.begin() );
    std::copy( uc.begin() , uc.end() , ucn.begin() );

    //auto start = std::chrono::high_resolution_clock::now();
    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    //auto end_s1 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh1.origin() );
    std::copy(  E.begin() ,  E.end() ,  E1.begin() );
    std::copy( uc.begin() , uc.end() , uc1.begin() );

    //auto start_s2 = std::chrono::high_resolution_clock::now();
    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    //auto end_s2 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh2.origin() );
    std::copy(  E.begin() ,  E.end() ,  E2.begin() );
    std::copy( uc.begin() , uc.end() , uc2.begin() );

    //auto start_s3 = std::chrono::high_resolution_clock::now();
    // Strang alpha3
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha3*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);
    //auto end_s3 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh3.origin() );
    std::copy(  E.begin() ,  E.end() ,  E3.begin() );
    std::copy( uc.begin() , uc.end() , uc3.begin() );

    //auto start_s4 = std::chrono::high_resolution_clock::now();
    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    //auto end_s4 = std::chrono::high_resolution_clock::now();

    std::copy( hfh.origin() , hfh.origin()+hfh.num_elements() , hfh4.origin() );
    std::copy(  E.begin() ,  E.end() ,  E4.begin() );
    std::copy( uc.begin() , uc.end() , uc4.begin() );

   //auto start_s5 = std::chrono::high_resolution_clock::now();
    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    //auto end_s5 = std::chrono::high_resolution_clock::now();

    //durations_s.push_back({ end_s5-start , end_s1-start , end_s2-start_s2 , end_s3-start_s3 , end_s4-start_s4 , end_s5-start_s5 });

    /**/

    double L_hfh = 0.;
    for ( auto k=0 ; k<Nv ; ++k ) {
      for ( auto i=0 ; i<Nx ; ++i ) {
        auto hfhtile_ik = -hfhn[k][i] + w1*(hfh1[k][i]+hfh4[k][i]) + w2*(hfh2[k][i]+hfh3[k][i]);
        L_hfh += SQ( std::abs( hfh[k][i]-hfhtile_ik ) )*fh.step.dx*fh.step.dv;
      }
    }
    L_hfh = std::sqrt(L_hfh);

    double L_E = 0.;
    for ( auto i=0 ; i<Nx ; ++i ) {
      double Etile_ik = -En[i] + w1*(E1[i]+E4[i]) + w2*(E2[i]+E3[i]);
      L_E += SQ( std::abs( E[i]-Etile_ik ) )*fh.step.dx;
    }
    L_E = std::sqrt(L_E);

    std::cout << " -- " << std::setw(10) << L_hfh << "        " << std::flush;

    //if ( std::abs(L_hfh - c.tol) <= c.tol ) // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    {
      // SAVE TIME STEP

      Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      double electric_energy = 0.;
      for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
      ee.push_back( std::sqrt(electric_energy) );
      double total_energy = energy(fh,E);
      {
        auto rhoh = fh.density();
        fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(&rhoh[0]);
        fft::spectrum_ hE(c.Nx); hE.fft(&E[0]);
        fft::spectrum_ hrhoc(c.Nx);
        hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
        for ( auto i=1 ; i<c.Nx ; ++i ) {
          hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
        }
        ublas::vector<double> rhoc (c.Nx,0.); hrhoc.ifft(&rhoc[0]);

        for ( auto i=0 ; i<c.Nx ; ++i ) {
          total_energy += rhoc[i]*uc[i]*uc[i];
        }
      }
      H.push_back( total_energy );

      current_time += dt;
      times.push_back( current_time );
      success_iterations.push_back( { i_t , dt , current_time , L_hfh } );
    }
    /*else {
      // REMAKE THE STEP
      std::copy( hfhn.origin() , hfhn.origin()+hfhn.num_elements() , hfh.origin() );
      std::copy( En.begin()  , En.end()  , E.begin() );
      std::copy( ucn.begin() , ucn.end() , uc.begin() );
    }*/

    iterations.push_back( { i_t , dt , current_time , L_hfh } );
    ++i_t;

    //dt = std::pow( c.tol/L_hfh , 0.25 )*dt;
    if ( current_time+dt > c.Tf ) { dt = c.Tf - current_time; }
  } // while current_time < c.Tf
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<<std::endl;

  std::ofstream of;
  std::size_t count = 0;

  /*
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };

  of.open( c.output_dir / "ee_vhls.dat" );
  std::transform( ee.begin() , ee.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "Emax_vhls.dat" );
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "H_vhls.dat" );
  std::transform( H.begin() , H.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();
  */
  save_data("vhls_suzuki");

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][Nx-1])+1,&(fh[k][0])); }
  fh.write( c.output_dir / "vp_vhls_suzuki.dat" );

  auto dx_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<< fh.step.dx*(count++) <<" "<<y; return ss.str(); };
  save(E,c.output_dir,"vhls",dx_y);
  save(uc,c.output_dir,"vhls",dx_y);

  return 0;
}


//  Copyright John Maddock 2006.
//  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)
//
// Basic sanity check that header
// #includes all the files that it needs to.
//
#include <boost/math/tools/cohen_acceleration.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

template<typename Real>
class G {
public:
    G(){
        k_ = 0;
    }

    Real operator()() {
        k_ += 1;
        return 1/(k_*k_);
    }

private:
    Real k_;
};

void compile_and_link_test()
{
    auto f_g = G<float>();
    check_result<float>(boost::math::tools::cohen_acceleration(f_g));

    auto d_g = G<double>();
    check_result<double>(boost::math::tools::cohen_acceleration(d_g));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    auto ld_g = G<long double>();
    check_result<long double>(boost::math::tools::cohen_acceleration(ld_g));
#endif
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm trigonometry asin
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/core/type_traits.h"
#include "fern/algorithm/trigonometry/asin.h"

namespace fa = fern::algorithm;

template<
    class Value>
using OutOfDomainPolicy = fa::asin::OutOfDomainPolicy<Value>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<double> policy;
        BOOST_CHECK(!policy.within_domain(-1.1));
        BOOST_CHECK(!policy.within_domain(1.1));
        BOOST_CHECK(policy.within_domain(0));
        BOOST_CHECK(policy.within_domain(-1));
        BOOST_CHECK(policy.within_domain(1));
    }
}

template<
    class Value,
    class Result>
void verify_value(
    Value const& value,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::trigonometry::asin(sequential, value, result_we_get);
    BOOST_CHECK_CLOSE(result_we_get, result_we_want, 1e-6);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_value<double, double>(-0.5, -fern::pi<double>() / 6.0);
    verify_value<double, double>(0.0, 0.0);
    verify_value<double, double>(1.0, fern::half_pi<double>());
}


///////////////////////////////////////////////////////////////
//  Copyright 2021 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt
//

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#include <thread>
#include <boost/detail/lightweight_test.hpp>
#include <boost/array.hpp>
#include <boost/math/special_functions/relative_difference.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include "test.hpp"

#if !defined(TEST_MPF_50) && !defined(TEST_MPFR_50)
#define TEST_MPF_50
#define TEST_MPFR_50

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#include <boost/multiprecision/mpfr.hpp>

#if defined(TEST_MPF_50)
#include <boost/multiprecision/gmp.hpp>
#endif

template <class T>
void thread_test_proc(unsigned digits)
{
   typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<1000> > mpfr_float_1000;

   if (!mpfr_buildopt_tls_p())
      return;

   T::thread_default_precision(digits);

   T result, value;

   value = boost::math::constants::pi<T>();
   result.assign(boost::math::constants::pi<mpfr_float_1000>().str());
   BOOST_CHECK_LE(boost::math::epsilon_difference(value, result), 20);
   BOOST_CHECK_EQUAL(value.precision(), digits);

   for (unsigned i = 20; i < 500; ++i)
   {
      T arg(i);
      arg /= 3;
      value = boost::math::tgamma(arg);
      BOOST_CHECK_EQUAL(value.precision(), digits);
      result.assign(boost::math::tgamma(mpfr_float_1000(arg)).str());
      BOOST_CHECK_LE(boost::math::epsilon_difference(value, result), 800);
   }

   //
   // Try tanh_sinh integration:
   //
   using namespace boost::math::constants;
   boost::math::quadrature::tanh_sinh<T> integrator;
   T                                     tol = 500;
   // Example 1:
   auto f1         = [](const T& t) { return static_cast<T>(t * boost::math::log1p(t)); };
   T Q          = integrator.integrate(f1, (T)0, (T)1);
   T Q_expected = half<T>() * half<T>();
   BOOST_CHECK_EQUAL(Q.precision(), digits);
   BOOST_CHECK_EQUAL(Q_expected.precision(), digits);
   BOOST_CHECK_LE(boost::math::epsilon_difference(Q, Q_expected), tol);

   // Example 2:
   auto f2    = [](const T& t) { return static_cast<T>(t * t * atan(t)); };
   Q          = integrator.integrate(f2, (T)0, (T)1);
   Q_expected = (pi<T>() - 2 + 2 * ln_two<T>()) / 12;
   BOOST_CHECK_EQUAL(Q.precision(), digits);
   BOOST_CHECK_EQUAL(Q_expected.precision(), digits);
   BOOST_CHECK_LE(boost::math::epsilon_difference(Q, Q_expected), tol);

   // Example 3:
   auto f3    = [](const T& t) { return static_cast<T>(exp(t) * cos(t)); };
   Q          = integrator.integrate(f3, (T)0, half_pi<T>());
   Q_expected = boost::math::expm1(half_pi<T>()) * half<T>();
   BOOST_CHECK_EQUAL(Q.precision(), digits);
   BOOST_CHECK_EQUAL(Q_expected.precision(), digits);
   BOOST_CHECK_LE(boost::math::epsilon_difference(Q, Q_expected), tol);

   // Example 4:
   auto f4    = [](T x) -> T { T t0 = sqrt(x*x + 2); return atan(t0)/(t0*(x*x+1)); };
   Q          = integrator.integrate(f4, (T)0, (T)1);
   Q_expected = 5 * pi<T>() * pi<T>() / 96;
   BOOST_CHECK_EQUAL(Q.precision(), digits);
   BOOST_CHECK_EQUAL(Q_expected.precision(), digits);
   BOOST_CHECK_LE(boost::math::epsilon_difference(Q, Q_expected), tol);

   // Example 5:
   auto f5    = [](const T& t)->T { return sqrt(t) * log(t); };
   Q          = integrator.integrate(f5, (T)0, (T)1);
   Q_expected = -4 / (T)9;
   BOOST_CHECK_EQUAL(Q.precision(), digits);
   BOOST_CHECK_EQUAL(Q_expected.precision(), digits);
   BOOST_CHECK_LE(boost::math::epsilon_difference(Q, Q_expected), tol);

   // Example 6:
   auto f6    = [](const T& t)->T { return sqrt(1 - t * t); };
   Q          = integrator.integrate(f6, (T)0, (T)1);
   Q_expected = pi<T>() / 4;
   BOOST_CHECK_EQUAL(Q.precision(), digits);
   BOOST_CHECK_EQUAL(Q_expected.precision(), digits);
   BOOST_CHECK_LE(boost::math::epsilon_difference(Q, Q_expected), tol);
}

int main()
{
#ifdef TEST_MPF_50
   {
      std::thread t1(thread_test_proc<boost::multiprecision::mpf_float>, 35);
      std::thread t2(thread_test_proc<boost::multiprecision::mpf_float>, 55);
      std::thread t3(thread_test_proc<boost::multiprecision::mpf_float>, 75);
      std::thread t4(thread_test_proc<boost::multiprecision::mpf_float>, 105);
      std::thread t5(thread_test_proc<boost::multiprecision::mpf_float>, 305);

      t1.join();
      t2.join();
      t3.join();
      t4.join();
      t5.join();
   }
#endif
#ifdef TEST_MPFR_50
   {
      std::thread t1(thread_test_proc<boost::multiprecision::mpfr_float>, 35);
      std::thread t2(thread_test_proc<boost::multiprecision::mpfr_float>, 55);
      std::thread t3(thread_test_proc<boost::multiprecision::mpfr_float>, 75);
      std::thread t4(thread_test_proc<boost::multiprecision::mpfr_float>, 105);
      std::thread t5(thread_test_proc<boost::multiprecision::mpfr_float>, 305);

      t1.join();
      t2.join();
      t3.join();
      t4.join();
      t5.join();
   }
#endif
   return boost::report_errors();
}


// workaround for the annoying boost message in boost 1.69
#define BOOST_PENDING_INTEGER_LOG2_HPP
#include <boost/integer/integer_log2.hpp>
// end workaround

#include <iostream>

#include <gudhi/Coxeter_triangulation.h>
#include <gudhi/Functions/Function_affine_plane_in_Rd.h>
#include <gudhi/Functions/Function_Sm_in_Rd.h>
#include <gudhi/Functions/Cartesian_product.h>
#include <gudhi/Functions/Linear_transformation.h>
#include <gudhi/Implicit_manifold_intersection_oracle.h>
#include <gudhi/Manifold_tracing.h>
#include <gudhi/Coxeter_triangulation/Cell_complex/Cell_complex.h>
#include <gudhi/Functions/random_orthogonal_matrix.h>  // requires CGAL

#include <gudhi/IO/build_mesh_from_cell_complex.h>
#include <gudhi/IO/output_meshes_to_medit.h>

using namespace Gudhi::coxeter_triangulation;

int main(int argc, char** argv) {
  // Creating a circle S1 in R2 of specified radius
  double radius = 1.0;
  Function_Sm_in_Rd fun_circle(radius, 1);

  // Creating a flat torus S1xS1 in R4 from two circle functions
  auto fun_flat_torus = make_product_function(fun_circle, fun_circle);

  // Apply a random rotation in R4
  auto matrix = random_orthogonal_matrix(4);
  auto fun_flat_torus_rotated = make_linear_transformation(fun_flat_torus, matrix);

  // Computing the seed of the function fun_flat_torus
  Eigen::VectorXd seed = fun_flat_torus_rotated.seed();

  // Defining a domain function that defines the boundary, which is a hyperplane passing by the origin and orthogonal to
  // x.
  Eigen::MatrixXd normal_matrix = Eigen::MatrixXd::Zero(4, 1);
  for (std::size_t i = 0; i < 4; i++) normal_matrix(i, 0) = -seed(i);
  Function_affine_plane_in_Rd fun_bound(normal_matrix, -seed / 2);

  // Defining the intersection oracle
  auto oracle = make_oracle(fun_flat_torus_rotated, fun_bound);

  // Define a Coxeter triangulation scaled by a factor lambda.
  // The triangulation is translated by a random vector to avoid violating the genericity hypothesis.
  double lambda = 0.2;
  Coxeter_triangulation<> cox_tr(oracle.amb_d());
  cox_tr.change_offset(Eigen::VectorXd::Random(oracle.amb_d()));
  cox_tr.change_matrix(lambda * cox_tr.matrix());

  // Manifold tracing algorithm
  using MT = Manifold_tracing<Coxeter_triangulation<> >;
  using Out_simplex_map = typename MT::Out_simplex_map;
  std::vector<Eigen::VectorXd> seed_points(1, seed);
  Out_simplex_map interior_simplex_map, boundary_simplex_map;
  manifold_tracing_algorithm(seed_points, cox_tr, oracle, interior_simplex_map, boundary_simplex_map);

  // Constructing the cell complex
  std::size_t intr_d = oracle.amb_d() - oracle.cod_d();
  Cell_complex<Out_simplex_map> cell_complex(intr_d);
  cell_complex.construct_complex(interior_simplex_map, boundary_simplex_map);

  // Output the cell complex to a file readable by medit
  output_meshes_to_medit(3, "flat_torus_with_boundary",
                         build_mesh_from_cell_complex(cell_complex, Configuration(true, true, true, 1, 5, 3),
                                                      Configuration(true, true, true, 2, 13, 14)));

  return 0;
}


#include <geometry.h>
#include <tiny_math_types.h>

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

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(raycast_dop)
{
  using std::sqrt;

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

  std::vector<V> corners(2u);

  corners[0] = V::make(-1.0,-1.0,-1.0);
  corners[1] = V::make( 1.0, 1.0, 1.0);

  geometry::DOP<T, 6u> const dop = geometry::make_dop(corners.begin(), corners.end(), geometry::make3<V>() );

  BOOST_CHECK(geometry::is_valid(dop));

  // Hit straigth on
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( 0.0, 0.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2.0, 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Hit straigth on corner
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( -1.0, -1.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2.0, 0.01);
    BOOST_CHECK_CLOSE( q(0), -1.0, 0.01);
    BOOST_CHECK_CLOSE( q(1), -1.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Hit straigth on edge
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( 0.0, -1.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2.0, 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1), -1.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Aligned ray no hitting
  {
    V const r      = V::make( 0.0, 0.0, 1.0);
    V const p      = V::make( -2.0, 0.0,-3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( !hit );

    BOOST_CHECK_CLOSE( length, VT::infinity(), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }

  // Obligue ray no hitting
  {
    V const r      = V::make( 1.0, 1.0, 1.0);
    V const p      = V::make( 0.0, 0.0,-10.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( !hit );

    BOOST_CHECK_CLOSE( length, VT::infinity(), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }

  // Obligue ray central hit
  {
    V const r      = V::make( 1.0,  1.0,  1.0);
    V const p      = V::make(-2.0, -2.0, -3.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 2*1.732050807568877, 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Obligue ray corner hit
  {
    V const r      = V::make( 1.0,  1.0,  1.0);
    V const p      = V::make( 0.0,  0.0, -2.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 1.732050807568877, 0.01);
    BOOST_CHECK_CLOSE( q(0),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

  // Obligue ray edge hit
  {
    V const r      = V::make( 1.0,  1.0,  1.0);
    V const p      = V::make( 0.0, -1.0, -2.0);

    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_dop(ray, dop, q, length);

    BOOST_CHECK( hit );

    BOOST_CHECK_CLOSE( length, 1.732050807568877, 0.01);
    BOOST_CHECK_CLOSE( q(0),  1.0, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.0, 0.01);
    BOOST_CHECK_CLOSE( q(2), -1.0, 0.01);
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/left.hpp>
#include <boost/numeric/bindings/right.hpp>
#include "print.hpp"
#include "random.hpp"

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

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    {
      size_type m=6, n=8;
      matrix A(m, m);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<m; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*ublas::prod(A, B)+beta*C);
      matrix C2(C);
      blas::symm(blas::left(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing boost::ublas containers\n"
		<< "using ublas (left multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (left multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
    {
      size_type m=6, n=8;
      matrix A(n, n);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*ublas::prod(B, A)+beta*C);
      matrix C2(C);
      blas::symm(blas::right(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing boost::ublas containers\n"
		<< "using ublas (right multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (right multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
  }
  {
    typedef std::complex<double> complex;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    {
      size_type m=6, n=8;
      matrix A(m, m);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<m; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*A*B+beta*C);
      matrix C2(C);
      blas::symm(blas::left(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing Eigen containers\n"
		<< "using Eigen (left multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (left multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
    {
      size_type m=6, n=8;
      matrix A(n, n);
      matrix B(m, n);
      matrix C(m, n);
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<=j; ++i) {
	  A(i, j)=rand_normal<complex>::get();
	  A(j, i)=A(i, j);
	}
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  B(i, j)=rand_normal<complex>::get();
      for (size_type j=0; j<n; ++j)
	for (size_type i=0; i<m; ++i)
	  C(i, j)=rand_normal<complex>::get();
      complex alpha(rand_normal<complex>::get());
      complex beta(rand_normal<complex>::get());
      matrix C1(alpha*B*A+beta*C);
      matrix C2(C);
      blas::symm(blas::right(), alpha, blas::upper(A), B, beta, C2);
      std::cout << "testing Eigen containers\n"
		<< "using Eigen (right multiply):\n" << print_mat(C1) << '\n'
		<< "using blas (right multiply):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
  }
  return EXIT_SUCCESS;
}