/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author:       François Godi, Vincent Rouvreau
 *
 *    Copyright (C) 2018  INRIA
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <iostream>
#include <vector>
#include <gudhi/Toplex_map.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "toplex map"
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(toplex_map) {
  using Vertex = Gudhi::Toplex_map::Vertex;

  Gudhi::Toplex_map tm;
  std::cout << "insert_simplex {1, 2, 3, 4}" << std::endl;
  std::vector<Vertex> sigma1 = {1, 2, 3, 4};
  tm.insert_simplex(sigma1);
  std::cout << "insert_simplex {5, 2, 3, 6}" << std::endl;
  std::vector<Vertex> sigma2 = {5, 2, 3, 6};
  tm.insert_simplex(sigma2);
  std::cout << "insert_simplex {5}" << std::endl;
  std::vector<Vertex> sigma3 = {5};
  tm.insert_simplex(sigma3);
  std::cout << "insert_simplex {4, 5, 3}" << std::endl;
  std::vector<Vertex> sigma6 = {4, 5, 3};
  tm.insert_simplex(sigma6);
  std::cout << "insert_simplex {4, 5, 9}" << std::endl;
  std::vector<Vertex> sigma7 = {4, 5, 9};
  tm.insert_simplex(sigma7);

  std::cout << "num_maximal_simplices" << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 4);
  // Browse maximal simplices
  std::cout << "Maximal simplices are :" << std::endl;
  for (auto simplex_ptr : tm.maximal_simplices()) {
    for (auto v : *simplex_ptr) {
      std::cout << v << ", ";
    }
    std::cout << std::endl;
    BOOST_CHECK(tm.maximality(*simplex_ptr));
  }

  BOOST_CHECK(tm.maximality(sigma1));
  BOOST_CHECK(tm.maximality(sigma2));
  BOOST_CHECK(!tm.maximality(sigma3));
  BOOST_CHECK(tm.maximality(sigma6));
  BOOST_CHECK(tm.maximality(sigma7));

  std::vector<Vertex> sigma4 = {5, 2, 3};
  std::vector<Vertex> sigma5 = {5, 2, 7};
  BOOST_CHECK(tm.membership(sigma4));
  BOOST_CHECK(!tm.membership(sigma5));
  std::cout << "insert_simplex {5, 2, 7}" << std::endl;
  tm.insert_simplex(sigma5);

  std::cout << "num_maximal_simplices" << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 5);
  // Browse maximal simplices
  std::cout << "Maximal simplices are :" << std::endl;
  for (auto simplex_ptr : tm.maximal_simplices()) {
    for (auto v : *simplex_ptr) {
      std::cout << v << ", ";
    }
    std::cout << std::endl;
    BOOST_CHECK(tm.maximality(*simplex_ptr));
  }

  BOOST_CHECK(tm.membership(sigma5));

  std::cout << "contraction(4,5)" << std::endl;
  auto r = tm.contraction(4, 5);
  std::cout << "r=" << r << std::endl;
  BOOST_CHECK(r == 5);

  std::cout << "num_maximal_simplices" << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 4);
  // Browse maximal simplices
  std::cout << "Maximal simplices are :" << std::endl;
  for (auto simplex_ptr : tm.maximal_simplices()) {
    for (auto v : *simplex_ptr) {
      std::cout << v << ", ";
    }
    std::cout << std::endl;
    BOOST_CHECK(tm.maximality(*simplex_ptr));
  }

  std::vector<Vertex> sigma8 = {1, 2, 3};
  std::vector<Vertex> sigma9 = {2, 7};

  sigma8.emplace_back(r);
  sigma9.emplace_back(r);
  BOOST_CHECK(!tm.membership(sigma6));
  BOOST_CHECK(tm.membership(sigma8));
  BOOST_CHECK(tm.membership(sigma9));

  std::cout << "remove_simplex({2, 7, r = 5})" << std::endl;
  tm.remove_simplex(sigma9);
  BOOST_CHECK(!tm.membership(sigma9));

  std::cout << "num_maximal_simplices" << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 5);
  // Browse maximal simplices
  std::cout << "Maximal simplices are :" << std::endl;
  for (auto simplex_ptr : tm.maximal_simplices()) {
    for (auto v : *simplex_ptr) {
      std::cout << v << ", ";
    }
    std::cout << std::endl;
    BOOST_CHECK(tm.maximality(*simplex_ptr));
  }
  // {2, 7, 5} is removed, but verify its edges are still there
  std::vector<Vertex> edge = {2, 7};
  BOOST_CHECK(tm.membership(edge));
  edge = {2, 5};
  BOOST_CHECK(tm.membership(edge));
  edge = {7, 5};
  BOOST_CHECK(tm.membership(edge));
}


// Copyright 2004 The Trustees of Indiana University.

// 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP
#define BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/simple_point.hpp>
#include <vector>
#include <list>
#include <algorithm> // for std::min and std::max

namespace boost {

struct square_distance_attractive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::edge_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return d * d / k;
  }
};

struct square_distance_repulsive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::vertex_descriptor,
             typename graph_traits<Graph>::vertex_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return k * k / d;
  }
};

template<typename T>
struct linear_cooling {
  typedef T result_type;

  linear_cooling(std::size_t iterations)
    : temp(T(iterations) / T(10)), step(0.1) { }

  linear_cooling(std::size_t iterations, T temp)
    : temp(temp), step(temp / T(iterations)) { }

  T operator()()
  {
    T old_temp = temp;
    temp -= step;
    if (temp < T(0)) temp = T(0);
    return old_temp;
  }

 private:
  T temp;
  T step;
};

struct all_force_pairs
{
  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    vertex_iterator v, end;
    for (tie(v, end) = vertices(g); v != end; ++v) {
      vertex_iterator u = v;
      for (++u; u != end; ++u) {
        apply_force(*u, *v);
        apply_force(*v, *u);
      }
    }
  }
};

template<typename Dim, typename PositionMap>
struct grid_force_pairs
{
  template<typename Graph>
  explicit
  grid_force_pairs(Dim width, Dim height, PositionMap position, const Graph& g)
    : width(width), height(height), position(position)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
    two_k = Dim(2) * sqrt(width*height / num_vertices(g));
  }

  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef std::list<vertex_descriptor> bucket_t;
    typedef std::vector<bucket_t> buckets_t;

    std::size_t columns = std::size_t(width / two_k + Dim(1));
    std::size_t rows = std::size_t(height / two_k + Dim(1));
    buckets_t buckets(rows * columns);
    vertex_iterator v, v_end;
    for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
      std::size_t column = std::size_t((position[*v].x + width  / 2) / two_k);
      std::size_t row    = std::size_t((position[*v].y + height / 2) / two_k);

      if (column >= columns) column = columns - 1;
      if (row >= rows) row = rows - 1;
      buckets[row * columns + column].push_back(*v);
    }

    for (std::size_t row = 0; row < rows; ++row)
      for (std::size_t column = 0; column < columns; ++column) {
        bucket_t& bucket = buckets[row * columns + column];
        typedef typename bucket_t::iterator bucket_iterator;
        for (bucket_iterator u = bucket.begin(); u != bucket.end(); ++u) {
          // Repulse vertices in this bucket
          bucket_iterator v = u;
          for (++v; v != bucket.end(); ++v) {
            apply_force(*u, *v);
            apply_force(*v, *u);
          }

          std::size_t adj_start_row = row == 0? 0 : row - 1;
          std::size_t adj_end_row = row == rows - 1? row : row + 1;
          std::size_t adj_start_column = column == 0? 0 : column - 1;
          std::size_t adj_end_column = column == columns - 1? column : column + 1;
          for (std::size_t other_row = adj_start_row; other_row <= adj_end_row;
               ++other_row)
            for (std::size_t other_column = adj_start_column;
                 other_column <= adj_end_column; ++other_column)
              if (other_row != row || other_column != column) {
                // Repulse vertices in this bucket
                bucket_t& other_bucket
                  = buckets[other_row * columns + other_column];
                for (v = other_bucket.begin(); v != other_bucket.end(); ++v)
                  apply_force(*u, *v);
              }
        }
      }
  }

 private:
  Dim width;
  Dim height;
  PositionMap position;
  Dim two_k;
};

template<typename Dim, typename PositionMap, typename Graph>
inline grid_force_pairs<Dim, PositionMap>
make_grid_force_pairs(Dim width, Dim height, const PositionMap& position,
                      const Graph& g)
{ return grid_force_pairs<Dim, PositionMap>(width, height, position, g); }

template<typename Graph, typename PositionMap, typename Dim>
void
scale_graph(const Graph& g, PositionMap position,
            Dim left, Dim top, Dim right, Dim bottom)
{
  if (num_vertices(g) == 0) return;

  if (bottom > top) {
    using std::swap;
    swap(bottom, top);
  }

  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;

  // Find min/max ranges
  Dim minX = position[*vertices(g).first].x, maxX = minX;
  Dim minY = position[*vertices(g).first].y, maxY = minY;
  vertex_iterator vi, vi_end;
  for (tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    BOOST_USING_STD_MIN();
    BOOST_USING_STD_MAX();
    minX = min BOOST_PREVENT_MACRO_SUBSTITUTION (minX, position[*vi].x);
    maxX = max BOOST_PREVENT_MACRO_SUBSTITUTION (maxX, position[*vi].x);
    minY = min BOOST_PREVENT_MACRO_SUBSTITUTION (minY, position[*vi].y);
    maxY = max BOOST_PREVENT_MACRO_SUBSTITUTION (maxY, position[*vi].y);
  }

  // Scale to bounding box provided
  for (tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    position[*vi].x = ((position[*vi].x - minX) / (maxX - minX))
                    * (right - left) + left;
    position[*vi].y = ((position[*vi].y - minY) / (maxY - minY))
                    * (top - bottom) + bottom;
  }
}

namespace detail {
  template<typename PositionMap, typename DisplacementMap,
           typename RepulsiveForce, typename Dim, typename Graph>
  struct fr_apply_force
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;

    fr_apply_force(const PositionMap& position,
                   const DisplacementMap& displacement,
                   RepulsiveForce repulsive_force, Dim k, const Graph& g)
      : position(position), displacement(displacement),
        repulsive_force(repulsive_force), k(k), g(g)
    { }

