//=======================================================================
// Copyright 2000 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// 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 EDMUNDS_KARP_MAX_FLOW_HPP
#define EDMUNDS_KARP_MAX_FLOW_HPP

#include <boost/config.hpp>
#include <vector>
#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/pending/queue.hpp>
#include <boost/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/breadth_first_search.hpp>

namespace boost {

  // The "labeling" algorithm from "Network Flows" by Ahuja, Magnanti,
  // Orlin.  I think this is the same as or very similar to the original
  // Edmunds-Karp algorithm.  This solves the maximum flow problem.

  namespace detail {

    template <class Graph, class ResCapMap>
    filtered_graph<Graph, is_residual_edge<ResCapMap> >
    residual_graph(Graph& g, ResCapMap residual_capacity) {
      return filtered_graph<Graph, is_residual_edge<ResCapMap> >
        (g, is_residual_edge<ResCapMap>(residual_capacity));
    }

    template <class Graph, class PredEdgeMap, class ResCapMap,
              class RevEdgeMap>
    inline void
    augment(Graph& g,
            typename graph_traits<Graph>::vertex_descriptor src,
            typename graph_traits<Graph>::vertex_descriptor sink,
            PredEdgeMap p,
            ResCapMap residual_capacity,
            RevEdgeMap reverse_edge)
    {
      typename graph_traits<Graph>::edge_descriptor e;
      typename graph_traits<Graph>::vertex_descriptor u;
      typedef typename property_traits<ResCapMap>::value_type FlowValue;

      // find minimum residual capacity along the augmenting path
      FlowValue delta = (std::numeric_limits<FlowValue>::max)();
      e = p[sink];
      do {
        BOOST_USING_STD_MIN();
        delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(delta, residual_capacity[e]);
        u = source(e, g);
        e = p[u];
      } while (u != src);

      // push delta units of flow along the augmenting path
      e = p[sink];
      do {
        residual_capacity[e] -= delta;
        residual_capacity[reverse_edge[e]] += delta;
        u = source(e, g);
        e = p[u];
      } while (u != src);
    }

  } // namespace detail

  template <class Graph,
            class CapacityEdgeMap, class ResidualCapacityEdgeMap,
            class ReverseEdgeMap, class ColorMap, class PredEdgeMap>
  typename property_traits<CapacityEdgeMap>::value_type
  edmunds_karp_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink,
     CapacityEdgeMap cap,
     ResidualCapacityEdgeMap res,
     ReverseEdgeMap rev,
     ColorMap color,
     PredEdgeMap pred)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;

    typename graph_traits<Graph>::vertex_iterator u_iter, u_end;
    typename graph_traits<Graph>::out_edge_iterator ei, e_end;
    for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
      for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
        res[*ei] = cap[*ei];

    color[sink] = Color::gray();
    while (color[sink] != Color::white()) {
      boost::queue<vertex_t> Q;
      breadth_first_search
        (detail::residual_graph(g, res), src, Q,
         make_bfs_visitor(record_edge_predecessors(pred, on_tree_edge())),
         color);
      if (color[sink] != Color::white())
        detail::augment(g, src, sink, pred, res, rev);
    } // while

    typename property_traits<CapacityEdgeMap>::value_type flow = 0;
    for (tie(ei, e_end) = out_edges(src, g); ei != e_end; ++ei)
      flow += (cap[*ei] - res[*ei]);
    return flow;
  } // edmunds_karp_max_flow()

  namespace detail {
    //-------------------------------------------------------------------------
    // Handle default for color property map

    // use of class here is a VC++ workaround
    template <class ColorMap>
    struct edmunds_karp_dispatch2 {
      template <class Graph, class PredMap, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply
      (Graph& g,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink,
       PredMap pred,
       const bgl_named_params<P, T, R>& params,
       ColorMap color)
      {
        return edmunds_karp_max_flow
          (g, src, sink,
           choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
           choose_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
           color, pred);
      }
    };
    template<>
    struct edmunds_karp_dispatch2<detail::error_property_not_found> {
      template <class Graph, class PredMap, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply
      (Graph& g,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink,
       PredMap pred,
       const bgl_named_params<P, T, R>& params,
       detail::error_property_not_found)
      {
        typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        size_type n = is_default_param(get_param(params, vertex_color)) ?
          num_vertices(g) : 1;
        std::vector<default_color_type> color_vec(n);
        return edmunds_karp_max_flow
          (g, src, sink,
           choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
           choose_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
           make_iterator_property_map(color_vec.begin(), choose_const_pmap
                                      (get_param(params, vertex_index),
                                       g, vertex_index), color_vec[0]),
           pred);
      }
    };

    //-------------------------------------------------------------------------
    // Handle default for predecessor property map

    // use of class here is a VC++ workaround
    template <class PredMap>
    struct edmunds_karp_dispatch1 {
      template <class Graph, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply(Graph& g,
            typename graph_traits<Graph>::vertex_descriptor src,
            typename graph_traits<Graph>::vertex_descriptor sink,
            const bgl_named_params<P, T, R>& params,
            PredMap pred)
      {
        typedef typename property_value< bgl_named_params<P,T,R>, vertex_color_t>::type C;
        return edmunds_karp_dispatch2<C>::apply
          (g, src, sink, pred, params, get_param(params, vertex_color));
      }
    };
    template<>
    struct edmunds_karp_dispatch1<detail::error_property_not_found> {

      template <class Graph, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply
      (Graph& g,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink,
       const bgl_named_params<P, T, R>& params,
       detail::error_property_not_found)
      {
        typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        size_type n = is_default_param(get_param(params, vertex_predecessor)) ?
          num_vertices(g) : 1;
        std::vector<edge_descriptor> pred_vec(n);

        typedef typename property_value< bgl_named_params<P,T,R>, vertex_color_t>::type C;
        return edmunds_karp_dispatch2<C>::apply
          (g, src, sink,
           make_iterator_property_map(pred_vec.begin(), choose_const_pmap
                                      (get_param(params, vertex_index),
                                       g, vertex_index), pred_vec[0]),
           params,
           get_param(params, vertex_color));
      }
    };

  } // namespace detail

  template <class Graph, class P, class T, class R>
  typename detail::edge_capacity_value<Graph, P, T, R>::type
  edmunds_karp_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink,
     const bgl_named_params<P, T, R>& params)
  {
    typedef typename property_value< bgl_named_params<P,T,R>, vertex_predecessor_t>::type Pred;
    return detail::edmunds_karp_dispatch1<Pred>::apply
      (g, src, sink, params, get_param(params, vertex_predecessor));
  }

  template <class Graph>
  typename property_traits<
    typename property_map<Graph, edge_capacity_t>::const_type
  >::value_type
  edmunds_karp_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink)
  {
    bgl_named_params<int, buffer_param_t> params(0);
    return edmunds_karp_max_flow(g, src, sink, params);
  }

} // namespace boost

#endif // EDMUNDS_KARP_MAX_FLOW_HPP


#include "scheme/nest/pmap/UnitMap.hh"
#include "scheme/nest/pmap/ScaleMap.hh"
#include "scheme/nest/pmap/DiscreteChoiceMap.hh"
#include "scheme/nest/NEST.hh"
#include "scheme/nest/NEST_test_util.hh"
#include <gtest/gtest.h>
#include <boost/assign/std/vector.hpp> // for 'operator+=()'
#include <random>
#include <boost/foreach.hpp>

