//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
/**
* @file
* @author Christian Winne
* @ingroup openOR_core
*/

#ifndef openOR_core_math_create_hpp
#define openOR_core_math_create_hpp

#include <openOR/Math/vector.hpp>
#include <openOR/Math/matrix.hpp>
#include <openOR/Math/matrixsetaxis.hpp>

#include <boost/mpl/assert.hpp>

namespace openOR {
	namespace Math {

		namespace Impl {
			/**
			* Functor for Impl::VectorCompileIterator in order to copy the content of source vector into destination vector
			* \internal
			*/
			template <class Vec, class VecSrc, int I>
			struct VectorCopy {
				void operator()(Vec& vec, const VecSrc& vecSrc) {
					get<I>(vec) = get<I>(vecSrc);
				}
			};

			template <class Mat, class MatSrc, int I, int J>
			struct MatrixCopy {
				void operator()(Mat& mat, const MatSrc& matSrc) {
					get<I, J>(mat) = get<I, J>(matSrc);
				}
			};

			/**
			* v0 is an vector because the destination vector is larger than Dimension 2.
			* \internal
			*/
			template<class Vec_type, class Type2, int Dim>
			struct CreateVec2 {
				static Vec_type create(const Type2& v0,
					typename VectorTraits<Vec_type>::ValueType v1)
				{
					BOOST_MPL_ASSERT((typename VectorTraits<Vec_type>::IsVector));
					Vec_type t;
					Impl::VectorCompileTimeIterator < Vec_type, 0, VectorTraits<Vec_type>::Dimension::value - 1 > ()
						.template apply<Impl::VectorCopy, Type2>(t, v0);
					get < VectorTraits<Vec_type>::Dimension::value - 1 > (t) = v1;
					return t;
				}
			};

			/**
			* v0 is a ValueType because the vector to create has dimension 2.
			* \internal
			*/
			template<class Type, class Type2>
			struct CreateVec2<Type, Type2, 2> {
				static Type create(const Type2& v0,
					typename VectorTraits<Type>::ValueType v1) {
						Type t;
						get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
						get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
						return t;
				}
			};

			template<class Mat, class MatSrc, class Vec>
			struct CreateMat2 {
				static Mat create(
					const MatSrc& matSrc,
					const Vec& v)
				{
					BOOST_MPL_ASSERT((typename MatrixTraits<Mat>::IsMatrix));
					BOOST_MPL_ASSERT((typename VectorTraits<Vec>::IsVector));
					Mat mat = MatrixTraits<Mat>::IDENTITY;
					Impl::MatrixCompileTimeIterator <Mat, 0, MatrixTraits<Mat>::RowDimension::value - 1, 0, MatrixTraits<Mat>::ColDimension::value - 1> ()
						.template apply<Impl::MatrixCopy, MatSrc>(mat, matSrc);

					for (int i = 0; i < VectorTraits<Vec>::Dimension::value; ++i)
					{
						mat(i, MatrixTraits<Mat>::ColDimension::value - 1) = v(i);
					}
					return mat;
				}
			};

			/**
			*
			* \internal
			*/
			template<class Type, class Type2, class Type3, class IsVector>
			struct Create2 {
				static Type create(const Type2& v0, const Type3& v1)
				{
					return CreateVec2<Type, Type2, VectorTraits<Type>::Dimension::value>::create(v0, v1);
				}
			};

			/**
			*
			* \internal
			*/
			template<class Type, class Type2, class Type3>
			struct Create2<Type, Type2, Type3, boost::mpl::false_> {
				static Type create(const Type2& v0, const Type3& v1)
				{
					return CreateMat2<Type, Type2, Type3>::create(v0, v1);
				}
			};

			/**
			* Type is a vector
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4, class IsVector>
			struct Create3 {
				static Type create(const Type2& v0,
					const Type3& v1,
					const Type4& v2) {
						BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
						Type t;
						get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
						get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
						get<2>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v2);
						return t;
				}
			};

			/**
			* Type is matrix
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4>
			struct Create3<Type, Type2, Type3, Type4, boost::mpl::false_> {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2) {
					BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));
					Type t = MatrixTraits<Type>::IDENTITY;
					setXAxis(t, v0);
					setYAxis(t, v1);
					setZAxis(t, v2);
					return t;
				}
			};

			/**
			* Type is a vector
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4, class Type5, class IsVector>
			struct Create4 {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3) {
					BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
					Type t;
					get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
					get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
					get<2>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v2);
					get<3>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v3);
					return t;
				}
			};

			/**
			* Type is matrix
			* \internal
			*/
			template<class Type, class Type2, class Type3, class Type4, class Type5>
			struct Create4<Type, Type2, Type3, Type4, Type5, boost::mpl::false_> {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3) {
					Type t = MatrixTraits<Type>::IDENTITY;
					setXAxis(t, v0);
					setYAxis(t, v1);
					setZAxis(t, v2);
					setTranslation(t, v3);
					return t;
				}
			};

			template<class Type, class Type2, class Type3, class Type4, class Type5, class Type6, class Type7, class IsVector>
			struct Create6 {
				static Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3, const Type6& v4, const Type7& v5) {
					BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
					Type t;
					get<0>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v0);
					get<1>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v1);
					get<2>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v2);
					get<3>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v3);
					get<4>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v4);
					get<5>(t) = static_cast<typename VectorTraits<Type>::ValueType>(v5);
					return t;
				}
			};
		}

		/**
		* \brief create
		* \ingroup openOR_core
		*/
		template <class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>))
			((Concept::ConstVector<Vec>)),
			(Type))
			create(const Vec& vec) {
				Type mat = MatrixTraits<Type>::IDENTITY;
				setTranslation(mat, vec);
				return mat;
		}

		/**
		* @brief Creation of a vector with two parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3>
		inline Type create(const Type2& v0,
			const Type3& v1) {
				return Impl::Create2<Type, Type2, Type3, typename VectorTraits<Type>::IsVector>::create(v0, v1);
		}

		/**
		* @brief Creation of a vector with three parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3, class Type4>
		inline Type create(const Type2& v0,
			const Type3& v1,
			const Type4& v2) {
				return Impl::Create3<Type, Type2, Type3, Type4, typename VectorTraits<Type>::IsVector>::create(v0, v1, v2);
		}

		/**
		* @brief Creation of a vector with four parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3, class Type4, class Type5>
		inline Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3) {
			return Impl::Create4<Type, Type2, Type3, Type4, Type5, typename VectorTraits<Type>::IsVector>::create(v0, v1, v2, v3);
		}

		/**
		* @brief Creation of a vector with six parameters.
		* @ingroup openOR_core
		*/
		template<class Type, class Type2, class Type3, class Type4, class Type5, class Type6, class Type7>
		inline Type create(const Type2& v0, const Type3& v1, const Type4& v2, const Type5& v3, const Type6& v4, const Type7& v5) {
			return Impl::Create6<Type, Type2, Type3, Type4, Type5, Type6, Type7, typename VectorTraits<Type>::IsVector>::create(v0, v1, v2, v3, v4, v5);
		}

	}
}

#endif


#include <iostream>
#include <pcl/io/io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>

int main (int argc, char** argv)
{
  pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),
  										 cloud_filtered_pass (new pcl::PointCloud<pcl::PointXYZRGB>),
  										 cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>),
  										 cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);

  // Fill in the cloud data
  pcl::PCDReader reader;
  reader.read ("dominant_cluster.pcd", *cloud_blob);

  int v1(0), v2(0), v3(0);
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer("3D Viewer"));
  viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);// viewer->createViewPort(0.0, 0.0, 0.4, 1.0, v1);
  viewer->setBackgroundColor(0.0, 0.0, 0.0, v1);
  viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);// viewer->createViewPort(0.4, 0.0, 0.7, 1.0, v2);
  viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
  // viewer->createViewPort(0.7, 0.0, 1.0, 1.0, v3);
  // viewer->setBackgroundColor(0.2, 0.2, 0.2, v3);
  viewer->addCoordinateSystem (0.5);
  viewer->initCameraParameters ();

  std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;

  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloud_blob);
  sor.setLeafSize (0.01f, 0.01f, 0.01f);
  sor.filter (*cloud_filtered_blob);

  // Convert to the templated PointCloud
  pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);

  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;

  // Write the downsampled version to disk
  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZRGB> ("test_pcd_downsampled.pcd", *cloud_filtered, false);

  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_filtered);
  viewer->addPointCloud<pcl::PointXYZRGB>(cloud_filtered, rgb, "Filtered cloud", v1);

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  Eigen::Vector3f perpendicular_to(1, 1, 0);
  seg.setAxis(perpendicular_to);
  seg.setMaxIterations (1000);
  seg.setDistanceThreshold (0.01);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;

  int i = 0, nr_points = (int) cloud_filtered->points.size ();
  // While 30% of the original cloud is still there
  while (cloud_filtered->points.size () > 0.6 * nr_points)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (cloud_filtered);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }

    // Extract the inliers
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (*cloud_p);
    std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

    std::stringstream ss;
    ss << "test_pcd_plane_" << i << ".pcd";
    writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_p, false);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> singleColor (cloud_p, 255 - (100*i)%256, (100*i)%256, (100*i)%256);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud_p, singleColor, ss.str(), v2);

    // Create the filtering object
    extract.setNegative (true);
    extract.filter (*cloud_f);
    cloud_filtered.swap (cloud_f);
    i++;
  }

  while(!viewer->wasStopped())
  {
    viewer->spinOnce();
    boost::this_thread::sleep (boost::posix_time::microseconds(100000));
  }
  viewer->close();

  return (0);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_MANTISSA_HPP_INCLUDED

#include <boost/simd/ieee/functions/mantissa.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mantissamask.hpp>
#include <boost/simd/include/functions/scalar/bitwise_and.hpp>
#include <boost/simd/include/functions/scalar/bitwise_or.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/include/functions/simd/is_invalid.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( mantissa_, tag::cpu_
                                    , (A0)
                                    , (scalar_< integer_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( mantissa_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if(!a0) return a0;
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_invalid(a0)) return a0;
      #endif
      return b_or(b_and(a0,Mantissamask<A0>()),One<A0>());
    }
  };
} } }

#endif


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

#include "test_utils.hpp"
#include <boost/random/mersenne_twister.hpp>

// https://github.com/s9w/articles/blob/master/perf%20cpp%20random.md
// according to this benchmark, boost mt is the fastest
// but anything fast will just be fine
boost::random::mt19937 rng(MGCPP_RAND_SEED);
std::normal_distribution<double> normal(0.0, 1.0);
std::uniform_real_distribution<double> uniform(-1.0, 1.0);

