/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2015 Basil Fierz
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

// VCL configuration
#include <vcl/config/global.h>
#include <vcl/config/eigen.h>

// C++ standard library
#include <random>

// Eigen library
#include <Eigen/Dense>

// Include the relevant parts from the library
#include <vcl/core/interleavedarray.h>
#include <vcl/math/math.h>
#include <vcl/math/jacobieigen33_selfadjoint.h>
#include <vcl/math/jacobieigen33_selfadjoint_quat.h>

// Google test
#include <gtest/gtest.h>

// Common functions
namespace
{
	template<typename REAL>
	void SortEigenvalues(Eigen::Matrix<REAL, 3, 1>& A, Eigen::Matrix<REAL, 3, 3>& B)
	{
		// Bubble sort
		bool swapped = true;
		int j = 0;

		while (swapped)
		{
			swapped = false;
			j++;

			for (int i = 0; i < 3 - j; i++)
			{
				if (A(i) > A(i + 1))
				{
					std::swap(A(i), A(i + 1));
					B.col(i).swap(B.col(i + 1));

					swapped = true;
				}
			}
		}
	}

	template<typename Scalar>
	Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> createProblems(size_t nr_problems)
	{
		// Random number generator
		std::mt19937_64 rng;
		std::uniform_real_distribution<float> d;

		Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> A(nr_problems);

		// Initialize data
		for (int i = 0; i < (int) nr_problems; i++)
		{
			Eigen::Matrix<Scalar, 3, 3> rnd;
			rnd << d(rng), d(rng), d(rng),
				   d(rng), d(rng), d(rng),
				   d(rng), d(rng), d(rng);
			A.at<Scalar>(i) = rnd.transpose() * rnd;
		}

		return std::move(A);
	}

	template<typename Scalar>
	void computeReferenceSolution
	(
		size_t nr_problems,
		const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& ATA,
		Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& U,
		Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& S
	)
	{
		// Compute reference using Eigen
		for (int i = 0; i < static_cast<int>(nr_problems); i++)
		{
			Vcl::Matrix3f A = ATA.at<Scalar>(i);

			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
			solver.compute(A, Eigen::ComputeEigenvectors);

			U.at<Scalar>(i) = solver.eigenvectors();
			S.at<Scalar>(i) = solver.eigenvalues();
		}
	}

	template<typename Scalar>
	void checkSolution
	(
		size_t nr_problems,
		Scalar tol,
		const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& refUa,
		const Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& refSa,
		const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& resUa,
		const Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& resSa
	)
	{
		using Vcl::Mathematics::equal;

		for (int i = 0; i < static_cast<int>(nr_problems); i++)
		{
			Vcl::Matrix3f refU = refUa.at<Scalar>(i);
			Vcl::Vector3f refS = refSa.at<Scalar>(i);

			Vcl::Matrix3f resU = resUa.at<Scalar>(i);
			Vcl::Vector3f resS = resSa.at<Scalar>(i);
			SortEigenvalues(resS, resU);

			Scalar sqLenRefUc0 = refU.col(0).squaredNorm();
			Scalar sqLenRefUc1 = refU.col(1).squaredNorm();
			Scalar sqLenRefUc2 = refU.col(2).squaredNorm();
			EXPECT_TRUE(equal(sqLenRefUc0, Scalar(1), tol)) << "Reference U(" << i << "): Column 0 is not normalized.";
			EXPECT_TRUE(equal(sqLenRefUc1, Scalar(1), tol)) << "Reference U(" << i << "): Column 1 is not normalized.";
			EXPECT_TRUE(equal(sqLenRefUc2, Scalar(1), tol)) << "Reference U(" << i << "): Column 2 is not normalized.";

			Scalar sqLenResUc0 = resU.col(0).squaredNorm();
			Scalar sqLenResUc1 = resU.col(1).squaredNorm();
			Scalar sqLenResUc2 = resU.col(2).squaredNorm();
			EXPECT_TRUE(equal(sqLenResUc0, Scalar(1), tol)) << "Result U(" << i << "): Column 0 is not normalized.";
			EXPECT_TRUE(equal(sqLenResUc1, Scalar(1), tol)) << "Result U(" << i << "): Column 1 is not normalized.";
			EXPECT_TRUE(equal(sqLenResUc2, Scalar(1), tol)) << "Result U(" << i << "): Column 2 is not normalized.";

			bool eqS = refS.array().abs().isApprox(resS.array().abs(), tol);
			bool eqU = refU.array().abs().isApprox(resU.array().abs(), tol);

			EXPECT_TRUE(eqS) << "S(" << i << ") - Ref: " << refS << ", Actual: " << resS;
			EXPECT_TRUE(eqU) << "U(" << i << ") - Ref: " << refU << ", Actual: " << resU;
		}
	}
}

template<typename WideScalar>
void runJacobiEigen33Test(float tol)
{
	using scalar_t = float;
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;
	using vector3_t = Eigen::Matrix<real_t, 3, 1>;

	size_t nr_problems = 128;
	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> resU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> resS(nr_problems);

	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> refU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> refS(nr_problems);

	auto A = createProblems<scalar_t>(nr_problems);
	computeReferenceSolution(nr_problems, A, refU, refS);

	// Strides
	size_t stride = nr_problems;
	size_t width = sizeof(real_t) / sizeof(scalar_t);

	for (int i = 0; i < static_cast<int>(stride / width); i++)
	{
		matrix3_t ATA = A.at<real_t>(i);
		matrix3_t U;
		vector3_t S;

		Vcl::Mathematics::SelfAdjointJacobiEigen(ATA, U);
		S = ATA.diagonal();

		resU.at<real_t>(i) = U;
		resS.at<real_t>(i) = S;
	}

	// Check against reference solution
	checkSolution(nr_problems, tol, refU, refS, resU, resS);
}

template<typename WideScalar>
void runJacobiEigenQuat33Test(float tol)
{
	using scalar_t = float;
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;
	using vector3_t = Eigen::Matrix<real_t, 3, 1>;

	size_t nr_problems = 128;
	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> resU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> resS(nr_problems);

	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> refU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> refS(nr_problems);

	auto A = createProblems<scalar_t>(nr_problems);
	computeReferenceSolution(nr_problems, A, refU, refS);

	// Strides
	size_t stride = nr_problems;
	size_t width = sizeof(real_t) / sizeof(scalar_t);

	for (int i = 0; i < static_cast<int>(stride / width); i++)
	{
		matrix3_t ATA = A.at<real_t>(i);
		matrix3_t U;
		vector3_t S;

		Vcl::Mathematics::SelfAdjointJacobiEigenQuat(ATA, U);
		S = ATA.diagonal();

		resU.at<real_t>(i) = U;
		resS.at<real_t>(i) = S;
	}

	// Check against reference solution
	checkSolution(nr_problems, tol, refU, refS, resU, resS);
}

TEST(Eigen33, EigenFloat)
{
	runJacobiEigen33Test<float>(1e-5f);
}
TEST(Eigen33, EigenFloat4)
{
	runJacobiEigen33Test<Vcl::float4>(1e-5f);
}
TEST(Eigen33, EigenFloat8)
{
	runJacobiEigen33Test<Vcl::float8>(1e-5f);
}
TEST(Eigen33, EigenFloat16)
{
	runJacobiEigen33Test<Vcl::float16>(1e-5f);
}

TEST(Eigen33, EigenQuatFloat)
{
	runJacobiEigenQuat33Test<float>(1e-5f);
}
TEST(Eigen33, EigenQuatFloat4)
{
	runJacobiEigenQuat33Test<Vcl::float4>(1e-5f);
}
TEST(Eigen33, EigenQuatFloat8)
{
	runJacobiEigenQuat33Test<Vcl::float8>(1e-5f);
}
TEST(Eigen33, EigenQuatFloat16)
{
	runJacobiEigenQuat33Test<Vcl::float16>(1e-5f);
}


/*! @file pitts_tensor2_qb_decomposition.hpp
* @brief QB-part of the SVQB orthogonalization algorithm (Stathopoulos and Wu, SISC 23 (6), pp. 2165-2182)
* @author Melven Roehrig-Zoellner <Melven.Roehrig-Zoellner@DLR.de>
* @date 2019-10-31
* @copyright Deutsches Zentrum fuer Luft- und Raumfahrt e. V. (DLR), German Aerospace Center
*
**/

// include guard
#ifndef PITTS_TENSOR2_QB_DECOMPOSITION_HPP
#define PITTS_TENSOR2_QB_DECOMPOSITION_HPP

// includes
#include <exception>
#include <cmath>
#include "pitts_tensor2.hpp"
#include "pitts_tensor2_eigen_adaptor.hpp"
#include "pitts_timer.hpp"
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options

//! namespace for the library PITTS (parallel iterative tensor train solvers)
namespace PITTS
{
  //! QB-part in the SVQB orthogonalization algorithm from Stathopoulos and Wu, SISC 23 (6), pp. 2165-2182
  //!
  //! Computes the decomposition B^TB = M using a SVD of M for a symmetric positive semi-definite matrix M
  //!
  //! @tparam T     underlying data type (double, complex, ...)
  //!
  //! @param  M     Symmetric positive semi-definite input matrix M, overwritten with the output matrix B
  //! @param  Binv  Pseudo-Inverse of the output matrix
  //! @return       detected rank of the matrix
  //!
  template<typename T>
  auto qb_decomposition(const Tensor2<T>& M, Tensor2<T>& B, Tensor2<T>& Binv, T rankTolerance)
  {
    const auto timer = PITTS::timing::createScopedTimer<Tensor2<T>>();

    // get dimension
    if( M.r1() != M.r2() )
      throw std::invalid_argument("qb_decomposition requires a quadratic matrix!");
    const auto n = M.r1();

    // Helpful types
    using vec = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
    using EigenSolver = Eigen::SelfAdjointEigenSolver<mat>;

    // use Eigen for matrix operations
    const auto mapM = ConstEigenMap(M);

    // compute sqrt(diag(M)) and its inverse
    vec d(n), dinv(n);
    for(int i = 0; i < n; i++)
    {
      if( std::abs(M(i,i)) <= rankTolerance )
      {
        d(i) = T(0);
        dinv(i) = T(0);
      }
      else
      {
        d(i) = std::sqrt(M(i,i));
        dinv(i) = T(1)/d(i);
      }
    }

    // scale input matrix from left and right by diag(dinv), so it's diagonal entries become 1
    const mat scaledM = dinv.asDiagonal() * mapM * dinv.asDiagonal();

    // compute eigenvalue decomposition
    EigenSolver eigSolv(scaledM);

    // determine rank of the input matrix and set w = sqrt(lambda), winv = 1/w
    // Eigen orders eigenvalues with increasing value (smallest first)
    int rank = n;
    const auto evMax = std::abs(eigSolv.eigenvalues()(n-1));
    vec w(n), winv(n);
    if( evMax <= rankTolerance )
    {
      w = vec::Zero(n);
      winv = vec::Zero(n);
      rank = 0;
    }
    else
    {
      for(int i = 0; i < n; i++)
      {
        if( eigSolv.eigenvalues()(i) <= rankTolerance*evMax )
        {
          rank--;
          w(i) = T(0);
          winv(i) = T(0);
        }
        else
        {
          w(i) = std::sqrt(eigSolv.eigenvalues()(i));
          winv(i) = T(1)/w(i);
        }
      }
    }

    // calculate B and Binv
    B.resize(n,n);
    Binv.resize(n,n);
    EigenMap(Binv) = dinv.asDiagonal() * eigSolv.eigenvectors() * winv.asDiagonal();
    EigenMap(B) = w.asDiagonal() * eigSolv.eigenvectors().transpose() * d.asDiagonal();

    // reorder, so the part corresponding to the largest eigenvalues are first
    for(int i = 0; i < n; i++)
    {
      for(int j = 0; j < n/2; j++)
      {
        int k = n-j-1;
        std::swap(B(j,i), B(k,i));
        std::swap(Binv(i,j), Binv(i,k));
      }
    }

    return rank;
  }

}