namespace scheme {
namespace nest {
namespace pmap {

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

using scheme::nest::StorePointer;

TEST(NEST_ScaleMap,particular_values){
	{
		typedef util::SimpleArray<1,double> VAL;
		VAL lb, ub;
		util::SimpleArray<1,size_t> bs;
		lb = -16.0;
		ub =  16.0;
		bs = 1;
		NEST<1,VAL,ScaleMap> nest1(lb,ub,bs);
		bs = 2;
		NEST<1,VAL,ScaleMap> nest2(lb,ub,bs);
		ASSERT_EQ( nest1.set_and_get(0,0)[0],  0.0 );
		ASSERT_EQ( nest1.set_and_get(0,1)[0], -8.0 );
		ASSERT_EQ( nest1.set_and_get(1,1)[0],  8.0 );

		ASSERT_EQ( nest2.set_and_get(0,0)[0], -8.0 );
		ASSERT_EQ( nest2.set_and_get(1,0)[0],  8.0 );
		ASSERT_EQ( nest2.set_and_get(0,1)[0],-12.0 );
		ASSERT_EQ( nest2.set_and_get(1,1)[0], -4.0 );
		ASSERT_EQ( nest2.set_and_get(2,1)[0],  4.0 );
		ASSERT_EQ( nest2.set_and_get(3,1)[0], 12.0 );
	}
	{
		typedef util::SimpleArray<2,double> VAL;
		util::SimpleArray<2,double> lb(-16.0,-24), ub(16.0, 24);
		util::SimpleArray<2,size_t> bs(2,3);
		NEST<2,VAL,ScaleMap> nest(lb,ub,bs);
		ASSERT_EQ( nest.set_and_get(0,0), VAL(-8,-16) );
		ASSERT_EQ( nest.set_and_get(1,0), VAL( 8,-16) );
		ASSERT_EQ( nest.set_and_get(2,0), VAL(-8,  0) );
		ASSERT_EQ( nest.set_and_get(3,0), VAL( 8,  0) );
		ASSERT_EQ( nest.set_and_get(4,0), VAL(-8, 16) );
		ASSERT_EQ( nest.set_and_get(5,0), VAL( 8, 16) );
		ASSERT_EQ( nest.set_and_get(0,1), VAL(-12,-20) );
		ASSERT_EQ( nest.set_and_get(1,1), VAL( -4,-20) );
		ASSERT_EQ( nest.set_and_get(2,1), VAL(-12,-12) );
		ASSERT_EQ( nest.set_and_get(3,1), VAL( -4,-12) );
		ASSERT_EQ( nest.set_and_get(0,2), VAL(-14,-22) );
		ASSERT_EQ( nest.set_and_get(1,2), VAL(-10,-22) );
		ASSERT_EQ( nest.set_and_get(2,2), VAL(-14,-18) );
		ASSERT_EQ( nest.set_and_get(3,2), VAL(-10,-18) );
		ASSERT_EQ( nest.set_and_get(0,3), VAL(-15,-23) );
		ASSERT_EQ( nest.set_and_get(1,3), VAL(-13,-23) );
		ASSERT_EQ( nest.set_and_get(2,3), VAL(-15,-21) );
		ASSERT_EQ( nest.set_and_get(3,3), VAL(-13,-21) );
		ASSERT_EQ( nest.set_and_get(5*64+60,3), VAL(13,21) );
		ASSERT_EQ( nest.set_and_get(5*64+61,3), VAL(15,21) );
		ASSERT_EQ( nest.set_and_get(5*64+62,3), VAL(13,23) );
		ASSERT_EQ( nest.set_and_get(5*64+63,3), VAL(15,23) );
		// cout << VAL(0,0) << endl;
	}
}

TEST(UnitMap,value_to_params_for_cell){
	typedef UnitMap<2> MapType;
	MapType umap(3);
	MapType::Params params;

	umap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, 0  );
	ASSERT_EQ( params[0], 1.5 );
	ASSERT_EQ( params[1], 0.5 );

	umap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, 1  );
	ASSERT_EQ( params[0], 0.5 );
	ASSERT_EQ( params[1], 0.5 );

	umap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, 2  );
	ASSERT_EQ( params[0], -0.5 );
	ASSERT_EQ( params[1],  0.5 );

	umap.value_to_params_for_cell( MapType::ValueType(0.2,0.5), 0, params, 2  );
	ASSERT_EQ( params[0], -1.8 );
	ASSERT_EQ( params[1],  0.5 );
}

TEST(ScaleMap,value_to_params_for_cell){
	{
		typedef ScaleMap<2> MapType;
		MapType smap(
			MapType::Params(0,0),
			MapType::Params(4,4),
			MapType::Indices(4,4)
		);

		MapType::Params params;

		smap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, smap.indices_to_cellindex(MapType::Indices(0,0)) );
		ASSERT_EQ( params[0], 1.5 );
		ASSERT_EQ( params[1], 0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, smap.indices_to_cellindex(MapType::Indices(1,0)) );
		ASSERT_EQ( params[0], 0.5 );
		ASSERT_EQ( params[1], 0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, smap.indices_to_cellindex(MapType::Indices(0,1)) );
		ASSERT_EQ( params[0],  1.5 );
		ASSERT_EQ( params[1], -0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, smap.indices_to_cellindex(MapType::Indices(2,0)) );
		ASSERT_EQ( params[0],-0.5 );
		ASSERT_EQ( params[1], 0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, smap.indices_to_cellindex(MapType::Indices(1,2)) );
		ASSERT_EQ( params[0],  0.5 );
		ASSERT_EQ( params[1], -1.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5,0.5), 0, params, smap.indices_to_cellindex(MapType::Indices(3,3)) );
		ASSERT_EQ( params[0], -1.5 );
		ASSERT_EQ( params[1], -2.5 );
	}
	{
		typedef ScaleMap<2> MapType;
		double scale = 2.0;
		MapType::Params shift(0.597,1.1243);
		MapType smap(
			MapType::Params(0.0,0.0)*scale-shift,
			MapType::Params(4.0,4.0)*scale-shift,
			MapType::Indices(4,4)
		);

		MapType::Params params;

		smap.value_to_params_for_cell( MapType::ValueType(1.5*scale-shift[0],0.5*scale-shift[1]), 0, params, smap.indices_to_cellindex(MapType::Indices(0,0)) );
		ASSERT_EQ( params[0], 1.5 );
		ASSERT_EQ( params[1], 0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5*scale-shift[0],0.5*scale-shift[1]), 0, params, smap.indices_to_cellindex(MapType::Indices(1,0)) );
		ASSERT_EQ( params[0], 0.5 );
		ASSERT_EQ( params[1], 0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5*scale-shift[0],0.5*scale-shift[1]), 0, params, smap.indices_to_cellindex(MapType::Indices(0,1)) );
		ASSERT_EQ( params[0],  1.5 );
		ASSERT_EQ( params[1], -0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5*scale-shift[0],0.5*scale-shift[1]), 0, params, smap.indices_to_cellindex(MapType::Indices(2,0)) );
		ASSERT_EQ( params[0],-0.5 );
		ASSERT_EQ( params[1], 0.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5*scale-shift[0],0.5*scale-shift[1]), 0, params, smap.indices_to_cellindex(MapType::Indices(1,2)) );
		ASSERT_EQ( params[0],  0.5 );
		ASSERT_EQ( params[1], -1.5 );

		smap.value_to_params_for_cell( MapType::ValueType(1.5*scale-shift[0],0.5*scale-shift[1]), 0, params, smap.indices_to_cellindex(MapType::Indices(3,3)) );
		ASSERT_EQ( params[0], -1.5 );
		ASSERT_EQ( params[1], -2.5 );
	}
}

template<int DIM>
void test_index_lookup_scaled(){
	typedef util::SimpleArray<DIM,double> VAL;
	util::SimpleArray<10,double> lb0(-1.3,2.2,0,-3,-5,-9.9,1.3,44,-13.3,99);
	util::SimpleArray<10,double> ub0(1.3,4.2,1,10,-3, 9.9,4.3,44, 13.3,199);
	util::SimpleArray<10,size_t> bs0(2,1,2,3,4,5,6,7,8,9);

	typedef util::SimpleArray<DIM,double> VAL;
	VAL lb, ub;
	util::SimpleArray<DIM,size_t> bs;
	for(size_t i = 0; i < DIM; ++i){
		lb[i] = lb0[i]; ub[i] = ub0[i]; bs[i] = bs0[i];
	}
	NEST<DIM,VAL,ScaleMap,StoreValue> nest(lb,ub,bs);
	size_t rmax = 6/DIM;
	for(size_t r = 0; r <= rmax; ++r){
		for(size_t i = 0; i < nest.size(r); ++i){
			ASSERT_TRUE( nest.set_state(i,r) );
			VAL value = nest.value();
			size_t index = nest.get_index(value,r);
			ASSERT_EQ( i, index );
			for(size_t r2 = 0; r2 <= r; ++r2){
				size_t index2 = nest.get_index(value,r2);
				ASSERT_EQ( i>>(DIM*(r-r2)), index2 );
			}
		}
	}
}

TEST(NEST_ScaleMap,index_lookup_scaled){
	test_index_lookup_scaled<1>();
	test_index_lookup_scaled<2>();
	test_index_lookup_scaled<3>();
	test_index_lookup_scaled<4>();
	test_index_lookup_scaled<5>();
	test_index_lookup_scaled<6>();
}

template<int DIM>
void test_map_scale_bounds(){
	BOOST_STATIC_ASSERT((DIM<10));
	util::SimpleArray<10,double> lb0(-1.3,2.2,0,-3,-5,-9.9,1.3,44,-13.3,99),ub0(1.3,4.2,1,10,-3, 9.9,4.3,44, 13.3,199);
	util::SimpleArray<10,size_t> bs0(1,2,3,4,5,6,7,8,9,10);

	typedef util::SimpleArray<DIM,double> VAL;
	VAL lb, ub;
	util::SimpleArray<DIM,size_t> bs;
	for(size_t i = 0; i < DIM; ++i){
		lb[i] = lb0[i]; ub[i] = ub0[i]; bs[i] = bs0[i];
	}
	NEST<DIM,VAL,ScaleMap,StoreValue> nest(lb,ub,bs);

	size_t resl = 8/DIM;
	for(size_t i = 0; i < nest.size(resl); ++i){
		ASSERT_TRUE( nest.set_state(i,resl) );
		for(size_t j = 0; j < DIM; ++j){
			ASSERT_LT( lb[j], nest.value()[j] ); ASSERT_LT( nest.value()[j] , ub[j] );
		}
		ASSERT_FALSE( nest.set_state(i+nest.size(resl),resl) );
		for(size_t j = 0; j < DIM; ++j){
			ASSERT_LT( lb[j], nest.value()[j] ); ASSERT_LT( nest.value()[j] , ub[j] );
		}
	}

}