    void operator()(vertex_descriptor u, vertex_descriptor v)
    {
#ifndef BOOST_NO_STDC_NAMESPACE
      using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
      if (u != v) {
        Dim delta_x = position[v].x - position[u].x;
        Dim delta_y = position[v].y - position[u].y;
        Dim dist = sqrt(delta_x * delta_x + delta_y * delta_y);
        Dim fr = repulsive_force(u, v, k, dist, g);
        displacement[v].x += delta_x / dist * fr;
        displacement[v].y += delta_y / dist * fr;
      }
    }

  private:
    PositionMap position;
    DisplacementMap displacement;
    RepulsiveForce repulsive_force;
    Dim k;
    const Graph& g;
  };

} // end namespace detail

template<typename Graph, typename PositionMap, typename Dim,
         typename AttractiveForce, typename RepulsiveForce,
         typename ForcePairs, typename Cooling, typename DisplacementMap>
void
fruchterman_reingold_force_directed_layout
 (const Graph&    g,
  PositionMap     position,
  Dim             width,
  Dim             height,
  AttractiveForce attractive_force,
  RepulsiveForce  repulsive_force,
  ForcePairs      force_pairs,
  Cooling         cool,
  DisplacementMap displacement)
{
  typedef typename graph_traits<Graph>::vertex_iterator   vertex_iterator;
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::edge_iterator     edge_iterator;

#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

  Dim area = width * height;
  // assume positions are initialized randomly
  Dim k = sqrt(area / num_vertices(g));

  detail::fr_apply_force<PositionMap, DisplacementMap,
                         RepulsiveForce, Dim, Graph>
    apply_force(position, displacement, repulsive_force, k, g);

  Dim temp = cool();
  if (temp) do {
    // Calculate repulsive forces
    vertex_iterator v, v_end;
    for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
      displacement[*v].x = 0;
      displacement[*v].y = 0;
    }
    force_pairs(g, apply_force);

    // Calculate attractive forces
    edge_iterator e, e_end;
    for (tie(e, e_end) = edges(g); e != e_end; ++e) {
      vertex_descriptor v = source(*e, g);
      vertex_descriptor u = target(*e, g);
      Dim delta_x = position[v].x - position[u].x;
      Dim delta_y = position[v].y - position[u].y;
      Dim dist = sqrt(delta_x * delta_x + delta_y * delta_y);
      Dim fa = attractive_force(*e, k, dist, g);

      displacement[v].x -= delta_x / dist * fa;
      displacement[v].y -= delta_y / dist * fa;
      displacement[u].x += delta_x / dist * fa;
      displacement[u].y += delta_y / dist * fa;
    }

    // Update positions
    for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
      BOOST_USING_STD_MIN();
      BOOST_USING_STD_MAX();
      Dim disp_size = sqrt(displacement[*v].x * displacement[*v].x
                           + displacement[*v].y * displacement[*v].y);
      position[*v].x += displacement[*v].x / disp_size
                     * min BOOST_PREVENT_MACRO_SUBSTITUTION (disp_size, temp);
      position[*v].y += displacement[*v].y / disp_size
                     * min BOOST_PREVENT_MACRO_SUBSTITUTION (disp_size, temp);
      position[*v].x = min BOOST_PREVENT_MACRO_SUBSTITUTION
                         (width / 2,
                          max BOOST_PREVENT_MACRO_SUBSTITUTION(-width / 2,
                                                               position[*v].x));
      position[*v].y = min BOOST_PREVENT_MACRO_SUBSTITUTION
                         (height / 2,
                          max BOOST_PREVENT_MACRO_SUBSTITUTION(-height / 2,
                                                               position[*v].y));
    }
  } while (temp = cool());
}

namespace detail {
  template<typename DisplacementMap>
  struct fr_force_directed_layout
  {
    template<typename Graph, typename PositionMap, typename Dim,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        Dim             width,
        Dim             height,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        DisplacementMap displacement,
        const bgl_named_params<Param, Tag, Rest>&)
    {
      fruchterman_reingold_force_directed_layout
        (g, position, width, height, attractive_force, repulsive_force,
         force_pairs, cool, displacement);
    }
  };

  template<>
  struct fr_force_directed_layout<error_property_not_found>
  {
    template<typename Graph, typename PositionMap, typename Dim,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        Dim             width,
        Dim             height,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        error_property_not_found,
        const bgl_named_params<Param, Tag, Rest>& params)
    {
      std::vector<simple_point<Dim> > displacements(num_vertices(g));
      fruchterman_reingold_force_directed_layout
        (g, position, width, height, attractive_force, repulsive_force,
         force_pairs, cool,
         make_iterator_property_map
         (displacements.begin(),
          choose_const_pmap(get_param(params, vertex_index), g,
                            vertex_index),
          simple_point<Dim>()));
    }
  };

} // end namespace detail

template<typename Graph, typename PositionMap, typename Dim, typename Param,
         typename Tag, typename Rest>
void
fruchterman_reingold_force_directed_layout
  (const Graph&    g,
   PositionMap     position,
   Dim             width,
   Dim             height,
   const bgl_named_params<Param, Tag, Rest>& params)
{
  typedef typename property_value<bgl_named_params<Param,Tag,Rest>,
                                  vertex_displacement_t>::type D;

  detail::fr_force_directed_layout<D>::run
    (g, position, width, height,
     choose_param(get_param(params, attractive_force_t()),
                  square_distance_attractive_force()),
     choose_param(get_param(params, repulsive_force_t()),
                  square_distance_repulsive_force()),
     choose_param(get_param(params, force_pairs_t()),
                  make_grid_force_pairs(width, height, position, g)),
     choose_param(get_param(params, cooling_t()),
                  linear_cooling<Dim>(100)),
     get_param(params, vertex_displacement_t()),
     params);
}

template<typename Graph, typename PositionMap, typename Dim>
void
fruchterman_reingold_force_directed_layout(const Graph&    g,
                                           PositionMap     position,
                                           Dim             width,
                                           Dim             height)
{
  fruchterman_reingold_force_directed_layout
    (g, position, width, height,
     attractive_force(square_distance_attractive_force()));
}

} // end namespace boost

#endif // BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP


#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <random>
#include <Eigen/Cholesky>
#include <fstream>
#include <iomanip>
#include <srrg_system_utils/parse_command_line.h>

#include "srrg_solver/solver_core/instances.h"
#include "srrg_solver/solver_core/factor_graph.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_se3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_point3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_point_offset_error_factor.h"

#include "srrg_solver/variables_and_factors/types_2d/variable_se2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/variable_point2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_point_error_factor.h"

#include "srrg_solver/variables_and_factors/types_projective/variable_sim3_ad.h"
#include "srrg_solver/variables_and_factors/types_projective/sim3_pose_pose_error_factor_ad.h"
#include "srrg_solver/variables_and_factors/types_projective/se3_pose_point_omni_ba_error_factor.h"

#include "srrg_solver/variables_and_factors/types_3d/instances.h"
#include "srrg_solver/variables_and_factors/types_2d/instances.h"
#include "srrg_solver/variables_and_factors/types_projective/instances.h"
#include "srrg_solver/utils/instances.h"
#include "srrg_solver/solver_incremental/factor_graph_incremental_sorter.h"

#include <Eigen/Cholesky>

using namespace srrg2_core;
using namespace srrg2_solver;
using namespace std;

const std::string exe_name = "solver_app_graph_sorter";
#define LOG std::cerr << exe_name << "|"

const char* banner[] = {
  "fuck yourself",
  0
};

// ia register types
void initTypes() {
  variables_and_factors_2d_registerTypes();
  variables_and_factors_3d_registerTypes();
  variables_and_factors_projective_registerTypes();
  solver_utils_registerTypes();
  solver_registerTypes();
}

// ia THE PROGRAM
int main(int argc, char** argv) {
  initTypes();

  ParseCommandLine cmd_line(argv, banner);
  ArgumentString output_file          (&cmd_line, "o",    "output-file",           "file where to save the output", "");
  ArgumentString path_type          (&cmd_line, "t",    "path-type",             "type of the variable used for sorting", "VariableSE3QuaternionRightAD");
  ArgumentString input_file          (&cmd_line, "i",    "input-file",             "file where to read the input ", "");
  cmd_line.parse();

  if (! input_file.isSet()) {
    cerr << "no input file provided, aborting" << endl;
    return 0;
  }
  std::cerr << "loding file: [" << input_file.value() << "]... ";
  FactorGraphPtr graph = FactorGraph::read(input_file.value());
  std::cerr << "done, factors:" << graph->factors().size() << " vars: " << graph->variables().size() << std::endl;

  cerr << "Sorting... ";
  FactorGraphIncrementalSorter sorter;
  sorter.setGraph(graph);
  sorter.setPathType(path_type.value());
  sorter.compute();
  std::cerr << "done" << std::endl;

  if (! output_file.isSet()) {
    cerr << "no output file provided, skipping output" << endl;
    return 0;
  }
  std::cerr << "writing output to file [" << output_file.value() << "]... "<<endl;
  Serializer ser;
  ser.setFilePath(output_file.value());
  int object_count=0;
  for (auto s: sorter.items()) {
    ++object_count;
    ser.writeObject(*s);
    if (! (object_count%1000) ){
      cerr << "\rwriting " << object_count << " objects";
    }
  }
  cerr << endl <<"Done, objects_written : " << object_count << endl;
  return 0;
}


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/split.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;
namespace tt = nsm::type_traits;

template<typename T, std::size_t N, typename Env>
void test( Env&, tt::false_type const& )
{}

template<typename T, std::size_t N, typename Env>
void test( Env& runtime, tt::true_type const& )
{
  using type = bd::upgrade_t<T>;

  std::array<T,N> data;
  for(std::size_t i=0;i<data.size();++i)  data[i] = T(1+i);

  std::array<type,N> dref;
  for(std::size_t i=0;i<dref.size();++i)  dref[i] = static_cast<type>(data[i]);

  bs::pack<T,N>                 value (&data[0]     , &data[0]+N    );
  bd::upgrade_t<bs::pack<T,N>>  valuel, valueh;
  bd::upgrade_t<bs::pack<T,N>>  refl  (&dref[0]     , &dref[0]+N/2  );
  bd::upgrade_t<bs::pack<T,N>>  refh  (&dref[0]+N/2 , &dref[0]+N    );

  auto values = bs::split(value);

  STF_EQUAL( values[0] , refl);
  STF_EQUAL( values[1] , refh);
}