namespace internal {
double uniform_rand() {
  return uniform(rng);
}

double normal_rand() {
  return normal(rng);
}
}  // namespace internal


// Copyright 2004, 2005 The Trustees of Indiana University.

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

//  Authors: Nick Edmonds
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_RMAT_GENERATOR_HPP
#define BOOST_GRAPH_RMAT_GENERATOR_HPP

#include <math.h>
#include <iterator>
#include <utility>
#include <vector>
#include <queue>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/assert.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/detail/mpi_include.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>
// #include <boost/test/floating_point_comparison.hpp>

using boost::shared_ptr;
using boost::uniform_01;

// Returns floor(log_2(n)), and -1 when n is 0
template <typename IntegerType>
inline int int_log2(IntegerType n) {
  int l = 0;
  while (n > 0) {++l; n >>= 1;}
  return l - 1;
}

struct keep_all_edges {
  template <typename T>
  bool operator()(const T&, const T&) { return true; }
};

template <typename Distribution, typename ProcessId>
struct keep_local_edges {

  keep_local_edges(const Distribution& distrib, const ProcessId& id)
    : distrib(distrib), id(id)
  { }

  template <typename T>
  bool operator()(const T& x, const T& y)
  { return distrib(x) == id || distrib(y) == id; }

private:
  const Distribution& distrib;
  const ProcessId&    id;
};

template <typename RandomGenerator, typename T>
void
generate_permutation_vector(RandomGenerator& gen, std::vector<T>& vertexPermutation, T n)
{
  using boost::uniform_int;

  vertexPermutation.resize(n);

  // Generate permutation map of vertex numbers
  uniform_int<T> rand_vertex(0, n-1);
  for (T i = 0; i < n; ++i)
    vertexPermutation[i] = i;

  // Can't use std::random_shuffle unless we create another (synchronized) PRNG
  for (T i = 0; i < n; ++i)
    std::swap(vertexPermutation[i], vertexPermutation[rand_vertex(gen)]);
}

template <typename RandomGenerator, typename T>
std::pair<T,T>
generate_edge(shared_ptr<uniform_01<RandomGenerator> > prob, T n,
              unsigned int SCALE, double a, double b, double c, double d)
{
  T u = 0, v = 0;
  T step = n/2;
  for (unsigned int j = 0; j < SCALE; ++j) {
    double p = (*prob)();

    if (p < a)
      ;
    else if (p >= a && p < a + b)
      v += step;
    else if (p >= a + b && p < a + b + c)
      u += step;
    else { // p > a + b + c && p < a + b + c + d
      u += step;
      v += step;
    }

    step /= 2;

    // 0.2 and 0.9 are hardcoded in the reference SSCA implementation.
    // The maximum change in any given value should be less than 10%
    a *= 0.9 + 0.2 * (*prob)();
    b *= 0.9 + 0.2 * (*prob)();
    c *= 0.9 + 0.2 * (*prob)();
    d *= 0.9 + 0.2 * (*prob)();

    double S = a + b + c + d;

    a /= S; b /= S; c /= S;
    // d /= S;
    // Ensure all values add up to 1, regardless of floating point errors
    d = 1. - a - b - c;
  }

  return std::make_pair(u, v);
}

namespace boost {

  /*
    Chakrabarti's R-MAT scale free generator.

    For all flavors of the R-MAT iterator a+b+c+d must equal 1 and for the
    unique_rmat_iterator 'm' << 'n^2'.  If 'm' is too close to 'n^2' the
    generator may be unable to generate sufficient unique edges

    To get a true scale free distribution {a, b, c, d : a > b, a > c, a > d}
  */

  template<typename RandomGenerator, typename Graph>
  class rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    rmat_iterator()
      : gen(), edge(0) { }

    // Initialize for edge generation
    rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                  edges_size_type m, double a, double b, double c,
                  double d, bool permute_vertices = true)
      : gen(), n(n), a(a), b(b), c(c), d(d), edge(m),
        permute_vertices(permute_vertices),
        SCALE(int_log2(n))

    {
      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      // TODO: Generate the entire adjacency matrix then "Clip and flip" if undirected graph

      // Generate the first edge
      vertices_size_type u, v;
      boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

      if (permute_vertices)
        current = std::make_pair(vertexPermutation[u],
                                 vertexPermutation[v]);
      else
        current = std::make_pair(u, v);

      --edge;
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    rmat_iterator& operator++()
    {
      vertices_size_type u, v;
      boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

      if (permute_vertices)
        current = std::make_pair(vertexPermutation[u],
                                 vertexPermutation[v]);
      else
        current = std::make_pair(u, v);

      --edge;

      return *this;
    }

    rmat_iterator operator++(int)
    {
      rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const rmat_iterator& other) const
    {
      return edge == other.edge;
    }

    bool operator!=(const rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    vertices_size_type n;
    double a, b, c, d;
    int edge;
    bool permute_vertices;
    int SCALE;

    // Internal data structures
    std::vector<vertices_size_type> vertexPermutation;
    value_type current;
  };

  // Sorted version for CSR
  template <typename T>
  struct sort_pair {
    bool operator() (const std::pair<T,T>& x, const std::pair<T,T>& y)
    {
      if (x.first == y.first)
        return x.second > y.second;
      else
        return x.first > y.first;
    }
  };

  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class sorted_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    sorted_rmat_iterator()
      : gen(), values(sort_pair<vertices_size_type>()), done(true)
    { }

    // Initialize for edge generation
    sorted_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, double a, double b, double c,
                         double d, bool permute_vertices = true,
                         EdgePredicate ep = keep_all_edges())
      : gen(), permute_vertices(permute_vertices),
        values(sort_pair<vertices_size_type>()), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      // TODO: "Clip and flip" if undirected graph
      int SCALE = int_log2(n);

      for (edges_size_type i = 0; i < m; ++i) {

        vertices_size_type u, v;
        boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        if (permute_vertices) {
          if (ep(vertexPermutation[u], vertexPermutation[v]))
            values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
        } else {
          if (ep(u, v))
            values.push(std::make_pair(u, v));
        }

      }

      current = values.top();
      values.pop();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.top();
        values.pop();
      } else
        done = true;

      return *this;
    }

    sorted_rmat_iterator operator++(int)
    {
      sorted_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const sorted_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    bool permute_vertices;

    // Internal data structures
    std::priority_queue<value_type, std::vector<value_type>, sort_pair<vertices_size_type> > values;
    value_type current;
    bool       done;
  };

  // This version is slow but guarantees unique edges
  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class unique_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    unique_rmat_iterator()
        : gen(), done(true)
    { }

    // Initialize for edge generation
    unique_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                         edges_size_type m, double a, double b, double c,
                         double d, bool permute_vertices = true,
                         EdgePredicate ep = keep_all_edges())
      : gen(), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      int SCALE = int_log2(n);

      std::map<value_type, bool> edge_map;

      edges_size_type edges = 0;
      do {
        vertices_size_type u, v;
        boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        // Lowest vertex number always comes first
        // (this means we don't have to worry about i->j and j->i being in the edge list)
        if (u > v && is_same<directed_category, undirected_tag>::value)
          std::swap(u, v);

        if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
          edge_map[std::make_pair(u, v)] = true;

          if (permute_vertices) {
            if (ep(vertexPermutation[u], vertexPermutation[v]))
              values.push_back(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
          } else {
            if (ep(u, v))
              values.push_back(std::make_pair(u, v));
          }

          edges++;
        }
      } while (edges < m);
      // NGE - Asking for more than n^2 edges will result in an infinite loop here
      //       Asking for a value too close to n^2 edges may as well

      current = values.back();
      values.pop_back();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    unique_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.back();
        values.pop_back();
      } else
        done = true;

      return *this;
    }

    unique_rmat_iterator operator++(int)
    {
      unique_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const unique_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const unique_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;

    // Internal data structures
    std::vector<value_type> values;
    value_type              current;
    bool                    done;
  };

  // This version is slow but guarantees unique edges
  template<typename RandomGenerator, typename Graph,
           typename EdgePredicate = keep_all_edges>
  class sorted_unique_rmat_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef std::ptrdiff_t difference_type; // Not used

    // No argument constructor, set to terminating condition
    sorted_unique_rmat_iterator()
      : gen(), values(sort_pair<vertices_size_type>()), done(true) { }

    // Initialize for edge generation
    sorted_unique_rmat_iterator(RandomGenerator& gen, vertices_size_type n,
                                edges_size_type m, double a, double b, double c,
                                double d, bool bidirectional = false,
                                bool permute_vertices = true,
                                EdgePredicate ep = keep_all_edges())
      : gen(), bidirectional(bidirectional),
        values(sort_pair<vertices_size_type>()), done(false)

    {
      // BOOST_ASSERT(boost::test_tools::check_is_close(a + b + c + d, 1., 1.e-5));

      this->gen.reset(new uniform_01<RandomGenerator>(gen));

      std::vector<vertices_size_type> vertexPermutation;
      if (permute_vertices)
        generate_permutation_vector(gen, vertexPermutation, n);

      int SCALE = int_log2(n);

      std::map<value_type, bool> edge_map;

      edges_size_type edges = 0;
      do {

        vertices_size_type u, v;
        boost::tie(u, v) = generate_edge(this->gen, n, SCALE, a, b, c, d);

        if (bidirectional) {
          if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
            edge_map[std::make_pair(u, v)] = true;
            edge_map[std::make_pair(v, u)] = true;

            if (ep(u, v)) {
              if (permute_vertices) {
                values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
                values.push(std::make_pair(vertexPermutation[v], vertexPermutation[u]));
              } else {
                values.push(std::make_pair(u, v));
                values.push(std::make_pair(v, u));
              }
           }

            ++edges;
          }
        } else {
          // Lowest vertex number always comes first
          // (this means we don't have to worry about i->j and j->i being in the edge list)
          if (u > v && is_same<directed_category, undirected_tag>::value)
            std::swap(u, v);

          if (edge_map.find(std::make_pair(u, v)) == edge_map.end()) {
            edge_map[std::make_pair(u, v)] = true;

            if (permute_vertices) {
              if (ep(vertexPermutation[u], vertexPermutation[v]))
                values.push(std::make_pair(vertexPermutation[u], vertexPermutation[v]));
            } else {
              if (ep(u, v))
                values.push(std::make_pair(u, v));
            }

            ++edges;
          }
        }

      } while (edges < m);
      // NGE - Asking for more than n^2 edges will result in an infinite loop here
      //       Asking for a value too close to n^2 edges may as well

      current = values.top();
      values.pop();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    sorted_unique_rmat_iterator& operator++()
    {
      if (!values.empty()) {
        current = values.top();
        values.pop();
      } else
        done = true;

      return *this;
    }

    sorted_unique_rmat_iterator operator++(int)
    {
      sorted_unique_rmat_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const sorted_unique_rmat_iterator& other) const
    {
      return values.empty() && other.values.empty() && done && other.done;
    }

    bool operator!=(const sorted_unique_rmat_iterator& other) const
    { return !(*this == other); }

  private:

    // Parameters
    shared_ptr<uniform_01<RandomGenerator> > gen;
    bool             bidirectional;

    // Internal data structures
    std::priority_queue<value_type, std::vector<value_type>,
                        sort_pair<vertices_size_type> > values;
    value_type current;
    bool       done;
  };

} // end namespace boost