TEST(NEST_ScaleMap,map_scale){
	test_map_scale_bounds<1>();
	test_map_scale_bounds<2>();
	test_map_scale_bounds<3>();
	test_map_scale_bounds<4>();
	test_map_scale_bounds<5>();
	test_map_scale_bounds<6>();
}

template<class NEST>
void test_bin_circumradius(
	NEST nest,
	typename NEST::ValueType lb,
	typename NEST::ValueType ub
){
    size_t NITER = 10*1000;
	#ifdef SCHEME_BENCHMARK
	NITER *= 50;
	#endif
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> uniform;
	typename NEST::ValueType randpt;
	for(size_t r = 0; r <= std::min((size_t)10,(size_t)NEST::MAX_RESL_ONE_CELL); ++r){
		double maxdis = 0;
		for(size_t iter=0; iter < NITER/NEST::DIMENSION; ++iter){
			for(size_t i = 0; i < NEST::DIMENSION; ++i)
				randpt[i] = uniform(rng)*(ub[i]-lb[i])+lb[i];
			nest.set_state( nest.get_index(randpt,r) ,r);
			double dist = (randpt-nest.value()).norm();
			// cout << randpt.transpose() << " " << nest.value().transpose() << endl;
			maxdis = fmax(maxdis,dist);
			ASSERT_LE( dist, nest.bin_circumradius(r) );
		}
		// covering radius should be reasonably tigth
		// cout << NEST::DIMENSION << " " << r << " " << nest.bin_circumradius(r) << " " << maxdis << endl;
		if( nest.bin_circumradius(r)*(1.0-(double)NEST::DIMENSION/20.0) > maxdis ){
			cout << "WARNING(PROBABILISTIC): covering radius may be too loose DIM=" << NEST::DIMENSION << " resl=" << r << endl;
			cout << "                        covering radius " << nest.bin_circumradius(r) << " max observerd: " << maxdis << endl;
			cout << "                        if you see a handful of these, don't worry. if you see lots, then worry" << endl;
		}
		// cout << DIM << " " << maxdis << " " << nest.bin_circumradius(r) << std::endl;
	}
}

TEST(NEST_UnitMap,NEST_bin_circumradius_unitmap){
	{ NEST<1>::ValueType lb,ub; lb.fill(0); ub.fill(1); test_bin_circumradius( NEST<1>(), lb, ub ); }
	{ NEST<2>::ValueType lb,ub; lb.fill(0); ub.fill(1); test_bin_circumradius( NEST<2>(), lb, ub ); }
	{ NEST<3>::ValueType lb,ub; lb.fill(0); ub.fill(1); test_bin_circumradius( NEST<3>(), lb, ub ); }
	{ NEST<4>::ValueType lb,ub; lb.fill(0); ub.fill(1); test_bin_circumradius( NEST<4>(), lb, ub ); }
	{ NEST<5>::ValueType lb,ub; lb.fill(0); ub.fill(1); test_bin_circumradius( NEST<5>(), lb, ub ); }
	{ NEST<6>::ValueType lb,ub; lb.fill(0); ub.fill(1); test_bin_circumradius( NEST<6>(), lb, ub ); }
}

template<int DIM>
void test_bin_circumradius_scalemap(){
	typedef NEST<DIM,util::SimpleArray<DIM,double>,ScaleMap> NestType;
	typename NestType::ValueType lb, ub;
	for(size_t i = 0; i < DIM; ++i){
		lb[i] = -1;
		ub[i] = 1+2*i;
	}
	NestType nest(lb,ub);
	test_bin_circumradius< NestType >( nest, lb, ub );
}

TEST(NEST_ScaleMap,NEST_bin_circumradius_scalemap){
	test_bin_circumradius_scalemap<1>();
	test_bin_circumradius_scalemap<2>();
	test_bin_circumradius_scalemap<3>();
	test_bin_circumradius_scalemap<4>();
	test_bin_circumradius_scalemap<5>();
	test_bin_circumradius_scalemap<6>();
}

template<class NestType>
void test_coverage_random_ScaleMap(){
    size_t NITER = 1*1000;
    double FUDGE = 0.06;
	#ifdef SCHEME_BENCHMARK
	NITER *= 50;
	FUDGE = 0.03;
	#endif

	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> uniform;

	// set up random bounds
	typename NestType::Params lb,ub;
	typename NestType::Indices cs;
	for(size_t i = 0; i < NestType::DIMENSION; ++i){
		double b1 = uniform(rng)*20.0 - 10.0;
		double b2 = uniform(rng)*20.0 - 10.0;
		lb[i] = std::min(b1,b2);
		ub[i] = std::max(b1,b2);
		cs[i] = (typename NestType::IndexType)(uniform(rng)*3.999) + 1;
		assert(lb[i] < ub[i]);
		assert(cs[i] > 0);
	}
	// cout << "LB " << lb.transpose() << endl;
	// cout << "UB " << ub.transpose() << endl;
	// cout << "CS " << cs.transpose() << endl;

	NestType nest(lb,ub,cs);
	size_t max_resl = std::min((size_t)10,(size_t)NestType::MAX_RESL_ONE_CELL-2); // -2 because we set cs up to 4 per dimension
	std::vector<double> largest_d2_for_r(max_resl+1,0.0);
	for(size_t i = 0; i < NITER; ++i){

		// set up random value within bounds
		typename NestType::ValueType val;
		for(size_t j = 0; j < NestType::DIMENSION; ++j){
			val[j] = (ub[j]-lb[j])*uniform(rng) + lb[j];
			assert(lb[j] <= val[j]);
			assert(ub[j] >= val[j]);
		}

		// run generic coverage test
		generic_test_coverage_of_value( nest, val, largest_d2_for_r, max_resl );

	}
	for(size_t r = 0; r <= max_resl; ++r){
		// cout << r << " " << largest_d2_for_r[r] << endl;
		// factor of 1.0+0.03*DIM is a *TOTAL* hack, errer does not scale this way by dimension
		// but it's enough to make sure the curcumradius is reasonably tight
		ASSERT_LT(  nest.bin_circumradius(r), (1.0+FUDGE*(double)NestType::DIMENSION)*sqrt(largest_d2_for_r[r]) );
	}

}

TEST(NEST_ScaleMap,test_coverage_DIM_1_to_9){
	test_coverage_random_ScaleMap<  NEST<1,util::SimpleArray<1,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<2,util::SimpleArray<2,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<3,util::SimpleArray<3,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<4,util::SimpleArray<4,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<5,util::SimpleArray<5,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<6,util::SimpleArray<6,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<7,util::SimpleArray<7,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<8,util::SimpleArray<8,double>,ScaleMap>  >();
	test_coverage_random_ScaleMap<  NEST<9,util::SimpleArray<9,double>,ScaleMap>  >();
}

}
}
}


#include <QApplication>
#include <QMessageBox>
#include <QMainWindow>
#include "Kernel_type.h"
#include "Polyhedron_type.h"
#include "Scene_polyhedron_item.h"
#include "Scene_polylines_item.h"

#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include <CGAL/Polygon_mesh_processing/stitch_borders.h>

#include <CGAL/boost/graph/split_graph_into_polylines.h>
#include <CGAL/boost/graph/helpers.h>
#include <boost/graph/filtered_graph.hpp>

template <typename G>
struct Is_border {
  const G& g;
  Is_border(const G& g)
    : g(g)
  {}

 template <typename Descriptor>
  bool operator()(const Descriptor& d) const {
   return is_border(d,g);
  }

  bool operator()(typename boost::graph_traits<G>::vertex_descriptor d) const {
    return is_border(d,g) != boost::none;
  }

};

using namespace CGAL::Three;
class Polyhedron_demo_polyhedron_stitching_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

  QAction* actionDetectBorders;
  QAction* actionStitchBorders;
public:
  QList<QAction*> actions() const { return QList<QAction*>() << actionDetectBorders << actionStitchBorders; }
  using Polyhedron_demo_plugin_helper::init;
  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface* /* m */)
  {
    actionDetectBorders= new QAction(tr("Detect Boundaries"), mainWindow);
    actionStitchBorders= new QAction(tr("Stitch Duplicated Boundaries"), mainWindow);
    actionDetectBorders->setObjectName("actionDetectBorders");
    actionStitchBorders->setObjectName("actionStitchBorders");
    actionStitchBorders->setProperty("subMenuName", "Polygon Mesh Processing");
    actionDetectBorders->setProperty("subMenuName", "Polygon Mesh Processing");
    Polyhedron_demo_plugin_helper::init(mainWindow, scene_interface);
  }

  bool applicable(QAction*) const {
    Q_FOREACH(int index, scene->selectionIndices())
    {
      if ( qobject_cast<Scene_polyhedron_item*>(scene->item(index)) )
        return true;
    }
    return false;
  }

public Q_SLOTS:
  void on_actionDetectBorders_triggered();
  void on_actionStitchBorders_triggered();

}; // end Polyhedron_demo_polyhedron_stitching_plugin