#endif // PITTS_TENSOR2_QB_DECOMPOSITION_HPP


#ifndef GENERATE_GALTON_WATSON_HPP
#define GENERATE_GALTON_WATSON_HPP

#include <cstdlib>
#include <string>
#include <queue>
#include <algorithm>
#include <numeric>
#include <cmath>

#ifdef _OPENMP
#include <omp.h>
#endif

#include <boost/random.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/factorials.hpp>

/**
 * Given:
 * (1) Distribution: Poisson, Negative Geometric, Binomial
 * (2) Number of nodes for the conditioned Galton-Watson process.
 * (3) Parameters needed for the distributions:
 *    (a) Probability of success (p)
 *    (b) Lambda if using Poisson distribution
 *    (c) 'k', the total number of trials if we are using Binomial
 *    (d) Seed value for the random number
 */
struct generate_Galton_Watson_t {
  typedef boost::mt19937 engine_type;
  typedef boost::binomial_distribution<int, double> binomial_type;
  typedef boost::uniform_real<double> uni_real_type;
  typedef boost::exponential_distribution<double> exp_real_type;
  typedef boost::uniform_int<int> uni_int_type;

  typedef boost::adjacency_list
                   <boost::listS,
                    boost::vecS,
                    boost::bidirectionalS,
                    boost::property<boost::vertex_name_t, std::string,
                    boost::property<boost::vertex_distance_t, double> >,
                    boost::property<boost::edge_weight_t, double> > graph_type;
    typedef boost::property_map<graph_type, boost::vertex_index_t>::type
                                                vertex_index_map_t;
    typedef boost::property_map<graph_type, boost::vertex_name_t>::type
                                                vertex_name_map_t;
    typedef boost::property_map<graph_type, boost::vertex_distance_t>::type
                                                vertex_distance_map_t;
    typedef boost::property_map<graph_type, boost::edge_weight_t>::type
                                                edge_weight_map_t;
    typedef boost::graph_traits<graph_type>::vertex_descriptor vertex_t;

  private:

  struct always_1_t {
    typedef double result_type;
    result_type operator()() { return 1.0; }
  };

  struct Poisson_computer_t {
    typedef boost::poisson_distribution<int, double> dist_type;
    int lambda;

    Poisson_computer_t (int lambda) : lambda(lambda) {}

    double operator()(int i) const {
      return (pow(lambda,i)*exp(-lambda)) / boost::math::factorial<double>(i);
    }

    dist_type operator()() const { return dist_type(lambda); }
  };

  struct geometric_computer_t {
    typedef boost::geometric_distribution<int, double> dist_type;
    double p;
    geometric_computer_t (double p) : p(p) {}

    double operator()(int i) const { return pow((1-p), i) * p; }

    dist_type operator()() const { return dist_type(p); }
  };

  struct binomial_computer_t {
    typedef boost::binomial_distribution<int, double> dist_type;
    int k;
    double p;
    binomial_computer_t (int k, double p) : k(k), p(p) {}

    double operator()(int i) const {
      return
      boost::math::binomial_coefficient<double>(k,i)*pow(p,i)*pow((1-p),(k-i));
    }

    dist_type operator()() const { return dist_type(k,p); }
  };

  struct binary_0_2_computer_t {
    struct binary_0_2_distribution {
      typedef int result_type;
      boost::bernoulli_distribution<double> distro;

      binary_0_2_distribution(double p) : distro(p) {}

      template <typename EngineType>
      int operator()(EngineType& engine) {return (distro(engine))?2:0;}
    };
    typedef binary_0_2_distribution dist_type;

    binary_0_2_computer_t (double p) : p(p) {}

    double operator()(int i) const {
      if (2==i) return p;
      else if (0==i) return (1-p);
      else return 0;
    }

    dist_type operator()() const { return dist_type(p); }

    private:
    double p;
  };

  struct binary_0_1_2_computer_t {
    typedef boost::uniform_int<int> dist_type;
    double operator()(int i) const {
      if (0==i || 1==i || 2==i) return 1./3.;
      else return 0;
    }

    dist_type operator()() const { return dist_type(0,2); }
  };

  /**
   * Thin wrapper around boost::uniform<> so that it conforms to random_shuffle.
   */
  struct random_functor_t : std::unary_function<int, int> {
    engine_type engine;

    random_functor_t (engine_type engine) : engine(engine) {}

    int operator()(const int& ULIMIT) {
      uni_int_type dist (0, ULIMIT-1);
      return dist(engine);
    }
  };

  static void rotate_to_get_tree (std::vector<int>& psi_vec) {
    /** Get the vector S for the queues in the tree */
    size_t n = psi_vec.size();
    std::vector<int> S(n);

    for (size_t i=0; i<n; ++i) S[i] = ((0==i)?1:(S[i-1]))+(psi_vec[i]-1);

    int min_ele=std::numeric_limits<int>::max();
    int min_index=-1;
    for (size_t i=0; i<n; ++i)
      if (min_ele>S[i]) { min_ele=S[i]; min_index=i; }

    std::rotate(psi_vec.begin(), psi_vec.begin()+min_index+1, psi_vec.end());
  }

  template <typename WDistribution>
  static graph_type generate_impl (int n,
                                   const std::vector<int>& psi_vec,
                                   WDistribution w_prng,
                                   int verbosity) {
    graph_type G;
    vertex_name_map_t name_map = boost::get (boost::vertex_name, G);
    vertex_distance_map_t dist_map = boost::get (boost::vertex_distance, G);
    edge_weight_map_t weight_map = boost::get (boost::edge_weight, G);

    /** Set up a queue for the children and add the root vertex */
    vertex_t root = boost::add_vertex(G);
    boost::put (name_map, root, boost::lexical_cast<std::string>(0));
    boost::put (dist_map, root, 0);

    std::queue<vertex_t> vertex_queue;
    vertex_queue.push(root);

    /** Count the number of children left */
    int n_left = (n-1);

    /** Iterate till we are left with nothing or we have maxed out children */
    while ((0<=n_left) && (false==vertex_queue.empty())) {
      const vertex_t parent = vertex_queue.front();
      vertex_queue.pop();

#pragma omp critical
      if (1<verbosity) std::cout << "Popped " << parent << std::endl;

      int num_children = psi_vec[parent];
#pragma omp critical
      if (1<verbosity) std::cout << "num children of " << parent << " = "
                               << num_children << std::endl;

      for (int child_number=0; child_number<num_children; ++child_number) {
        const vertex_t child = boost::add_vertex(G);
        vertex_queue.push(child);
        boost::put (name_map,
                    child,
                    boost::get (name_map, parent) +
                    boost::lexical_cast<std::string>(child_number));

        /**
         * Duplicating a map here, but I think it's fine. We need the vertex
         * weights to easily be able to get the Dyck/Harris paths.
         */
        double edge_weight = w_prng();
        boost::put (dist_map, child, edge_weight);
        boost::put (weight_map,
                    (boost::add_edge (parent, child, G)).first,
                    edge_weight);

        --n_left;
      }

#pragma omp critical
      if (1<verbosity) std::cout << "N Left = " << n_left << std::endl;
    }

    return G;
  }

  template <typename ProbabilityComputer>
  static graph_type generate_tree_stupid (int n,
                                          int seed,
                                          ProbabilityComputer compute,
                                          bool use_random_weights,
                                          int verbosity) {
    // This is the stupid way of generating the required random tree.
    // We simply want to fill up the tree as long as it has at least
    // the required number of nodes, we are happy. Please use it only
    // as a counter example.

    // A vector to hold the number of children for each node
    std::vector<int> psi_vec (n, 0);

    // Get a new random engine
    engine_type engine(seed);

    // Get the distribution that we are interested in
    typename ProbabilityComputer::dist_type prng = compute();

    // Now, all we need to do is iterate over and over again
    int num_children;
    do {

      // Start with 0 edges in the beginning
      num_children = 0;

      // Loop to assign edges/children to nodes
      for (int i=0;
           (num_children<(n-1)) && (i<psi_vec.size());
           ++i) {

        // Figure out how many children we want to give this node.
        int num_children_for_i = prng(engine);
        if ((num_children_for_i+num_children) > (n-1)) {
          num_children_for_i = (n-1) - num_children;
        }

        num_children += num_children_for_i;
        psi_vec[i] = num_children_for_i;
      }

    } while (num_children != (n-1));

    random_functor_t shuffle_prng (engine);
    std::random_shuffle (psi_vec.begin(), psi_vec.end(), shuffle_prng);

    if (2<verbosity) {
      for (int i=0; i<n; ++i) std::cout<<psi_vec[i]<<" ";
      std::cout<<std::endl;
    }

    rotate_to_get_tree (psi_vec);

    if (2<verbosity) {
      for (int i=0; i<n; ++i) std::cout<<psi_vec[i]<<" ";
      std::cout<<std::endl;
    }

    if (use_random_weights) {
      boost::variate_generator<engine_type, uni_real_type>
                            w_prng (engine_type(), uni_real_type(0.0,1.0));
      return generate_impl (n, psi_vec, w_prng, verbosity);
    } else {
      return generate_impl (n, psi_vec, always_1_t(), verbosity);
    }
  }