#include BOOST_GRAPH_MPI_INCLUDE(<boost/graph/distributed/rmat_graph_generator.hpp>)

#endif // BOOST_GRAPH_RMAT_GENERATOR_HPP


#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <stdexcept>
#include <stddef.h>

//#include <opengm/learning/loss/hammingloss.hxx>
//#include <opengm/learning/loss/generalized-hammingloss.hxx>
#include <opengm/learning/loss/flexibleloss.hxx>
#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>

using namespace boost::python;

namespace opengm{

void pySetNodeLossMultiplier(opengm::learning::FlexibleLoss::Parameter& p,
                             const opengm::python::NumpyView<double,1>& m)
{
    p.nodeLossMultiplier_ = std::vector<double>(m.begin(), m.end());
}

void pySetLabelLossMultiplier(opengm::learning::FlexibleLoss::Parameter& p,
                             const opengm::python::NumpyView<double,1>& m)
{
    p.labelLossMultiplier_ = std::vector<double>(m.begin(), m.end());
}
void pySetFactorLossMultiplier(opengm::learning::FlexibleLoss::Parameter& p,
                               const opengm::python::NumpyView<double,1>& m)
{
    p.labelLossMultiplier_ = std::vector<double>(m.begin(), m.end());
}

template <class GM>
void export_loss(){
   typedef typename std::vector<typename GM::LabelType>::const_iterator Literator;
   typedef typename std::vector<typename GM::LabelType>::const_iterator Niterator;
   typedef opengm::learning::HammingLoss PyHammingLoss;
   typedef opengm::learning::FlexibleLoss PyFlexibleLoss;
   typedef opengm::learning::GeneralizedHammingLoss PyGeneralizedHammingLoss;
   typedef opengm::learning::NoLoss PyNoLoss;

    class_<PyFlexibleLoss >("FlexibleLoss")
        //.def("loss", &PyHammingLoss::loss<const GM &, Literator,Literator>)
        //.def("addLoss", &PyHammingLoss::addLoss<GM, Literator>)
    ;

    // learner param enum
    enum_<PyFlexibleLoss::Parameter::LossType>("LossType")
      .value("hamming", PyFlexibleLoss::Parameter::Hamming)
      .value("l1",  PyFlexibleLoss::Parameter::L1)
      .value("l2",  PyFlexibleLoss::Parameter::L2)
      .value("partition",  PyFlexibleLoss::Parameter::Partition)
      .value("ConfMat",  PyFlexibleLoss::Parameter::ConfMat)
    ;

    class_<PyFlexibleLoss::Parameter>("FlexibleLossParameter")
        .def_readwrite("lossType", &PyFlexibleLoss::Parameter::lossType_)
        .def("setNodeLossMultiplier", &pySetNodeLossMultiplier)
        .def("setLabelLossMultiplier", &pySetLabelLossMultiplier)
        .def("setFactorLossMultiplier", &pySetFactorLossMultiplier)
    ;

    class_<std::vector< PyFlexibleLoss::Parameter > >("FlexibleLossParameterVector")
        .def(vector_indexing_suite<std::vector< PyFlexibleLoss::Parameter> >())
    ;

}

template void export_loss<opengm::python::GmAdder>();

}


#include <NTL/ZZ_p.h>
#include <NTL/FFT.h>

#include <NTL/new.h>

NTL_START_IMPL

NTL_THREAD_LOCAL SmartPtr<ZZ_pInfoT> ZZ_pInfo = 0;
NTL_THREAD_LOCAL SmartPtr<ZZ_pTmpSpaceT> ZZ_pTmpSpace = 0;
NTL_THREAD_LOCAL bool ZZ_pInstalled = false;

ZZ_pInfoT::ZZ_pInfoT(const ZZ& NewP)
{
   if (NewP <= 1) LogicError("ZZ_pContext: p must be > 1");

   p = NewP;
   size = p.size();

   ExtendedModulusSize = 2*size +
                 (NTL_BITS_PER_LONG + NTL_ZZ_NBITS - 1)/NTL_ZZ_NBITS;

}

// we use a lazy strategy for initializing and installing
// FFTInfo and TmpSpace related to a ZZ_p modulus.
// The routines GetFFTInfo and GetTmpSpace make sure this process
// is complete.

void ZZ_p::DoInstall()
{
   SmartPtr<ZZ_pTmpSpaceT> tmps = 0;

   do { // NOTE: thread safe lazy init
      Lazy<ZZ_pFFTInfoT>::Builder builder(ZZ_pInfo->FFTInfo);
      if (!builder()) break;

      UniquePtr<ZZ_pFFTInfoT> FFTInfo;
      FFTInfo.make();

      ZZ B, M, M1, M2, M3;
      long n, i;
      long q, t;

      sqr(B, ZZ_pInfo->p);

      LeftShift(B, B, NTL_FFTMaxRoot+NTL_FFTFudge);

      // FIXME: the following is quadratic time...would
      // be nice to get a faster solution...
      // One could estimate the # of primes by summing logs,
      // then multiply using a tree-based multiply, then
      // adjust up or down...

      // Assuming IEEE floating point, the worst case estimate
      // for error guarantees a correct answer +/- 1 for
      // numprimes up to 2^25...for sure we won't be
      // using that many primes...we can certainly put in
      // a sanity check, though.

      // If I want a more accuaruate summation (with using Kahan,
      // which has some portability issues), I could represent
      // numbers as x = a + f, where a is integer and f is the fractional
      // part.  Summing in this representation introduces an *absolute*
      // error of 2 epsilon n, which is just as good as Kahan
      // for this application.

      // same strategy could also be used in the ZZX HomMul routine,
      // if we ever want to make that subquadratic

      set(M);
      n = 0;
      while (M <= B) {
         UseFFTPrime(n);
         q = GetFFTPrime(n);
         n++;
         mul(M, M, q);
      }

      FFTInfo->NumPrimes = n;
      FFTInfo->MaxRoot = CalcMaxRoot(q);

      double fn = double(n);

      if (8.0*fn*(fn+32) > NTL_FDOUBLE_PRECISION)
         ResourceError("modulus too big");

      if (8.0*fn*(fn+32) > NTL_FDOUBLE_PRECISION/double(NTL_SP_BOUND))
         FFTInfo->QuickCRT = false;
      else
         FFTInfo->QuickCRT = true;

      FFTInfo->x.SetLength(n);
      FFTInfo->u.SetLength(n);

      FFTInfo->rem_struct.init(n, ZZ_pInfo->p, GetFFTPrime);

      FFTInfo->crt_struct.init(n, ZZ_pInfo->p, GetFFTPrime);

      if (!FFTInfo->crt_struct.special()) {
         ZZ qq, rr;

         DivRem(qq, rr, M, ZZ_pInfo->p);

         NegateMod(FFTInfo->MinusMModP, rr, ZZ_pInfo->p);

         for (i = 0; i < n; i++) {
            q = GetFFTPrime(i);

            long tt = rem(qq, q);

            mul(M2, ZZ_pInfo->p, tt);
            add(M2, M2, rr);
            div(M2, M2, q);  // = (M/q) rem p

            div(M1, M, q);
            t = rem(M1, q);
            t = InvMod(t, q);

            mul(M3, M2, t);
            rem(M3, M3, ZZ_pInfo->p);

            FFTInfo->crt_struct.insert(i, M3);

            FFTInfo->x[i] = ((double) t)/((double) q);
            FFTInfo->u[i] = t;
         }
      }

      tmps = MakeSmart<ZZ_pTmpSpaceT>();
      tmps->crt_tmp_vec.fetch(FFTInfo->crt_struct);
      tmps->rem_tmp_vec.fetch(FFTInfo->rem_struct);

      builder.move(FFTInfo);
   } while (0);

   if (!tmps) {
      const ZZ_pFFTInfoT *FFTInfo = ZZ_pInfo->FFTInfo.get();
      tmps = MakeSmart<ZZ_pTmpSpaceT>();
      tmps->crt_tmp_vec.fetch(FFTInfo->crt_struct);
      tmps->rem_tmp_vec.fetch(FFTInfo->rem_struct);
   }

   ZZ_pTmpSpace = tmps;
}

void ZZ_p::init(const ZZ& p)
{
   ZZ_pContext c(p);
   c.restore();
}

void ZZ_pContext::restore() const
{
   ZZ_pInfo = ptr;
   ZZ_pTmpSpace = 0;
   ZZ_pInstalled = false;
}

ZZ_pBak::~ZZ_pBak()
{
   if (MustRestore) c.restore();
}

void ZZ_pBak::save()
{
   c.save();
   MustRestore = true;
}

void ZZ_pBak::restore()
{
   c.restore();
   MustRestore = false;
}

const ZZ_p& ZZ_p::zero()
{
   NTL_THREAD_LOCAL static ZZ_p z(INIT_NO_ALLOC);
   return z;
}

NTL_THREAD_LOCAL
ZZ_p::DivHandlerPtr ZZ_p::DivHandler = 0;

ZZ_p::ZZ_p(INIT_VAL_TYPE, const ZZ& a)  // NO_ALLOC
{
   conv(*this, a);
}

ZZ_p::ZZ_p(INIT_VAL_TYPE, long a) // NO_ALLOC
{
   conv(*this, a);
}

void conv(ZZ_p& x, long a)
{
   if (a == 0)
      clear(x);
   else if (a == 1)
      set(x);
   else {
      NTL_ZZRegister(y);

      conv(y, a);
      conv(x, y);
   }
}