struct Polyline_visitor
{
  Scene_polylines_item* new_item;

  Polyline_visitor(Scene_polylines_item* new_item)
    : new_item(new_item)
  {}

  void start_new_polyline()
  {
    new_item->polylines.push_back( Scene_polylines_item::Polyline() );
  }

  void add_node(boost::graph_traits<Polyhedron>::vertex_descriptor vd)
  {
    new_item->polylines.back().push_back(vd->point());
  }
  void end_polyline(){}
};

void Polyhedron_demo_polyhedron_stitching_plugin::on_actionDetectBorders_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices())
  {
    Scene_polyhedron_item* item =
      qobject_cast<Scene_polyhedron_item*>(scene->item(index));

    if(item)
    {
      Scene_polylines_item* new_item = new Scene_polylines_item();

      Polyhedron* pMesh = item->polyhedron();
      pMesh->normalize_border();

#if 0
      for (Polyhedron::Halfedge_iterator
              it=pMesh->border_halfedges_begin(), it_end=pMesh->halfedges_end();
              it!=it_end; ++it)
      {
        if (!it->is_border()) continue;
        /// \todo build cycles and graph with nodes of valence 2.
        new_item->polylines.push_back( Scene_polylines_item::Polyline() );
        new_item->polylines.back().push_back( it->opposite()->vertex()->point() );
        new_item->polylines.back().push_back( it->vertex()->point() );
      }
#else
      typedef boost::filtered_graph<Polyhedron,Is_border<Polyhedron>, Is_border<Polyhedron> > BorderGraph;

      Is_border<Polyhedron> ib(*pMesh);
      BorderGraph bg(*pMesh,ib,ib);
      Polyline_visitor polyline_visitor(new_item);
      CGAL::split_graph_into_polylines( bg,
                                        polyline_visitor,
                                        CGAL::internal::IsTerminalDefault() );
#endif

      if (new_item->polylines.empty())
      {
        delete new_item;
      }
      else
      {
        new_item->setName(tr("Boundary of %1").arg(item->name()));
        new_item->setColor(Qt::red);
        scene->addItem(new_item);
        new_item->invalidateOpenGLBuffers();
      }
    }
  }
}

void Polyhedron_demo_polyhedron_stitching_plugin::on_actionStitchBorders_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices())
  {
    Scene_polyhedron_item* item =
      qobject_cast<Scene_polyhedron_item*>(scene->item(index));

    if(item)
    {
      Polyhedron* pMesh = item->polyhedron();
      CGAL::Polygon_mesh_processing::stitch_borders(*pMesh);
      item->invalidateOpenGLBuffers();
      scene->itemChanged(item);
    }
  }
}

#include "Polyhedron_stitching_plugin.moc"


// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/* least_squares_test.cc
   Jeremy Barnes, 25 February 2008
   Copyright (c) 2008 Jeremy Barnes.  All rights reserved.

   Test of the least squares class.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

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

#include <vector>
#include <stdint.h>
#include <iostream>

#include "mldb/plugins/jml/algebra/irls.h"
#include "mldb/utils/vector_utils.h"

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;
using namespace boost::test_tools;

namespace ML {
extern __thread std::ostream * debug_remove_dependent;
}

template<typename Float>
void do_test_identity()
{
    boost::multi_array<Float, 2> array(boost::extents[2][2]);
    array[0][0] = 1;
    array[1][1] = 1;

    vector<int> result = remove_dependent(array);
    BOOST_CHECK_EQUAL(array[0][0], 1.0);
    BOOST_CHECK_EQUAL(array[0][1], 0.0);
    BOOST_CHECK_EQUAL(array[1][0], 0.0);
    BOOST_CHECK_EQUAL(array[1][1], 1.0);

    BOOST_REQUIRE_EQUAL(result.size(), 2);
    BOOST_CHECK_EQUAL(result[0], 0);
    BOOST_CHECK_EQUAL(result[1], 1);
}

BOOST_AUTO_TEST_CASE( test_identity )
{
    do_test_identity<double>();
    do_test_identity<float>();
}

template<typename Float>
void do_test_null()
{
    boost::multi_array<Float, 2> array(boost::extents[2][2]);

    BOOST_CHECK_EQUAL(array.shape()[0], 2);
    BOOST_CHECK_EQUAL(array.shape()[1], 2);

    debug_remove_dependent = &cerr;
    vector<int> result = remove_dependent(array);
    debug_remove_dependent = 0;

    BOOST_CHECK_EQUAL(array.shape()[0], 0);
    BOOST_CHECK_EQUAL(array.shape()[1], 2);

    BOOST_REQUIRE_EQUAL(result.size(), 2);
    BOOST_CHECK_EQUAL(result[0], -1);
    BOOST_CHECK_EQUAL(result[1], -1);
}

BOOST_AUTO_TEST_CASE( test_null )
{
    do_test_null<double>();
    do_test_null<float>();
}

template<typename Float>
void do_test_uniform()
{
    boost::multi_array<Float, 2> array(boost::extents[2][2]);
    array[0][0] = array[0][1] = array[1][0] = array[1][1] = 1.0;

    BOOST_CHECK_EQUAL(array.shape()[0], 2);
    BOOST_CHECK_EQUAL(array.shape()[1], 2);

    debug_remove_dependent = &cerr;
    vector<int> result = remove_dependent(array);
    debug_remove_dependent = 0;

    BOOST_CHECK_EQUAL(array.shape()[0], 1);
    BOOST_CHECK_EQUAL(array.shape()[1], 2);

    BOOST_CHECK_EQUAL(array[0][0], 1.0);
    BOOST_CHECK_EQUAL(array[0][1], 1.0);

    BOOST_REQUIRE_EQUAL(result.size(), 2);
    BOOST_CHECK_EQUAL(result[0],  -1);
    BOOST_CHECK_EQUAL(result[1],   0);
}

BOOST_AUTO_TEST_CASE( test_uniform )
{
    do_test_uniform<double>();
    do_test_uniform<float>();
}

template<typename Float>
void do_test_dependent()
{
    boost::multi_array<Float, 2> array(boost::extents[3][2]);
    array[0][0] = 1;
    array[1][1] = 1;
    array[2][1] = 1;

    BOOST_CHECK_EQUAL(array.shape()[0], 3);
    BOOST_CHECK_EQUAL(array.shape()[1], 2);

    vector<int> result = remove_dependent(array);

    cerr << "result = " << result << endl;

    BOOST_CHECK_EQUAL(array.shape()[0], 2);
    BOOST_CHECK_EQUAL(array.shape()[1], 2);

    BOOST_CHECK_EQUAL(array[0][0], 1.0);
    BOOST_CHECK_EQUAL(array[0][1], 0.0);
    BOOST_CHECK_EQUAL(array[1][0], 0.0);
    BOOST_CHECK_EQUAL(array[1][1], 1.0);

    BOOST_REQUIRE_EQUAL(result.size(), 3);

    BOOST_CHECK_EQUAL(result[0],  0);
    BOOST_CHECK_EQUAL(result[1], -1);
    BOOST_CHECK_EQUAL(result[2],  1);
}

BOOST_AUTO_TEST_CASE( test_dependent )
{
    do_test_dependent<double>();
    do_test_dependent<float>();
}


/// \file
// --------------------------------------------------------------------------
// This file is part of the reference implementation for the paper
//    QFib: Fast and Efficient Brain Tractogram Compression
//    C. Mercier*, S. Rousseau*, P. Gori, I. Bloch and T. Boubekeur
//    NeuroInformatics 2020
//    DOI: 10.1007/s12021-020-09452-0
//
// All rights reserved. Use of this source code is governed by a
// MIT license that can be found in the LICENSE file.
// --------------------------------------------------------------------------
#pragma once

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

#include "saveload.hpp"
#include <vector>