  template <typename ProbabilityComputer>
  static graph_type generate_tree (int n,
                                   int seed,
                                   ProbabilityComputer compute,
                                   bool use_random_weights,
                                   int verbosity) {

    std::vector<int> N_j_vec;
    engine_type engine(seed);
    int j_times_N_j, j;

    do {

      j_times_N_j = j = 0;
      int sum_of_N_i = 0;
      int i = 0;
      double sum_of_p_i = 0;
      N_j_vec.resize(0);
      while (0 != (n-sum_of_N_i)) {
        const double p_i = compute(i);
        engine_type new_engine(engine());
        binomial_type dist((n-sum_of_N_i), p_i/(1-sum_of_p_i));
        const int N_j = dist(new_engine);
        N_j_vec.push_back (N_j);
        sum_of_N_i += N_j;
        sum_of_p_i += p_i;
        j_times_N_j += j*N_j;
        ++i;
        ++j;
      }

      if (2 < verbosity) std::cout << "j*N_j = " << j_times_N_j << std::endl;

    } while (j_times_N_j != (n-1));

    std::vector<int> psi_vec (n, 0);
    int offset = 0;
    for (int j=0; j<N_j_vec.size(); ++j) {
      for (int i=0; i<N_j_vec[j]; ++i) psi_vec[offset+i] = j;
      offset += N_j_vec[j];
    }

    random_functor_t shuffle_prng (engine);
    std::random_shuffle (psi_vec.begin(), psi_vec.end(), shuffle_prng);

    if (2<verbosity) {
      for (int i=0; i<n; ++i) std::cout<<psi_vec[i]<<" ";
      std::cout<<std::endl;
    }

    rotate_to_get_tree (psi_vec);

    if (2<verbosity) {
      for (int i=0; i<n; ++i) std::cout<<psi_vec[i]<<" ";
      std::cout<<std::endl;
    }

    if (use_random_weights) {
      boost::variate_generator<engine_type, uni_real_type>
                            w_prng (engine_type(), uni_real_type(0.0,1.0));
      return generate_impl (n, psi_vec, w_prng, verbosity);
    } else {
      return generate_impl (n, psi_vec, always_1_t(), verbosity);
    }
  }

  template <typename ProbabilityComputer>
  static graph_type generate_tree_dispatcher (int n,
                                              int seed,
                                              ProbabilityComputer compute,
                                              bool use_random_weights,
                                              int verbosity,
                                              bool use_stupid_tree_gen) {
    if (use_stupid_tree_gen) {
      return generate_tree_stupid (n, seed, compute, use_random_weights,
                                   verbosity);
    } else {
      return generate_tree (n, seed, compute, use_random_weights, verbosity);
    }
  }

  public:

  static graph_type generate_Poisson (int n,
                                      int seed,
                                      int lambda,
                                      bool use_random_weights,
                                      int verbosity,
                                      bool use_stupid_tree_gen) {
    return generate_tree_dispatcher (n, seed, Poisson_computer_t(lambda),
                          use_random_weights, verbosity, use_stupid_tree_gen);
  }

  static graph_type generate_Geometric (int n,
                                        int seed,
                                        double p,
                                        bool use_random_weights,
                                        int verbosity,
                                        bool use_stupid_tree_gen) {
    return generate_tree_dispatcher (n, seed, geometric_computer_t(p),
                          use_random_weights, verbosity, use_stupid_tree_gen);
  }

  static graph_type generate_Binomial (int n,
                                       int seed,
                                       int k,
                                       double p,
                                       bool use_random_weights,
                                       int verbosity,
                                       bool use_stupid_tree_gen) {
    return generate_tree_dispatcher (n, seed, binomial_computer_t(k, p),
                          use_random_weights, verbosity, use_stupid_tree_gen);
  }

  static graph_type generate_Binary_0_2 (int n,
                                         int seed,
                                         double p,
                                         bool use_random_weights,
                                         int verbosity,
                                         bool use_stupid_tree_gen) {
    return generate_tree_dispatcher (n, seed, binary_0_2_computer_t(p),
                          use_random_weights, verbosity, use_stupid_tree_gen);
  }

  static graph_type generate_Binary_0_1_2 (int n,
                                           int seed,
                                           bool use_random_weights,
                                           int verbosity,
                                           bool use_stupid_tree_gen) {
    return generate_tree_dispatcher (n, seed, binary_0_1_2_computer_t(),
               use_random_weights, verbosity, use_stupid_tree_gen);
  }
};

#endif // GENERATE_GALTON_WATSON_HPP


// Copyright (c) 2009  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// $URL$
// $Id$
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Sebastien Loriot, Sylvain Pion

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/CGAL_Ipelet_base.h>
#include <CGAL/linear_least_squares_fitting_2.h>
#include <boost/utility.hpp>

namespace CGAL_pca{

//~ typedef CGAL::Exact_predicates_exact_constructions_kernel                 Kernel;
typedef CGAL::Simple_cartesian<double>                 Kernel;

const std::string Slab[] = {
  "PCA","Help"
};

const std::string Hmsg[] = {
  "(Principal Component Analysis) given a set of points, draw a segment that is on the line defined by the eigen vector associated to the highest eigen value of the covariance matrix of the input points"
};

class pcaIpelet
  : public CGAL::Ipelet_base<Kernel,2>{
public:
  pcaIpelet()
    :CGAL::Ipelet_base<Kernel,2>("PCA",Slab,Hmsg){}
  void protected_run(int);
};

void pcaIpelet::protected_run(int fn)
{
  if (fn==1) {
    show_help();
    return;
  }

  std::list<Point_2> pt_list;
  std::list<Circle_2> cir_list;
  std::list<Polygon_2> poly_list;
  std::list<Kernel::Triangle_2> tri_list;
  std::list<Segment_2> sg_list;

  Iso_rectangle_2 bbox=
    read_active_objects(
      CGAL::dispatch_or_drop_output<Point_2,Polygon_2,Circle_2,Segment_2>(
        std::back_inserter(pt_list),
        std::back_inserter(poly_list),
        std::back_inserter(cir_list),
        std::back_inserter(sg_list)
      )
    );

  for (std::list<Polygon_2>::iterator it=poly_list.begin();it!=poly_list.end();++it)
    if (it->size()==3){
      tri_list.push_back(Kernel::Triangle_2(*(it->vertices_begin()),
                                            *boost::next(it->vertices_begin()),
                                            *boost::next(it->vertices_begin(),2)
                                            ));
    }
    else{
      print_error_message("This implementation is limited to triangles");
      return;
    }

  int s=0;

  if (!pt_list.empty()) s=1;
  if (!cir_list.empty()) s+=2;
  if (!tri_list.empty()) s+=4;
  if (!sg_list.empty()) s+=8;

  if (s==0) {
    print_error_message("Nothing is selected");
    return;
  }

  Kernel::Line_2 line;
  Kernel::Point_2 centroid;

  switch (s){
    case 1://points
      linear_least_squares_fitting_2(pt_list.begin(),pt_list.end(),line,centroid,CGAL::Dimension_tag<0>());
      break;
    case 2://circles
      linear_least_squares_fitting_2(cir_list.begin(),cir_list.end(),line,centroid,CGAL::Dimension_tag<2>());
      break;
    case 4://triangles
      linear_least_squares_fitting_2(tri_list.begin(),tri_list.end(),line,centroid,CGAL::Dimension_tag<2>());
      break;
    case 8://segments
      linear_least_squares_fitting_2(sg_list.begin(),sg_list.end(),line,centroid,CGAL::Dimension_tag<1>());
      break;
    default:
      print_error_message("Please select a set of points or segments or triangles or circles");
      return;
  }

  CGAL::Object obj_cgal = CGAL::intersection(line,bbox);
  Segment_2 seg;
  if (CGAL::assign(seg, obj_cgal))
    draw_in_ipe(seg);

}

}

CGAL_IPELET(CGAL_pca::pcaIpelet)


/*
 * Copyright 2011-2013 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/ublas/vector.hpp>

typedef boost::numeric::ublas::vector< double > state_type;

void lorenz( const state_type &x , state_type &dxdt , const double t )
{
    const double sigma( 10.0 );
    const double R( 28.0 );
    const double b( 8.0 / 3.0 );

    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

using namespace boost::numeric::odeint;

//[ublas_main
int main()
{
    state_type x(3);
    x[0] = 10.0; x[1] = 5.0 ; x[2] = 0.0;
    typedef runge_kutta_dopri5< state_type > stepper;
    integrate_const( make_dense_output< stepper >( 1E-6 , 1E-6 ) , lorenz , x ,
                     0.0 , 10.0 , 0.1 );
}
//]


#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operations.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <random>
#include "mnist/reader.hpp"
#include "perceptron/PerceptronMonoLayer.hpp"
#include "perceptron/act_funs.hpp"
#include "perceptron/ItemNN.hpp"

using namespace cv;
using namespace boost;
using namespace boost::numeric;
using namespace std;

void randomizeMatrix(boost::numeric::ublas::matrix<double> &matrix);

void randomizeVector(boost::numeric::ublas::vector<double> &vect);

ublas::vector<double> imageToVector(cv::Mat const &mat) {
    size_t nbPixels = mat.rows * mat.cols;
    ublas::vector<double> v(nbPixels);
    for (size_t i = 0; i < nbPixels; i++) {
        auto a = (double) mat.at<unsigned char>(i);
        v.insert_element(i, a>(255./2.)?1:0);
    }
    return v;
}

int main(int argc, char ** argv) {

    using namespace boost::numeric::ublas;

    int numCycles;
    if(argc > 1)
        numCycles = atoi(argv[1]);
    else
        numCycles =25;

    double (*seuil)(double) = act_f::noActFun<double>;

    string filename = "../res/train-images-idx3-ubyte";
    size_t image_size = 28 * 28;

    std::vector<cv::Mat> vec_entrees;
    std::vector<double> vec_labels;
    std::vector<cv::Mat> vec_entrees_test;
    std::vector<double> vec_labels_test;
    read_Mnist(filename, vec_entrees);
    read_Mnist_Label("../res/train-labels-idx1-ubyte", vec_labels);
    read_Mnist("../res/t10k-images-idx3-ubyte", vec_entrees_test);
    read_Mnist_Label("../res/t10k-labels-idx1-ubyte", vec_labels_test);

    std::vector<ItemNN<double>> vec_train;
    for (int i = 0; i < vec_entrees.size(); ++i) {
        vec_train.emplace_back(vec_labels[i], vec_entrees[i], imageToVector);
    }
    vec_entrees.clear();
    vec_labels.clear();
    std::vector<ItemNN<double>> vec_test;
    for (int i = 0; i < vec_entrees_test.size(); ++i) {
        vec_test.emplace_back(vec_labels_test[i], vec_entrees_test[i], imageToVector);
    }
    vec_entrees_test.clear();
    vec_labels_test.clear();

    std::vector<Neuron> neuronnes;
    for (int i = 0; i < 10; ++i) {
        ublas::vector<double> weigthN(image_size);
        randomizeVector(weigthN);
        neuronnes.emplace_back(weigthN, seuil);
    }

    PerceptronMonoLayer perceptronMonoLayer(neuronnes);
    perceptronMonoLayer.learn(vec_train,numCycles);
    cout << "End of learning" << endl;
    cout << "Begin tests" << endl;
    perceptronMonoLayer.test(vec_test);

    neuronnes.clear();

    cv::Mat imageTest = imread("../res/test.png",cv::ImreadModes::IMREAD_GRAYSCALE);

    ItemNN<double> itemNN(5,imageTest,imageToVector);

    cout << "test with gimp image(5):" <<(
         perceptronMonoLayer.test(itemNN)? "true":"false") << endl;

    return 0;
}

/**
 * Fonction qui permet d'initialiser un vecteur a des poids aléatoires
 * entre -1 et 1
 * @param vect à initialiser
 */