istream& operator>>(istream& s, ZZ_p& x)
{
   NTL_ZZRegister(y);

   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

void div(ZZ_p& x, const ZZ_p& a, const ZZ_p& b)
{
   NTL_ZZ_pRegister(T);

   inv(T, b);
   mul(x, a, T);
}

void inv(ZZ_p& x, const ZZ_p& a)
{
   NTL_ZZRegister(T);

   if (InvModStatus(T, a._ZZ_p__rep, ZZ_p::modulus())) {
      if (!IsZero(a._ZZ_p__rep) && ZZ_p::DivHandler)
         (*ZZ_p::DivHandler)(a);

      InvModError("ZZ_p: division by non-invertible element",
                   a._ZZ_p__rep, ZZ_p::modulus());
   }

   x._ZZ_p__rep = T;
}

long operator==(const ZZ_p& a, long b)
{
   if (b == 0)
      return IsZero(a);

   if (b == 1)
      return IsOne(a);

   NTL_ZZ_pRegister(T);
   conv(T, b);
   return a == T;
}

void add(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   add(x, a, T);
}

void sub(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   sub(x, a, T);
}

void sub(ZZ_p& x, long a, const ZZ_p& b)
{
   NTL_ZZ_pRegister(T);
   conv(T, a);
   sub(x, T, b);
}

void mul(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   mul(x, a, T);
}

void div(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   div(x, a, T);
}

void div(ZZ_p& x, long a, const ZZ_p& b)
{
   if (a == 1) {
      inv(x, b);
   }
   else {
      NTL_ZZ_pRegister(T);
      conv(T, a);
      div(x, T, b);
   }
}

NTL_END_IMPL


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at http://www.somewhere.org/somepath/license.html.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2020  Jeff Jackowski
 */
/**
 * @file
 * Test of the duds::general::SignExtend template.
 */

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <duds/general/SignExtend.hpp>

BOOST_AUTO_TEST_SUITE(SignExtend)

BOOST_AUTO_TEST_CASE(SignExtend_Signed) {
	int v = 0xF;
	BOOST_CHECK_EQUAL(duds::general::SignExtend<4>(v), -1);
	v = 0x8;
	BOOST_CHECK_EQUAL(duds::general::SignExtend<4>(v), -8);
	v = 0x7;
	BOOST_CHECK_EQUAL(duds::general::SignExtend<4>(v), 7);
}

BOOST_AUTO_TEST_CASE(SignExtend_Unsigned) {
	unsigned int v = 0xF;
	BOOST_CHECK_EQUAL(duds::general::SignExtend<4>(v), -1);
	v = 0x8;
	BOOST_CHECK_EQUAL(duds::general::SignExtend<4>(v), -8);
	v = 0x7;
	BOOST_CHECK_EQUAL(duds::general::SignExtend<4>(v), 7);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef YASMIC_BINARY_IFSTREAM_MATRIX
#define YASMIC_BINARY_IFSTREAM_MATRIX

#ifdef BOOST_MSVC
#if _MSC_VER >= 1400
	// disable the warning for ifstream::read
	#pragma warning( push )
	#pragma warning( disable : 4996 )
#endif // _MSC_VER >= 1400
#endif // BOOST_MSVC

#include <fstream>
#include <boost/tuple/tuple.hpp>
#include <iterator>
#include <boost/iterator/iterator_facade.hpp>

#include <yasmic/generic_matrix_operations.hpp>

namespace yasmic
{
	namespace impl
	{

		template <class i_index_type, class i_value_type>
		class binary_ifstream_matrix_const_iterator
		: public boost::iterator_facade<
            binary_ifstream_matrix_const_iterator<i_index_type, i_value_type>,
            boost::tuple<
                i_index_type, i_index_type, i_value_type> const,
            boost::forward_traversal_tag,
            boost::tuple<
                i_index_type, i_index_type, i_value_type> const >
        {
        public:
            binary_ifstream_matrix_const_iterator()
				: _str(0), _r(0), _c(0), _v(0)
			{}

            binary_ifstream_matrix_const_iterator(std::istream &str)
				: _str(&str), _r(0), _c(0), _v(0)
            { increment(); }

        private:
            friend class boost::iterator_core_access;

            void increment()
            {
				if (_str != 0)
				{
            		_str->read((char *)&_r, sizeof(i_index_type));
					_str->read((char *)&_c, sizeof(i_index_type));
					_str->read((char *)&_v, sizeof(i_value_type));

					if (_str->eof()) { _str = 0; }
				}
            }

            bool equal(binary_ifstream_matrix_const_iterator const& other) const
            {
				return (_str == other._str);
            }

            boost::tuple<
                i_index_type, i_index_type, i_value_type>
            dereference() const
            {
            	return boost::make_tuple(_r, _c, _v);
            }

			i_index_type _r, _c;
			i_value_type _v;

			std::istream* _str;
        };

	}

	template <class index_type = int, class value_type = double, class size_type = int>
	struct binary_ifstream_matrix
	{
		std::istream& _f;

		binary_ifstream_matrix(std::istream& f)
			: _f(f)
		{}

		binary_ifstream_matrix(const binary_ifstream_matrix& bifm)
			: _f(bifm._f)
		{}
	};

	template <class i_index_type, class i_value_type, class i_size_type>
    struct smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> >
    {
    	typedef i_size_type size_type;
    	typedef i_index_type index_type;
		typedef i_value_type value_type;

		typedef boost::tuple<index_type, index_type, value_type> nonzero_descriptor;

		typedef impl::binary_ifstream_matrix_const_iterator<i_index_type, i_value_type> nonzero_iterator;

		typedef size_type nz_index_type;

		typedef void row_iterator;

		typedef void row_nonzero_descriptor;
		typedef void row_nonzero_iterator;
    };

	template <class i_index_type, class i_value_type, class i_size_type>
    inline std::pair<typename smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> >::size_type,
                     typename smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> >::size_type >
    dimensions(binary_ifstream_matrix<i_index_type, i_value_type, i_size_type>& m)
    {
		typedef smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> > traits;
    	typename traits::size_type nrows,ncols;

    	m._f.clear();
    	m._f.seekg(0, std::ios_base::beg);

        m._f.read((char *)&nrows, sizeof(i_index_type));
		m._f.read((char *)&ncols, sizeof(i_index_type));

        return (std::make_pair(nrows, ncols));
    }

	template <class i_index_type, class i_value_type, class i_size_type>
	typename smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> >::size_type
	nnz(binary_ifstream_matrix<i_index_type, i_value_type, i_size_type>& m)
	{
		typedef smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> > traits;
    	typename traits::size_type nnz;

		// clear any error bits
		m._f.clear();

		// seek after nrows, ncols
        m._f.seekg(2*sizeof(i_index_type), std::ios_base::beg);

        m._f.read((char *)&nnz, sizeof(i_size_type));

        return (nnz);
	}

	template <class i_index_type, class i_value_type, class i_size_type>
	std::pair<typename smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> >::nonzero_iterator,
              typename smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> >::nonzero_iterator>
    nonzeros(binary_ifstream_matrix<i_index_type, i_value_type, i_size_type>& m)
    {
    	typedef smatrix_traits<binary_ifstream_matrix<i_index_type, i_value_type, i_size_type> > traits;

    	m._f.clear();

		// seek after nrows,ncols,nnz
    	m._f.seekg(2*sizeof(i_index_type)+sizeof(i_size_type), std::ios_base::beg);

        typedef typename traits::nonzero_iterator nz_iter;

        return (std::make_pair(nz_iter(m._f), nz_iter()));
    }
}

#ifdef BOOST_MSVC
#if _MSC_VER >= 1400
	// restore the warning for ifstream::read
	#pragma warning( pop )
#endif // _MSC_VER >= 1400
#endif // BOOST_MSVC

#endif //YASMIC_BINARY_IFSTREAM_MATRIX


//
// $Id: bin1d.hpp 5313 2013-12-17 18:06:54Z chambm $
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef BIN1D_H
#define BIN1D_H

#include <vector>
#include <algorithm>
#include <functional>
#include <vector>
#include <utility>
#include <stdexcept>
#include <boost/cstdint.hpp>

namespace ralab
{
  namespace base
  {
    namespace resample
    {
      typedef boost::int32_t int32_t;
      typedef boost::int64_t int64_t;

      //Class returns the index a value belongs too....
      struct Bin1D
      {
        std::vector<double> breaks_; // boundaries
        double * begbreaks_;
        double * endbreaks_;
      public:
        Bin1D():breaks_(),begbreaks_(0),endbreaks_(0){}

        Bin1D(
            std::vector<double> & breaks // breaks
            ):breaks_(), begbreaks_(0), endbreaks_(0)
        {
          this->setBreaks(breaks);
        }

        Bin1D(
            const Bin1D & rhs // breaks
            ):breaks_(), begbreaks_(0), endbreaks_(0)
        {
          this->setBreaks(rhs.breaks_);
        }

        void setBreaks(const std::vector<double> & breaks){
          breaks_.assign( breaks.begin(), breaks.end() );
          std::sort(breaks_.begin(),breaks_.end());
          reset();
        }

        void reset(){
          begbreaks_ = &breaks_[0];
          endbreaks_ = begbreaks_ + breaks_.size();
        }

        void getBreaks(std::vector<double> & breaks) const{
          breaks.assign(begbreaks_,endbreaks_);
        }

        const std::vector<double> & getBreaks() const{
          return breaks_;
        }

        bool inRange(double dat) const{
          return (dat > breaks_.front() && dat < breaks_.back());
        }

        std::size_t operator()(double dat) const
        {
          double * it2 = std::lower_bound(begbreaks_,endbreaks_,dat);
          size_t ub = std::distance(begbreaks_,it2);
          return ub;
        }

        void operator()(double dat1,
                        double dat2,
                        std::vector<int32_t> & idx,
                        std::vector<double> & dist
                        ) const
        {
          double * it1 = std::lower_bound(begbreaks_,endbreaks_,dat1);
          double * it2 = std::lower_bound(begbreaks_,endbreaks_,dat2);

          size_t ub1 = std::distance(begbreaks_,it1);
          size_t ub2 = std::distance(begbreaks_,it2);
          int64_t n = static_cast<int64_t>(ub2-ub1)+1;
          idx.resize(n);
          dist.resize(n);

          if(ub1 == ub2 ){
              idx[0] = ub1-1;
              dist[0] = dat2 - dat1;
              return;
            }
          else{
              for(int64_t i = 0; it1 != (it2+1) ; i++, it1++){
                  idx[i] = ub1 + i - 1;
                  if(i == 0)
                    {
                      dist[i] = *(it1) - dat1;
                    }
                  else if( i < n - 1 )
                    {
                      dist[i] = *(it1) - *(it1-1);
                    }
                  else
                    {
                      dist[i] = dat2 - *(it1-1);
                    }
                }
            }

        }
      };

    }//resample
  }//base
}//ralab

#endif // BUCKET1D_H


// Copyright (C) 2009-2012 The Trustees of Indiana University.

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

//  Authors: Nick Edmonds
//           Andrew Lumsdaine

#include <am++/am++.hpp>
#include <am++/mpi_transport.hpp>

// A test of the distributed compressed sparse row graph type
#include <boost/graph/use_mpi.hpp>
#include <boost/config.hpp>
#include <boost/graph/distributed/compressed_sparse_row_graph.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/test/minimal.hpp>

/****************************************************************************
 * Edge weight generator iterator                                           *
 ****************************************************************************/
template<typename F, typename RandomGenerator>
class generator_iterator
{
public:
  typedef std::input_iterator_tag iterator_category;
  typedef typename F::result_type value_type;
  typedef const value_type&       reference;
  typedef const value_type*       pointer;
  typedef void                    difference_type;

  explicit
  generator_iterator(RandomGenerator& gen, const F& f = F())
    : f(f), gen(&gen)
  {
    value = this->f(gen);
  }

  reference operator*() const  { return value; }
  pointer   operator->() const { return &value; }

  generator_iterator& operator++()
  {
    value = f(*gen);
    return *this;
  }

  generator_iterator operator++(int)
  {
    generator_iterator temp(*this);
    ++(*this);
    return temp;
  }

  bool operator==(const generator_iterator& other) const
  { return f == other.f; }

  bool operator!=(const generator_iterator& other) const
  { return !(*this == other); }

private:
  F f;
  RandomGenerator* gen;
  value_type value;
};

template<typename F, typename RandomGenerator>
inline generator_iterator<F, RandomGenerator>
make_generator_iterator( RandomGenerator& gen, const F& f)
{ return generator_iterator<F, RandomGenerator>(gen, f); }

using namespace boost;

typedef int weight_type;

struct EdgeProperty {
  EdgeProperty(weight_type weight = 0) : weight(weight) { }

  weight_type weight;
};

struct VertexProperty {
  VertexProperty(int d = 0)
    : distance(d) { }

  int distance;
};

int test_main(int argc, char* argv[])
{
  amplusplus::environment env = amplusplus::mpi_environment(argc, argv, true);
  amplusplus::transport trans = env.create_transport();

  trans.set_nthreads(1);

  int n = 40;
  double prob = 0.1;
  int c = 10;

  minstd_rand gen;

  // No properties
  {
    typedef compressed_sparse_row_graph<directedS, no_property, no_property,
                                        no_property, distributedS<> >
      Digraph;

    parallel::block<graph_traits<Digraph>::vertices_size_type> dist(trans, n);

    typedef sorted_erdos_renyi_iterator<minstd_rand, Digraph> SortedERIter;
    typedef erdos_renyi_iterator<minstd_rand, Digraph> ERIter;

    std::vector<ERIter::value_type> edges;
    std::vector<graph_traits<Digraph>::vertex_descriptor> sources, targets;
    ERIter start(gen, n, prob);
    for ( ; start != ERIter() ; ++start) {
      edges.push_back(*start);

      if (dist(start->first) == trans.rank()) {
        sources.push_back(start->first);
        targets.push_back(start->second);
      }
    }

    Digraph g(edges_are_unsorted, ERIter(gen, n, prob), ERIter(), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs(g);
    bfs.run(vertex(0, g));

    Digraph g2(edges_are_sorted, SortedERIter(gen, n, prob), SortedERIter(), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs2(g2);
    bfs2.run(vertex(0, g2));

    Digraph g3(edges_are_unsorted_multi_pass, edges.begin(), edges.end(), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs3(g3);
    bfs3.run(vertex(0, g3));

    Digraph g4(distributed_construct_inplace_from_sources_and_targets, sources, targets, n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs4(g4);
    bfs4.run(vertex(0, g4));

    Digraph g5(n, trans, parallel::oned_block_cyclic<graph_traits<Digraph>::vertices_size_type>(trans, 1));
    add_edges(edges.begin(), edges.end(), g5);
    boost::graph::distributed::breadth_first_search<Digraph> bfs5(g5);
    bfs5.run(vertex(0, g5));

    // TODO (NGE): Test add_edges_sorted()?
  }

  // Edge properties
  {
    typedef compressed_sparse_row_graph<directedS, no_property, EdgeProperty, no_property,
                                        distributedS<> >
      Digraph;

    parallel::block<graph_traits<Digraph>::vertices_size_type> dist(trans, n);

    typedef sorted_erdos_renyi_iterator<minstd_rand, Digraph> SortedERIter;
    typedef erdos_renyi_iterator<minstd_rand, Digraph> ERIter;

    std::vector<ERIter::value_type> edges;
    std::vector<graph_traits<Digraph>::vertex_descriptor> sources, targets;
    std::vector<EdgeProperty> edge_props;
    uniform_int<int> edge_weight(0, c);

    ERIter start(gen, n, prob);
    for ( ; start != ERIter() ; ++start) {
      edges.push_back(*start);

      if (dist(start->first) == trans.rank()) {
        sources.push_back(start->first);
        targets.push_back(start->second);
        edge_props.push_back(EdgeProperty(edge_weight(gen)));
      }
    }

    Digraph g(edges_are_unsorted, ERIter(gen, n, prob), ERIter(), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs(g);
    bfs.run(vertex(0, g));

    Digraph g2(edges_are_sorted, SortedERIter(gen, n, prob), SortedERIter(), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs2(g2);
    bfs2.run(vertex(0, g2));

    Digraph g3(edges_are_unsorted_multi_pass, edges.begin(), edges.end(),
	       make_generator_iterator(gen, uniform_int<int>(0, c)), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs3(g3);
    bfs3.run(vertex(0, g3));

    Digraph g4(distributed_construct_inplace_from_sources_and_targets, sources,
	       targets, edge_props, n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs4(g4);
    bfs4.run(vertex(0, g4));

    Digraph g5(n, trans, parallel::oned_block_cyclic<graph_traits<Digraph>::vertices_size_type>(trans, 1));
    add_edges(edges.begin(), edges.end(), g5);
    boost::graph::distributed::breadth_first_search<Digraph> bfs5(g5);
    bfs5.run(vertex(0, g5));

    // TODO (NGE): Test add_edges_sorted()?
  }

  // Edge and vertex properties
  {
    typedef compressed_sparse_row_graph<directedS, VertexProperty, EdgeProperty,
                                        no_property,
                                        distributedS<> >
      Digraph;

    parallel::block<graph_traits<Digraph>::vertices_size_type> dist(trans, n);

    typedef sorted_erdos_renyi_iterator<minstd_rand, Digraph> SortedERIter;
    typedef erdos_renyi_iterator<minstd_rand, Digraph> ERIter;

    std::vector<ERIter::value_type> edges;
    std::vector<graph_traits<Digraph>::vertex_descriptor> sources, targets;
    std::vector<EdgeProperty> edge_props;
    uniform_int<int> edge_weight(0, c);

    ERIter start(gen, n, prob);
    for ( ; start != ERIter() ; ++start) {
      edges.push_back(*start);

      if (dist(start->first) == trans.rank()) {
        sources.push_back(start->first);
        targets.push_back(start->second);
        edge_props.push_back(EdgeProperty(edge_weight(gen)));
      }
    }

    Digraph g(edges_are_unsorted, ERIter(gen, n, prob), ERIter(), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs(g);
    bfs.run(vertex(0, g));

    Digraph g2(edges_are_sorted, SortedERIter(gen, n, prob), SortedERIter(), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs2(g2);
    bfs2.run(vertex(0, g2));

    Digraph g3(edges_are_unsorted_multi_pass, edges.begin(), edges.end(),
	       make_generator_iterator(gen, uniform_int<int>(0, c)), n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs3(g3);
    bfs3.run(vertex(0, g3));

    Digraph g4(distributed_construct_inplace_from_sources_and_targets, sources,
               targets, edge_props, n, trans, dist);
    boost::graph::distributed::breadth_first_search<Digraph> bfs4(g4);
    bfs4.run(vertex(0, g4));

    // NGE: Both add_edges() and bfs5.run() below fail
#if 0
    Digraph g5(n, trans, parallel::oned_block_cyclic(trans, 1));
    add_edges(edges.begin(), edges.end(), edge_props.begin(), edge_props.end(), g5);
    boost::graph::distributed::breadth_first_search<Digraph> bfs5(g5);
    bfs5.run(vertex(0, g5));
#endif

    // TODO (NGE): Test add_edges_sorted()?
  }

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::meta::include.hpp                             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_META_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_META_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/data/meta/failure_distribution.hpp>
#include <boost/statistics/survival/data/meta/failure_random.hpp>
#include <boost/statistics/survival/data/meta/random_input.hpp>

#endif


/**
 * @file SimPolygon2D.cpp
 * @author Alex Cunningham
 */

#include <iostream>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <gtsam_unstable/geometry/SimPolygon2D.h>

namespace gtsam {

using namespace std;

const size_t max_it = 100000;
boost::minstd_rand SimPolygon2D::rng(42u);

/* ************************************************************************* */
void SimPolygon2D::seedGenerator(unsigned long seed) {
  rng = boost::minstd_rand(seed);
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::createTriangle(const Point2& pA, const Point2& pB, const Point2& pC) {
  SimPolygon2D result;
  result.landmarks_.push_back(pA);
  result.landmarks_.push_back(pB);
  result.landmarks_.push_back(pC);
  return result;
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, double width) {
  SimPolygon2D result;
  result.landmarks_.push_back(p);
  result.landmarks_.push_back(p + Point2(width, 0.0));
  result.landmarks_.push_back(p + Point2(width, height));
  result.landmarks_.push_back(p + Point2(0.0, height));
  return result;
}

/* ************************************************************************* */
bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
  if (p.size() != size()) return false;
  for (size_t i=0; i<size(); ++i)
    if (!traits<Point2>::Equals(landmarks_[i], p.landmarks_[i], tol))
      return false;
  return true;
}

/* ************************************************************************* */
void SimPolygon2D::print(const string& s) const {
  cout << "SimPolygon " << s << ": " << endl;
  for(const Point2& p: landmarks_)
    traits<Point2>::Print(p, "   ");
}

/* ************************************************************************* */
vector<SimWall2D> SimPolygon2D::walls() const {
  vector<SimWall2D> result;
  for (size_t i=0; i<size()-1; ++i)
    result.push_back(SimWall2D(landmarks_[i], landmarks_[i+1]));
  result.push_back(SimWall2D(landmarks_[size()-1], landmarks_[0]));
  return result;
}

/* ************************************************************************* */
bool SimPolygon2D::contains(const Point2& c) const {
  vector<SimWall2D> edges = walls();
  bool initialized = false;
  bool lastSide = false;
  for(const SimWall2D& ab: edges) {
    // compute cross product of ab and ac
    Point2 dab = ab.b() - ab.a();
    Point2 dac = c - ab.a();
    double cross = dab.x() * dac.y() - dab.y() * dac.x();
    if (fabs(cross) < 1e-6) // check for on one of the edges
      return true;
    bool side = cross > 0;
    // save the first side found
    if (!initialized) {
      lastSide = side;
      initialized = true;
      continue;
    }

    // to be inside the polygon, point must be on the same side of all lines
    if (lastSide != side)
      return false;
  }
  return true;
}

/* ************************************************************************* */
bool SimPolygon2D::overlaps(const SimPolygon2D& p) const {
  for(const Point2& a: landmarks_)
    if (p.contains(a))
      return true;
  for(const Point2& a: p.landmarks_)
    if (contains(a))
      return true;
  return false;
}

/* ***************************************************************** */
bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obstacles) {
  for(const SimPolygon2D& poly: obstacles)
    if (poly.contains(p))
      return true;
  return false;
}

/* ************************************************************************* */
bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector<SimPolygon2D>& obstacles) {
  for(const SimPolygon2D& poly: obstacles)
    if (poly.overlaps(p))
      return true;
  return false;
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::randomTriangle(
    double side_len, double mean_side_len, double sigma_side_len,
    double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
  // get the current set of landmarks
  std::vector<Point2> lms;
  double d2 = side_len/2.0;
  lms.push_back(Point2( d2, d2));
  lms.push_back(Point2(-d2, d2));
  lms.push_back(Point2(-d2,-d2));
  lms.push_back(Point2( d2,-d2));

  for(const SimPolygon2D& poly: existing_polys)
    lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());

  for (size_t i=0; i<max_it; ++i) {

    // find a random pose for line AB
    Pose2 xA(randomAngle(), randomBoundedPoint2(side_len, lms, existing_polys, min_vertex_dist));

    // extend line by random dist and angle to get BC
    double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
    double tABC = randomAngle().theta();
    Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC).finished());

    // extend from B to find C
    double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);
    Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC);

    // use triangle equality to verify non-degenerate triangle
    double dAC = distance2(xA.t(), xC.t());

    // form a triangle and test if it meets requirements
    SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());

    // check inside walls, long enough edges, far away from landmarks
    const double thresh = mean_side_len / 2.0;
    if ((dAB + dBC + thresh > dAC) &&  // triangle inequality
        (dAB + dAC + thresh > dBC) &&
        (dAC + dBC + thresh > dAB) &&
        insideBox(side_len, test_tri.landmark(0)) &&
        insideBox(side_len, test_tri.landmark(1)) &&
        insideBox(side_len, test_tri.landmark(2)) &&
        distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len &&
        !nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
        !nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
        !nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
        !anyOverlaps(test_tri, existing_polys)) {
      return test_tri;
    }
  }
  throw runtime_error("Could not find space for a triangle");
  return SimPolygon2D::createTriangle(Point2(99,99), Point2(99,99), Point2(99,99));
}

/* ************************************************************************* */
SimPolygon2D SimPolygon2D::randomRectangle(
    double side_len, double mean_side_len, double sigma_side_len,
    double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
  // get the current set of landmarks
  std::vector<Point2> lms;
  double d2 = side_len/2.0;
  lms.push_back(Point2( d2, d2));
  lms.push_back(Point2(-d2, d2));
  lms.push_back(Point2(-d2,-d2));
  lms.push_back(Point2( d2,-d2));
  for(const SimPolygon2D& poly: existing_polys)
    lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());

  const Point2 lower_corner(-side_len,-side_len);
  const Point2 upper_corner( side_len, side_len);

  for (size_t i=0; i<max_it; ++i) {

    // pick height and width to be viable distances
    double height = randomDistance(mean_side_len, sigma_side_len, min_side_len);
    double width = randomDistance(mean_side_len, sigma_side_len, min_side_len);

    // find a starting point - limited to region viable for this height/width
    Point2 pA = randomBoundedPoint2(lower_corner, upper_corner - Point2(width, height),
        lms, existing_polys, min_vertex_dist);

    // verify
    SimPolygon2D rect = SimPolygon2D::createRectangle(pA, height, width);

    // check inside walls, long enough edges, far away from landmarks
    if (insideBox(side_len, rect.landmark(0)) &&
        insideBox(side_len, rect.landmark(1)) &&
        insideBox(side_len, rect.landmark(2)) &&
        insideBox(side_len, rect.landmark(3)) &&
        !nearExisting(lms, rect.landmark(0), min_vertex_dist) &&
        !nearExisting(lms, rect.landmark(1), min_vertex_dist) &&
        !nearExisting(lms, rect.landmark(2), min_vertex_dist) &&
        !nearExisting(lms, rect.landmark(3), min_vertex_dist) &&
        !anyOverlaps(rect, existing_polys)) {
      return rect;
    }
  }
  throw runtime_error("Could not find space for a rectangle");
  return SimPolygon2D::createRectangle(Point2(99,99), 100, 100);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomPoint2(double s) {
  boost::uniform_real<>  gen_t(-s/2.0, s/2.0);
  return Point2(gen_t(rng), gen_t(rng));
}

/* ***************************************************************** */
Rot2 SimPolygon2D::randomAngle() {
  boost::uniform_real<>  gen_r(-M_PI, M_PI); // modified range to avoid degenerate cases in triangles
  return Rot2::fromAngle(gen_r(rng));
}

/* ***************************************************************** */
double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
  boost::normal_distribution<double> norm_dist(mu, sigma);
  boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > gen_d(rng, norm_dist);
  double d = -10.0;
  for (size_t i=0; i<max_it; ++i) {
    d = fabs(gen_d());
    if (d > min_dist)
      return d;
  }
  cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma
       << " min_dist = " << min_dist << endl;
  throw runtime_error("Failed to find a viable distance");
  return fabs(norm_dist(rng));
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
      const vector<SimPolygon2D>& obstacles) {
  for (size_t i=0; i<max_it; ++i) {
    Point2 p = randomPoint2(boundary_size);
    if (!anyContains(p, obstacles))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
    const std::vector<Point2>& landmarks, double min_landmark_dist) {
  for (size_t i=0; i<max_it; ++i) {
    Point2 p = randomPoint2(boundary_size);
    if (!nearExisting(landmarks, p, min_landmark_dist))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
    const std::vector<Point2>& landmarks,
    const vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
  for (size_t i=0; i<max_it; ++i) {
    Point2 p = randomPoint2(boundary_size);
    if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(
    const Point2& LL_corner, const Point2& UR_corner,
    const std::vector<Point2>& landmarks,
    const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {

  boost::uniform_real<>  gen_x(0.0, UR_corner.x() - LL_corner.x());
  boost::uniform_real<>  gen_y(0.0, UR_corner.y() - LL_corner.y());

  for (size_t i=0; i<max_it; ++i) {
    Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
    if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
      return p;
  }
  throw runtime_error("Failed to find a place for a landmark!");
  return Point2(0,0);
}

/* ***************************************************************** */
Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector<SimPolygon2D>& obstacles) {
  return Pose2(randomAngle(), randomBoundedPoint2(boundary_size, obstacles));
}

/* ***************************************************************** */
bool SimPolygon2D::insideBox(double s, const Point2& p) {
  return fabs(p.x()) < s/2.0 && fabs(p.y()) < s/2.0;
}

/* ***************************************************************** */
bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
    const Point2& p, double threshold) {
  for(const Point2& Sp: S)
    if (distance2(Sp, p) < threshold)
      return true;
  return false;
}

} //\namespace gtsam


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE AliasZeroCopyTest

#include <boost/filesystem.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <random_util.hpp>
#include <popart/aliaszerocopy.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/graph.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/call.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/popx/devicex.hpp>
#include <popart/popx/irlowering.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>

#include <algorithm>
#include <map>
#include <tuple>
#include <vector>

using namespace popart;

enum class ZeroCopyTestModel {
  //      B              C
  //      |              |
  // A - Call(0) - X0 - Call(0) - X1 - Loss
  // X1 aliased as subgraph output
  // (meaning that "Subgraph(0)/X0" will be aliased to "X1"),
  // but not X0, since X0 will be required
  // in the backward pass and is therefore still alive through the second
  // Call(0)
  TwoMatMuls = 0,

  //      B              C
  //      |              |
  // A - Call(0) - X0 - Call(0) - X1 - Add - X2 - Loss
  //                \__________________/
  // X0/X1 - same as TwoMatMuls
  TwoMatMulsAddOutput,

  //      B -------------.
  //      |              |
  // A - Call(0) - X0 - Call(0) - X1 - Loss
  // B aliased as subgraph input, since both Call(0) use the same weight
  // X0/X1 - same as TwoMatMuls
  TwoMatMulsSharedWeight
};

void compare(const std::vector<std::vector<float>> &a,
             const std::vector<std::vector<float>> &b) {
  for (size_t i = 0; i < a.size(); ++i) {
    BOOST_CHECK_EQUAL_COLLECTIONS(
        a.at(i).begin(), a.at(i).end(), b.at(i).begin(), b.at(i).end());
  }
}

// Test zero-copy through aliasing behaviour
// Test models from ZeroCopyTestModel
// Verify that the right subgraph input/output tensors are aliased and
// that the weight update result is numerically identical to the same graph
// trained without alias zero copy enabled
BOOST_AUTO_TEST_CASE(AliasZeroCopyTest0) {
  auto test = [](bool aliasZeroCopy, ZeroCopyTestModel model) {
    std::vector<std::vector<float>> collection;

    int N = 8;

    // we will generate random initializations
    int seed = 1013;
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdis(-4.f, 4.f);

    // prepare a Builder for creating onnx model
    auto bder   = Builder::create();
    auto aiOnnx = bder->aiOnnxOpset9();

    // matrix A of shape N
    TensorInfo A_info{"FLOAT", std::vector<int64_t>{1, 1, N}};
    std::vector<float> v_A_init(A_info.nelms());
    for (auto &val : v_A_init) {
      val = fdis(eng);
    }
    TensorId A_id =
        bder->addInitializedInputTensor({v_A_init.data(), A_info}, "A");

    // matrix B of shape N x N
    TensorInfo B_info{"FLOAT", std::vector<int64_t>{1, N, N}};
    std::vector<float> v_B_init(B_info.nelms());
    for (auto &val : v_B_init) {
      val = fdis(eng);
    }
    TensorId B_id =
        bder->addInitializedInputTensor({v_B_init.data(), B_info}, "B");

    // matrix C of shape N x N
    TensorInfo C_info{"FLOAT", std::vector<int64_t>{1, N, N}};
    std::vector<float> v_C_init(C_info.nelms());
    for (auto &val : v_C_init) {
      val = fdis(eng);
    }
    TensorId C_id =
        bder->addInitializedInputTensor({v_C_init.data(), C_info}, "C");

    TensorInfo X_info{"FLOAT", std::vector<int64_t>{N}};

    TensorId X_id;

    switch (model) {
    case ZeroCopyTestModel::TwoMatMuls: {
      X_id = aiOnnx.matmul({A_id, B_id});
      X_id = aiOnnx.matmul({X_id, C_id});
      break;
    };
    case ZeroCopyTestModel::TwoMatMulsAddOutput: {
      TensorId X1_id = aiOnnx.matmul({A_id, B_id});
      TensorId X2_id = aiOnnx.matmul({X1_id, C_id});
      X_id           = aiOnnx.add({X1_id, X2_id});
      break;
    };
    case ZeroCopyTestModel::TwoMatMulsSharedWeight: {
      X_id = aiOnnx.matmul({A_id, B_id});
      X_id = aiOnnx.matmul({X_id, B_id});
      break;
    };
    }

    // l1 loss with penalty term, will be applied to C
    float lossLambda = 0.26;
    auto l1          = bder->aiGraphcoreOpset1().l1loss({X_id}, lossLambda);

    auto proto      = bder->getModelProto();
    auto modelProto = io::getModelFromString(proto);
    auto art        = AnchorReturnType("All");

    // one batch per step
    int batchesPerStep = 1;
    AnchorReturnTypeMap anchorIds;
    auto dataFlow = DataFlow(batchesPerStep, anchorIds);

    auto device = popart::createTestDevice(TEST_TARGET);

    auto opts                           = SessionOptions();
    opts.explicitRecomputation          = true;
    opts.enableOutlining                = true;
    opts.outlineThreshold               = -1.0;
    opts.enableOutliningCopyCostPruning = false;
    opts.aliasZeroCopy                  = aliasZeroCopy;
    opts.delayVarUpdates                = false;

    // training info
    float learnRate = 0.321;
    auto optimizer  = ConstSGD(learnRate);

    auto session = popart::TrainingSession::createFromOnnxModel(
        proto,
        dataFlow,
        l1,
        optimizer,
        device,
        popart::InputShapeInfo(),
        opts,
        popart::Patterns(PatternsLevel::Default));

    std::map<popart::TensorId, popart::IArray &> anchors = {};

    auto &ir = session->getIr();

    session->prepareDevice();

    // inputs:
    popart::NDArrayWrapper<float> A_wrapper(v_A_init.data(), A_info);
    popart::NDArrayWrapper<float> B_wrapper(v_B_init.data(), B_info);
    popart::NDArrayWrapper<float> C_wrapper(v_C_init.data(), C_info);
    std::map<popart::TensorId, popart::IArray &> inputs = {
        {A_id, A_wrapper}, {B_id, B_wrapper}, {C_id, C_wrapper}};

    popart::StepIO stepio(inputs, anchors);

    session->weightsFromHost();
    session->run(stepio);

    // Verify alias zero copy
    if (aliasZeroCopy) {
      auto &dev = session->getDevice();
      auto azc  = dev.lowering().getAliasZeroCopy();

      std::set<TensorId> tensorIdsWithAliases;
      for (Tensor *t : azc->getTensorsWithPostIRAliases()) {
        tensorIdsWithAliases.insert(t->id);
      }
      // For debugging purposes
      logging::trace("Tensors with aliases: {}", tensorIdsWithAliases);

      auto checkAliased = [&ir, &dev, &azc](CallOp *call,
                                            std::pair<int, bool> index,
                                            bool expectAliased) {
        Graph &calledGraph = call->getCalledGraph();
        Tensor *t0;
        Tensor *t1;
        if (index.second) {
          t0 = call->input->tensor(index.first);
          t1 = ir.getTensor(calledGraph.getInputId(index.first));
        } else {
          t0 = call->output->tensor(index.first);
          t1 = ir.getTensor(calledGraph.getOutputId(index.first));
        }
        auto aliases = azc->getActiveAliasedTensors({t0}, true);
        bool aliased = aliases.find(t1) != aliases.end();
        logging::trace(
            "Testing aliasing of {} <-> {}, expected: {}, observed: {}",
            t0->id,
            t1->id,
            expectAliased ? "yes" : "no",
            aliased ? "yes" : "no");
        if (expectAliased) {
          // AliasZeroCopy reports tensors as actively aliased
          BOOST_CHECK(aliased);
          // Additionally verify the poplar tensors agree
          BOOST_CHECK(
              poplar::concat(
                  {dev.lowering().tensors().get(t0->id).getPoplarTensor(),
                   dev.lowering().tensors().get(t1->id).getPoplarTensor()},
                  0)
                  .containsAliases());
        } else {
          BOOST_CHECK(!aliased);
        }
      };

      auto inTensor = [](InIndex index) {
        return std::pair<InIndex, bool>(index, true);
      };

      auto outTensor = [](OutIndex index) {
        return std::pair<OutIndex, bool>(index, false);
      };

      // Count the number of calls
      size_t callIndex = 0;

      for (Op *op :
           ir.getMainGraph().getOpSchedule({}, RequireOptimalSchedule::Yes)) {
        logging::trace("Op: {}", op->debugName());

        if (CallOp *call = dynamic_cast<CallOp *>(op)) {
          // Depending on the model, we expect different inputs/outputs to be
          // aliased
          switch (model) {
          case ZeroCopyTestModel::TwoMatMuls: {
            if (callIndex == 0) {
              checkAliased(call, inTensor(0), false);  // A
              checkAliased(call, inTensor(1), false);  // B
              checkAliased(call, outTensor(0), false); // X0
            }
            if (callIndex == 1) {
              checkAliased(call, inTensor(0), false); // X0
              checkAliased(call, inTensor(1), false); // C
              checkAliased(call, outTensor(0), true); // X1
            }
            if (callIndex == 2) {
              checkAliased(call, inTensor(0), false); // grad_X1
              checkAliased(call, inTensor(1), false); // C
              checkAliased(call, inTensor(2), false); // X0
              checkAliased(call, outTensor(0), true); // grad_X0
            }
            if (callIndex == 3) {
              checkAliased(call, inTensor(0), false); // grad_X0
              checkAliased(call, inTensor(1), false); // B
              checkAliased(call, inTensor(2), false); // A
              checkAliased(call, outTensor(0), true); // grad_A
            }
            break;
          };
          case ZeroCopyTestModel::TwoMatMulsAddOutput: {
            if (callIndex == 0) {
              checkAliased(call, inTensor(0), false);  // A
              checkAliased(call, inTensor(1), false);  // B
              checkAliased(call, outTensor(0), false); // X0
            }
            if (callIndex == 1) {
              checkAliased(call, inTensor(0), false);  // X0
              checkAliased(call, inTensor(1), false);  // C
              checkAliased(call, outTensor(0), false); // X1
            }
            if (callIndex == 2) {
              checkAliased(call, inTensor(0), false); // X0
              checkAliased(call, inTensor(1), false); // X1
              checkAliased(call, outTensor(0), true); // X2
            }
            if (callIndex == 3) {
              checkAliased(call, inTensor(0), false); // grad_X2
              checkAliased(call, inTensor(1), false); // X0
              checkAliased(call, inTensor(2), false); // C
              checkAliased(call, outTensor(0), true); // grad_X1
            }
            if (callIndex == 4) {
              checkAliased(call, inTensor(0), false); // grad_X2
              checkAliased(call, inTensor(1), false); // grad_X1
              checkAliased(call, outTensor(0), true); // grad_X0
            }
            if (callIndex == 5) {
              checkAliased(call, inTensor(0), false); // grad_X0
              checkAliased(call, inTensor(1), false); // A
              checkAliased(call, inTensor(2), false); // B
              checkAliased(call, outTensor(0), true); // grad_A
            }
            break;
          };
          case ZeroCopyTestModel::TwoMatMulsSharedWeight: {
            if (callIndex == 0) {
              checkAliased(call, inTensor(0), false);  // A
              checkAliased(call, inTensor(1), true);   // B
              checkAliased(call, outTensor(0), false); // X0
            }
            if (callIndex == 1) {
              checkAliased(call, inTensor(0), false); // X0
              checkAliased(call, inTensor(1), true);  // B
              checkAliased(call, outTensor(0), true); // X1
            }
            if (callIndex == 2) {
              checkAliased(call, inTensor(0), false);  // grad_X1
              checkAliased(call, inTensor(1), true);   // B
              checkAliased(call, outTensor(0), false); // grad_X0
            }
            if (callIndex == 3) {
              checkAliased(call, inTensor(0), false); // grad_X0
              checkAliased(call, inTensor(1), true);  // B
              checkAliased(call, outTensor(0), true); // grad_A
            }
            if (callIndex == 4) {
              checkAliased(call, inTensor(0), false);  // grad_X1
              checkAliased(call, inTensor(1), false);  // X0
              checkAliased(call, outTensor(0), false); // grad_B_part
            }
            if (callIndex == 5) {
              checkAliased(call, inTensor(0), false); // grad_X0
              checkAliased(call, inTensor(1), false); // A
              checkAliased(call, outTensor(0), true); // grad_B_part
            }
            break;
          };
          }

          ++callIndex;
        }
      }
    }

    WeightsIO weightsRead;
    // to be readback:
    std::vector<float> A_readback(A_info.nelms(), -1.0f);
    std::vector<float> B_readback(B_info.nelms(), -1.0f);
    std::vector<float> C_readback(C_info.nelms(), -1.0f);
    weightsRead.insert(A_id, {A_readback.data(), A_info});
    weightsRead.insert(B_id, {B_readback.data(), B_info});
    weightsRead.insert(C_id, {C_readback.data(), C_info});

    session->weightsToHost();
    session->readWeights(weightsRead);

    collection.push_back(A_readback);
    collection.push_back(B_readback);
    collection.push_back(C_readback);
    return collection;
  };

  {
    auto baseline = test(false, ZeroCopyTestModel::TwoMatMuls);
    auto zerocopy = test(true, ZeroCopyTestModel::TwoMatMuls);
    compare(baseline, zerocopy);
  }
  {
    auto baseline = test(false, ZeroCopyTestModel::TwoMatMulsAddOutput);
    auto zerocopy = test(true, ZeroCopyTestModel::TwoMatMulsAddOutput);
    compare(baseline, zerocopy);
  }
  {
    auto baseline = test(false, ZeroCopyTestModel::TwoMatMulsSharedWeight);
    auto zerocopy = test(true, ZeroCopyTestModel::TwoMatMulsSharedWeight);
    compare(baseline, zerocopy);
  }
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
//using namespace std;

#include <boost/core/ignore_unused.hpp>
#include <boost/type_traits.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/equal_to.hpp>

using namespace boost;
using namespace boost::mpl;

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

#include <boost/mpl/assert.hpp>

void case1()
{
    typedef mpl::vector<int, char> vec;

    BOOST_MPL_ASSERT((is_same<int,
                front<vec>::type> ));

    //BOOST_MPL_ASSERT((equal_to<
    //            size<vec>::type, int_<3>>));

}

///////////////////////////////////////
template<typename T>
struct my_operation
{
    BOOST_MPL_ASSERT_NOT((is_integral<T>));
    BOOST_MPL_ASSERT((
            is_integral<typename T::value_type>));

    typedef typename next<T>::type type;
};

void case2()
{
    typedef my_operation<int_<3>>::type t;
    //typedef my_operation<int>::type error;

    boost::ignore_unused<t>();

    BOOST_MPL_ASSERT_RELATION(int_<5>::value, >, 0);
    //BOOST_MPL_ASSERT_RELATION(sizeof(int), >, sizeof(long));
}

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

template<typename T>
struct my_operation2
{
    BOOST_MPL_ASSERT_MSG(!is_pod<T>::value,
        IS_POD_ERROR, (T));
};

void case3()
{
    //BOOST_MPL_ASSERT_MSG(1>2, DEMO_MESSAGE, ());
    //my_operation2<int>::type;
}

///////////////////////////////////////
#include <boost/mpl/print.hpp>
#include <boost/mpl/transform.hpp>

void case4()
{
    typedef mpl::print<int>::type x;
    typedef mpl::vector<char, int_<5>, float> vec;
    typedef transform<vec, print<_>>::type none;

    boost::ignore_unused<x, none>();
}

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

int main()
{
    std::cout << "hello debug" << std::endl;

    case1();
    case2();
    case3();
    //case4();
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_FUNCTION_DERIVATIVE_NUMERICAL_DERIVATIVE_HPP__
#define __OPTIMISATION2_FUNCTION_DERIVATIVE_NUMERICAL_DERIVATIVE_HPP__

#include "detail_derivative.hpp"
#include <libv/lma/ttt/traits/unroll1.hpp>
#include <boost/fusion/include/vector_tie.hpp>
#include <libv/lma/lm/trait/accessor.hpp>

template<class T >
struct Container;

namespace lma
{
  //! calcul de dérivée numérique avec déroulage static sur les blocks, puis sur les paramètres de chaque block, puis sur chaque résidu

  namespace detail
  {
    template<class Float, std::size_t I, std::size_t Fin> struct TupleDerivator;
    template<class Float, std::size_t Fin> struct TupleDerivator<Float,Fin,Fin>
    {
      template<class A, class B, class C, class D> static void compute(const A&, const B&, const C&, const D&){}
      template<class A, class B, class C> static void compute(const A&, const B&, const C&){}
    };

    template<class Float, std::size_t K, size_t I, std::size_t F> struct TupleDerivatorInternal;
    template<class Float, std::size_t K, size_t F> struct TupleDerivatorInternal<Float,K,F,F>
    {
      template<class Fonction, class Result, class Tuple, class R1> static void compute(const Fonction&, Result&, Tuple&, const R1&){}
      template<class Fonction, class Result, class Tuple> static void compute(const Fonction&, Result&, Tuple&){}
    };

    template<class Float, size_t I, class TIE> struct FinalUnroll
    {
      TIE& tie;
      FinalUnroll(TIE& tie_):tie(tie_){}

      template<size_t K, class A, class D> inline void compute(A& a, const Float& b, const Float& c, const D& d) const
      {
      	static_assert( K==0, "K==0" );
      	std::get<K,I>(a) = ( b - c ) * d;
        //a(K,I) = ( b - c ) * d;
      }

      template<size_t K, class A, class B, class C, class D> inline void compute(A& a, const B& b, const C& c, const D& d) const
      {
	       //a(K,I) = ( std::get<K>(b) - std::get<K>(c) ) * d;
         std::get<K,I>(a) = ( std::get<K>(b) - std::get<K>(c) ) * d;
      }

      template<size_t K> inline void operator()(Int<K> const &) const
      {
	       compute<K>(bf::at_c<0>(tie),bf::at_c<1>(tie),bf::at_c<2>(tie),bf::at_c<3>(tie));
      }
    };

    template<class Float, std::size_t K, size_t I, std::size_t Fin> struct TupleDerivatorInternal
    {
      // forward derivative
      template<class Fonction, class Result, class Tuple, class R> static void compute(const Fonction& fonction, Result& result, Tuple& tuple, const R& r1)
      {
        static const Float h = Float(2.0)*std::sqrt( std::numeric_limits<Float>::epsilon() );
        static const Float _h = Float(1.0) / h ;
        //! to_ref : on crée une référence vers l'objet contenu dans le tuple (uniquement pour simplifier l'écriture)
        //! -> to_ref renvoie T& que l'objet contenu soit T ou T*
        //! at -> renvoie at_c<I>(tuple) ou at_c<I>(map).second

        auto& ref_objet = ttt::to_ref(bf::at_c<K>(tuple));
        auto& jacob = bf::at_c<K>(result).second;

        BOOST_MPL_ASSERT((boost::is_reference<decltype(ref_objet)>));
        assert( (&ref_objet == &ttt::to_ref(bf::at_c<K>(tuple))) );

        auto backup = back_up<I>(ref_objet);
        detail::internal_apply_small_increment(ref_objet,h,v::numeric_tag<I>());
        typedef typename Fonction::ErreurType Residu;
        Residu r2;
        bool b2 = fonction(tuple,r2);
        backup.restore();

        if(b2) // functor evaluation didn't failed
        {
          auto tie = bf::vector_tie(jacob,r2,r1,_h);
          ttt::unroll<0,Size<R>::value>(FinalUnroll<Float,I,decltype(tie)>(tie));
          TupleDerivatorInternal<Float,K,I+1,Fin>::template compute(fonction,result,tuple,r1);
          return;
        }

        set_zero(jacob);
      }

      // central derivative
      template<class Fonction, class Result, class Tuple> static void compute(const Fonction& fonction, Result& result, Tuple& tuple)
      {
        static const Float h = Float(2.0)*std::sqrt( std::numeric_limits<Float>::epsilon() );
        static const Float _h = Float(1.0) / (2.0*h) ;
        //! to_ref : on crée une référence vers l'objet contenu dans le tuple (uniquement pour simplifier l'écriture)
        //! -> to_ref renvoie T& que l'objet contenu soit T ou T*
        //! at -> renvoie at_c<I>(tuple) ou at_c<I>(map).second

        auto& ref_objet = ttt::to_ref(bf::at_c<K>(tuple));
        auto& jacob = bf::at_c<K>(result).second;

        BOOST_MPL_ASSERT((boost::is_reference<decltype(ref_objet)>));
        assert( (&ref_objet == &ttt::to_ref(bf::at_c<K>(tuple))) );

        typedef typename Fonction::ErreurType Residu;
        Residu r2;

        auto backup = back_up<I>(ref_objet);
        detail::internal_apply_small_increment(ref_objet,h,v::numeric_tag<I>());
        bool b2 = fonction(tuple,r2);
        backup.restore();
        if (b2)
        {
          detail::internal_apply_small_increment(ref_objet,-h,v::numeric_tag<I>());
          Residu r1;
          bool b1 = fonction(tuple,r1);
          backup.restore();

          if(b1)
          {
            auto tie = bf::vector_tie(jacob,r2,r1,_h);
            ttt::unroll<0,Size<Residu>::value>(FinalUnroll<Float,I,decltype(tie)>(tie));
            TupleDerivatorInternal<Float,K,I+1,Fin>::template compute(fonction,result,tuple);
            return;
          }
        }
        set_zero(jacob);
      }
    };

    template<class Float, std::size_t I, std::size_t Fin> struct TupleDerivator
    {
      // numerical central derivative
      template<class Fonction, class Result, class Tuple> static void compute(const Fonction& fonction, Result& result, Tuple& tuple)
      {
        static const size_t F = Size<decltype(ttt::to_ref(bf::at_c<I>(tuple)))>::value;
        TupleDerivatorInternal<Float,I,0,F>::template compute(fonction,result,tuple);
        TupleDerivator<Float,I+1,Fin>::template compute(fonction,result,tuple);
      }

      // numerical foward derivative
      template<class Fonction, class Result, class Tuple, class R1> static void compute(const Fonction& fonction, Result& result, Tuple& tuple, const R1& r1)
      {
        static const size_t F = Size<decltype(ttt::to_ref(bf::at_c<I>(tuple)))>::value;
        TupleDerivatorInternal<Float,I,0,F>::template compute(fonction,result,tuple,r1);
        TupleDerivator<Float,I+1,Fin>::template compute(fonction,result,tuple,r1);
      }
    };
  }// eon detail

  template<class Tag> struct NumericalDerivator
  {
    //! le tuple en entrée ne doit pas être constant car il sera modifié, et remis à l'état d'origine
    template<class Fonctor, class Tuple, class Jacob>
    static void derive(const Function<Fonctor>& fonctor, Tuple tuple, Jacob& result)
    {
      detail::TupleDerivator<typename Tag::second_type, 0,br::size<Jacob>::value>::template compute(fonctor,result,tuple);
    }

    template<class Fonctor, class Tuple, class Jacob, class Residual>
    static void derive(const Function<Fonctor>& fonctor, Tuple tuple, Jacob& result, const Residual& residual)
    {
      detail::TupleDerivator<typename Tag::second_type, 0,br::size<Jacob>::value>::template compute(fonctor,result,tuple,residual);
    }
  };

}// eon

namespace ttt
{
  template<class T> struct Name<lma::NumericalDerivator<T>> { static std::string name(){ return "NumericalDerivator<" + ttt::name<T>() + ">"; } };
}

#endif