namespace utl
{
///
/// \brief meanMaxError Function to compute average and maximum error between two sets of fibers containing the exact same number of points and fibers
/// \param fibersA Vector containing the first set of fibers
/// \param fibersB Vector containing the second set of fibers
/// \param meanError Average error
/// \param maxError Maximum error
/// \param totalNbPts Variable containing the total number of points in the bundle, used for average error computation
///

void meanMaxError(const std::vector< std::vector<Vector3f> > & fibersA, const std::vector< std::vector<Vector3f> > & fibersB, double & meanError, float & maxError, uint64_t & totalNbPts)
{
	meanError = 0;
	maxError = 0.f;
	assert(fibersA.size() == fibersB.size());
	for(unsigned f = 0; f < fibersA.size(); ++f)
	{
		assert(fibersA[f].size() == fibersB[f].size());
		for(unsigned p = 0; p < fibersA[f].size(); ++p)
		{
			float err = (fibersA[f][p] - fibersB[f][p]).norm();
			maxError = err > maxError ? err : maxError;
			meanError  += err / double(totalNbPts);
		}
	}
}

///
/// \brief error Function to compute the error between two sets of fibers with the exact same number of points and fibers
/// This error is computed depending on the length of the fibers
/// \param fibersA Vector containing the first set of fibers
/// \param fibersB Vector containing the second set of fibers
/// \param minError Minimum error
/// \param meanError Average error
/// \param maxError Maximum error
/// \param minEndPointError Minimum error on every end point
/// \param meanEndPointError Average error on every end point
/// \param maxEndPointError Maximum error on every end point
/// \param totalNbPts Variable containing the total number of points in the bundle, used for average error computation
/// \param steps Number of length separation to perform
/// \param minValue Minimum length to consider
/// \param maxValue Maximum length to consider
///

void error(const std::vector<std::vector<Vector3f> > & fibersA, const std::vector< std::vector<Vector3f> > & fibersB,
		   vector<float>& minError, vector<double>& meanError, vector<float>& maxError,
		   float & minEndPointError, float & meanEndPointError, float & maxEndPointError, uint64_t & totalNbPts,
		   unsigned steps = 10, float minValue = 35, float maxValue = 260)
{
	minEndPointError = std::numeric_limits<float>::max();
	meanEndPointError = 0.f;
	maxEndPointError = 0.f;
	unsigned nbSteps = (maxValue - minValue)/steps;
	minError.resize(nbSteps, std::numeric_limits<float>::max());
	meanError.resize(nbSteps, 0);
	maxError.resize(nbSteps, 0);

	unsigned nbEndPoints = fibersA.size();
	assert(fibersA.size() == fibersB.size());
	for(unsigned f = 0; f < fibersA.size(); ++f)
	{
		unsigned tablePos = (float)((fibersA[f].size() - 1) * (fibersA[f][1]-fibersA[f][0]).norm() - minValue) / (float)(maxValue - minValue) * (nbSteps-1);
		if (tablePos>nbSteps-1) tablePos = nbSteps - 1;
		assert(fibersA[f].size() == fibersB[f].size());
		for(unsigned p = 0; p < fibersA[f].size()-1; ++p)
		{
			float err = (fibersA[f][p] - fibersB[f][p]).norm();
			meanError[tablePos]  += err / double(totalNbPts);
			maxError[tablePos] = err > maxError[tablePos] ? err : maxError[tablePos];
			minError[tablePos] = err < minError[tablePos] ? err : minError[tablePos];
		}
		float err = (fibersA[f][fibersA[f].size()-1] - fibersB[f][fibersA[f].size()-1]).norm();
		meanError[tablePos]  += err / double(totalNbPts);
		maxError[tablePos] = err > maxError[tablePos] ? err : maxError[tablePos];
		minError[tablePos] = err < minError[tablePos] ? err : minError[tablePos];
		minEndPointError = err < minEndPointError ? err : minEndPointError;
		meanEndPointError += err / double(nbEndPoints);
		maxEndPointError = err > maxEndPointError ? err : maxEndPointError;
	}
}
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_DETAIL_SCALAR_D_EXPO_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_DETAIL_SCALAR_D_EXPO_REDUCTION_HPP_INCLUDED

#include <boost/simd/arch/detail/scalar/horner.hpp>
#include <boost/simd/constant/invlog10_2.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/constant/log10_2lo.hpp>
#include <boost/simd/constant/log_10.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/maxlog10.hpp>
#include <boost/simd/constant/maxlog2.hpp>
#include <boost/simd/constant/minlog.hpp>
#include <boost/simd/constant/minlog10.hpp>
#include <boost/simd/constant/minlog2.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/simd/fma.hpp>
#include <boost/simd/function/simd/fnms.hpp>
#include <boost/simd/function/simd/inc.hpp>
#include <boost/simd/function/simd/oneminus.hpp>
#include <boost/simd/function/simd/oneplus.hpp>
#include <boost/simd/function/simd/round2even.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/simd/logical.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0> struct exp_reduction < A0, natural_tag, double>
    {
      using l_t = logical<A0>;
      static BOOST_FORCEINLINE l_t isgemaxlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 >= Maxlog<A0>());
      }

      static BOOST_FORCEINLINE l_t isleminlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 <= Minlog<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce( A0 a0
                                        , A0& hi, A0& lo, A0& x) BOOST_NOEXCEPT
      {
        A0 k = round2even(Invlog_2<A0>()*a0);
        hi = fnms(k, Log_2hi<A0>(), a0); //a0-k*L
        lo = k*Log_2lo<A0>();
        x  = hi-lo;
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        A0 const t = sqr(x);
        return fnms(t, horner<BOOST_SIMD_HORNER_COEFF_T(A0, 5,
                                                        ( 0x3e66376972bea4d0ull
                                                        , 0xbebbbd41c5d26bf1ull
                                                        , 0x3f11566aaf25de2cull
                                                        , 0xbf66c16c16bebd93ull
                                                        , 0x3fc555555555553eull
                                                        )
                                                       )>(t), x); //x-h*t
      }

      static BOOST_FORCEINLINE A0 finalize(A0 x, A0 c, A0 hi, A0 lo) BOOST_NOEXCEPT
      {
        return oneminus(((lo-(x*c)/(Two<A0>()-c))-hi));
      }

    };

    template < class A0 > struct exp_reduction < A0, two_tag, double>
    {
      using l_t = logical<A0>;
      static BOOST_FORCEINLINE l_t isgemaxlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 >= Maxlog2<A0>());
      }

      static BOOST_FORCEINLINE l_t isleminlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 <= Minlog2<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 a0, A0, A0, A0& x) BOOST_NOEXCEPT
      {
        A0 k = round2even(a0);
        x = (a0 - k)*Log_2<A0>();
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        const A0 t =  sqr(x);
        return fnms(t, horner<BOOST_SIMD_HORNER_COEFF_T(A0, 5,
                                                        ( 0x3e66376972bea4d0ull
                                                        , 0xbebbbd41c5d26bf1ull
                                                        , 0x3f11566aaf25de2cull
                                                        , 0xbf66c16c16bebd93ull
                                                        , 0x3fc555555555553eull
                                                        )
                                                       )> (t), x); //x-h*t
      }

      static BOOST_FORCEINLINE A0 finalize( A0 x, A0 c, A0, A0& ) BOOST_NOEXCEPT
      {
        return oneminus(((-(x*c)/(Two<A0>()-c))-x));
      }
    };

    template < class A0 > struct exp_reduction < A0, ten_tag, double>
    {
      using l_t = logical<A0>;
      static BOOST_FORCEINLINE l_t isgemaxlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 >= Maxlog10<A0>());
      }

      static BOOST_FORCEINLINE l_t isleminlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 <= Minlog10<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 a0, A0&, A0&, A0& x) BOOST_NOEXCEPT
      {
        A0 k  = round2even(Invlog10_2<A0>()*a0);
        x = fnms(k, Log10_2hi<A0>(), a0);
        x = fnms(k, Log10_2lo<A0>(), x);
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        A0 xx = sqr(x);
        A0 px = x*horner<BOOST_SIMD_HORNER_COEFF_T(A0, 4,
                                                   (0x3fa4fd75f3062dd4ull,
                                                    0x40277d9474c55934ull,
                                                    0x40796b7a050349e4ull,
                                                    0x40a2b4798e134a01ull)
                                                  )> (xx);
        A0 x2 =  px/(horner<BOOST_SIMD_HORNER_COEFF_T(A0, 4,
                                                      (0x3ff0000000000000ull,
                                                       0x405545fdce51ca08ull,
                                                       0x4093e05eefd67782ull,
                                                       0x40a03f37650df6e2ull)
                                                     )> (xx)-px);
        return oneplus(x2+x2);
      }

      static BOOST_FORCEINLINE A0 finalize(A0, A0 c, A0,  A0 ) BOOST_NOEXCEPT
      {
        return c;
      }
    };
  }
} }
#endif


#include <benchmark/benchmark.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"
#include "UKF/Core.h"

enum MyStateFields {
    Position,
    Velocity,
    Attitude,
    AngularVelocity
};

using MyStateVector = UKF::StateVector<
    UKF::Field<Position, UKF::Vector<3>>,
    UKF::Field<Velocity, UKF::Vector<3>>,
    UKF::Field<Attitude, UKF::Quaternion>,
    UKF::Field<AngularVelocity, UKF::Vector<3>>
>;

namespace UKF {
namespace Parameters {
template <> constexpr real_t AlphaSquared<MyStateVector> = 1e-6;
}

/*
State vector process model. One version takes body frame kinematic
acceleration and angular acceleration as inputs, the other doesn't (assumes
zero accelerations).
*/
template <> template <>
MyStateVector MyStateVector::derivative<UKF::Vector<3>, UKF::Vector<3>>(
        const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) const {
    MyStateVector temp;

    /* Position derivative. */
    temp.set_field<Position>(get_field<Velocity>());

    /* Velocity derivative. */
    temp.set_field<Velocity>(get_field<Attitude>().conjugate() * acceleration);

    /* Attitude derivative. */
    UKF::Quaternion temp_q;
    temp_q.vec() = get_field<AngularVelocity>();
    temp_q.w() = 0;
    temp.set_field<Attitude>(temp_q);

    /* Angular velocity derivative. */
    temp.set_field<AngularVelocity>(angular_acceleration);

    return temp;
}

template <> template <>
MyStateVector MyStateVector::derivative<>() const {
    return derivative(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0));
}

}