void randomizeVector(boost::numeric::ublas::vector<double> &vect) {
    std::random_device rd;
    std::mt19937 mt(rd());
    std::uniform_real_distribution<double> dist(0, 1);

    for (size_t i = 0; i < vect.size(); i++) {
        vect(i) = dist(mt);
    }
}


#define BOOST_TEST_MODULE SRF
#include <boost/test/included/unit_test.hpp>

#include <limits>
#include <tuple>

typedef std::tuple<float, double, long double> test_types;

#include <triumf/numpy.hpp>
#include <triumf/superconductivity/phenomenology.hpp>

//
BOOST_AUTO_TEST_CASE_TEMPLATE(halbritter_gap, T, test_types) {
  BOOST_TEST(triumf::superconductivity::phenomenology::reduced_gap<T>(-10.0) ==
             static_cast<T>(1.0));
  BOOST_TEST(triumf::superconductivity::phenomenology::reduced_gap<T>(0.0) ==
             static_cast<T>(1.0));
  BOOST_TEST(triumf::superconductivity::phenomenology::reduced_gap<T>(1.0) ==
             static_cast<T>(0.0));
  BOOST_TEST(triumf::superconductivity::phenomenology::reduced_gap<T>(10.0) ==
             static_cast<T>(0.0));
}

//
BOOST_AUTO_TEST_CASE_TEMPLATE(penetration_depth, T, test_types) {
  BOOST_TEST(
      triumf::superconductivity::phenomenology::reduced_penetration_depth<T>(
          -10.0, 4) == static_cast<T>(1.0));
  BOOST_TEST(
      triumf::superconductivity::phenomenology::reduced_penetration_depth<T>(
          0.0, 4) == static_cast<T>(1.0));
  BOOST_TEST(
      triumf::superconductivity::phenomenology::reduced_penetration_depth<T>(
          1.0, 4) == std::numeric_limits<T>::infinity());
  BOOST_TEST(
      triumf::superconductivity::phenomenology::reduced_penetration_depth<T>(
          10.0, 4) == std::numeric_limits<T>::infinity());
}

//
BOOST_AUTO_TEST_CASE_TEMPLATE(critical_temperature, T, test_types) {
  // define some constants to use in the tests
  constexpr T T_c = 9.25; // K
  constexpr T B_c2 = 400; // mT
  constexpr T gamma = 2.0;
  // explicit tests for edge cases
  BOOST_TEST(triumf::superconductivity::phenomenology::critical_temperature<T>(
                 -1.0 * B_c2, T_c, B_c2, gamma) == static_cast<T>(T_c));
  BOOST_TEST(triumf::superconductivity::phenomenology::critical_temperature<T>(
                 0.0 * B_c2, T_c, B_c2, gamma) == static_cast<T>(T_c));
  BOOST_TEST(triumf::superconductivity::phenomenology::critical_temperature<T>(
                 1.1 * B_c2, T_c, B_c2, gamma) == static_cast<T>(0.0));
  // tests for valid input range
  for (const auto &B : triumf::numpy::linspace<T>(0.0, B_c2, 100)) {
    BOOST_TEST(
        triumf::superconductivity::phenomenology::critical_temperature<T>(
            B, T_c, B_c2, gamma) <= T_c);
  }
}

//
BOOST_AUTO_TEST_CASE_TEMPLATE(critical_temperature_II, T, test_types) {
  // define some constants to use in the tests
  constexpr T T_c = 9.25; // K
  constexpr T B_c2 = 400; // mT
  constexpr T gamma = 2.0;
  // explicit tests for edge cases
  BOOST_TEST(triumf::superconductivity::phenomenology::critical_temperature_II<T>(
                 -1.0 * B_c2, T_c, B_c2) == static_cast<T>(T_c));
  BOOST_TEST(triumf::superconductivity::phenomenology::critical_temperature_II<T>(
                 0.0 * B_c2, T_c, B_c2) == static_cast<T>(T_c));
  BOOST_TEST(triumf::superconductivity::phenomenology::critical_temperature_II<T>(
                 1.1 * B_c2, T_c, B_c2) == static_cast<T>(0.0));
  // tests for valid input range
  for (const auto &B : triumf::numpy::linspace<T>(0.0, B_c2, 100)) {
    BOOST_TEST(
        triumf::superconductivity::phenomenology::critical_temperature_II<T>(
            B, T_c, B_c2) <= T_c);
  }
}


#include <seir.h>

#include <boost/numeric/odeint.hpp>

#include <iostream>
#include <functional>
#include <algorithm>
#include <limits>

#include <math.h>
#if !defined(max)
#define max(a, b) (a > b ? a : b)
#endif

#if !defined(min)
#define min(a, b) (a < b ? a : b)
#endif

#define NSTATES 9
typedef std::array<double, NSTATES> StateType;

// odeing namespace access
namespace odeint = boost::numeric::odeint;

SEIR::SEIR()
    : R0Table_(), Tinc_(2.5), Tinf_(5.0), Tfat_(30), Tmrec_(12), Threc_(30),
      pMild_(0.8), pFat_(0.01), duration_(300), population_(1000000), dt_(1),
      Hc_(1), pfat_increase_nohospital_(1.0)
{
    SetR0(2.5);
}

static bool operator<(const R0TableElement &a, const R0TableElement &b)
{
    return (a.time < b.time);
}

void SEIR::SetR0(double R0, double time)
{
    if (time < 0) {
        R0Table_.clear();
        R0Table_.push_back(R0TableElement(R0, 0));
        return;
    }
    R0Table_.push_back(R0TableElement(R0, time));
    R0Table_.sort();
}

ResultsType SEIR::compute(void)
{
    StateType state0;
    memset(state0.data(), 0, sizeof(double) * NSTATES);
    state0[0] = 1.0;
    state0[1] = 1.0 / (double)population_;

    typedef odeint::runge_kutta_cash_karp54<StateType, double, StateType,
                                            double>
        error_stepper_type;
    typedef odeint::controlled_runge_kutta<error_stepper_type>
        controlled_stepper_type;
    // typedef odeint::dense_output_runge_kutta<controlled_stepper_type>
    //   dense_stepper_type;

    // Create an output
    double nsteps = (double)duration_ / (double)dt_;
    if (::fmod(nsteps, 1.0) < std::numeric_limits<double>::epsilon())
        nsteps = nsteps + 1;
    else
        nsteps = ::ceil(nsteps);

    ResultsType results;
    results.resize((int)nsteps);
    uint32_t counter = 0;

    error_stepper_type stepper;
    odeint::integrate_const(
        stepper,
        [this](const StateType &state, StateType &statedot, double t) {
            // statedot.resize(NSTATES);

            double S = state[0];   // susceptible
            double E = state[1];   // exposed
            double Inf = state[2]; // Infectious
            double M = state[3];   // Mild case
            double Hrec = state[4];
            double Hfat = state[5];

            // Probability of fatal given hospitalized
            // and sufficient hospital capacity
            double pfs_hos = pFat_ / (1.0 - pMild_);

            // Probability of fatal given hospitalized,
            // and no hospital capacity
            double pfs_nohos = pfs_hos * (1.0 + pfat_increase_nohospital_);

            // weighted probability of fatal
            // given hospital capacity
            double H = Hrec + Hfat;
            double pfs = pfs_hos;
            if ((H > Hc_) && (H > 0)) {
                pfs = (Hc_ / H) * pfs_hos + (H - Hc_) / H * pfs_nohos;
            }

            // Get the current R0 from the table
            auto upper = std::lower_bound(R0Table_.begin(), R0Table_.end(),
                                          R0TableElement(0, t));
            //[](const R0TableElement &a, double b) { return a.second < b; });
            if (upper != R0Table_.begin())
                upper--;
            double R0 = upper->R0;

            // Update time derivates:

            // Susceptible population:
            // out: susceptible to exposed
            auto dS = -R0 / Tinf_ * Inf * S;
            // Exposed population:
            // in: susceptible to exposed
            // out: exposed to infectious
            auto dE = -dS - E / Tinc_;
            // Infectious population:
            // in: exposed to infectious
            // out: infectious to mild or hospital
            auto dI = E / Tinc_ - Inf / Tinf_;
            // Mild case population:
            // in: infectious to mild
            // out: mild to recovered (removed)
            auto dM = pMild_ * Inf / Tinf_ - M / Tmrec_;
            // Hospitalized population that will recover
            // in: infectious to hospitalized
            // out: hospitalized to recovered
            auto dHrec =
                (1.0 - pMild_) * (1.0 - pfs) * Inf / Tinf_ - Hrec / Threc_;
            // Hospitalized population that will not recover
            // in: infectious to hospitalized
            // out: hospitalized to fatal
            auto dHfat = (1.0 - pMild_) * pfs * Inf / Tinf_ - Hfat / Tfat_;
            // Fatal population:
            // in: hospitalized to fatal
            auto dF = Hfat / Tfat_;
            // Removed population:
            // in: mild to recovered
            // in: hospitalized to recovered
            auto dRm = M / Tmrec_;
            auto dRh = Hrec / Threc_;

            statedot[0] = dS;
            statedot[1] = dE;
            statedot[2] = dI;
            statedot[3] = dM;
            statedot[4] = dHrec;
            statedot[5] = dHfat;
            statedot[6] = dF;
            statedot[7] = dRm;
            statedot[8] = dRh;
        },
        state0, 0.0, (double)duration_, dt_,
        [this, &counter, &results](const StateType &s, double t) {
            results[counter] = std::vector<double>(s.begin(), s.end());
            counter++;
        });

    // Multiply results by population
    double population = this->population_;
    for (auto &res : results) {
        std::transform(res.begin(), res.end(), res.begin(),
                       [population](double &c) { return population * c; });
    }
    return results;
}

#if 0
int main(int argc, char *argv[])
{
    SEIR seir;
    seir.Hc_ = .001;
    seir.R0Table_.clear();
    seir.R0Table_.push_back(R0TableElement(4.5, 0));
    seir.R0Table_.push_back(R0TableElement(1.5, 150));

    auto results = seir.compute();
    auto lr = results[results.size() - 1];
#if 0
    for (int i = 0; i < NSTATES; i++) {
        std::cout << lr[i] << std::endl;
    }
#endif
    return 0;
}
#endif


#include <cradle/geometry/polygonal.hpp>
#include <boost/assign/std/vector.hpp>
#include <cradle/anonymous.hpp>

#define BOOST_TEST_MODULE polygonal
#include <cradle/test.hpp>

using namespace cradle;
using namespace boost::assign;

// polygon2

BOOST_AUTO_TEST_CASE(simple_ccw_poly_test)
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(0, 0),
            make_vector<double>(5, 0),
            make_vector<double>(2, 2),
            make_vector<double>(0, 2);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 7.);
    BOOST_CHECK(!is_inside(poly, make_vector<double>(-1, 1)));
    BOOST_CHECK(is_inside(poly, make_vector<double>(1, 1)));
}