STF_CASE_TPL("split pack<T,N> into a pair of pack<T*2,N/2>", STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T,N  >(runtime, bd::is_upgradable< bs::pack<T,N/2> >{});
  test<T,N/2>(runtime, bd::is_upgradable< bs::pack<T,N/2> >{});
  test<T,N*2>(runtime, bd::is_upgradable< bs::pack<T,N/2> >{});
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef CACHE_HH
#define CACHE_HH

#include "config.h"

#include <vector>
#include <cmath>

#include <boost/math/special_functions/gamma.hpp>

namespace graph_tool
{
using namespace std;

// Repeated computation of x*log(x) and log(x) actually adds up to a lot of
// time. A significant speedup can be made by caching pre-computed values.

extern vector<double> __safelog_cache;
extern vector<double> __xlogx_cache;
extern vector<double> __lgamma_cache;

void init_safelog(size_t x);

template <class T>
inline double safelog(T x)
{
    if (x == 0)
        return 0;
    return log(x);
}

template <bool Init=true, class T>
inline double safelog_fast(T x)
{
    if (size_t(x) >= __safelog_cache.size())
    {
        if (Init)
            init_safelog(x);
        else
            return safelog(x);
    }
    return __safelog_cache[x];
}

void init_xlogx(size_t x);

template <class T>
inline double xlogx(T x)
{
    return x * safelog(x);
}

template <bool Init=true, class T>
inline double xlogx_fast(T x)
{
    if (size_t(x) >= __xlogx_cache.size())
    {
        if (Init)
            init_xlogx(x);
        else
            return xlogx(x);
    }
    return __xlogx_cache[x];
}

void init_lgamma(size_t x);

template <bool Init=true, class T>
inline double lgamma_fast(T x)
{
    if (size_t(x) >= __lgamma_cache.size())
    {
        if (Init)
            init_lgamma(x);
        else
            return lgamma(x);
    }
    return __lgamma_cache[x];
}

void init_cache(size_t E);

} // graph_tool namespace

#endif //CACHE_HH


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostBiconnectedComponents.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
#include "vtkBoostBiconnectedComponents.h"

#include "vtkCellData.h"
#include "vtkIdTypeArray.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkOutEdgeIterator.h"
#include "vtkPointData.h"
#include "vtkSmartPointer.h"
#include "vtkVertexListIterator.h"

#include "vtkBoostGraphAdapter.h"
#include "vtkGraph.h"
#include <boost/graph/biconnected_components.hpp>
#include <boost/version.hpp>
#include <utility>
#include <vector>

using namespace boost;

vtkStandardNewMacro(vtkBoostBiconnectedComponents);

vtkBoostBiconnectedComponents::vtkBoostBiconnectedComponents()
{
  this->OutputArrayName = 0;
}

vtkBoostBiconnectedComponents::~vtkBoostBiconnectedComponents()
{
  // release mem
  this->SetOutputArrayName(0);
}

int vtkBoostBiconnectedComponents::RequestData(vtkInformation* vtkNotUsed(request),
  vtkInformationVector** inputVector, vtkInformationVector* outputVector)
{
  // get the info objects
  vtkInformation* inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation* outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkUndirectedGraph* input =
    vtkUndirectedGraph::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkUndirectedGraph* output =
    vtkUndirectedGraph::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));

  // Send the data to output.
  output->ShallowCopy(input);

  // Create edge biconnected component array.
  // This will be populated directly by the boost algorithm.
  vtkSmartPointer<vtkIntArray> edgeCompArr = vtkSmartPointer<vtkIntArray>::New();
  edgeCompArr->SetNumberOfTuples(input->GetNumberOfEdges());
  for (vtkIdType i = 0; i < input->GetNumberOfEdges(); ++i)
  {
    edgeCompArr->SetValue(i, -1);
  }
  if (this->OutputArrayName)
  {
    edgeCompArr->SetName(this->OutputArrayName);
  }
  else
  {
    edgeCompArr->SetName("biconnected component");
  }
  vtkGraphEdgePropertyMapHelper<vtkIntArray*> helper(edgeCompArr);

  // Create vector of articulation points and set it up for insertion
  // by the algorithm.
  std::vector<vtkIdType> artPoints;
  std::pair<size_t, std::back_insert_iterator<std::vector<vtkIdType>>> res(
    0, std::back_inserter(artPoints));

  // Call BGL biconnected_components.
  // It appears that the signature for this
  // algorithm has changed in 1.32, 1.33, and 1.34 ;p
#if BOOST_VERSION < 103300 // Boost 1.32.x
  // TODO I have no idea what the 1.32 signature is suppose to be
  // res = biconnected_components(
  //  output, helper, std::back_inserter(artPoints), vtkGraphIndexMap());
#elif BOOST_VERSION < 103400 // Boost 1.33.x
  res = biconnected_components(output, helper, std::back_inserter(artPoints), vtkGraphIndexMap());
#else                        // Anything after Boost 1.34.x
  res = biconnected_components(
    output, helper, std::back_inserter(artPoints), vertex_index_map(vtkGraphIndexMap()));
#endif

  size_t numComp = res.first;

  // Assign component values to vertices based on the first edge.
  // If isolated, assign a new value.
  vtkSmartPointer<vtkIntArray> vertCompArr = vtkSmartPointer<vtkIntArray>::New();
  if (this->OutputArrayName)
  {
    vertCompArr->SetName(this->OutputArrayName);
  }
  else
  {
    vertCompArr->SetName("biconnected component");
  }
  vertCompArr->SetNumberOfTuples(output->GetNumberOfVertices());
  vtkSmartPointer<vtkVertexListIterator> vertIt = vtkSmartPointer<vtkVertexListIterator>::New();
  vtkSmartPointer<vtkOutEdgeIterator> edgeIt = vtkSmartPointer<vtkOutEdgeIterator>::New();
  output->GetVertices(vertIt);
  while (vertIt->HasNext())
  {
    vtkIdType u = vertIt->Next();
    output->GetOutEdges(u, edgeIt);
    int comp = -1;
    while (edgeIt->HasNext() && comp == -1)
    {
      vtkOutEdgeType e = edgeIt->Next();
      int value = edgeCompArr->GetValue(e.Id);
      comp = value;
    }
    if (comp == -1)
    {
      comp = static_cast<int>(numComp);
      numComp++;
    }
    vertCompArr->SetValue(u, comp);
  }

  // Articulation points belong to multiple biconnected components.
  // Indicate these by assigning a component value of -1.
  // It belongs to whatever components its incident edges belong to.
  std::vector<vtkIdType>::size_type i;
  for (i = 0; i < artPoints.size(); i++)
  {
    vertCompArr->SetValue(artPoints[i], -1);
  }

  // Add edge and vertex component arrays to the output
  output->GetEdgeData()->AddArray(edgeCompArr);
  output->GetVertexData()->AddArray(vertCompArr);

  return 1;
}

void vtkBoostBiconnectedComponents::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "OutputArrayName: " << (this->OutputArrayName ? this->OutputArrayName : "(none)")
     << endl;
}


/*
 * Copyright 2019,
 * Noëlie Ramuzat,
 *
 *
 */

#include <iostream>
#include <sot/core/debug.hh>

#ifndef WIN32
#include <unistd.h>
#endif

using namespace std;

#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <sot/core/admittance-control-op-point.hh>
#include <sstream>

using namespace dynamicgraph;
using namespace dynamicgraph::sot;

#define BOOST_TEST_MODULE debug - control - admittance

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