/* Set up measurement vector class. */
enum MyMeasurementFields {
    GPS_Position,
    GPS_Velocity,
    Accelerometer,
    Magnetometer,
    Gyroscope
};

using MyMeasurementVector = UKF::DynamicMeasurementVector<
    UKF::Field<GPS_Position, UKF::Vector<3>>,
    UKF::Field<GPS_Velocity, UKF::Vector<3>>,
    UKF::Field<Accelerometer, UKF::Vector<3>>,
    UKF::Field<Magnetometer, UKF::FieldVector>,
    UKF::Field<Gyroscope, UKF::Vector<3>>
>;

using MyCore = UKF::Core<
    MyStateVector,
    MyMeasurementVector,
    UKF::IntegratorRK4
>;

namespace UKF {
/*
Define measurement model to be used in tests. NOTE: These are just for
testing, don't expect them to make any physical sense whatsoever.
*/
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Position>(const MyStateVector& state) {
    return state.get_field<Position>();
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, GPS_Velocity>(const MyStateVector& state) {
    return state.get_field<Velocity>();
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Accelerometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8);
}

template <> template <>
UKF::FieldVector MyMeasurementVector::expected_measurement
<MyStateVector, Magnetometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::FieldVector(1, 0, 0);
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Gyroscope>(const MyStateVector& state) {
    return state.get_field<AngularVelocity>();
}

}

MyCore create_initialised_test_filter() {
    MyCore test_filter;
    test_filter.state.set_field<Position>(UKF::Vector<3>(0, 0, 0));
    test_filter.state.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));
    test_filter.state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_filter.state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));
    test_filter.covariance = MyStateVector::CovarianceMatrix::Zero();
    test_filter.covariance.diagonal() << 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10;
    test_filter.measurement_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05;

    real_t a, b;
    real_t dt = 0.01;
    a = std::sqrt(0.1*dt*dt);
    b = std::sqrt(0.1*dt);
    test_filter.process_noise_covariance << a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                            0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                            0, 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                            0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 0,
                                            0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0,
                                            0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0,
                                            0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0,
                                            0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0,
                                            0, 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0,
                                            0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 0,
                                            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0,
                                            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b;

    return test_filter;
}

void Core_APrioriStep(benchmark::State& state) {
    MyCore test_filter = create_initialised_test_filter();

    while(state.KeepRunning()) {
        test_filter.a_priori_step(0.01);
    }
}

BENCHMARK(Core_APrioriStep);

void Core_InnovationStep(benchmark::State& state) {
    MyCore test_filter = create_initialised_test_filter();
    MyMeasurementVector m;

    m.set_field<GPS_Position>(UKF::Vector<3>(100, 10, -50));
    m.set_field<GPS_Velocity>(UKF::Vector<3>(20, 0, 0));
    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.a_priori_step(0.01);

    while(state.KeepRunning()) {
        test_filter.innovation_step(m);
    }
}

BENCHMARK(Core_InnovationStep);

void Core_APosterioriStep(benchmark::State& state) {
    MyCore test_filter = create_initialised_test_filter();
    MyMeasurementVector m;

    m.set_field<GPS_Position>(UKF::Vector<3>(100, 10, -50));
    m.set_field<GPS_Velocity>(UKF::Vector<3>(20, 0, 0));
    m.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    m.set_field<Magnetometer>(UKF::FieldVector(0, -1, 0));
    m.set_field<Gyroscope>(UKF::Vector<3>(0.5, 0, 0));

    test_filter.a_priori_step(0.01);
    test_filter.innovation_step(m);

    MyStateVector::CovarianceMatrix initial_cov = test_filter.covariance;
    MyStateVector initial_state = test_filter.state;

    while(state.KeepRunning()) {
        test_filter.covariance = initial_cov;
        test_filter.state = initial_state;
        test_filter.a_posteriori_step();
    }
}

BENCHMARK(Core_APosterioriStep);


#ifndef YQVMC_EXTERNAL_LIBRARY_ADAPTOR_EIGEN3_HPP
#define YQVMC_EXTERNAL_LIBRARY_ADAPTOR_EIGEN3_HPP

#include <Eigen/Core>
#include "../impl_/mae_traits.hpp"

namespace yqvmc {
  namespace impl_ {
    template <typename S_, int R_, int C_, int O_, int MR_, int MC_>
    struct MeanAndErrorTraits<Eigen::Array<S_, R_, C_, O_, MR_, MC_>, void> {
    public:
      typedef Eigen::Array<S_, R_, C_, O_, MR_, MC_> input_type;
      typedef typename input_type::Scalar Scalar;
      typedef typename std::conditional<input_type::ColsAtCompileTime == 1,
        Eigen::Array<Scalar, Eigen::Dynamic, 1>,
        Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> >::type sum_type;
      typedef sum_type result_type;

      static void set_zero(sum_type& x) { x.setZero(); }
      static void add_to(sum_type& x, const input_type& dx) {
        if (x.size() == 0)
          x = dx;
        else
          x += dx;
      }
      static input_type square(const input_type& x) { return x*x; }
      static result_type mean(const sum_type& x, std::size_t n) {
        return x/n;
      }
      static result_type standard_error(const result_type& x2mean,
        const result_type& xmean, std::size_t n) {
        return ((x2mean - xmean*xmean) / n).sqrt();
      }
    };
  }
}

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOG1P_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOG1P_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

#include <boost/simd/function/any.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/two.hpp>

#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;

 BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) BOOST_NOEXCEPT
    {
      return musl_(log1p)(a0);
    }
  };

 BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::double_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) BOOST_NOEXCEPT
    {
      return plain_(log1p)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::musl_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

      uiA0 iu = bitwise_cast<uiA0>(uf);
      iu += 0x3f800000 - 0x3f3504f3;
      iA0 k = bitwise_cast<iA0>(iu>>23) - 0x7f;
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iu = (iu&0x007fffff) + 0x3f3504f3;
      A0 f =  dec(bitwise_cast<A0>(iu));
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
      A0 dk = tofloat(k);
      A0  c = if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::musl_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      uiA0 hu = bitwise_cast<uiA0>(uf)>>32;
      hu += 0x3ff00000 - 0x3fe6a09e;
      iA0 k = bitwise_cast<iA0>(hu>>20) - 0x3ff;
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      A0  c =  if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;
      hu =  (hu&0x000fffff) + 0x3fe6a09e;
      A0 f = bitwise_cast<A0>( bitwise_cast<uiA0>(hu<<32) | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(uf))));
      f = dec(f);

      A0 hfsq = Half<A0>()*sqr(f);
      A0 s = f/(2.0 + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 dk = tofloat(k);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };
//=================================================================================================================
  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::plain_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

     /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 k;
      A0 x;
      std::tie(x, k) = ifrexp(uf);
      A0  x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() >  x);
      k += bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      A0  c = if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;

      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
      A0 dk = tofloat(k);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log1p_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::plain_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0,   signed>;
      const A0 uf =  inc(a0);
      auto isnez = is_nez(uf);

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 k;
      A0 x;
      std::tie(x, k) = ifrexp(uf);
      A0  x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() >  x);
      k += bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
      A0  c = if_else( k >= 2, oneminus(uf-a0), a0-dec(uf))/uf;

      A0 hfsq = Half<A0>()*sqr(f);
      A0 s = f/(2.0 + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 dk = tofloat(k);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(uf), zz);
    }
  };

} } }

#endif


#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <limits.h>
#include <Eigen/Eigen>
#include <opengv/triangulation/methods.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#include <opengv/math/cayley.hpp>
#include <sstream>
#include <fstream>
#include <boost/concept_check.hpp>

#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

using namespace std;
using namespace Eigen;
using namespace opengv;