BOOST_AUTO_TEST_CASE(simple_cw_poly_test)
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(0, 0),
            make_vector<double>(0, 2),
            make_vector<double>(2, 2),
            make_vector<double>(5, 0);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 7.);
    BOOST_CHECK(!is_inside(poly, make_vector<double>(-1, 1)));
    BOOST_CHECK(is_inside(poly, make_vector<double>(1, 1)));
}

BOOST_AUTO_TEST_CASE(triangle_test)
{
    triangle<3,double> tri(
        make_vector<double>(0, 0, 0),
        make_vector<double>(0, 0, 2),
        make_vector<double>(0, 1, 0));

    CRADLE_CHECK_ALMOST_EQUAL(get_normal(tri),
        make_vector<double>(-1, 0, 0));
}

BOOST_AUTO_TEST_CASE(square_test)
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-2, -2),
            make_vector<double>(-2,  2),
            make_vector<double>( 2,  2),
            make_vector<double>( 2, -2);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 16.);
    BOOST_CHECK(!is_inside(poly, make_vector<double>(-3, 3)));
    BOOST_CHECK(is_inside(poly, make_vector<double>(-1, 1)));
}

BOOST_AUTO_TEST_CASE(concave_poly_test)
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-2, -2),
            make_vector<double>(-2,  2),
            make_vector<double>( 2,  2),
            make_vector<double>( 0,  0),
            make_vector<double>( 2, -2);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 12.);
    BOOST_CHECK(!is_inside(poly, make_vector<double>(-3, 3)));
    BOOST_CHECK(!is_inside(poly, make_vector<double>(1, 0)));
    BOOST_CHECK(is_inside(poly, make_vector<double>(-1, 1)));
}

BOOST_AUTO_TEST_CASE(edge_view_test)
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(0, 0),
            make_vector<double>(0, 1),
            make_vector<double>(1, 1);
        initialize(&poly.vertices, vertices);
    }

    vertex2_array::const_iterator
        vertex_i = poly.vertices.begin(), vertex_j = poly.vertices.end() - 1;
    for (polygon2_edge_view ev(poly); !ev.done(); ev.advance(),
        vertex_j = vertex_i++)
    {
        BOOST_CHECK_EQUAL(ev.p0(), *vertex_j);
        BOOST_CHECK_EQUAL(ev.p1(), *vertex_i);
    }
}

BOOST_AUTO_TEST_CASE(circle_test)
{
    circle<double> circle(make_vector<double>(0, 0), 2);
    polygon2 poly = as_polygon(circle, 8);

    std::vector<vector<2,double> > correct_vertices;
    double srt = std::sqrt(2.);
    correct_vertices +=
        make_vector<double>(   2,    0),
        make_vector<double>( srt,  srt),
        make_vector<double>(   0,    2),
        make_vector<double>(-srt,  srt),
        make_vector<double>(  -2,    0),
        make_vector<double>(-srt, -srt),
        make_vector<double>(   0,   -2),
        make_vector<double>( srt, -srt);

    CRADLE_CHECK_RANGES_ALMOST_EQUAL(poly.vertices, correct_vertices);
}

BOOST_AUTO_TEST_CASE(box_test)
{
    box2d box(make_vector<double>(0, 0), make_vector<double>(2, 2));
    polygon2 poly = as_polygon(box);

    std::vector<vector<2,double> > correct_vertices;
    correct_vertices +=
        make_vector<double>(0, 0),
        make_vector<double>(2, 0),
        make_vector<double>(2, 2),
        make_vector<double>(0, 2);

    CRADLE_CHECK_RANGES_ALMOST_EQUAL(poly.vertices, correct_vertices);
}

// polyset

double tolerance = 0.00001;

BOOST_AUTO_TEST_CASE(two_polygons_test)
{
    polygon2 poly0, poly1;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-2, -2),
            make_vector<double>(-2,  2),
            make_vector<double>( 2,  2),
            make_vector<double>( 2, -2);
        initialize(&poly0.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(4, 4),
            make_vector<double>(4, 5),
            make_vector<double>(5, 5),
            make_vector<double>(5, 4);
        initialize(&poly1.vertices, vertices);
    }

    polyset region;
    add_polygon(region, poly0);
    add_polygon(region, poly1);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(region), 17., tolerance);

    BOOST_CHECK(!is_inside(region, make_vector<double>(-3, 3)));
    BOOST_CHECK(!is_inside(region, make_vector<double>(3, 3)));
    BOOST_CHECK(!is_inside(region, make_vector<double>(6, 6)));
    BOOST_CHECK(is_inside(region, make_vector<double>(-1, 1)));
    BOOST_CHECK(is_inside(region, make_vector<double>(4.5, 4.5)));
    BOOST_CHECK(is_inside(region, make_vector<double>(0, 0)));
}

BOOST_AUTO_TEST_CASE(polygon_with_hole_test)
{
    polygon2 poly, hole;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-2, -2),
            make_vector<double>(-2,  2),
            make_vector<double>( 2,  2),
            make_vector<double>( 2, -2);
        initialize(&poly.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-1, -1),
            make_vector<double>(-1,  1),
            make_vector<double>( 1,  1),
            make_vector<double>( 1, -1);
        initialize(&hole.vertices, vertices);
    }
    polyset frame;
    add_polygon(frame, poly);
    add_hole(frame, hole);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(frame), 12., tolerance);

    BOOST_CHECK(!is_inside(frame, make_vector<double>(-3, 3)));
    BOOST_CHECK(!is_inside(frame, make_vector<double>(0, 0)));
    BOOST_CHECK(is_inside(frame, make_vector<double>(-1.5, 1.5)));
}

BOOST_AUTO_TEST_CASE(polyset_as_polygon_list_test)
{
    polygon2 outside, hole, inside;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-3, -3),
            make_vector<double>(-3,  3),
            make_vector<double>( 3,  3),
            make_vector<double>( 3, -3);
        initialize(&outside.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-2, -2),
            make_vector<double>(-2,  2),
            make_vector<double>( 2,  2),
            make_vector<double>( 2, -2);
        initialize(&hole.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-1, -1),
            make_vector<double>(-1,  1),
            make_vector<double>( 1,  1),
            make_vector<double>( 1, -1);
        initialize(&inside.vertices, vertices);
    }

    polyset frame;
    add_polygon(frame, outside);
    add_hole(frame, hole);
    add_polygon(frame, inside);

    std::vector<polygon2> polys = as_polygon_list(frame);
    BOOST_CHECK_EQUAL(polys.size(), 2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(polys[0]) + get_area(polys[1]),
        get_area(frame), tolerance);

    polyset reconstructed;
    add_polygon(reconstructed, polys[0]);
    add_polygon(reconstructed, polys[1]);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(reconstructed), get_area(frame),
        tolerance);
}

BOOST_AUTO_TEST_CASE(polyset_set_operations_test)
{
    polygon2 p;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-6, -3),
            make_vector<double>(-6,  3),
            make_vector<double>( 6,  3),
            make_vector<double>( 6, -3);
        initialize(&p.vertices, vertices);
    }
    polyset wide_rect;
    create_polyset(&wide_rect, p);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(wide_rect), 72., tolerance);

    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-3, -6),
            make_vector<double>(-3,  6),
            make_vector<double>( 3,  6),
            make_vector<double>( 3, -6);
        initialize(&p.vertices, vertices);
    }
    polyset tall_rect;
    create_polyset(&tall_rect, p);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(tall_rect), 72., tolerance);

    polyset cross, square;
    do_set_operation(&cross, set_operation::UNION, wide_rect, tall_rect);
    do_set_operation(&square, set_operation::INTERSECTION, wide_rect, tall_rect);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(cross), 108., tolerance);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(square), 36., tolerance);

    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-1, -1),
            make_vector<double>(-1,  1),
            make_vector<double>( 1,  1),
            make_vector<double>( 1, -1);
        initialize(&p.vertices, vertices);
    }
    polyset small_square;
    create_polyset(&small_square, p);
    do_set_operation(&cross, set_operation::DIFFERENCE, cross, small_square);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(cross), 104., tolerance);
    BOOST_CHECK(!is_inside(cross, make_vector<double>(0, 0)));
    BOOST_CHECK(is_inside(cross, make_vector<double>(2, 0)));
    BOOST_CHECK(!is_inside(cross, make_vector<double>(8, 0)));
    BOOST_CHECK(is_inside(cross, make_vector<double>(0, -5)));
}

BOOST_AUTO_TEST_CASE(polyset_comparisons_test)
{
    polygon2 poly1, poly2;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-2, -2),
            make_vector<double>(-2,  2),
            make_vector<double>( 2,  2),
            make_vector<double>( 2, -2);
        initialize(&poly1.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-1, -1),
            make_vector<double>(-1,  1),
            make_vector<double>( 1,  1),
            make_vector<double>( 1, -1);
        initialize(&poly2.vertices, vertices);
    }

    polyset region1;
    add_polygon(region1, poly1);
    add_hole(region1, poly2);

    polyset region2;
    add_polygon(region2, poly1);
    add_hole(region2, poly2);

    polyset region3;
    add_polygon(region3, poly1);
    add_polygon(region3, poly2);

    BOOST_CHECK(almost_equal(region1, region2, 0.001));
    BOOST_CHECK(!almost_equal(region1, region3, 0.001));
}

// structure_geometry

BOOST_AUTO_TEST_CASE(structure_geometry_test0)
{
    polyset area0, area1, area2;

    add_polygon(area0, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 6),
        make_vector<double>(3, 3))));
    BOOST_CHECK_EQUAL(area0.polygons.size(), 1);

    add_polygon(area1, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(1, 0), make_vector<double>(0, 0),
        make_vector<double>(0, 1))));
    add_polygon(area1, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(1, 3), make_vector<double>(0, 3),
        make_vector<double>(0, 4))));
    BOOST_CHECK_EQUAL(area1.polygons.size(), 2);

    add_polygon(area2, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 1),
        make_vector<double>(1, 0))));
    add_polygon(area2, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(2, 0), make_vector<double>(2, 1),
        make_vector<double>(3, 0))));
    add_polygon(area2, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(4, 0), make_vector<double>(4, 1),
        make_vector<double>(5, 0))));
    BOOST_CHECK_EQUAL(area2.polygons.size(), 3);

    structure_geometry volume;
    volume.slices +=
        structure_geometry_slice(0.0, 1.5, area0),
        structure_geometry_slice(1.5, 1.5, area1),
        structure_geometry_slice(3.0, 1.5, area2);

    BOOST_CHECK(get_slice(volume, -0.8) == 0);
    BOOST_CHECK(get_slice(volume, -0.7) != 0);
    BOOST_CHECK(get_slice(volume, 3.7) != 0);
    BOOST_CHECK(get_slice(volume, 3.8) == 0);

    BOOST_CHECK_EQUAL(get_slice(volume, 1  )->position, 1.5);
    BOOST_CHECK_EQUAL(get_slice(volume, 1  )->region.polygons.size(), 2);
    BOOST_CHECK_EQUAL(get_slice(volume, 2.5)->position, 3);
    BOOST_CHECK_EQUAL(get_slice(volume, 2.5)->region.polygons.size(), 3);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume), 17.25, tolerance);
}