BOOST_AUTO_TEST_CASE(control_admittance) {
  sot::core::AdmittanceControlOpPoint *aControlAdm =
      new sot::core::AdmittanceControlOpPoint("acontrol_admittance");

  std::istringstream Kp("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
  std::istringstream Kd("[6](0.0,0.0,0.0,0.0,0.0,0.0)");
  aControlAdm->m_KpSIN.set(Kp);
  aControlAdm->m_KdSIN.set(Kd);
  std::istringstream dqSaturation("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
  aControlAdm->m_dqSaturationSIN.set(dqSaturation);
  std::istringstream w_forceDes("[6](100.0,0.0,0.0,0.0,0.0,0.0)");
  aControlAdm->m_w_forceDesSIN.set(w_forceDes);
  std::istringstream force("[6](10.0,0.0,10.0,0.0,0.0,0.0)");
  aControlAdm->m_forceSIN.set(force);
  MatrixHomogeneous opPose;
  opPose.translation() << 0.3, 0.0, 0.0;
  opPose.linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
  aControlAdm->m_opPoseSIN = opPose;
  MatrixHomogeneous sensorPose;
  sensorPose.translation() << 0.3, 0.0, 0.0;
  sensorPose.linear() << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
  aControlAdm->m_sensorPoseSIN = sensorPose;
  aControlAdm->init(0.001);

  aControlAdm->m_dqSOUT.recompute(0);
  {
    dynamicgraph::Vector expected(6);
    expected << 1.1, 0.0, -0.109, 0.0, 0.03, 0.0;
    BOOST_CHECK(aControlAdm->m_dqSOUT(0).isApprox(expected));
  }
}


//=======================================================================
// Copyright 2001 University of Notre Dame.
// Author: Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================

/*
   Sample output:

   filtered edge set: (A,B) (C,D) (D,B)
   filtered out-edges:
   A --> B
   B -->
   C --> D
   D --> B
   E -->
 */

#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_utility.hpp>

template <typename EdgeWeightMap>
struct positive_edge_weight {
  positive_edge_weight() { }
  positive_edge_weight(EdgeWeightMap weight) : m_weight(weight) { }
  template <typename Edge>
  bool operator()(const Edge& e) const {
    return 0 < boost::get(m_weight, e);
  }
  EdgeWeightMap m_weight;
};

int main()
{
  using namespace boost;

  typedef adjacency_list<vecS, vecS, directedS,
    no_property, property<edge_weight_t, int> > Graph;
  typedef property_map<Graph, edge_weight_t>::type EdgeWeightMap;

  enum { A, B, C, D, E, N };
  const char* name = "ABCDE";
  Graph g(N);
  add_edge(A, B, 2, g);
  add_edge(A, C, 0, g);
  add_edge(C, D, 1, g);
  add_edge(C, E, 0, g);
  add_edge(D, B, 3, g);
  add_edge(E, C, 0, g);

  positive_edge_weight<EdgeWeightMap> filter(get(edge_weight, g));
  filtered_graph<Graph, positive_edge_weight<EdgeWeightMap> >
    fg(g, filter);

  std::cout << "filtered edge set: ";
  print_edges(fg, name);

  std::cout << "filtered out-edges:" << std::endl;
  print_graph(fg, name);

  return 0;
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file types.hpp
 * \date June 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

namespace fl
{

#if defined(fl_USE_FLOAT)
    typedef float FloatingPoint;
#elif defined(fl_USE_DOUBLE)
    typedef double FloatingPoint;
#elif defined(fl_USE_LONG_DOUBLE)
    typedef long double FloatingPoint;
#else
    typedef double FloatingPoint;
#endif

/**
 * \ingroup types
 * \brief Common floating point type. The default type of Real is \c double.
 *        This type is used throughout the entire ::fl library.
 *
 * In order or use other basic floating point types please compile with one of
 * the following defines:
 *
 *  - \c FL_USE_FLOAT:       defines fl::Real as \c float
 *  - \c FL_USE_DOUBLE:      defines fl::Real as \c double (default)
 *  - \c FL_USE_LONG_DOUBLE: defines fl::Real as \c long double
 */
typedef FloatingPoint Real;

typedef Eigen::Matrix<Real, 1, 1> Vector1d;

/**
 * \ingroup types
 */
template <typename Model_> struct Additive
{
    typedef Model_ Model;
};

/**
 * \ingroup types
 */
template <typename Model_> struct AdditiveUncorrelated
{
    typedef Model_ Model;
};

/**
 * \ingroup types
 */
template <typename Model_> struct NonAdditive
{
    typedef Model_ Model;
};

/**
 * \internal
 */
namespace internal
{

/**
 * \internal
 * \ingroup types
 *
 * \brief Linear model type identifier
 */
struct LinearModelType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Observation model type identifier
 */
struct SensorType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Process model type identifier
 */
struct TransitionType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Adaptive model type identifier
 */
struct AdaptiveModelType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Represents the base type of any model with additive noise term
 * \f$ x_{t+1} = f(x_t) + v_t\f$ while \f$v_t\f$ is the additive noise.
 */
struct AdditiveNoiseModelType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Represents the base type of any model with additive uncorrelated
 * Gaussian white noise term in \f$ x_{t+1} = f(x_t) + v_t\f$ while \f$v_t\f$ is
 * the additive noise with \f$v_t \sim {\cal N}(v_t; 0, Q_t)\f$. Here, the
 * covariance matrix has a diagonal form \f$Q_t = \text{diag}(q_1, q_2, \ldots,
 * q_n)\f$ and \f$n\f$ is the dimension of \f$v_t \in \mathbb{R}^n\f$.
 */
struct AdditiveUncorrelatedNoiseModelType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Represents the base type of any model with non-additive noise term
 * \f$ x_{t+1} = f(x_t, v_t) \f$ while \f$v_t\f$ is the additive noise.
 */
struct NonAdditiveNoiseModelType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Base type of every iid JointSensor
 */
struct JointSensorIidType { };

/**
 * \internal
 * \ingroup types
 *
 * \brief Base type of every id JointSensor
 */
struct JointSensorIdType { };

}

}


/*
 [auto_generated]
 boost/numeric/odeint/external/mtl4/mtl4_algebra_dispatcher.hpp

 [begin_description]
 specialization of the algebra dispatcher for mtl4
 [end_description]

 Copyright 2009-2013 Karsten Ahnert
 Copyright 2009-2013 Mario Mulansky

 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_NUMERIC_ODEINT_MTL4_MTL4_ALGEBRA_DISPATCHER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_MTL4_MTL4_ALGEBRA_DISPATCHER_HPP_INCLUDED

#include <boost/numeric/mtl/mtl.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<typename Value, typename Parameters>
struct algebra_dispatcher< mtl::dense_vector< Value , Parameters > >
{
    typedef vector_space_algebra algebra_type;
};

template<typename Value, typename Parameters>
struct algebra_dispatcher< mtl::dense2D< Value , Parameters > >
{
    typedef vector_space_algebra algebra_type;
};

template<typename Value , size_t BitMask , typename Parameters>
struct algebra_dispatcher< mtl::morton_dense< Value , BitMask, Parameters > >
{
    typedef vector_space_algebra algebra_type;
};

template<typename Value, typename Parameters>
struct algebra_dispatcher< mtl::compressed2D< Value , Parameters > >
{
    typedef vector_space_algebra algebra_type;
};

// specialization of infinity norm calculation

template<typename Value, typename Parameters>
struct vector_space_norm_inf< mtl::dense_vector< Value , Parameters > >
{
    typedef Value result_type;
    Value operator()( const mtl::dense_vector< Value , Parameters > &x ) const
    {
        return mtl::infinity_norm(x);
    }
};

template<typename Value, typename Parameters>
struct vector_space_norm_inf< mtl::dense2D< Value , Parameters > >
{
    typedef Value result_type;
    Value operator()( const mtl::dense2D< Value , Parameters > &x ) const
    {
        return mtl::infinity_norm(x);
    }
};

template<typename Value , size_t BitMask , typename Parameters>
struct vector_space_norm_inf< mtl::morton_dense< Value , BitMask , Parameters > >
{
    typedef Value result_type;
    Value operator()( const mtl::morton_dense< Value , BitMask , Parameters > &x ) const
    {
        return mtl::infinity_norm(x);
    }
};

template<typename Value, typename Parameters>
struct vector_space_norm_inf< mtl::compressed2D< Value , Parameters > >
{
    typedef Value result_type;
    Value operator()( const mtl::compressed2D< Value , Parameters > &x ) const
    {
        return mtl::infinity_norm(x);
    }
};

}
}
}

#endif // BOOST_NUMERIC_ODEINT_MTL4_MTL4_ALGEBRA_DISPATCHER_INCLUDED


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014, Oracle and/or its affiliates.

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_union_pointlike_pointlike
#endif

#ifdef BOOST_GEOMETRY_TEST_DEBUG
#define BOOST_GEOMETRY_DEBUG_TURNS
#define BOOST_GEOMETRY_DEBUG_SEGMENT_IDENTIFIER
#endif

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

#include "test_set_ops_pl_pl.hpp"

#include <boost/geometry/multi/geometries/multi_point.hpp>

typedef bg::model::point<double,2,bg::cs::cartesian>  point_type;
typedef bg::model::multi_point<point_type>  multi_point_type;

//===========================================================================
//===========================================================================
//===========================================================================

BOOST_AUTO_TEST_CASE( test_union_point_point )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** POINT / POINT UNION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef point_type P;
    typedef multi_point_type MP;

    typedef test_set_op_of_pointlike_geometries
        <
            P, P, MP, bg::overlay_union
        > tester;

    tester::apply
        (from_wkt<P>("POINT(0 0)"),
         from_wkt<P>("POINT(1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0,1 1)"),
         "ppu01");

    tester::apply
        (from_wkt<P>("POINT(0 0)"),
         from_wkt<P>("POINT(0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0)"),
         "ppu02");
}

BOOST_AUTO_TEST_CASE( test_union_multipoint_point )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** MULTIPOINT / POINT UNION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef point_type P;
    typedef multi_point_type MP;

    typedef test_set_op_of_pointlike_geometries
        <
            MP, P, MP, bg::overlay_union
        > tester;

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0)"),
         from_wkt<P>("POINT(1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0,1 1)"),
         "mppu01");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0)"),
         from_wkt<P>("POINT(0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0)"),
         "mppu02");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,0 0)"),
         from_wkt<P>("POINT(1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0,1 1)"),
         "mppu03");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,0 0)"),
         from_wkt<P>("POINT(0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0)"),
         "mppu04");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         from_wkt<P>("POINT(1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0,1 1)"),
         "mppu05");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         from_wkt<P>("POINT(1 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         "mppu06");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         from_wkt<P>("POINT(0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,1 0)"),
         "mppu07");

    tester::apply
        (from_wkt<MP>("MULTIPOINT()"),
         from_wkt<P>("POINT(0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0)"),
         "mppu08");
}

BOOST_AUTO_TEST_CASE( test_union_multipoint_multipoint )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** MULTIPOINT / MULTIPOINT UNION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef multi_point_type MP;

    typedef test_set_op_of_pointlike_geometries
        <
            MP, MP, MP, bg::overlay_union
        > tester;

    tester::apply
        (from_wkt<MP>("MULTIPOINT(2 2,3 3,0 0,0 0,2 2,1 1,1 1,1 0,1 0)"),
         from_wkt<MP>("MULTIPOINT(1 0,1 1,1 1,1 1)"),
         from_wkt<MP>("MULTIPOINT(2 2,3 3,0 0,0 0,2 2,1 1,1 1,1 0,1 0)"),
         from_wkt<MP>("MULTIPOINT(1 0,1 1,1 1,1 1,2 2,3 3,0 0,0 0,2 2)"),
         "mpmpu01");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,1 1,1 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(1 0,0 0,1 1,0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,1 1,1 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(1 0,0 0,1 1,0 0)"),
         "mpmpu02");

    tester::apply
        (from_wkt<MP>("MULTIPOINT()"),
         from_wkt<MP>("MULTIPOINT(1 0,0 0,1 1,0 0)"),
         from_wkt<MP>("MULTIPOINT(1 0,0 0,1 1,0 0)"),
         "mpmpu03");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,1 1,1 0,1 1)"),
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<MP>("MULTIPOINT(0 0,1 1,1 0,1 1)"),
         "mpmpu04");

    tester::apply
        (from_wkt<MP>("MULTIPOINT()"),
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<MP>("MULTIPOINT()"),
         "mpmpu05");

    tester::apply
        (from_wkt<MP>("MULTIPOINT(0 0,1 0,2 0,3 0,0 0,1 0,2 0)"),
         from_wkt<MP>("MULTIPOINT(0 1,0 2,1 0,0 0,2 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,1 0,2 0,3 0,0 0,1 0,2 0,0 1,0 2)"),
         from_wkt<MP>("MULTIPOINT(0 1,0 2,1 0,0 0,2 0,3 0)"),
         "mpmpu06");
}


/*
 *          Copyright Schwenk, Kurt <Kurt.Schwenk@gmx.de> 2020
 * 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)
 */

#define BOOST_TEST_MODULE test_block_output_predicate
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include <dataflow/blocks/Block.h>
#include <dataflow/channels/Channel.h>
#include <dataflow/executors/ExecutorSeq.h>

using namespace dataflow::blocks;
using namespace dataflow::channels;
using namespace dataflow::executors;

BOOST_AUTO_TEST_CASE(BasicOutputPredicate)
{
    using ValueType = int;
    Channel<ValueType> chanIn;
    Channel<ValueType> chanOut;

    ExecutorSeq executor;

    //Definition passThroughOnlyEvenBlock
    auto passThrough = [](ValueType&& val) { return std::forward<ValueType>(val); };

    auto outputOnlyEven = [](const auto&... inputPack) {
        auto isEven = [](const auto& number) { return number % 2 == 0; };
        return std::array { isEven(inputPack)... };
    };

    Block passThroughOnlyEven { ChannelBundle { chanIn },
        passThrough, ChannelBundle { chanOut }, executor, outputOnlyEven };

    //Test run
    const size_t cycles = 101;

    for (size_t cycle = 0; cycle < cycles; ++cycle) {
        chanIn.push(cycle);
    };

    //Test evaluation
    const size_t cyclesEven = cycles % 2 == 0 ? cycles / 2 : cycles / 2 + 1;

    BOOST_CHECK_EQUAL(chanOut.size(), cyclesEven);

    for (size_t i = 0; i < cyclesEven; ++i) {
        BOOST_CHECK_EQUAL(chanOut.pop().value() % 2, 0);
    }
}


#include <cmath>
#include <boost/function.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>
#include <boost/phoenix/stl/cmath.hpp>
#include <boost/detail/lightweight_test.hpp>

int main()
{
    double eps = 0.000001;
    using namespace boost::phoenix::arg_names;
    boost::function<bool(double, double)> f = fabs(_1 - _2) < eps;

    BOOST_TEST(f(0.0, 0 * eps));
    BOOST_TEST(!f(0.0, eps));
}


/* Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  * Neither the name of NVIDIA CORPORATION nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <boost/multiprecision/cpp_int.hpp>

#include "util/numeric.hpp"

using namespace boost::multiprecision;

//------------------------------------
//              Factors
//------------------------------------

unsigned long Factors::ISqrt_(unsigned long x)
{
  register unsigned long op, res, one;

  op = x;
  res = 0;

  one = 1 << 30;
  while (one > op) one >>= 2;

  while (one != 0)
  {
    if (op >= res + one)
    {
      op -= res + one;
      res += one << 1;
    }
    res >>= 1;
    one >>= 2;
  }
  return res;
}

void Factors::CalculateAllFactors_()
{
  all_factors_.clear();
  for (unsigned long i = 1; i <= ISqrt_(n_); i++)
  {
    if (n_ % i == 0)
    {
      all_factors_.push_back(i);
      if (i * i != n_)
      {
        all_factors_.push_back(n_ / i);
      }
    }
  }
}

// Return a vector of all order-way cofactor sets of n.
std::vector<std::vector<unsigned long>>
Factors::MultiplicativeSplitRecursive_(unsigned long n, int order)
{
  if (order == 0)
  {
    return {{}};
  }
  else if (order == 1)
  {
    return {{n}};
  }
  else
  {
    std::vector<std::vector<unsigned long>> retval;
    for (auto factor = all_factors_.begin(); factor != all_factors_.end(); factor++)
    {
      // This factor is only acceptable if the residue is divisible by it.
      if (n % (*factor) == 0)
      {
        // Recursive call.
        std::vector<std::vector<unsigned long>> subproblem =
          MultiplicativeSplitRecursive_(n / (*factor), order - 1);

        // Append this factor to the end of each vector returned by the
        // recursive call.
        for (auto vector = subproblem.begin(); vector != subproblem.end(); vector++)
        {
          vector->push_back(*factor);
        }

        // Add all these appended vectors to my growing vector of vectors.
        retval.insert(retval.end(), subproblem.begin(), subproblem.end());
      }
      else
      {
        // Discard this factor.
      }
    }
    return retval;
  }
}

Factors::Factors() :
    n_(0)
{
}

Factors::Factors(const unsigned long n, const int order) :
    n_(n), cofactors_()
{
  CalculateAllFactors_();
  cofactors_ = MultiplicativeSplitRecursive_(n, order);
}

Factors::Factors(const unsigned long n, const int order, std::map<unsigned, unsigned long> given) :
    n_(n), cofactors_()
{
  assert(given.size() <= std::size_t(order));

  // If any of the given factors is not a factor of n, forcibly reset that to
  // be a free variable, otherwise accumulate them into a partial product.
  unsigned long partial_product = 1;
  for (auto f = given.begin(); f != given.end(); f++)
  {
    auto factor = f->second;
    if (n % (factor * partial_product) == 0)
    {
      partial_product *= factor;
    }
    else
    {
      std::cerr << "WARNING: cannot accept " << factor << " as a factor of " << n
                << " with current partial product " << partial_product;
#define SET_NONFACTOR_TO_FREE_VARIABLE
#ifdef SET_NONFACTOR_TO_FREE_VARIABLE
      // Ignore mapping constraint and set the factor to a free variable.
      std::cerr << ", ignoring mapping constraint and setting to a free variable."
                << std::endl;
      f = given.erase(f);
#else
      // Try to find the next *lower* integer that is a factor of n.
      // FIXME: there are multiple exceptions that can cause us to be here:
      // (a) factor doesn't divide into n.
      // (b) factor does divide into n but causes partial product to exceed n.
      // (c) factor does divide into n but causes partial product to not
      //     divide into n.
      // The following code only solves case (a).
      std::cerr << "FIXME: please fix this code." << std::endl;
      assert(false);

      for (; factor >= 1 && (n % factor != 0); factor--);
      std::cerr << ", setting this to " << factor << " instead." << std::endl;
      f->second = factor;
      partial_product *= factor;
#endif
    }
    assert(n % partial_product == 0);
  }

  CalculateAllFactors_();

  cofactors_ = MultiplicativeSplitRecursive_(n / partial_product, order - given.size());

  // Insert the given factors at the specified indices of each of the solutions.
  for (auto& cofactors : cofactors_)
  {
    for (auto& given_factor : given)
    {
      // Insert the given factor, pushing all existing factors back.
      auto index = given_factor.first;
      auto value = given_factor.second;
      assert(index <= cofactors.size());
      cofactors.insert(cofactors.begin() + index, value);
    }
  }
}

void Factors::PruneMax(std::map<unsigned, unsigned long>& max)
{
  // Prune the vector of cofactor sets by removing those sets that have factors
  // outside user-specified min/max range. We should really have done this during
  // MultiplicativeSplitRecursive. However, the "given" map complicates things
  // because given factors may be scattered, and we'll need a map table to
  // find the original rank from the "compressed" rank seen by
  // MultiplicativeSplitRecursive. Doing it now is slower but cleaner and less
  // bug-prone.

  auto cofactors_it = cofactors_.begin();
  while (cofactors_it != cofactors_.end())
  {
    bool illegal = false;
    for (auto& max_factor : max)
    {
      auto index = max_factor.first;
      auto max = max_factor.second;
      assert(index <= cofactors_it->size());
      auto value = cofactors_it->at(index);
      if (value > max)
      {
        illegal = true;
        break;
      }
    }

    if (illegal)
      cofactors_it = cofactors_.erase(cofactors_it);
    else
      cofactors_it++;
  }
}

std::vector<unsigned long>& Factors::operator[](int index)
{
  return cofactors_[index];
}

std::size_t Factors::size()
{
  return cofactors_.size();
}

void Factors::Print()
{
  PrintAllFactors();
  PrintCoFactors();
}

void Factors::PrintAllFactors()
{
  std::cout << "All factors of " << n_ << ": ";
  bool first = true;
  for (auto f = all_factors_.begin(); f != all_factors_.end(); f++)
  {
    if (first)
    {
      first = false;
    }
    else
    {
      std::cout << ", ";
    }
    std::cout << (*f);
  }
  std::cout << std::endl;
}

void Factors::PrintCoFactors()
{
  std::cout << *this;
}

std::ostream& operator<<(std::ostream& out, const Factors& f)
{
  out << "Co-factors of " << f.n_ << " are: " << std::endl;
  for (auto cset = f.cofactors_.begin(); cset != f.cofactors_.end(); cset++)
  {
    out << "    " << f.n_ << " = ";
    bool first = true;
    for (auto i = cset->begin(); i != cset->end(); i++)
    {
      if (first)
      {
        first = false;
      }
      else
      {
        out << " * ";
      }
      out << (*i);
    }
    out << std::endl;
  }
  return out;
}
//------------------------------------
//              ResidualFactors
//------------------------------------

unsigned long ResidualFactors::ISqrt_(unsigned long x)
{
  unsigned long op, res, one;

  op = x;
  res = 0;

  one = 1 << 30;
  while (one > op) one >>= 2;

  while (one != 0)
  {
    if (op >= res + one)
    {
      op -= res + one;
      res += one << 1;
    }
    res >>= 1;
    one >>= 2;
  }
  return res;
}

void ResidualFactors::ClearAllFactors_()
{
  all_factors_.clear();
}
void ResidualFactors::CalculateAllFactors_()
{
  for (unsigned long i = 1; i <= ISqrt_(n_); i++)
  {
    if (n_ % i == 0)
    {
      all_factors_.insert(i);
      if (i * i != n_)
      {
        all_factors_.insert(n_ / i);
      }
    }
  }
}

// Generate all additional potential factors given the remainder bounds
void ResidualFactors::CalculateAdditionalFactors_()
{
  std::vector<unsigned long> reaminder_possible;
  for (auto& n : remainder_bounds_){
    for(unsigned long i = 1; i <= n; i++)
      reaminder_possible.push_back(i);
  }

  for (auto& n : reaminder_possible){
    unsigned long g = n * n_ * ceil((double)n_/(double)n);
    for (unsigned long i = 1; i <= n_; i++)
    {
      if (g % i == 0)
      {
        if (i < n_)
        all_factors_.insert(i);
        if (i * i != g)
        {
          if ((g/i) < n_)
          all_factors_.insert(g / i);
        }
      }
    }
  }
}

std::vector<std::vector<unsigned long>> ResidualFactors::CartProduct_ (const std::vector<std::vector<unsigned long>> v) {
  std::vector<std::vector<unsigned long>> s = {{}};
  for (const auto u : v) {
      std::vector<std::vector<unsigned long>> r;
      for (const auto x : s) {
          for (const auto y : u) {
              r.push_back(x);
              r.back().push_back(y);
          }
      }
      s = r;
  }
  return s;
}

// Replicate all factors possibilities accross each level n, disregarding sets with that include more than dimension size n * sqrt(n)
void ResidualFactors::GenerateFactorProduct_(const unsigned long n, const int order)
{
  for(auto rec = 0; rec < order; rec++){
    std::vector<std::vector<unsigned long>> inter_factors;
    std::vector<std::vector<unsigned long>> product_factors;
    for (auto i = 0; i < order; i++)
    {
      std::vector<unsigned long> v2;
      for(auto a : all_factors_){
        if (i == 0 && rec == 0){
          v2.push_back(a);
        }else if(a <= ((unsigned int)(pow((double)n_, 1.0/(2.0)) + 1.5)) && i > 0){
          v2.push_back(a);
        }else if (rec > 0 && i == 0 && a >= ((unsigned int)(pow((double)n_, 1.0/(2.0)) + 1.5))){
          v2.push_back(a);
        }
      }
      inter_factors.push_back(v2);
    }

    std::swap(inter_factors[0], inter_factors[rec]);

    product_factors = CartProduct_(inter_factors);
    replicated_factors_.reserve(replicated_factors_.size() + product_factors.size());
    replicated_factors_.insert(replicated_factors_.end(), product_factors.begin(), product_factors.end());
  }

  for(auto t : replicated_factors_){

      unsigned long product = 1;
      for(auto p : t){
        if (p != 1){
          product *= (p-1);
        }
      }
      if (product <= n){
          pruned_product_factors_.push_back(t);
      }
  }

}

// Replicate all possible residual combiniations, and remove possibilities that sum up to size greater than n
void ResidualFactors::GenerateResidual_(const unsigned long n, const int order)
{
  std::vector<std::vector<unsigned long>> residuals;

  for (auto i : remainder_bounds_){
    std::vector<unsigned long> r;
    for (unsigned j = 1; j <= i; j++){
      r.push_back(j);
    }
    residuals.push_back(r);
  }

  residuals = CartProduct_(residuals);

  for(auto t : residuals){
    unsigned long sum = 0;
    for(auto p : t){
      sum += p;
    }
    if(sum <= n+(unsigned)order){
      pruned_residuals_.push_back(t);
    }
  }

}

// Replicate all possible residual combiniations, and remove possibilities that sum up to size greater than n
void ResidualFactors::ValidityChecker_(const unsigned long n, std::map<unsigned, unsigned long> given)
{

  for (unsigned i = pruned_product_factors_.size(); i > 0 ; i--)
  {

    for (auto it = given.begin(); it != given.end(); it++)
    {
      // Insert the given factor, pushing all existing factors back.
      auto index = it->first;
      auto value = it->second;
      pruned_product_factors_[i-1].insert(pruned_product_factors_[i-1].begin() + index, value);
    }

  }

  for(auto f : pruned_product_factors_){
    for(auto r : pruned_residuals_){
      std::vector<unsigned long> valid_residuals;
      int s_i = 0;
      bool valid = true;
      for(unsigned long i = 0; i < (f.size()); i++){
        if(std::count(remainder_ix_.begin(),remainder_ix_.end(), i) > 0){
          valid_residuals.push_back(r.at(s_i));

          if(f.at(i) > remainder_bounds_[s_i]){
            valid = false;
          }
          s_i++;
        }else{
          valid_residuals.push_back(f.at(i));
        }
      }

      //Solve for generic is L_{n} = L{n+1}*P{n} + R_{n} - 1
      unsigned long equation_answer = 0;
      for(unsigned j = (f.size()); j > 0; j--){
          equation_answer = f.at(j-1)*equation_answer + (valid_residuals.at(j-1) - 1);

          if (f.at(j-1) < valid_residuals.at(j-1))
            valid = false;
          if(equation_answer == 0 && valid_residuals.at(j-1) != f.at(j-1))
            valid = false;
        }

        if ((equation_answer + 1 == n) and valid){

          cofactors_.push_back(f);
          rfactors_.push_back(valid_residuals);

      }
    }
  }

}

void ResidualFactors::PruneMax()
{
  // Prune the vector of cofactor sets by removing those sets that have factors
  // outside user-specified min/max range. We should really have done this during
  // MultiplicativeSplitRecursive. However, the "given" map complicates things
  // because given factors may be scattered, and we'll need a map table to
  // find the original rank from the "compressed" rank seen by
  // MultiplicativeSplitRecursive. Doing it now is slower but cleaner and less
  // bug-prone.

}

ResidualFactors::ResidualFactors() : n_(0) {}

/***
First, we cacluate all the factors same as Uber
Next, we add in additional factors based on user defined loop bounds and take the cross product of these (eliminating impossible mappings)
Then, we calculate all the valid mapspace points that fit the expanded formula L_{n} = L{n+1}*P{n} + R_{n} - 1
***/
ResidualFactors::ResidualFactors(const unsigned long n, const int order, std::vector<unsigned long> remainder_bounds,
    std::vector<unsigned long> remainder_ix) : n_(n), remainder_bounds_(remainder_bounds), remainder_ix_(remainder_ix)
{
  ClearAllFactors_();
  CalculateAllFactors_();
  CalculateAdditionalFactors_();
  GenerateFactorProduct_(n, order);
  GenerateResidual_(n, order);
  std::map<unsigned, unsigned long> given = {{}};
  ValidityChecker_(n, given);

  for (unsigned i = 0; i < cofactors_.size(); i++)
  {
    std::reverse(cofactors_[i].begin(), cofactors_[i].end());
    std::reverse(rfactors_[i].begin(), rfactors_[i].end());
  }
}

ResidualFactors::ResidualFactors(const unsigned long n, const int order, std::vector<unsigned long> remainder_bounds,
    std::vector<unsigned long> remainder_ix, std::map<unsigned, unsigned long> given)
    : n_(n), remainder_bounds_(remainder_bounds), remainder_ix_(remainder_ix)
{

  const unsigned int given_size = given.size();

  assert(given_size <= std::size_t(order));
  // If any of the given factors is not a factor of n, forcibly reset that to
  // be a free variable, otherwise accumulate them into a partial product.
  unsigned long partial_product = 1;
  for (auto f = given.begin(); f != given.end(); f++)
  {
    auto factor = f->second;
    if (n % (factor * partial_product) == 0)
    {
      partial_product *= factor;
    }
    else
    {
      std::cerr << "WARNING: cannot accept " << factor << " as a factor of " << n
                << " with current partial product " << partial_product;
#define SET_NONFACTOR_TO_FREE_VARIABLE
#ifdef SET_NONFACTOR_TO_FREE_VARIABLE
      // Ignore mapping constraint and set the factor to a free variable.
      std::cerr << ", ignoring mapping constraint and setting to a free variable."
                << std::endl;
      f = given.erase(f);
#else
      // Try to find the next *lower* integer that is a factor of n.
      // FIXME: there are multiple exceptions that can cause us to be here:
      // (a) factor doesn't divide into n.
      // (b) factor does divide into n but causes partial product to exceed n.
      // (c) factor does divide into n but causes partial product to not
      //     divide into n.
      // The following code only solves case (a).
      std::cerr << "FIXME: please fix this code." << std::endl;
      assert(false);

      for (; factor >= 1 && (n % factor != 0); factor--);
      std::cerr << ", setting this to " << factor << " instead." << std::endl;
      f->second = factor;
      partial_product *= factor;
#endif
    }
    assert(n % partial_product == 0);
  }

  ClearAllFactors_();
  CalculateAllFactors_();
  CalculateAdditionalFactors_();

  GenerateFactorProduct_(n / partial_product, order - given.size());

  GenerateResidual_(n / partial_product, order - given.size());

  // Insert the given factors at the specified indices of each of the solutions.

  ValidityChecker_(n, given);

  remainder_bounds_.resize(0);
  remainder_ix_.resize(0);
  pruned_product_factors_.resize(0);
  pruned_residuals_.resize(0);
  replicated_factors_.resize(0);

}

std::vector<std::vector<unsigned long>> ResidualFactors::operator[](int index)
{
  std::vector<std::vector<unsigned long>> ret;
  std::vector<unsigned long> cfm = cofactors_.at(index);
  std::vector<unsigned long> rfm = rfactors_.at(index);

  ret.push_back(cfm);
  ret.push_back(rfm);
  return ret;
}

std::size_t ResidualFactors::size() { return cofactors_.size(); }

void ResidualFactors::Print()
{
  PrintAllFactors();
  PrintCoFactors();
}

void ResidualFactors::PrintAllFactors()
{
  std::cout << "All factors of " << n_ << ": ";
  bool first = true;
  for (auto f = all_factors_.begin(); f != all_factors_.end(); f++) {
    if (first) {
      first = false;
    } else {
      std::cout << ", ";
    }
    std::cout << (*f);
  }
  std::cout << std::endl;
}

void ResidualFactors::PrintCoFactors() { std::cout << *this; }

std::ostream& operator<<(std::ostream& out, const ResidualFactors& f) {
  out << "Co-factors of " << f.n_ << " are: " << std::endl;
  for (auto cset = f.cofactors_.begin(); cset != f.cofactors_.end(); cset++) {
    out << "    " << f.n_ << " = ";
    bool first = true;
    for (auto i = cset->begin(); i != cset->end(); i++) {
      if (first) {
        first = false;
      } else {
        out << " * ";
      }
      out << (*i);
    }
    out << std::endl;
  }
  return out;
}

//------------------------------------
//        PatternGenerator128
//------------------------------------

PatternGenerator128::PatternGenerator128(uint128_t bound) :
    bound_(bound)
{
}

SequenceGenerator128::SequenceGenerator128(uint128_t bound, bool autoloop) :
    PatternGenerator128(bound),
    autoloop_(autoloop),
    cur_(0)
{
}

uint128_t SequenceGenerator128::Next()
{
  auto retval = cur_;
  if (cur_ == bound_-1)
  {
    assert(autoloop_);
    cur_ = 0;
  }
  else
  {
    cur_++;
  }
  return retval;
}

RandomGenerator128::RandomGenerator128(uint128_t bound) :
    PatternGenerator128(bound),
    use_two_generators_(bound > uint128_t(uint64_max_)),
    low_gen_(0, use_two_generators_ ? uint64_max_ : (std::uint64_t)(bound - 1)),
    high_gen_(0, (std::uint64_t)(bound/uint64_max_ - 1))
{
}

uint128_t RandomGenerator128::Next()
{
  std::uint64_t low = low_gen_(engine_);
  std::uint64_t high = 0;

  if (use_two_generators_)
  {
    high = high_gen_(engine_);
  }

  uint128_t rand = low + ((uint128_t)high * uint64_max_);
  assert(rand < bound_);

  return rand;
}

//------------------------------------
//           Miscellaneous
//------------------------------------

// Returns the smallest factor of an integer and the quotient after
// division with the smallest factor.
void SmallestFactor(uint64_t n, uint64_t& factor, uint64_t& residue)
{
  for (uint64_t i = 2; i < n; i++)
  {
    if (n % i == 0)
    {
      factor = i;
      residue = n / i;
      return;
    }
  }
  factor = n;
  residue = 1;
}

// Helper function to get close-to-square layouts of arrays
// containing a given number of nodes.
void GetTiling(uint64_t num_elems, uint64_t& height, uint64_t& width)
{
  std::vector<uint64_t> factors;
  uint64_t residue = num_elems;
  uint64_t cur_factor;
  while (residue > 1)
  {
    SmallestFactor(residue, cur_factor, residue);
    factors.push_back(cur_factor);
  }

  height = 1;
  width = 1;
  for (uint64_t i = 0; i < factors.size(); i++)
  {
    if (i % 2 == 0)
      height *= factors[i];
    else
      width *= factors[i];
  }

  if (height > width)
  {
    uint64_t temp = height;
    height = width;
    width = temp;
  }
}

double LinearInterpolate(double x,
                         double x0, double x1,
                         double q0, double q1)
{
  double slope = (x0 == x1) ? 0 : (q1 - q0) / double(x1 - x0);
  return q0 + slope * (x - x0);
}

double BilinearInterpolate(double x, double y,
                           double x0, double x1,
                           double y0, double y1,
                           double q00, double q01, double q10, double q11)
{
  // Linear interpolate along x dimension.
  double qx0 = LinearInterpolate(x, x0, x1, q00, q10);
  double qx1 = LinearInterpolate(x, x0, x1, q01, q11);

  // Linear interpolate along y dimension.
  return LinearInterpolate(y, y0, y1, qx0, qx1);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_NUM_SEGMENTS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_NUM_SEGMENTS_HPP

#include <cstddef>

#include <boost/mpl/size_t.hpp>
#include <boost/mpl/times.hpp>

#include <boost/range.hpp>

#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/variant_fwd.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/util/range.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>

#include <boost/geometry/algorithms/detail/counting.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace num_segments
{

struct range_count
{
    template <typename Range>
    static inline std::size_t apply(Range const& range)
    {
        std::size_t n = boost::size(range);
        if ( n <= 1 )
        {
            return 0;
        }

        return
            geometry::closure<Range>::value == open
            ?
            n
            :
            static_cast<std::size_t>(n - 1);
    }
};

}} // namespace detail::num_segments
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct num_segments
    : not_implemented<Tag>
{};

template <typename Geometry>
struct num_segments<Geometry, point_tag>
    : detail::counting::other_count<0>
{};

// the number of segments (1-dimensional faces) of the hypercube is
// given by the formula: d * 2^(d-1), where d is the dimension of the
// hypercube; see also:
//            http://en.wikipedia.org/wiki/Hypercube
template <typename Geometry>
struct num_segments<Geometry, box_tag>
    : detail::counting::other_count
        <
            geometry::dimension<Geometry>::value
            * (1 << (geometry::dimension<Geometry>::value - 1))
        >
{};

template <typename Geometry>
struct num_segments<Geometry, segment_tag>
    : detail::counting::other_count<1>
{};

template <typename Geometry>
struct num_segments<Geometry, linestring_tag>
    : detail::num_segments::range_count
{};

template <typename Geometry>
struct num_segments<Geometry, ring_tag>
    : detail::num_segments::range_count
{};

template <typename Geometry>
struct num_segments<Geometry, polygon_tag>
    : detail::counting::polygon_count<detail::num_segments::range_count>
{};

template <typename Geometry>
struct num_segments<Geometry, multi_point_tag>
    : detail::counting::other_count<0>
{};

template <typename Geometry>
struct num_segments<Geometry, multi_linestring_tag>
    : detail::counting::multi_count
        <
            num_segments< typename boost::range_value<Geometry>::type>
        >
{};

template <typename Geometry>
struct num_segments<Geometry, multi_polygon_tag>
    : detail::counting::multi_count
        <
            num_segments< typename boost::range_value<Geometry>::type>
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_variant
{

template <typename Geometry>
struct num_segments
{
    static inline std::size_t apply(Geometry const& geometry)
    {
        concept::check<Geometry const>();

        return dispatch::num_segments<Geometry>::apply(geometry);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct num_segments<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    struct visitor: boost::static_visitor<std::size_t>
    {
        template <typename Geometry>
        inline std::size_t operator()(Geometry const& geometry) const
        {
            return num_segments<Geometry>::apply(geometry);
        }
    };

    static inline std::size_t
    apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry)
    {
        return boost::apply_visitor(visitor(), geometry);
    }
};

} // namespace resolve_variant

/*!
\brief \brief_calc{number of segments}
\ingroup num_segments
\details \details_calc{num_segments, number of segments}.
\tparam Geometry \tparam_geometry
\param geometry \param_geometry
\return \return_calc{number of segments}

\qbk{[include reference/algorithms/num_segments.qbk]}
*/
template <typename Geometry>
inline std::size_t num_segments(Geometry const& geometry)
{
    return resolve_variant::num_segments<Geometry>::apply(geometry);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_NUM_SEGMENTS_HPP


#include <Eigen/Core>
#include <boost/math/constants/constants.hpp>
#include <limits>
#include <nlopt.hpp>
#include <vector>

#include "common.hh"
#include "scenegraph.hh"
#include "solver.hh"

namespace planner::solver {
namespace {
  std::unique_ptr<Vec<double>> lower_bounds;
  std::unique_ptr<Vec<double>> upper_bounds;

  void make_bounds(const int dim, const bool robot_movable, const Transform3r* const base_pose) {
    if (lower_bounds != nullptr && upper_bounds != nullptr) {
      return;
    }

    lower_bounds = std::make_unique<Vec<double>>(dim, 0.0);
    upper_bounds = std::make_unique<Vec<double>>(dim, 0.0);
    // TODO(Wil): Spooky scary global state
    const size_t bounds_size = cspace::workspace_bounds->low.size();
    int offset               = 0;
    if (robot_movable) {
      for (size_t i = 0; i < bounds_size; ++i) {
        lower_bounds->at(i) = cspace::workspace_bounds->low.at(i);
        upper_bounds->at(i) = cspace::workspace_bounds->high.at(i);
      }

      // Fix the Z coordinate of the robot translation
      lower_bounds->at(2) = base_pose->translation().z();
      upper_bounds->at(2) = base_pose->translation().z();
      lower_bounds->at(3) = -boost::math::double_constants::pi;
      upper_bounds->at(3) = boost::math::double_constants::pi;
      offset += bounds_size + 1;
    }

    // Add pi bounds for the continuous joints
    for (size_t i = 0; i < cspace::cont_joint_idxs.size(); ++i) {
      lower_bounds->at(offset + i) = -boost::math::double_constants::pi;
      upper_bounds->at(offset + i) = boost::math::double_constants::pi;
    }

    offset += cspace::cont_joint_idxs.size();

    // Handle remaining joints
    for (size_t i = 0; i < cspace::joint_bounds.size(); ++i) {
      lower_bounds->at(offset + i) = cspace::joint_bounds[i].first;
      upper_bounds->at(offset + i) = cspace::joint_bounds[i].second;
    }
  }
  struct OptData {
    OptData(Vec<addn::DN>& diff_state,
            const structures::robot::Robot* const robot,
            Transform3<addn::DN>& base_tf,
            structures::scenegraph::Graph* const sg,
            const addn::DN* const cont_vals,
            const addn::DN* const joint_vals,
            pred::LuaEnv<double>& grad_env,
            const spec::Formula& formula)
    : diff_state(diff_state)
    , robot(robot)
    , base_tf(base_tf)
    , sg(sg)
    , cont_vals(cont_vals)
    , joint_vals(joint_vals)
    , grad_env(grad_env)
    , formula(formula) {}

    Vec<addn::DN>& diff_state;
    const structures::robot::Robot* const robot;
    Transform3<addn::DN>& base_tf;
    structures::scenegraph::Graph* const sg;
    const addn::DN* const cont_vals;
    const addn::DN* const joint_vals;
    pred::LuaEnv<double>& grad_env;
    const spec::Formula& formula;
  };
}  // namespace

double grad_fn(const Vec<double>& inp_val, Vec<double>& grad_out, void* f_data) {
  auto* data = static_cast<OptData*>(f_data);
  auto& [diff_state, robot, base_tf, sg, cont_vals, joint_vals, grad_env, formula] = *data;
  // Convert to DNs
  for (size_t i = 0; i < diff_state.size(); ++i) {
    diff_state[i] = inp_val[i];
  }

  if (robot->base_movable) {
    pred::make_base_tf(diff_state, base_tf);
  }

  addn::DN diff_result;
  // Update the pose cache
  if (!grad_out.empty()) {
    sg->update_transforms(cont_vals,
                          joint_vals,
                          base_tf,
                          [](const structures::scenegraph::Node* const node,
                             const bool new_value,
                             const Transform3<addn::DN>& pose,
                             const Transform3r& coll_pose) {});
    // Compute each component of the gradient
    for (size_t i = 0; i < diff_state.size(); ++i) {
      diff_state[i].a = 1.0;
      // Rebuild the base pose when we're computing those components of the gradient, then once
      // after to set it back to normal
      if (robot->base_movable && i <= 4) {
        pred::make_base_tf(diff_state, base_tf);
      }

      sg->update_transforms(
      cont_vals,
      joint_vals,
      base_tf,
      [&](const structures::scenegraph::Node* const node,
          const bool new_value,
          const Transform3<addn::DN>& pose,
          const Transform3r& coll_pose) { data->grad_env.update_object(node->name, pose); },
      false);

      diff_result     = grad_env.call_formula<addn::DN>(formula);
      grad_out[i]     = diff_result.a;
      diff_state[i].a = 0.0;
    }
  } else {
    sg->update_transforms(cont_vals,
                          joint_vals,
                          base_tf,
                          [&](const structures::scenegraph::Node* const node,
                              const bool new_value,
                              const Transform3<addn::DN>& pose,
                              const Transform3r& coll_pose) {
                            data->grad_env.update_object(node->name, pose);
                          });
    diff_result = grad_env.call_formula<addn::DN>(formula);
  }

  return diff_result.v;
}

bool gradient_solve(const ob::StateSpace* const space,
                    pred::LuaEnv<double>& grad_env,
                    const structures::robot::Robot* const robot,
                    const cspace::CompositeSpace::StateType* start,
                    const spec::Formula& formula,
                    const Map<Str, Str>& bindings,
                    ob::State* result,
                    double& last_value) {
  // Separate out the state spaces for dimension and index information
  auto full_space  = space->as<ob::CompoundStateSpace>();
  auto robot_space = full_space->getSubspace(cspace::ROBOT_SPACE)->as<ob::CompoundStateSpace>();

  auto& sg = start->sg;

  // Make a double vector of the continuous parts of the state.
  // It will have the form [robot base pose (if it exists), continuous joints, other joints]
  auto state = pred::generate_state_vector(space, start, robot->base_movable);

  // Make the space bounds (only happens once)
  make_bounds(cspace::num_dims, robot->base_movable, robot->base_pose.get());

  Vec<addn::DN> diff_state(state.size(), addn::DN());
  const addn::DN* const cont_vals  = &(*diff_state.begin()) + (robot->base_movable ? 4 : 0);
  const addn::DN* const joint_vals = cont_vals + cspace::cont_joint_idxs.size();

  Transform3<addn::DN> base_tf(*(robot->base_pose));
  nlopt::opt opt_problem(nlopt::AUGLAG, state.size());
  OptData opt_data(diff_state, robot, base_tf, sg, cont_vals, joint_vals, grad_env, formula);
  opt_problem.set_min_objective(grad_fn, &opt_data);
  opt_problem.set_lower_bounds(*lower_bounds);
  opt_problem.set_upper_bounds(*upper_bounds);
  opt_problem.set_xtol_rel(1e-6);
  opt_problem.set_xtol_abs(1e-6);
  nlopt::opt local_opt(nlopt::LD_LBFGS, state.size());
  local_opt.set_lower_bounds(*lower_bounds);
  local_opt.set_upper_bounds(*upper_bounds);
  local_opt.set_xtol_rel(1e-6);
  local_opt.set_xtol_abs(1e-6);
  opt_problem.set_local_optimizer(local_opt);

  // Set up Lua objects
  grad_env.setup_world<addn::DN>(formula, bindings, sg);

  // Run gradient descent
  nlopt::result opt_result = nlopt::FAILURE;
  try {
    opt_result = opt_problem.optimize(state, last_value);
  } catch (const std::runtime_error& e) {
    // dbg(e.what());
    // dbg(opt_problem.get_errmsg());
  }

  // Tear down Lua objects
  grad_env.teardown_world(formula);

  // If we succeeded, copy the result into the output
  if (opt_result > 0) {
    // NOTE: We assume that `result` has been initialized to be the same space as `start` by the
    // caller
    // This copies the discrete part of the state (and the rest, but we overwrite that), which
    // hasn't changed
    // NOTE: This does *not* update the object poses (for manipulated objects)
    full_space->copyState(result, start);
    auto robot_state = result->as<ob::CompoundState>()->as<ob::CompoundState>(
    full_space->getSubspaceIndex(cspace::ROBOT_SPACE));
    int offset = 0;
    if (robot->base_movable) {
      auto robot_base_state = robot_state->as<cspace::RobotBaseSpace::StateType>(
      robot_space->getSubspaceIndex(cspace::BASE_SPACE));
      robot_base_state->setXYZ(state[0], state[1], state[2]);

      ob::SO2StateSpace::StateType& base_rotation = robot_base_state->rotation();
      base_rotation.value                         = state[3];
      offset += 4;
    }

    for (size_t i = 0; i < cspace::cont_joint_idxs.size(); ++i) {
      const auto idx          = cspace::cont_joint_idxs[i];
      auto* cont_joint_state  = robot_state->as<ob::SO2StateSpace::StateType>(idx);
      cont_joint_state->value = state[offset + i];
    }

    offset += cspace::cont_joint_idxs.size();

    auto* joint_state = robot_state->as<cspace::RobotJointSpace::StateType>(
    robot_space->getSubspaceIndex(cspace::JOINT_SPACE));
    for (size_t i = 0; i < cspace::joint_bounds.size(); ++i) {
      joint_state->values[i] = state[offset + i];
    }
  }

  return opt_result > 0;
}
}  // namespace planner::solver


//==============================================================================
//         Copyright 2009 - 2013 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
//==============================================================================

#include <nt2/table.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/adds.hpp>
#include <nt2/include/functions/minus.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/selinc.hpp>
#include <nt2/include/functions/seldec.hpp>
#include <nt2/include/functions/if_zero_else_one.hpp>
#include <nt2/include/functions/is_less.hpp>
#include <nt2/include/functions/is_greater.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <boost/fusion/include/at.hpp>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/fps.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;
template<class Pixel>
BOOST_FORCEINLINE void do_work(Pixel &bkg, const Pixel &fr, Pixel &var, Pixel &diff_img, Pixel &mul_img, Pixel &out)
{
  typename Pixel::value_type zero = 0;

  bkg = nt2::selinc( bkg < fr, nt2::seldec( bkg > fr
                                          , bkg
                                          )
                   );
  diff_img = nt2::max(bkg, fr) - nt2::min(bkg, fr);

  mul_img = nt2::adds(nt2::adds(diff_img,diff_img),diff_img);

  var = nt2::if_else( diff_img != zero, nt2::selinc( var < mul_img
                                                   , nt2::seldec( var > mul_img
                                                                , var
                                                                )
                                                   )
                    , var
                    );
  out = nt2::if_zero_else_one( diff_img < var );
}

template<typename T> struct sigmadelta_nt2
{
  template<typename Setup>
  sigmadelta_nt2(Setup const& s)
                    :  height(boost::fusion::at_c<1>(s))
                    ,  width(boost::fusion::at_c<2>(s))
                    ,  size_(height*width)
                    ,  nb_frames(boost::fusion::at_c<0>(s))
  {
    frames.resize(nb_frames);
    image_variance.resize(nt2::of_size(height,width));
    diff_img.resize(nt2::of_size(height,width));
    mul_img.resize(nt2::of_size(height,width));
    background_img.resize(nt2::of_size(height,width));
    binary_label.resize(nt2::of_size(height,width));

    for(std::size_t k=0; k<nb_frames; k++)
      frames[k].resize(nt2::of_size(height,width));

    for(std::size_t k=0; k<nb_frames; k++)
    {
      for(std::size_t j=1; j<=width; j++)
      {
        for(std::size_t i=1; i<=height;i++)
        {
          if(i-1>(height/4) && i-1<(height/2) && j-1>((width/4)+k%10) && j-1<((width/2)+k%10))
            frames[k](i,j) = 255;
          else
            frames[k](i,j) = 0;
        }
      }
    }
    image_variance = ones(nt2::of_size(height,width),nt2::meta::as_<T>());
    for (std::size_t i=1;i<=height;i++)
      for (std::size_t j=1;j<=width;j++)
        background_img(i,j) = frames[0](i,j);

    binary_label = zeros(nt2::of_size(height,width),nt2::meta::as_<T>());
  }

  void operator()()
  {
    for(std::size_t k=1; k<nb_frames; k++)
    {
      do_work(background_img, frames[k], image_variance, diff_img, mul_img, binary_label);
    }
  }

  friend std::ostream& operator<<(std::ostream& os, sigmadelta_nt2<T> const& p)
  {
    return os << "(" << p.height << " x " << p.width << " @" << p.nb_frames << ")";
  }

  std::size_t size() const { return size_ * nb_frames; }
  std::size_t frame_count()  const { return nb_frames; }

private:
  std::size_t height;
  std::size_t width;
  std::size_t size_;
  std::vector<table<T> > frames;
  nt2::table<T> image_variance, background_img, binary_label, mul_img, diff_img;
  std::size_t nb_frames;
};

NT2_REGISTER_BENCHMARK( sigmadelta_nt2 )
{
  std::size_t frame = args("frame", 5);
  std::size_t hmin = args("hmin", 32);
  std::size_t hmax = args("hmax", 128);
  std::size_t hstep = args("hstep", 2);
  std::size_t wmin = args("wmin", 32);
  std::size_t wmax = args("wmax", 128);
  std::size_t wstep = args("wstep", 2);

  run_during_with< sigmadelta_nt2<nt2::uint8_t> > ( 1.
                                                  , and_( constant( frame )
                                                        , geometric(hmin,hmax,hstep)
                                                        , geometric(wmin,wmax,wstep)
                                                        )
                                                  , cycles_per_element<stats::median_>()
                                                  , fps<stats::median_>()
                                                  );
}


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman

    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 <boost/detail/lightweight_test.hpp>
#include <boost/config/warning_disable.hpp>

#include <input/sexpr.hpp>
#include <input/parse_sexpr_impl.hpp>
#include <scheme/compiler.hpp>

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int main()
{
    using scheme::interpreter;
    using scheme::utree;

    {
        utree src = "(define n 123)";
        scheme::interpreter program(src);
        BOOST_TEST(program["n"]() == 123);
    }

    {
        utree src = "(define (factorial n) (if (<= n 0) 1 (* n (factorial (- n 1)))))";
        scheme::interpreter program(src);
        BOOST_TEST(program["factorial"](10) == 3628800);
    }

    {
        // test forward declaration (a scheme extension)
        utree src =
            "(define (dbl n))" // multiple forward declarations allowed
            "(define (dbl n))"
            "(define foo (dbl 10))"
            "(define (dbl n) (* n 2))"
            ;
        scheme::interpreter program(src);
        BOOST_TEST(program["foo"](10) == 20);
    }

    return boost::report_errors();
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#include <nt2/hyperbolic/include/functions/acosh.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

NT2_TEST_CASE_TPL ( acosh,  NT2_SIMD_REAL_TYPES)
{
  using nt2::acosh;
  using nt2::tag::acosh_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::call<acosh_(vT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(acosh(nt2::Inf<vT>()), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(acosh(nt2::Minf<vT>()), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(acosh(nt2::Nan<vT>()), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(acosh(nt2::Zero<vT>()), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(acosh(nt2::One<vT>()), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(acosh(nt2::Valmax<vT>()), nt2::splat<vT>(nt2::acosh(nt2::Valmax<T>())), 0.5);
}