int main( int argc, char** argv )
{
	//initialize random seed
	initializeRandomSeed();

	//set experiment parameters
	double noise = 2.5;
	double outlierFraction = 0.3;
	size_t numberPoints = 200;

	//generate a random pose for viewpoint 1
	translation_t position1 = Eigen::Vector3d::Zero();
	rotation_t rotation1 = Eigen::Matrix3d::Identity();

	//generate a random pose for viewpoint 2
	translation_t position2 = generateRandomTranslation(2.0);
	rotation_t rotation2 = generateRandomRotation(0.25);

	//create a fake central camera
	translations_t camOffsets;
	rotations_t camRotations;
	generateCentralCameraSystem( camOffsets, camRotations );

	//derive correspondences based on random point-cloud
	bearingVectors_t bearingVectors1;
	bearingVectors_t bearingVectors2;
	std::vector<int> camCorrespondences1; //unused in the central case
	std::vector<int> camCorrespondences2; //unused in the central case
	Eigen::MatrixXd gt(3,numberPoints);
	generateRandom2D2DCorrespondences(
		position1, rotation1, position2, rotation2,
		camOffsets, camRotations, numberPoints, noise, outlierFraction,
		bearingVectors1, bearingVectors2,
		camCorrespondences1, camCorrespondences2, gt );

	std::cout << "*************************************************" << std::endl;
	std::cout << "		Ground Truth" << std::endl;
	std::cout << "*************************************************" << std::endl;

	//print the experiment characteristics
	printExperimentCharacteristics(
		position2 / position2.norm(), rotation2, noise, outlierFraction );

	/*********************************
	 * 		Compute Relative Pose	 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "Computing relative Pose between View 0 and View 1" << std::endl;
	std::cout << "*************************************************" << std::endl;

	relative_pose::CentralRelativeAdapter rel_adapter(
		bearingVectors1,
		bearingVectors2);

	sac::Ransac<
	sac_problems::relative_pose::CentralRelativePoseSacProblem> rel_ransac;
	std::shared_ptr<
	sac_problems::relative_pose::CentralRelativePoseSacProblem> relposeproblem_ptr(
		new sac_problems::relative_pose::CentralRelativePoseSacProblem(
			rel_adapter,
			sac_problems::relative_pose::CentralRelativePoseSacProblem::NISTER));
	rel_ransac.sac_model_ = relposeproblem_ptr;
	rel_ransac.threshold_ = 2.*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
	rel_ransac.max_iterations_ = 1000;

	//Run the experiment
	struct timeval tic;
	struct timeval toc;
	gettimeofday( &tic, 0 );
	rel_ransac.computeModel();
	gettimeofday( &toc, 0 );
	double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	//print results for ransac 1
	std::cout << "the ransac threshold is: " << rel_ransac.threshold_ << std::endl;
	std::cout << "the normalized translation result is: " << std::endl;
	std::cout << rel_ransac.model_coefficients_.col(3)/
	rel_ransac.model_coefficients_.col(3).norm() << std::endl << std::endl;
	std::cout << "Ransac needed " << rel_ransac.iterations_ << " iterations and ";
	std::cout << ransac_time << " seconds" << std::endl << std::endl;
	std::cout << "the number of inliers is: " << rel_ransac.inliers_.size() << " out of " << numberPoints;;
	std::cout << std::endl << std::endl;

	translation_t t = rel_ransac.model_coefficients_.col(3) / rel_ransac.model_coefficients_.col(3).norm();
	rotation_t R = rel_ransac.model_coefficients_.block<3,3>(0,0);

	bearingVectors1.clear();
	bearingVectors2.clear();

	for(size_t i = 0 ; i < rel_ransac.inliers_.size() ; ++i)
	{
		bearingVectors1.push_back(rel_adapter.getBearingVector1(rel_ransac.inliers_[i]));
		bearingVectors2.push_back(rel_adapter.getBearingVector2(rel_ransac.inliers_[i]));
	}

	relative_pose::CentralRelativeAdapter adapter(
		bearingVectors1,
		bearingVectors2,
		rel_adapter.gett12() / rel_adapter.gett12().norm(),
		rel_adapter.getR12());

	/*********************************
	 * 		Triangulate Points		 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "Triangulating Points between View 0 and View 1" << std::endl;
	std::cout << "*************************************************" << std::endl;

	std::cout << "running triangulation algorithm 2" << std::endl << std::endl;
	MatrixXd triangulate2_results(3,(int) rel_ransac.inliers_.size());
	gettimeofday( &tic, 0 );

	for(size_t j = 0; j < rel_ransac.inliers_.size(); j++)
	{
		triangulate2_results.block<3,1>(0,j) = triangulation::triangulate2(adapter,j);
	}

	gettimeofday( &toc, 0 );

	ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	/*
	std::cout << "triangulation truth: " << std::endl;
	std::cout << gt << std::endl;
	*/

	std::cout << "triangulation result: " << std::endl;
	std::cout << triangulate2_results.col(0) << std::endl << std::endl;

	std::cout << " in " << ransac_time << " seconds" << std::endl << std::endl;

	/*
	MatrixXd error(1,numberPoints);
	for(size_t i = 0; i < numberPoints; i++)
	{
		Vector3d singleError = triangulate2_results.col(i) - gt.col(i);
		error(0,i) = singleError.norm();
	}
	std::cout << "triangulation error is: " << std::endl << error << std::endl;
	*/

	/*********************************
	 * 		Compute Absolute Pose	 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "Computing absolute Pose of View 1 from 3D points" << std::endl;
	std::cout << "*************************************************" << std::endl;

	points_t points;

	for(size_t i = 0 ; i < rel_ransac.inliers_.size() ; ++i)
	{
		//store the point
		points.push_back(triangulate2_results.col(i));
	}

	//create a central absolute adapter
	absolute_pose::CentralAbsoluteAdapter abs_adapter(
		bearingVectors2,
		points);

	transformation_t optimized_pose;

	//Create an AbsolutePoseSac problem and Ransac
	//The method can be set to KNEIP, GAO or EPNP
	sac::Ransac<sac_problems::absolute_pose::AbsolutePoseSacProblem> abs_ransac;
	std::shared_ptr<
	sac_problems::absolute_pose::AbsolutePoseSacProblem> abspose_kneip_ptr(
		new sac_problems::absolute_pose::AbsolutePoseSacProblem(
			abs_adapter,
			sac_problems::absolute_pose::AbsolutePoseSacProblem::KNEIP));
	abs_ransac.sac_model_ = abspose_kneip_ptr;
	abs_ransac.threshold_ = 1.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
	abs_ransac.max_iterations_ = 1000;

	//Run the experiment
	gettimeofday( &tic, 0 );
	abs_ransac.computeModel();
	gettimeofday( &toc, 0 );
	ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	//print the results
	std::cout << "KNEIP: " << std::endl;
	std::cout << "the ransac results is: " << std::endl;
	std::cout << abs_ransac.model_coefficients_ << std::endl << std::endl;
	std::cout << "Ransac needed " << abs_ransac.iterations_ << " iterations and ";
	std::cout << ransac_time << " seconds" << std::endl << std::endl;
	std::cout << "the number of inliers is: " << abs_ransac.inliers_.size() << " out of " << rel_ransac.inliers_.size();
	std::cout << std::endl << std::endl;

	abs_ransac.sac_model_->optimizeModelCoefficients(abs_ransac.inliers_, abs_ransac.model_coefficients_, optimized_pose);

	std::cout << "the sac optimized ransac results is: " << std::endl;
	std::cout <<  optimized_pose << std::endl << std::endl;

	t = abs_ransac.model_coefficients_.col(3);
	R = abs_ransac.model_coefficients_.block<3,3>(0,0);

	abs_adapter.sett(t);
	abs_adapter.setR(R);

	transformation_t nonlinear_transformation =
	absolute_pose::optimize_nonlinear(abs_adapter);

	std::cout << "results from nonlinear algorithm with every points:" << std::endl;
	std::cout << nonlinear_transformation << std::endl << std::endl;

	std::cout << "Truth is: " << std::endl  << std::endl;
	std::cout << position2 / position2.norm() << std::endl << std::endl;
	std::cout << rotation2 << std::endl << std::endl;

	/*********************************
	 * 		Simulation of view 3	 *
	 * *******************************/

	std::cout << "*************************************************" << std::endl;
	std::cout << "		Simulating a 3rd View " << std::endl;
	std::cout << "*************************************************" << std::endl;

	//generate a random pose for viewpoint 3
	translation_t position3 = position2 + generateRandomTranslation(5.0);
	rotation_t rotation3 = generateRandomRotation(0.25);

	//derive correspondences based on random point-cloud
	bearingVectors_t bearingVectors3;
	std::vector<int> camCorrespondences22; //unused in the central case
	std::vector<int> camCorrespondences33; //unused in the central case

	for( size_t i = 0; i < points.size(); i++ )
	{
		//get the point in viewpoint 3
		point_t bodyPoint3 = rotation3.transpose()*(points[i] - position3);

		//get the point in the camera in viewpoint 2
		bearingVectors3.push_back(bodyPoint3);

		//normalize the bearing-vectors
		bearingVectors3[i] = bearingVectors3[i] / bearingVectors3[i].norm();

		//add noise
		bearingVectors3[i] = addNoise(2.5,bearingVectors3[i]);
	}

	//print the experiment characteristics
	printExperimentCharacteristics(
		position3, rotation3, noise, outlierFraction );

	//create a central absolute adapter
	absolute_pose::CentralAbsoluteAdapter next_abs_adapter(
		bearingVectors3,
		points);

	//Create an AbsolutePoseSac problem and Ransac
	//The method can be set to KNEIP, GAO or EPNP
	sac::Ransac<sac_problems::absolute_pose::AbsolutePoseSacProblem> next_abs_ransac;
	std::shared_ptr<
	sac_problems::absolute_pose::AbsolutePoseSacProblem> next_abspose_kneip_ptr(
		new sac_problems::absolute_pose::AbsolutePoseSacProblem(
			next_abs_adapter,
			sac_problems::absolute_pose::AbsolutePoseSacProblem::KNEIP));
	next_abs_ransac.sac_model_ = next_abspose_kneip_ptr;
	next_abs_ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
	next_abs_ransac.max_iterations_ = 1000;

	//Run the experiment
	gettimeofday( &tic, 0 );
	next_abs_ransac.computeModel();
	gettimeofday( &toc, 0 );
	ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

	//print the results
	std::cout << "KNEIP: " << std::endl;
	std::cout << "the ransac results is: " << std::endl;
	std::cout << next_abs_ransac.model_coefficients_ << std::endl << std::endl;
	std::cout << "Ransac needed " << next_abs_ransac.iterations_ << " iterations and ";
	std::cout << ransac_time << " seconds" << std::endl << std::endl;
	std::cout << "the number of inliers is: " << next_abs_ransac.inliers_.size() << " out of " << rel_ransac.inliers_.size();
	std::cout << std::endl << std::endl;

	next_abs_ransac.sac_model_->optimizeModelCoefficients(next_abs_ransac.inliers_, next_abs_ransac.model_coefficients_, optimized_pose);

	std::cout << "the sac optimized ransac results is: " << std::endl;
	std::cout <<  optimized_pose << std::endl << std::endl;

	t = next_abs_ransac.model_coefficients_.col(3);
	R = next_abs_ransac.model_coefficients_.block<3,3>(0,0);

	next_abs_adapter.sett(t);
	next_abs_adapter.setR(R);

	nonlinear_transformation =
	absolute_pose::optimize_nonlinear(next_abs_adapter);

	std::cout << "results from nonlinear algorithm with every points:" << std::endl;
	std::cout << nonlinear_transformation << std::endl << std::endl;

	std::cout << "Truth is: " << std::endl  << std::endl;
	std::cout << position3 << std::endl << std::endl;
	std::cout << rotation3 << std::endl << std::endl;

}


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