BOOST_AUTO_TEST_CASE(structure_geometry_test1)
{
    polyset area0, area1, area2, area3;

    add_polygon(area0, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 6),
        make_vector<double>(3, 3))));

    add_polygon(area1, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(1, 0), make_vector<double>(0, 0),
        make_vector<double>(0, 1))));
    add_polygon(area1, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(1, 3), make_vector<double>(0, 3),
        make_vector<double>(0, 4))));

    add_polygon(area2, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 1),
        make_vector<double>(1, 0))));
    add_polygon(area2, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(2, 0), make_vector<double>(2, 1),
        make_vector<double>(3, 0))));

    add_polygon(area3, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(4, 0), make_vector<double>(4, 1),
        make_vector<double>(5, 0))));

    structure_geometry volume1;
    volume1.slices +=
        structure_geometry_slice(1.0, 1, area1),
        structure_geometry_slice(2.0, 1, area2),
        structure_geometry_slice(2.5, 1, area3);

    structure_geometry volume2;
    volume2.slices +=
        structure_geometry_slice(0.0, 1, area0),
        structure_geometry_slice(1.0, 1, area1),
        structure_geometry_slice(2.0, 1, area2),
        structure_geometry_slice(2.5, 1, area3);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 11.125, tolerance);

    BOOST_CHECK(almost_equal(volume2, volume2, tolerance));
    BOOST_CHECK(!almost_equal(volume1, volume2, tolerance));
}

BOOST_AUTO_TEST_CASE(set_operation_test)
{
    polyset area0, area1, area2, area3, area4, area5;

    add_polygon(area0, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 5),
        make_vector<double>(4, 5), make_vector<double>(4, 0))));
    add_polygon(area1, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 5),
        make_vector<double>(4, 5), make_vector<double>(4, 0))));
    add_polygon(area2, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 5),
        make_vector<double>(4, 5), make_vector<double>(4, 0))));

    add_polygon(area3, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(6, 0), make_vector<double>(6, 5),
        make_vector<double>(10, 5), make_vector<double>(10, 0))));
    add_polygon(area4, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(2, 0), make_vector<double>(2, 5),
        make_vector<double>(6, 5), make_vector<double>(6, 0))));
    add_polygon(area5, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(0, 5),
        make_vector<double>(4, 5), make_vector<double>(4, 0))));

    {
    structure_geometry volume1;
    volume1.slices +=
        structure_geometry_slice(1.0, 2, area0),
        structure_geometry_slice(3.0, 2, area1),
        structure_geometry_slice(4.5, 1, area2),
        structure_geometry_slice(6.0, 2, polyset());
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

    structure_geometry volume2;
    volume2.slices +=
        structure_geometry_slice(1.0, 2, polyset()),
        structure_geometry_slice(3.0, 2, area3),
        structure_geometry_slice(4.5, 1, area4),
        structure_geometry_slice(6.0, 2, area5);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

    structure_geometry union_;
    do_set_operation(&union_, set_operation::UNION, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 190., tolerance);

    structure_geometry intersection;
    do_set_operation(&intersection, set_operation::INTERSECTION, volume1,
        volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(intersection), 10., tolerance);

    structure_geometry xor_;
    do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 180., tolerance);

    structure_geometry difference;
    do_set_operation(&difference, set_operation::DIFFERENCE, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 90., tolerance);
    }

    {
    structure_geometry volume;
    volume.slices +=
        structure_geometry_slice(1.0, 2, area0),
        structure_geometry_slice(3.0, 2, area1),
        structure_geometry_slice(4.5, 1, area2),
        structure_geometry_slice(6.0, 2, polyset());

    structure_geometry result;

    structure_geometry mismatched_volume1;
    mismatched_volume1.slices +=
        structure_geometry_slice(1.0, 2, polyset()),
        structure_geometry_slice(3.0, 2, area3),
        structure_geometry_slice(4.5, 1, area4);
    BOOST_CHECK_THROW(
        do_set_operation(&result, set_operation::UNION,
            volume, mismatched_volume1),
        std::exception);

    structure_geometry mismatched_volume2;
    mismatched_volume2.slices +=
        structure_geometry_slice(1.0, 2, polyset()),
        structure_geometry_slice(3.0, 2, area1),
        structure_geometry_slice(4.5, 1, area2),
        structure_geometry_slice(6.0, 1, area3);
    BOOST_CHECK_THROW(
        do_set_operation(&result, set_operation::UNION,
            volume, mismatched_volume2),
        std::exception);

    structure_geometry mismatched_volume3;
    mismatched_volume3.slices +=
        structure_geometry_slice(1.1, 2, polyset()),
        structure_geometry_slice(3.0, 2, area1),
        structure_geometry_slice(4.5, 1, area2),
        structure_geometry_slice(6.0, 2, area3);
    BOOST_CHECK_THROW(
        do_set_operation(&result, set_operation::UNION,
            volume, mismatched_volume3),
        std::exception);
    }

    {
    structure_geometry volume1;
    volume1.slices +=
        structure_geometry_slice(1.0, 2, polyset()),
        structure_geometry_slice(3.0, 2, area0),
        structure_geometry_slice(4.5, 1, area1),
        structure_geometry_slice(6.0, 2, area2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

    structure_geometry volume2;
    volume2.slices +=
        structure_geometry_slice(1.0, 2, area3),
        structure_geometry_slice(3.0, 2, area4),
        structure_geometry_slice(4.5, 1, area5),
        structure_geometry_slice(6.0, 2, polyset());
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

    structure_geometry union_;
    do_set_operation(&union_, set_operation::UNION, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 160., tolerance);

    structure_geometry intersection;
    do_set_operation(&intersection, set_operation::INTERSECTION, volume1,
        volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(intersection), 40., tolerance);

    structure_geometry xor_;
    do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 120., tolerance);

    structure_geometry difference;
    do_set_operation(&difference, set_operation::DIFFERENCE, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 60., tolerance);
    }

    {
    structure_geometry volume1;
    volume1.slices +=
        structure_geometry_slice(1.0, 2, polyset()),
        structure_geometry_slice(3.0, 2, area0),
        structure_geometry_slice(4.5, 1, area1),
        structure_geometry_slice(6.0, 2, area2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

    structure_geometry volume2;
    volume2.slices +=
        structure_geometry_slice(1.0, 2, area3),
        structure_geometry_slice(3.0, 2, area4),
        structure_geometry_slice(4.5, 1, area5),
        structure_geometry_slice(6.0, 2, polyset());
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

    structure_geometry union_;
    do_set_operation(&union_, set_operation::UNION, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 160., tolerance);

    structure_geometry intersection;
    do_set_operation(&intersection, set_operation::INTERSECTION, volume1,
        volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(intersection), 40., tolerance);

    structure_geometry xor_;
    do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 120., tolerance);

    structure_geometry difference;
    do_set_operation(&difference, set_operation::DIFFERENCE, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 60., tolerance);
    }

    {
    structure_geometry volume1;
    volume1.slices +=
        structure_geometry_slice(1.0, 2, area0),
        structure_geometry_slice(3.0, 2, polyset()),
        structure_geometry_slice(4.5, 1, area1),
        structure_geometry_slice(6.0, 2, area2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

    structure_geometry volume2;
    volume2.slices +=
        structure_geometry_slice(1.0, 2, area3),
        structure_geometry_slice(3.0, 2, area4),
        structure_geometry_slice(4.5, 1, area5),
        structure_geometry_slice(6.0, 2, polyset());
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

    structure_geometry union_;
    do_set_operation(&union_, set_operation::UNION, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 180., tolerance);

    structure_geometry intersection;
    do_set_operation(&intersection, set_operation::INTERSECTION, volume1,
        volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(intersection), 20., tolerance);

    structure_geometry xor_;
    do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 160., tolerance);

    structure_geometry difference;
    do_set_operation(&difference, set_operation::DIFFERENCE, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 80., tolerance);
    }

    {
    structure_geometry volume1;
    volume1.slices +=
        structure_geometry_slice(1.0, 2, area0),
        structure_geometry_slice(3.0, 2, area1),
        structure_geometry_slice(4.5, 1, area2),
        structure_geometry_slice(6.0, 2, polyset());
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

    structure_geometry volume2;
    volume2.slices +=
        structure_geometry_slice(1.0, 2, area3),
        structure_geometry_slice(3.0, 2, polyset()),
        structure_geometry_slice(4.5, 1, area4),
        structure_geometry_slice(6.0, 2, area5);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

    structure_geometry union_;
    do_set_operation(&union_, set_operation::UNION, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 190., tolerance);

    structure_geometry intersection;
    do_set_operation(&intersection, set_operation::INTERSECTION, volume1,
        volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(intersection), 10., tolerance);

    structure_geometry xor_;
    do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 180., tolerance);

    structure_geometry difference;
    do_set_operation(&difference, set_operation::DIFFERENCE, volume1, volume2);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 90., tolerance);
    }
}

BOOST_AUTO_TEST_CASE(expansion_2d)
{
    polyset area0, area1, area2, area3;

    add_polygon(area1, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(6, 0),
        make_vector<double>(6, 6), make_vector<double>(0, 6))));

    add_polygon(area2, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(1, 0),
        make_vector<double>(1, 1), make_vector<double>(0, 1))));

    structure_geometry original;
    original.slices +=
        structure_geometry_slice(1, 1, area1),
        structure_geometry_slice(2, 1, area2);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(original), 37., tolerance);

    structure_geometry expanded;
    expand_in_2d(&expanded, original, 1.);

    BOOST_CHECK(get_volume(expanded) > 65);
    BOOST_CHECK(get_volume(expanded) < 83);
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6.9, 1, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.9, 1, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1, 6.9, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1, -0.9, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.7, -0.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6.7, -0.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6.7, 6.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.7, 6.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1.9, 1, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.9, 1, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1, 1.9, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1, -0.9, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.7, -0.7, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1.7, -0.7, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1.7, 1.7, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.7, 1.7, 2)));
}

BOOST_AUTO_TEST_CASE(expansion_3d)
{
    polyset area0, area1, area2, area3;

    add_polygon(area1, make_polygon2(anonymous<std::vector<vector2d> >(
        make_vector<double>(0, 0), make_vector<double>(6, 0),
        make_vector<double>(6, 6), make_vector<double>(0, 6))));

    structure_geometry original;
    original.slices +=
        structure_geometry_slice(-1, 1, polyset()),
        structure_geometry_slice( 0, 1, polyset()),
        structure_geometry_slice( 1, 1, area1),
        structure_geometry_slice( 2, 1, polyset()),
        structure_geometry_slice( 3, 1, polyset());

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(original), 36., tolerance);

    structure_geometry expanded;
    expand_in_3d(&expanded, original, 1.);

    BOOST_CHECK(get_volume(expanded) > 136);
    BOOST_CHECK(get_volume(expanded) < 197);
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6.9, 1, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.9, 1, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1, 6.9, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(1, -0.9, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.7, -0.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6.7, -0.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6.7, 6.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(-0.7, 6.7, 1)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(0, 0, 0)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6, 0, 0)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6, 6, 0)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(0, 6, 0)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(0, 0, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6, 0, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(6, 6, 2)));
    BOOST_CHECK(is_inside(expanded, make_vector<double>(0, 6, 2)));

    structure_geometry contracted;
    expand_in_3d(&contracted, original, -1.);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(contracted), 0., tolerance);
}

BOOST_AUTO_TEST_CASE(polygon_bounding_box_test)
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(0, 0),
            make_vector<double>(-1, -1),
            make_vector<double>(-3, 0),
            make_vector<double>(0, 7),
            make_vector<double>(3, 3),
            make_vector<double>(3, 2);
        initialize(&poly.vertices, vertices);
    }
    BOOST_CHECK_EQUAL(bounding_box(poly),
        box2d(make_vector<double>(-3, -1), make_vector<double>(6, 8)));
}

BOOST_AUTO_TEST_CASE(polyset_bounding_box_test)
{
    polygon2 p;
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(-6, -3),
            make_vector<double>(-6, 3),
            make_vector<double>(-4, 3),
            make_vector<double>(-4, -3);
        initialize(&p.vertices, vertices);
    }
    polyset area;
    create_polyset(&area, p);
    {
        std::vector<vertex2> vertices;
        vertices +=
            make_vector<double>(2, -6),
            make_vector<double>(2, -2),
            make_vector<double>(4, -2),
            make_vector<double>(4, -6);
        initialize(&p.vertices, vertices);
    }
    add_polygon(area, p);

    box<2,double> bb = bounding_box(area);
    BOOST_CHECK(almost_equal(bb.corner, make_vector<double>(-6, -6), 0.00001));
    BOOST_CHECK(almost_equal(bb.size, make_vector<double>(10, 9), 0.00001));
}


#include "k_points_util.h"

#include <algorithm>
#include <boost/functional/hash.hpp>
#include <cmath>
#include <exception>
#include <unordered_set>
#include "../array_math.h"

int KPointsUtil::get_n_k_points(const double rcut) {
  int count = 0;
  const int n_max = std::floor(static_cast<float>(rcut));
  double rcut_square = rcut * rcut;
  for (int i = -n_max; i <= n_max; i++) {
    for (int j = -n_max; j <= n_max; j++) {
      for (int k = -n_max; k <= n_max; k++) {
        if (i * i + j * j + k * k > rcut_square) continue;
        count++;
      }
    }
  }
  return count;
}

std::vector<std::array<int8_t, 3>> KPointsUtil::generate_k_points(
    const double rcut) {
  std::vector<std::array<int8_t, 3>> k_points;
  const int n_max = std::floor(static_cast<float>(rcut));
  assert(n_max < 127);
  double rcut_square = rcut * rcut;
  for (int i = -n_max; i <= n_max; i++) {
    for (int j = -n_max; j <= n_max; j++) {
      for (int k = -n_max; k <= n_max; k++) {
        if (i * i + j * j + k * k > rcut_square) continue;
        k_points.push_back(std::array<int8_t, 3>({static_cast<int8_t>(i),
                                                  static_cast<int8_t>(j),
                                                  static_cast<int8_t>(k)}));
      }
    }
  }
  std::stable_sort(
      k_points.begin(),
      k_points.end(),
      [](const auto& a, const auto& b) -> bool {
        return squared_norm(a) < squared_norm(b);
      });
  return k_points;
}

std::vector<std::array<int8_t, 3>> KPointsUtil::get_k_diffs(
    const std::vector<std::array<int8_t, 3>>& k_points) {
  // Generate all possible differences between two different k points.
  std::unordered_set<std::array<int8_t, 3>, boost::hash<std::array<int8_t, 3>>>
      k_diffs_set;
  std::vector<std::array<int8_t, 3>> k_diffs;
  const int n_k_points = k_points.size();
  for (int p = 0; p < n_k_points; p++) {
    for (int q = 0; q < n_k_points; q++) {
      if (p == q) continue;
      const auto& diff_pq = k_points[q] - k_points[p];
      if (k_diffs_set.count(diff_pq) == 1) continue;
      k_diffs.push_back(diff_pq);
      k_diffs_set.insert(diff_pq);
    }
  }

  // Sort k_diffs into ascending order so that later sorting hci queue will be
  // faster.
  std::stable_sort(
      k_diffs.begin(), k_diffs.end(), [](const auto& a, const auto& b) -> bool {
        return squared_norm(a) < squared_norm(b);
      });

  return k_diffs;
}


#ifndef COORDINATE_CALCULATION
#define COORDINATE_CALCULATION

#include "util/coordinate.hpp"

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

#include <algorithm>
#include <cmath>
#include <numeric>
#include <utility>
#include <vector>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

namespace detail
{
const constexpr double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
const constexpr double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;

inline double degToRad(const double degree)
{
    using namespace boost::math::constants;
    return degree * (pi<double>() / 180.0);
}

inline double radToDeg(const double radian)
{
    using namespace boost::math::constants;
    return radian * (180.0 * (1. / pi<double>()));
}
}

//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs);

double fccApproximateDistance(const Coordinate first_coordinate,
                              const Coordinate second_coordinate);

double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

// get the length of a full coordinate vector, using one of our basic functions to compute distances
template <class BinaryOperation, typename iterator_type>
double getLength(iterator_type begin, const iterator_type end, BinaryOperation op);

// Find the closest distance and location between coordinate and the line connecting source and
// target:
//             coordinate
//                 |
//                 |
// source -------- x -------- target.
// returns x as well as the distance between source and x as ratio ([0,1])
inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate);

// find the closest distance between a coordinate and a segment
// O(1)
double findClosestDistance(const Coordinate coordinate,
                           const Coordinate segment_begin,
                           const Coordinate segment_end);

// find the closest distance between a coordinate and a set of coordinates
// O(|coordinates|)
template <typename iterator_type>
double findClosestDistance(const Coordinate coordinate,
                           const iterator_type begin,
                           const iterator_type end);

// find the closes distance between two sets of coordinates
// O(|lhs| * |rhs|)
template <typename iterator_type>
double findClosestDistance(const iterator_type lhs_begin,
                           const iterator_type lhs_end,
                           const iterator_type rhs_begin,
                           const iterator_type rhs_end);

// checks if two sets of coordinates describe a parallel set of ways
template <typename iterator_type>
bool areParallel(const iterator_type lhs_begin,
                 const iterator_type lhs_end,
                 const iterator_type rhs_begin,
                 const iterator_type rhs_end);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio);

Coordinate centroid(const Coordinate lhs, const Coordinate rhs);

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate);

// Get angle of line segment (A,C)->(C,B)
double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third);

// find the center of a circle through three coordinates
boost::optional<Coordinate> circleCenter(const Coordinate first_coordinate,
                                         const Coordinate second_coordinate,
                                         const Coordinate third_coordinate);

// find the radius of a circle through three coordinates
double circleRadius(const Coordinate first_coordinate,
                    const Coordinate second_coordinate,
                    const Coordinate third_coordinate);

// factor in [0,1]. Returns point along the straight line between from and to. 0 returns from, 1
// returns to
Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to);

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate);

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate);

template <typename iterator_type>
std::pair<Coordinate, Coordinate> leastSquareRegression(const iterator_type begin,
                                                        const iterator_type end);

// rotates a coordinate around the point (0,0). This function can be used to normalise a few
// computations around regression vectors
Coordinate rotateCCWAroundZero(Coordinate coordinate, double angle_in_radians);

// compute the difference vector of two coordinates lhs - rhs
Coordinate difference(const Coordinate lhs, const Coordinate rhs);

// TEMPLATE/INLINE DEFINITIONS
inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate)
{
    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
    // dot product of two un-normed vectors
    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
    // squared length of the slope vector
    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
                                static_cast<double>(slope_vector.lat * slope_vector.lat);

    if (squared_length < std::numeric_limits<double>::epsilon())
    {
        return {0, source};
    }

    const double normed_ratio = unnormed_ratio / squared_length;
    double clamped_ratio = normed_ratio;
    if (clamped_ratio > 1.)
    {
        clamped_ratio = 1.;
    }
    else if (clamped_ratio < 0.)
    {
        clamped_ratio = 0.;
    }

    return {clamped_ratio,
            {
                FloatLongitude{1.0 - clamped_ratio} * source.lon +
                    target.lon * FloatLongitude{clamped_ratio},
                FloatLatitude{1.0 - clamped_ratio} * source.lat +
                    target.lat * FloatLatitude{clamped_ratio},
            }};
}

template <class BinaryOperation, typename iterator_type>
double getLength(iterator_type begin, const iterator_type end, BinaryOperation op)
{
    double result = 0;
    const auto functor = [&result, op](const Coordinate lhs, const Coordinate rhs) {
        result += op(lhs, rhs);
        return false;
    };
    // side-effect find adding up distances
    std::adjacent_find(begin, end, functor);

    return result;
}

template <typename iterator_type>
double
findClosestDistance(const Coordinate coordinate, const iterator_type begin, const iterator_type end)
{
    double current_min = std::numeric_limits<double>::max();

    // comparator updating current_min without ever finding an element
    const auto compute_minimum_distance = [&current_min, coordinate](const Coordinate lhs,
                                                                     const Coordinate rhs) {
        current_min = std::min(current_min, findClosestDistance(coordinate, lhs, rhs));
        return false;
    };

    std::adjacent_find(begin, end, compute_minimum_distance);
    return current_min;
}

template <typename iterator_type>
double findClosestDistance(const iterator_type lhs_begin,
                           const iterator_type lhs_end,
                           const iterator_type rhs_begin,
                           const iterator_type rhs_end)
{
    double current_min = std::numeric_limits<double>::max();

    const auto compute_minimum_distance_in_rhs = [&current_min, rhs_begin, rhs_end](
        const Coordinate coordinate) {
        current_min = std::min(current_min, findClosestDistance(coordinate, rhs_begin, rhs_end));
        return false;
    };

    std::find_if(lhs_begin, lhs_end, compute_minimum_distance_in_rhs);
    return current_min;
}