#include <string>
#include <utility>
#include <Eigen/Core>
#include <limits>
#include <cassert>
#include "metro/regression/LogLikelihood.hpp"
#include "metro/ModifiedCholesky.hpp"
#include "metro/ModifiedNewtonRaphson.hpp"
#include "metro/CholeskyStepper.hpp"

#define DEBUG 1

namespace metro {
	CholeskyStepper::CholeskyStepper( double tolerance, int max_iterations, Tracer tracer ):
		m_tolerance( tolerance ),
		m_max_iterations( max_iterations ),
		m_tracer( tracer ),
		m_iteration( -1 ),
		m_target_ll( -std::numeric_limits< double >::infinity() )
	{
		assert( tolerance > 0 ) ;
	}

	bool CholeskyStepper::step( Function& function, Vector* result ) {
		++m_iteration ;
		//std::cerr << "CholeskyStepper::step(): calling evaluate()...\n" ;
		function.evaluate( 2 ) ;
		double const ll = function.get_value_of_function() ;
		m_solver.compute( -function.get_value_of_second_derivative() ) ;

		// Different possible stopping conditions are possible.
		// One is to stop when the predicted improvement in function
		// value is small.  This amount is encoded by the directional derivative.
		// However, for statistical model fitting we would like a rule that is invariant to
		// various types of rescaling the function.  For example
		// - scaling parameters (which scales the derivative)
		// - multiplying the function by a constant (which is a bit like adding more data)
		// A rule like directional_derivative < tolerance does not have these properties.

		// Instead, we compute the derivative normalised to 'standard Gaussian' space.
		// Theory: if the function 2nd derivative H = LLᵗ then Σ=(Lᵗ)⁻¹L⁻¹.
		// We assume the log-likelihood looks like this:
		// f(x) = s(Lᵗ(x-x₀)) + O((x-x₀)³)
		// near the true maximum x₀, where s is the standard multivariate normal
		// log-density.  Then
		// f'(x) = -L Lᵗ (x-x₀) + O((x-x₀)²)   (expressed as a column vector).
		// We put a convergence condition on the corresponding derivative in s-space, given by
		// z = L⁻¹ f'(x) = -Lᵗ(x-x₀) + O((x-x₀)²)
		// This has the nice interpretation of being interpretable as a distance
		// to the maximum in the uncorrelated-variable space that is the domain of s.

		double const derivativeOneNorm = function.get_value_of_first_derivative().array().abs().maxCoeff() ;
		Vector step = m_solver.solve( function.get_value_of_first_derivative() ) ;
		//Vector const sqrtStep = m_solver.halfSolve( function.get_value_of_first_derivative() ) ;
		double directional_derivative = function.get_value_of_first_derivative().transpose() * step ;

		bool converged = (
			(derivativeOneNorm < m_tolerance)
			&& ( ll >= ( m_target_ll - m_tolerance ) )
			//&& (sqrtStep.array().abs().maxCoeff() < m_tolerance )
			&& (directional_derivative < m_tolerance )
		) ;

		if( m_tracer ) {
			m_tracer( m_iteration, ll, m_target_ll, function.parameters(), function.get_value_of_first_derivative(), step, converged ) ;
		}

		if(
			converged || (m_iteration) >= m_max_iterations
		) {
			return false ;
		} else {
			(*result) = step ;
			m_target_ll = std::max( m_target_ll, ll ) ;
			return true ;
		}
	}

	bool CholeskyStepper::diverged() const {
		return m_iteration >= m_max_iterations || m_target_ll != m_target_ll ;
	}

	std::size_t CholeskyStepper::number_of_iterations() const {
		return m_iteration ;
	}

	void CholeskyStepper::reset() {
		m_iteration = -1 ;
		m_target_ll = -std::numeric_limits< double >::infinity() ;
	}
}


#ifndef CY_NNQS_JASTROW_HPP
#define CY_NNQS_JASTROW_HPP
#include <random>
#include <Eigen/Dense>
#include "Utilities/type_traits.hpp"
#include <nlohmann/json.hpp>
namespace yannq
{
template<typename T>
class Jastrow
{
public:
	using Scalar=T;
	typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
	typedef Eigen::Matrix<T, Eigen::Dynamic, 1>  Vector;

private:
	int n_;

	Vector a_;
	Matrix J_;

public:

	nlohmann::json desc() const
	{
		return nlohmann::json
		{
			{"name", "Jastrow"},
			{"n", n_},
		};
	}

	inline int getN() const
	{
		return n_;
	}

	Jastrow(int n)
		: n_(n), a_(n_), J_(n,n)
	{
	}

	void resize(int n)
	{
		n_ = n;
		a_.resize(n);
		J_.resize(n,n);
	}

	int getDim() const
	{
		return n_ + n_*(n_-1)/2;
	}

	Vector getA() const
	{
		return a_;
	}
	Matrix getJ() const
	{
		return J_;
	}

	void setA(const Vector& a) const
	{
		a_ = a;
	}

	void setJ(const Matrix& J) const
	{
		J_ = J;
	}

	T A(int i) const
	{
		return a_(i);
	}
	T J(int i, int j) const
	{
		return J_(i,j);
	}

	template <typename RandomEngine, class U=T,
               typename std::enable_if < !is_complex_type<U>::value, int >::type = 0 >
	void initializeRandom(RandomEngine& re, T weight = 0.001)
	{
		std::normal_distribution<double> nd{};

		for(int i = 0; i < n_; i++)
		{
			a_(i) = weight*nd(re);
		}
		J_.setZero();
		for(int j = n_-1; j >=0; --j)
		{
			for(int i = 0; i < j; i++)
			{
				J_(i, j) = weight*nd(re);
			}
		}
	}
	template <typename RandomEngine, class U=T,
               typename std::enable_if < is_complex_type<U>::value, int >::type = 0 >
	void initializeRandom(RandomEngine& re, T weight = 0.001)
	{
		std::normal_distribution<double> nd{};

		for(int i = 0; i < n_; i++)
		{
			a_(i) = weight*T(nd(re),nd(re));
		}
		J_.setZero();
		for(int j = n_-1; j >=0; --j)
		{
			for(int i = 0; i < j; i++)
			{
				J_(i, j) = weight*T(nd(re),nd(re));
			}
		}
	}

	T calcTheta(const Eigen::VectorXi& sigma) const
	{
		Vector s = sigma.cast<T>();
		T res = a_.transpose()*s;
		res += T(s.transpose()*J_*s);
		return res;
	}

	std::tuple<Eigen::VectorXi, T> makeData(const Eigen::VectorXi& sigma) const
	{
		return std::make_tuple(sigma, calcTheta(sigma));
	}

	Vector logDeriv(const std::tuple<Eigen::VectorXi, T>& t) const
	{
		Vector res(getDim());
		for(int i = 0; i < n_; i++)
		{
			res(i) = std::get<0>(t)(i);
		}
		for(int i = 0; i < n_; i++)
		{
			for(int j = i+1; i < n_; j++)
			{
				res(i*n_+j+n_) = std::get<0>(t)(i)*std::get<0>(t)(j);
			}
		}
		return res;
	}

	void updateParams(const Vector& u)
	{
		a_ += u.segment(0, n_);
		for(int i = 0; i < n_; i++)
		{
			for(int j = i+1; i < n_; j++)
			{
				J_(i, j) += u(i*n_ + j + n_);
			}
		}
	}

};
}
#endif//CY_NNQS_JASTROW_HPP


// 多倍長整数エイリアス (補完・コンパイルが重くなる)
#include <boost/multiprecision/cpp_int.hpp>
using mll = boost::multiprecision::cpp_int;