template <typename iterator_type>
std::pair<Coordinate, Coordinate> leastSquareRegression(const iterator_type begin,
                                                        const iterator_type end)
{
    // following the formulas of https://faculty.elgin.edu/dkernler/statistics/ch04/4-2.html
    const auto number_of_coordinates = std::distance(begin, end);
    BOOST_ASSERT(number_of_coordinates >= 2);
    const auto extract_lon = [](const Coordinate coordinate) {
        return static_cast<double>(toFloating(coordinate.lon));
    };

    const auto extract_lat = [](const Coordinate coordinate) {
        return static_cast<double>(toFloating(coordinate.lat));
    };

    double min_lon = extract_lon(*begin);
    double max_lon = extract_lon(*begin);
    double min_lat = extract_lat(*begin);
    double max_lat = extract_lat(*begin);

    for (auto coordinate_iterator = begin; coordinate_iterator != end; ++coordinate_iterator)
    {
        const auto c = *coordinate_iterator;
        const auto lon = extract_lon(c);
        min_lon = std::min(min_lon, lon);
        max_lon = std::max(max_lon, lon);
        const auto lat = extract_lat(c);
        min_lat = std::min(min_lat, lat);
        max_lat = std::max(max_lat, lat);
    }
    // very small difference in longitude -> would result in inaccurate calculation, check if lat is
    // better
    if ((max_lat - min_lat) > 2 * (max_lon - min_lon))
    {
        std::vector<util::Coordinate> rotated_coordinates(number_of_coordinates);
        // rotate all coordinates to the right
        std::transform(begin, end, rotated_coordinates.begin(), [](const auto coordinate) {
            return rotateCCWAroundZero(coordinate, detail::degToRad(-90));
        });
        const auto rotated_regression =
            leastSquareRegression(rotated_coordinates.begin(), rotated_coordinates.end());
        return {rotateCCWAroundZero(rotated_regression.first, detail::degToRad(90)),
                rotateCCWAroundZero(rotated_regression.second, detail::degToRad(90))};
    }

    const auto make_accumulate = [](const auto extraction_function) {
        return [extraction_function](const double sum_so_far, const Coordinate coordinate) {
            return sum_so_far + extraction_function(coordinate);
        };
    };

    const auto accumulated_lon = std::accumulate(begin, end, 0., make_accumulate(extract_lon));

    const auto accumulated_lat = std::accumulate(begin, end, 0., make_accumulate(extract_lat));

    const auto mean_lon = accumulated_lon / number_of_coordinates;
    const auto mean_lat = accumulated_lat / number_of_coordinates;
    const auto make_variance = [](const auto mean, const auto extraction_function) {
        return [extraction_function, mean](const double sum_so_far, const Coordinate coordinate) {
            const auto difference = extraction_function(coordinate) - mean;
            return sum_so_far + difference * difference;
        };
    };

    // using the unbiased version, we divide by num_samples - 1 (see
    // http://mathworld.wolfram.com/SampleVariance.html)
    const auto sample_variance_lon =
        std::sqrt(std::accumulate(begin, end, 0., make_variance(mean_lon, extract_lon)) /
                  (number_of_coordinates - 1));

    // if we don't change longitude, return the vertical line as is
    if (std::abs(sample_variance_lon) <
        std::numeric_limits<decltype(sample_variance_lon)>::epsilon())
        return {*begin, *(end - 1)};

    const auto sample_variance_lat =
        std::sqrt(std::accumulate(begin, end, 0., make_variance(mean_lat, extract_lat)) /
                  (number_of_coordinates - 1));

    if (std::abs(sample_variance_lat) <
        std::numeric_limits<decltype(sample_variance_lat)>::epsilon())
        return {*begin, *(end - 1)};
    const auto linear_correlation =
        std::accumulate(begin,
                        end,
                        0.,
                        [&](const auto sum_so_far, const auto current_coordinate) {
                            return sum_so_far +
                                   (extract_lon(current_coordinate) - mean_lon) *
                                       (extract_lat(current_coordinate) - mean_lat) /
                                       (sample_variance_lon * sample_variance_lat);
                        }) /
        (number_of_coordinates - 1);

    const auto slope = linear_correlation * sample_variance_lat / sample_variance_lon;
    const auto intercept = mean_lat - slope * mean_lon;

    const auto GetLatAtLon = [intercept,
                              slope](const util::FloatLongitude longitude) -> util::FloatLatitude {
        return {intercept + slope * static_cast<double>((longitude))};
    };

    const double offset = 0.00001;
    const Coordinate regression_first = {
        toFixed(util::FloatLongitude{min_lon - offset}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{min_lon - offset})))};
    const Coordinate regression_end = {
        toFixed(util::FloatLongitude{max_lon + offset}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{max_lon + offset})))};

    return {regression_first, regression_end};
}

template <typename iterator_type>
bool areParallel(const iterator_type lhs_begin,
                 const iterator_type lhs_end,
                 const iterator_type rhs_begin,
                 const iterator_type rhs_end)
{
    const auto regression_lhs = leastSquareRegression(lhs_begin, lhs_end);
    const auto regression_rhs = leastSquareRegression(rhs_begin, rhs_end);

    const auto null_island = Coordinate(FixedLongitude{0}, FixedLatitude{0});
    const auto difference_lhs = difference(regression_lhs.first, regression_lhs.second);
    const auto difference_rhs = difference(regression_rhs.first, regression_rhs.second);

    // we normalise the left slope to be zero, so we rotate the coordinates around 0,0 to match 90
    // degrees
    const auto bearing_lhs = bearing(null_island, difference_lhs);

    // we rotate to have one of the lines facing horizontally to the right (bearing 90 degree)
    const auto rotation_angle_radians = detail::degToRad(bearing_lhs - 90);
    const auto rotated_difference_rhs = rotateCCWAroundZero(difference_rhs, rotation_angle_radians);

    const auto get_slope = [](const Coordinate from, const Coordinate to) {
        const auto diff_lat = static_cast<int>(from.lat) - static_cast<int>(to.lat);
        const auto diff_lon = static_cast<int>(from.lon) - static_cast<int>(to.lon);
        if (diff_lon == 0)
            return std::numeric_limits<double>::max();
        return static_cast<double>(diff_lat) / static_cast<double>(diff_lon);
    };

    const auto slope_rhs = get_slope(null_island, rotated_difference_rhs);
    // the left hand side has a slope of `0` after the rotation. We can check the slope of the right
    // hand side to ensure we only considering slight slopes
    return std::abs(slope_rhs) < 0.20; // twenty percent incline at the most
}

double computeArea(const std::vector<Coordinate> &polygon);

} // ns coordinate_calculation
} // ns util
} // ns osrm

#endif // COORDINATE_CALCULATION


// 2018-03-24
#include <iostream>
#include <time.h>
#include <boost/multiprecision/gmp.hpp>

using namespace std;
using namespace boost::multiprecision;

mpz_int func10(int n)
{
  mpz_int it = 1;
  mpz_int sum = 1;
  for (int i = 0; i < 10; i++)
  {
    it *= (-n);
    sum += it;
  }
  return sum;
}

mpz_int

void work()
{
  for (int i = 0; i < 20; i++)
  {
    cout << func10(i) << endl;
  }
}

int main(int argc, char const *argv[])
{
  clock_t t1,t2;
  t1=clock();
  work();
  t2=clock();
  float diff ((float)t2-(float)t1);
  cout << "Running time (s): " << diff/CLOCKS_PER_SEC << endl;
  return 0;
}


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

//[ guide_axis_with_transform

#include <boost/histogram/axis/regular.hpp>
#include <limits>

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

  // make a regular axis with a log transform over [10, 100), [100, 1000), [1000, 10000)
  axis::regular<double, axis::transform::log> r_log{3, 10., 10000.};
  // log transform:
  // - useful when values vary dramatically in magnitude, like brightness of stars
  // - edges are not exactly at 10, 100, 1000, because of finite floating point precision
  // - value >= 0 below is mapped to -1
  // - value < 0 below is mapped to `size()`, because the result of std::log is NaN
  assert(r_log.index(10.1) == 0);
  assert(r_log.index(100.1) == 1);
  assert(r_log.index(1000.1) == 2);
  assert(r_log.index(1) == -1);
  assert(r_log.index(0) == -1);
  assert(r_log.index(-1) == 3);

  // make a regular axis with a sqrt transform over [4, 9), [9, 16), [16, 25)
  axis::regular<double, axis::transform::sqrt> r_sqrt{3, 4., 25.};
  // sqrt transform:
  // - bin widths are more mildly increasing compared to log transform
  // - starting axis at value == 0 is ok, sqrt(0) == 0 unlike log transform
  // - value < 0 is mapped to `size()`, because the result of std::sqrt is NaN
  assert(r_sqrt.index(0) == -1);
  assert(r_sqrt.index(4.1) == 0);
  assert(r_sqrt.index(9.1) == 1);
  assert(r_sqrt.index(16.1) == 2);
  assert(r_sqrt.index(25.1) == 3);
  assert(r_sqrt.index(-1) == 3);

  // make a regular axis with a power transform x^1/3 over [1, 8), [8, 27), [27, 64)
  using pow_trans = axis::transform::pow;
  axis::regular<double, pow_trans> r_pow(pow_trans{1. / 3.}, 3, 1., 64.);
  // pow transform:
  // - generalization of the sqrt transform, power index is first argument of ctor
  // - starting the axis at value == 0 is ok, 0^p == 0 for p != 0
  // - value < 0 is mapped to `size()`, because the result of std::pow is NaN
  assert(r_pow.index(0) == -1);
  assert(r_pow.index(1.1) == 0);
  assert(r_pow.index(8.1) == 1);
  assert(r_pow.index(27.1) == 2);
  assert(r_pow.index(64.1) == 3);
  assert(r_pow.index(-1) == 3);
}

//]


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             : 视觉SLAM十四讲/ch4 学习记录
 *                           李代数库
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2018.03.26, William Yu

=================================================================================*/
/// Include files
#include <iostream>
#include <cmath>
using namespace std;

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

#include "sophus/so3.h"
#include "sophus/se3.h"

/// Global Variables

/// Function definitions

/**
 * @function main
 * @author William Yu
 * @brief
 * @param  None
 * @retval None
 */
int main( int argc, char** argv )
{
    //--------------------------------------------------------------------------------------------------
    //  旋转矩阵
    //--------------------------------------------------------------------------------------
    // 沿Z轴转90度的旋转矩阵
    // 　                                  角度　　                  轴　　罗德里格公式进行转换为旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    //--------------------------------------------------------------------------------------------------
    //  李群
    //--------------------------------------------------------------------------------------
    Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造
    Eigen::Quaterniond q(R);            // 或者四元数
    Sophus::SO3 SO3_q( q );
    // 上述表达方式都是等价的
    // 输出SO(3)时，以so(3)形式输出
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from vector: "<<SO3_v<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;

    //--------------------------------------------------------------------------------------------------
    //  李代数
    //--------------------------------------------------------------------------------------
    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // hat 为向量到反对称矩阵
    cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
    // 相对的，vee为反对称到向量
    cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些

    //--------------------------------------------------------------------------------------------------
    //  李代数求导 更新
    //--------------------------------------------------------------------------------------
    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 updated = "<<SO3_updated<<endl;

    /********************我是萌萌的分割线*****萌萌是谁？************************/
    //说实话，在这份代码里面你可能经常看到一些略微卖萌的成分，
    //要么是高翔博士的原版代码在卖萌，
    //要么是我被高博的代码带上了一条不归路，总之，开心就好：）

    cout<<"************我是分割线*************"<<endl;

    //--------------------------------------------------------------------------------------------------
    //  特殊欧式群  变换矩阵群
    //--------------------------------------------------------------------------------------
    // 对SE(3)操作大同小异
    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    // 李代数se(3) 是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;

    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

    return 0;
}