/*
 *  Copyright Nick Thompson, 2017
 *  Use, modification and distribution are subject to the
 *  Boost Software License, Version 1.0. (See accompanying file
 *  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 *  Given N samples (t_i, y_i) which are irregularly spaced, this routine constructs an
 *  interpolant s which is constructed in O(N) time, occupies O(N) space, and can be evaluated in O(N) time.
 *  The interpolation is stable, unless one point is incredibly close to another, and the next point is incredibly far.
 *  The measure of this stability is the "local mesh ratio", which can be queried from the routine.
 *  Pictorially, the following t_i spacing is bad (has a high local mesh ratio)
 *  ||             |      | |                           |
 *  and this t_i spacing is good (has a low local mesh ratio)
 *  |   |      |    |     |    |        |    |  |    |
 *
 *
 *  If f is C^{d+2}, then the interpolant is O(h^(d+1)) accurate, where d is the interpolation order.
 *  A disadvantage of this interpolant is that it does not reproduce rational functions; for example, 1/(1+x^2) is not interpolated exactly.
 *
 *  References:
 *  Floater, Michael S., and Kai Hormann. "Barycentric rational interpolation with no poles and high rates of approximation."
*      Numerische Mathematik 107.2 (2007): 315-331.
 *  Press, William H., et al. "Numerical recipes third edition: the art of scientific computing." Cambridge University Press 32 (2007): 10013-2473.
 */

#ifndef BOOST_MATH_INTERPOLATORS_BARYCENTRIC_RATIONAL_HPP
#define BOOST_MATH_INTERPOLATORS_BARYCENTRIC_RATIONAL_HPP

#include <memory>
#include <boost/math/interpolators/detail/barycentric_rational_detail.hpp>

namespace boost{ namespace math{

template<class Real>
class barycentric_rational
{
public:
    barycentric_rational(const Real* const x, const Real* const y, size_t n, size_t approximation_order = 3);

    barycentric_rational(std::vector<Real>&& x, std::vector<Real>&& y, size_t approximation_order = 3);

    template <class InputIterator1, class InputIterator2>
    barycentric_rational(InputIterator1 start_x, InputIterator1 end_x, InputIterator2 start_y, size_t approximation_order = 3, typename boost::disable_if_c<boost::is_integral<InputIterator2>::value>::type* = 0);

    Real operator()(Real x) const;

    Real prime(Real x) const;

    std::vector<Real>&& return_x()
    {
        return m_imp->return_x();
    }

    std::vector<Real>&& return_y()
    {
        return m_imp->return_y();
    }

private:
    std::shared_ptr<detail::barycentric_rational_imp<Real>> m_imp;
};

template <class Real>
barycentric_rational<Real>::barycentric_rational(const Real* const x, const Real* const y, size_t n, size_t approximation_order):
 m_imp(std::make_shared<detail::barycentric_rational_imp<Real>>(x, x + n, y, approximation_order))
{
    return;
}

template <class Real>
barycentric_rational<Real>::barycentric_rational(std::vector<Real>&& x, std::vector<Real>&& y, size_t approximation_order):
 m_imp(std::make_shared<detail::barycentric_rational_imp<Real>>(std::move(x), std::move(y), approximation_order))
{
    return;
}

template <class Real>
template <class InputIterator1, class InputIterator2>
barycentric_rational<Real>::barycentric_rational(InputIterator1 start_x, InputIterator1 end_x, InputIterator2 start_y, size_t approximation_order, typename boost::disable_if_c<boost::is_integral<InputIterator2>::value>::type*)
 : m_imp(std::make_shared<detail::barycentric_rational_imp<Real>>(start_x, end_x, start_y, approximation_order))
{
}

template<class Real>
Real barycentric_rational<Real>::operator()(Real x) const
{
    return m_imp->operator()(x);
}

template<class Real>
Real barycentric_rational<Real>::prime(Real x) const
{
    return m_imp->prime(x);
}

}}
#endif


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

#include <boost/math/concepts/real_concept.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class Real, class T>
void do_test_sinc(const T& data, const char* type_name, const char* test_name)
{
#if !(defined(ERROR_REPORTING_MODE) && !defined(DIGAMMA_FUNCTION_TO_TEST))
   typedef Real                   value_type;

   typedef value_type (*pg)(value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::sinc_pi<value_type>;
#else
   pg funcp = boost::math::sinc_pi;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test sinc against data:
   //
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(funcp, 0),
      extract_result<Real>(1));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "sinc_pi", test_name);
   std::cout << std::endl;
#endif
}

template <class T>
void test_sinc(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // three items, input value a, input value b and erf(a, b):
   //
#  include "sinc_data.ipp"

   do_test_sinc<T>(sinc_data, name, "Sinc Function");
}


#pragma once
/*
 * MeanShift implementation created by Matt Nedrich.
 * project: https://github.com/mattnedrich/MeanShift_cpp
 *
 */
#include <vector>
#include <bitset>
#include <Eigen/Geometry>

using namespace std;

class MeanShift {
public:
    MeanShift() { set_kernel(NULL); }
    MeanShift(double (*_kernel_func)(double,double)) { set_kernel(kernel_func); }
    vector<Eigen::Vector3d> cluster(vector<Eigen::Vector3d>, double);
private:
    double (*kernel_func)(double,double);
    void set_kernel(double (*_kernel_func)(double,double));
    Eigen::Vector3d shift_point(const Eigen::Vector3d &, const vector<Eigen::Vector3d> &, double);
};


#include <df/optimization/icp.h>

#include <df/camera/poly3.h> // TODO

#include <df/optimization/linearSystems.h>

#include <df/util/cudaHelpers.h>

#include <assert.h>

#include <Eigen/Cholesky>

namespace df {

template <typename Scalar,
          typename CameraModelT,
          int DPred,
          typename ... DebugArgsT>
Sophus::SE3<Scalar> icp(const DeviceTensor2<Eigen::UnalignedVec3<Scalar> > & liveVertices,
                             const DeviceTensor2<Eigen::UnalignedVec<Scalar,DPred> > & predVertices,
                             const DeviceTensor2<Eigen::UnalignedVec<Scalar,DPred> > & predNormals,
                             const CameraModelT & cameraModel,
                             const Sophus::SE3<Scalar> & predictionPose,
                             const Eigen::Matrix<Scalar,2,1> & depthRange,
                             const Scalar maxError,
                             const uint numIterations,
                             DebugArgsT ... debugArgs) {

    typedef Sophus::SE3<Scalar> SE3;

    const uint width = liveVertices.dimensionSize(0);
    const uint height = liveVertices.dimensionSize(1);

    assert(predVertices.dimensionSize(0) == width);
    assert(predVertices.dimensionSize(1) == height);
    assert(predNormals.dimensionSize(0) == width);
    assert(predNormals.dimensionSize(1) == height);

    const dim3 grid(128,8,1);
    const dim3 block(intDivideAndCeil(width,grid.x),intDivideAndCeil(height,grid.y));

    Eigen::Matrix<Scalar,6,1> initialPose = predictionPose.log();
    Sophus::SE3<Scalar> accumulatedUpdate;

    for (uint iter = 0; iter < numIterations; ++iter) {

        // std::cout << iter << std::endl;

        internal::LinearSystem<Scalar,6> system = internal::icpIteration(liveVertices,
                                                                         predVertices,
                                                                         predNormals,
                                                                         cameraModel,
                                                                         accumulatedUpdate,
                                                                         initialPose,
                                                                         depthRange,maxError,
                                                                         grid,block,
                                                                         debugArgs ...);

        Eigen::Matrix<Scalar,6,6,Eigen::DontAlign> fullJTJ = internal::SquareMatrixReconstructor<Scalar,6>::reconstruct(system.JTJ);

//        Eigen::Matrix<Scalar,6,6> fullJTJ = Eigen::Matrix<Scalar,6,6>::Zero();
//        fullJTJ.template block<1,6>(0,0) = system.JTJ.head;
//        fullJTJ.template block<1,5>(1,1) = system.JTJ.tail.head;
//        fullJTJ.template block<1,4>(2,2) = system.JTJ.tail.tail.head;
//        fullJTJ.template block<1,3>(3,3) = system.JTJ.tail.tail.tail.head;
//        fullJTJ.template block<1,2>(4,4) = system.JTJ.tail.tail.tail.tail.head;
//        fullJTJ.template block<1,1>(5,5) = system.JTJ.tail.tail.tail.tail.tail.head;

//        for (int i=0; i<6; ++i) {
//            for (int j=0; j<6; ++j) {
//                fullJTJ(j,i) = fullJTJ(i,j);
//            }
//        }

//        for (int i = 0; i < 6; ++i) {
//            system.JTr(i) = 0;
//        }

//        std::cout << fullJTJ << std::endl;

//        std::cout << std::endl << system.JTr << std::endl << std::endl << std::endl;

//        Eigen::Matrix<Scalar,6,6> sav = fullJTJ.template selfadjointView<Eigen::Upper>();
//        std::cout << sav << std::endl;
//        Eigen::Matrix<Scalar,6,1> solution = sav.ldlt().solve(system.JTr);
//        std::cout << solution << std::endl;

        Eigen::Matrix<Scalar,6,1> solution = fullJTJ.template selfadjointView<Eigen::Upper>().ldlt().solve(system.JTr);
//        std::cout << std::endl << solution2 << std::endl;

        // std::cout << fullJTJ << std::endl;

        // std::cout << system.JTr << std::endl;

        // std::cout << solution.transpose() << std::endl;

        SE3 update = SE3::exp(solution);
        accumulatedUpdate = update*accumulatedUpdate;

//        std::cout << accumulatedUpdate.matrix() << std::endl << std::endl;

    }

    return accumulatedUpdate;

}

template Sophus::SE3f icp(const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const Poly3CameraModel<float> &,
                          const Sophus::SE3f &,
                          const Eigen::Vector2f &,
                          const float,
                          const uint);

template Sophus::SE3f icp(const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const Poly3CameraModel<float> &,
                          const Sophus::SE3f &,
                          const Eigen::Vector2f &,
                          const float,
                          const uint,
                          DeviceTensor2<Eigen::UnalignedVec4<uchar> >);

template Sophus::SE3f icp(const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec4<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec4<float> > &,
                          const Poly3CameraModel<float> &,
                          const Sophus::SE3f &,
                          const Eigen::Vector2f &,
                          const float,
                          const uint);

template Sophus::SE3f icp(const DeviceTensor2<Eigen::UnalignedVec3<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec4<float> > &,
                          const DeviceTensor2<Eigen::UnalignedVec4<float> > &,
                          const Poly3CameraModel<float> &,
                          const Sophus::SE3f &,
                          const Eigen::Vector2f &,
                          const float,
                          const uint,
                          DeviceTensor2<Eigen::UnalignedVec4<uchar> >);

} // namespace df


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015-2016.
// Modifications copyright (c) 2015-2016, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_BOX_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/within.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

namespace boost { namespace geometry { namespace strategy
{

namespace within
{

template <typename Geometry, std::size_t Dimension, typename CSTag>
struct within_range
{
    template <typename Value1, typename Value2>
    static inline bool apply(Value1 const& value, Value2 const& min_value, Value2 const& max_value)
    {
        return value > min_value && value < max_value;
    }
};

template <typename Geometry, std::size_t Dimension, typename CSTag>
struct covered_by_range
{
    template <typename Value1, typename Value2>
    static inline bool apply(Value1 const& value, Value2 const& min_value, Value2 const& max_value)
    {
        return value >= min_value && value <= max_value;
    }
};

// NOTE: the result would be the same if instead of structs defined below
// the above xxx_range were used with the following arguments:
// (min_value + diff_min, min_value, max_value)
struct within_longitude_range
{
    template <typename CalcT>
    static inline bool apply(CalcT const& diff_min, CalcT const& min_value, CalcT const& max_value)
    {
        CalcT const c0 = 0;
        return diff_min > c0 && min_value + diff_min < max_value;
    }
};

struct covered_by_longitude_range
{
    template <typename CalcT>
    static inline bool apply(CalcT const& diff_min, CalcT const& min_value, CalcT const& max_value)
    {
        return min_value + diff_min <= max_value;
    }
};

template <typename Geometry,
          typename ResultCheck>
struct longitude_range
{
    template <typename Value1, typename Value2>
    static inline bool apply(Value1 const& value, Value2 const& min_value, Value2 const& max_value)
    {
        typedef typename select_most_precise
            <
                Value1, Value2
            >::type calc_t;
        typedef typename coordinate_system<Geometry>::type::units units_t;
        typedef math::detail::constants_on_spheroid<calc_t, units_t> constants;

        // min <= max <=> diff >= 0
        calc_t const diff_ing = max_value - min_value;

        // if containing covers the whole globe it contains all
        if (diff_ing >= constants::period())
        {
            return true;
        }

        // calculate positive longitude translation with min_value as origin
        calc_t const diff_min = math::longitude_distance_unsigned<units_t, calc_t>(min_value, value);

        return ResultCheck::template apply<calc_t>(diff_min, min_value, max_value);
    }
};

// spherical_equatorial_tag, spherical_polar_tag and geographic_cat are casted to spherical_tag
template <typename Geometry>
struct within_range<Geometry, 0, spherical_tag>
    : longitude_range<Geometry, within_longitude_range>
{};

template <typename Geometry>
struct covered_by_range<Geometry, 0, spherical_tag>
    : longitude_range<Geometry, covered_by_longitude_range>
{};

template
<
    template <typename, std::size_t, typename> class SubStrategy,
    typename Point,
    typename Box,
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct relate_point_box_loop
{
    static inline bool apply(Point const& point, Box const& box)
    {
        typedef typename tag_cast<typename cs_tag<Point>::type, spherical_tag>::type cs_tag_t;

        if (! SubStrategy<Point, Dimension, cs_tag_t>::apply(get<Dimension>(point),
                    get<min_corner, Dimension>(box),
                    get<max_corner, Dimension>(box))
            )
        {
            return false;
        }

        return relate_point_box_loop
            <
                SubStrategy,
                Point, Box,
                Dimension + 1, DimensionCount
            >::apply(point, box);
    }
};

template
<
    template <typename, std::size_t, typename> class SubStrategy,
    typename Point,
    typename Box,
    std::size_t DimensionCount
>
struct relate_point_box_loop<SubStrategy, Point, Box, DimensionCount, DimensionCount>
{
    static inline bool apply(Point const& , Box const& )
    {
        return true;
    }
};

template
<
    typename Point,
    typename Box,
    template <typename, std::size_t, typename> class SubStrategy = within_range
>
struct point_in_box
{
    static inline bool apply(Point const& point, Box const& box)
    {
        return relate_point_box_loop
            <
                SubStrategy,
                Point, Box,
                0, dimension<Point>::type::value
            >::apply(point, box);
    }
};

} // namespace within

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace within { namespace services
{

template <typename Point, typename Box>
struct default_strategy
    <
        point_tag, box_tag,
        point_tag, areal_tag,
        cartesian_tag, cartesian_tag,
        Point, Box
    >
{
    typedef within::point_in_box<Point, Box> type;
};

// spherical_equatorial_tag, spherical_polar_tag and geographic_cat are casted to spherical_tag
template <typename Point, typename Box>
struct default_strategy
    <
        point_tag, box_tag,
        point_tag, areal_tag,
        spherical_tag, spherical_tag,
        Point, Box
    >
{
    typedef within::point_in_box<Point, Box> type;
};

}} // namespace within::services

namespace covered_by { namespace services
{

template <typename Point, typename Box>
struct default_strategy
    <
        point_tag, box_tag,
        point_tag, areal_tag,
        cartesian_tag, cartesian_tag,
        Point, Box
    >
{
    typedef within::point_in_box
                <
                    Point, Box,
                    within::covered_by_range
                > type;
};

// spherical_equatorial_tag, spherical_polar_tag and geographic_cat are casted to spherical_tag
template <typename Point, typename Box>
struct default_strategy
    <
        point_tag, box_tag,
        point_tag, areal_tag,
        spherical_tag, spherical_tag,
        Point, Box
    >
{
    typedef within::point_in_box
                <
                    Point, Box,
                    within::covered_by_range
                > type;
};

}} // namespace covered_by::services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_BOX_HPP


#include <kdop.h>
#include <mesh_array.h>
#include <geometry.h>
#include <tiny.h>

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

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

class GeometryInfo
{
public:

  kdop::Tree<T,6>                                   m_tree;
  mesh_array::T4Mesh                                m_mesh;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> m_X;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> m_Y;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> m_Z;

  mesh_array::TetrahedronAttribute<mesh_array::TetrahedronSurfaceInfo,mesh_array::T4Mesh> m_surface_map;

};

void make_geometry( GeometryInfo & info )
{
  mesh_array::T3Mesh surface;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  mesh_array::make_box<MT>( 2.0f, 2.0f, 2.0f, surface, sX, sY, sZ);

  mesh_array::T4Mesh mesh_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z_in;

  mesh_array::tetgen(surface, sX, sY, sZ, mesh_in, X_in, Y_in, Z_in);

  kdop::mesh_reorder( mesh_in, X_in, Y_in, Z_in, info.m_mesh, info.m_X, info.m_Y, info.m_Z );

  mesh_array::compute_surface_map( info.m_mesh, info.m_X, info.m_Y, info.m_Z, info.m_surface_map );

  info.m_tree = kdop::make_tree<V,6,T>( 32000, info.m_mesh, info.m_X, info.m_Y, info.m_Z );

  BOOST_CHECK( info.m_surface_map.size()>0u );

}

bool compute_raycast(geometry::Ray<V> const & ray, GeometryInfo const & object, V & hit_point, T & length)
{
  return kdop::raycast(
                       ray
                       , object.m_tree
                       , object.m_mesh
                       , object.m_X
                       , object.m_Y
                       , object.m_Z
                       , object.m_surface_map
                       , hit_point
                       , length
                       );
}

BOOST_AUTO_TEST_SUITE(kdop);

BOOST_AUTO_TEST_CASE(raycast_test)
{

  GeometryInfo object;

  make_geometry(object);

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( hit );

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

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( hit );

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

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( hit );

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

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( !hit );

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

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( !hit );

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

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( hit );

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

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( hit );

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

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

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

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

    bool const hit = compute_raycast(ray, object, q, length);

    BOOST_CHECK( hit );

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

}

BOOST_AUTO_TEST_SUITE_END();


#ifndef __solver_Random_h__
#define __solver_Random_h__

#include <Eigen/Dense>
#include <vector>
#include <iostream>

namespace hcod{
    class RandStackWithWeight{
        public:
            RandStackWithWeight(const unsigned int & nh, const unsigned int &p, const Eigen::VectorXi & m, const Eigen::VectorXi & r, const bool & eq_only = true);
            ~RandStackWithWeight(){};

        private:
            void compute_random();
            Eigen::MatrixXd mrand(int n, int m, Eigen::Vector2d sbound);

        public:
            std::vector<Eigen::MatrixXd> getA(){
                return A_;
            };
            std::vector<Eigen::MatrixXd> getb(){
                return b_;
            };
            std::vector<Eigen::MatrixXd> getW(){
                return W_;
            };
            std::vector<Eigen::VectorXi> getbtype(){
                return btype_;
            };
            Eigen::MatrixXd getAu(){
                return Au_;
            };
            Eigen::MatrixXd getbu(){
                return bu_;
            };

        private:
            Eigen::Vector2d svbound_;
            std::vector<Eigen::MatrixXd> A_;
            std::vector<Eigen::MatrixXd> b_;
            std::vector<Eigen::MatrixXd> W_;
            std::vector<Eigen::VectorXi> btype_;
            Eigen::MatrixXd Au_;
            Eigen::MatrixXd bu_;

            unsigned int nh_;
            unsigned int p_;
            Eigen::VectorXi m_;
            Eigen::VectorXi r_;
            bool eq_only_;
    };
}

#endif


#include "binary_search_tree.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <vector>

// test data version: 1.0.0

template<typename T>
using tree_ptr = typename std::unique_ptr<binary_tree::binary_tree<T>>;

template<typename T>
static void test_leaf(const tree_ptr<T> &tree,
                      const T& data, bool has_left, bool has_right)
{
    BOOST_REQUIRE_EQUAL(data, tree->data());
    BOOST_REQUIRE_EQUAL((bool) tree->left(), has_left);
    BOOST_REQUIRE_EQUAL((bool) tree->right(), has_right);
}

template<typename T>
static tree_ptr<T> make_tree(const std::vector<T> &data)
{
    if (data.empty())
        return tree_ptr<T>(nullptr);

    auto data_iter = data.begin();
    auto tree = tree_ptr<T>(new binary_tree::binary_tree<T>(*data_iter));
    ++data_iter;

    for (; data_iter != data.end(); ++data_iter)
    {
        tree->insert(*data_iter);
    }

    return tree;
}

BOOST_AUTO_TEST_CASE(data_is_retained)
{
    auto tested = make_tree<uint32_t>({4});
    test_leaf<uint32_t>(tested, 4, false, false);
}

#if defined(EXERCISM_RUN_ALL_TESTS)

BOOST_AUTO_TEST_CASE(smaller_number_at_left_node)
{
    auto tested = make_tree<uint32_t>({4, 2});

    test_leaf<uint32_t>(tested, 4, true, false);
    test_leaf<uint32_t>(tested->left(), 2, false, false);
}

BOOST_AUTO_TEST_CASE(same_number_at_left_node)
{
    auto tested = make_tree<uint32_t>({4, 4});

    test_leaf<uint32_t>(tested, 4, true, false);
    test_leaf<uint32_t>(tested->left(), 4, false, false);
}

BOOST_AUTO_TEST_CASE(greater_number_at_right_node)
{
    auto tested = make_tree<uint32_t>({4, 5});

    test_leaf<uint32_t>(tested, 4, false, true);
    test_leaf<uint32_t>(tested->right(), 5, false, false);
}

BOOST_AUTO_TEST_CASE(can_create_complex_tree)
{
    auto tested = make_tree<uint32_t>({4, 2, 6, 1, 3, 5, 7});

    test_leaf<uint32_t>(tested, 4, true, true);
    test_leaf<uint32_t>(tested->left(), 2, true, true);
    test_leaf<uint32_t>(tested->right(), 6, true, true);

    test_leaf<uint32_t>(tested->left()->left(), 1, false, false);
    test_leaf<uint32_t>(tested->left()->right(), 3, false, false);

    test_leaf<uint32_t>(tested->right()->left(), 5, false, false);
    test_leaf<uint32_t>(tested->right()->right(), 7, false, false);
}

// The tests below require an implementation of an iterator.
// You can get more details here: http://www.cplusplus.com/reference/iterator/

template<typename T>
static void test_sort(const tree_ptr<T> &tree, const std::vector<T> &expected)
{
    BOOST_REQUIRE_EQUAL_COLLECTIONS(tree->begin(), tree->end(), expected.begin(), expected.end());
}

BOOST_AUTO_TEST_CASE(can_sort_single_number)
{
    test_sort(make_tree<uint32_t>({2}), {2});
}

BOOST_AUTO_TEST_CASE(can_sort_if_second_number_is_smaller_than_first)
{
    test_sort(make_tree<uint32_t>({2, 1}), {1, 2});
}

BOOST_AUTO_TEST_CASE(can_sort_if_second_number_is_same_as_first)
{
    test_sort(make_tree<uint32_t>({2, 2}), {2, 2});
}

BOOST_AUTO_TEST_CASE(can_sort_if_second_number_is_greater_than_first)
{
    test_sort(make_tree<uint32_t>({2, 3}), {2, 3});
}

BOOST_AUTO_TEST_CASE(can_sort_complex_tree)
{
    test_sort(make_tree<uint32_t>({2, 1, 3, 6, 7, 5}), {1, 2, 3, 5, 6, 7});
}

// strings

BOOST_AUTO_TEST_CASE(can_create_complex_tree_strings)
{
    auto tested = make_tree<std::string>({"delicious", "ballon", "flag", "apple", "cat", "elispsis", "grains"});

    test_leaf<std::string>(tested, "delicious", true, true);
    test_leaf<std::string>(tested->left(), "ballon", true, true);
    test_leaf<std::string>(tested->right(), "flag", true, true);

    test_leaf<std::string>(tested->left()->left(), "apple", false, false);
    test_leaf<std::string>(tested->left()->right(), "cat", false, false);

    test_leaf<std::string>(tested->right()->left(), "elispsis", false, false);
    test_leaf<std::string>(tested->right()->right(), "grains", false, false);
}

BOOST_AUTO_TEST_CASE(can_sort_complex_tree_strings)
{
    test_sort(make_tree<std::string>({"A", "few", "random", "strings", "that", "should", "be", "sorted"}), {"A", "be", "few", "random", "should", "sorted", "strings", "that"});
}

#endif


// Copyright Aleksey Gurtovoy 2001-2006
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/next_prior.hpp>
#include <boost/mpl/aux_/test.hpp>
#include <boost/mpl/aux_/config/workaround.hpp>

#include <cassert>

#if !BOOST_WORKAROUND(__BORLANDC__, < 0x600)
#   define INTEGRAL_WRAPPER_RUNTIME_TEST(i, T) \
    BOOST_TEST(( WRAPPER(T,i)() == i )); \
    BOOST_TEST(( WRAPPER(T,i)::value == i )); \
    /**/
#else
#   define INTEGRAL_WRAPPER_RUNTIME_TEST(i, T) \
    BOOST_TEST(( WRAPPER(T,i)::value == i )); \
    /**/
#endif

#if defined(BOOST_NO_CXX11_CONSTEXPR)
#define CONSTEXPR_INTEGRAL_TEST(T, i)
#else
#define CONSTEXPR_INTEGRAL_TEST(T, i) { static_assert(T() == i, "Constexpr for integral constant failed"); }
#endif

#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
// agurt 20/nov/06: see http://article.gmane.org/gmane.comp.lib.boost.devel/151065
#define INTEGRAL_WRAPPER_TEST(unused1, i, T) \
    { \
      typedef WRAPPER(T,i) borland_tested_type; \
    { typedef is_same< borland_tested_type::value_type, T > borland_test_type; \
      MPL_ASSERT(( borland_test_type )); } \
    { MPL_ASSERT(( is_same< borland_tested_type::type, WRAPPER(T,i) > )); } \
    { MPL_ASSERT(( is_same< next< borland_tested_type >::type, WRAPPER(T,i+1) > )); } \
    { MPL_ASSERT(( is_same< prior< borland_tested_type >::type, WRAPPER(T,i-1) > )); } \
    { MPL_ASSERT_RELATION( (borland_tested_type::value), ==, i ); } \
    CONSTEXPR_INTEGRAL_TEST(borland_tested_type, i) \
    INTEGRAL_WRAPPER_RUNTIME_TEST(i, T) \
    } \
/**/
#else
#define INTEGRAL_WRAPPER_TEST(unused1, i, T) \
    { MPL_ASSERT(( is_same< WRAPPER(T,i)::value_type, T > )); } \
    { MPL_ASSERT(( is_same< WRAPPER(T,i)::type, WRAPPER(T,i) > )); } \
    { MPL_ASSERT(( is_same< next< WRAPPER(T,i) >::type, WRAPPER(T,i+1) > )); } \
    { MPL_ASSERT(( is_same< prior< WRAPPER(T,i) >::type, WRAPPER(T,i-1) > )); } \
    { MPL_ASSERT_RELATION( (WRAPPER(T,i)::value), ==, i ); } \
    CONSTEXPR_INTEGRAL_TEST(WRAPPER(T,i), i) \
    INTEGRAL_WRAPPER_RUNTIME_TEST(i, T) \
/**/
#endif


// Copyright (c) 1998  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Christophe Delage (Christophe.Delage@sophia.inria.fr)

#include <CGAL/Regular_triangulation_3.h>

#include <iostream>
#include <fstream>
#include <cassert>
//#include <cstdlib>
#include <set>

#include <boost/random/linear_congruential.hpp>
#include <boost/cstdint.hpp>

#include <CGAL/_test_types.h>
//#include <CGAL/_test_cls_regular_3.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

int degeneracy_counter = 0;

// This traits class counts the number of times a power_test has returned 0.
// This gives a rough idea of how degenerate a data set is.
struct traits : public K
{
    struct Power_side_of_oriented_power_sphere_3 : public K::Power_side_of_oriented_power_sphere_3
    {
      typedef K::Weighted_point_3 Weighted_point;
        typedef K::Power_side_of_oriented_power_sphere_3 P3;
        Oriented_side operator() (const Weighted_point &p0,
                                  const Weighted_point &p) const
        {
            Oriented_side result = P3::operator()(p0, p);
            if (result == 0) ++degeneracy_counter;
            return result;
        }
        Oriented_side operator() (const Weighted_point &p0,
                                  const Weighted_point &p1,
                                  const Weighted_point &p) const
        {
            Oriented_side result = P3::operator()(p0, p1, p);
            if (result == 0) ++degeneracy_counter;
            return result;
        }
        Oriented_side operator() (const Weighted_point &p0,
                                  const Weighted_point &p1,
                                  const Weighted_point &p2,
                                  const Weighted_point &p) const
        {
            Oriented_side result = P3::operator()(p0, p1, p2, p);
            if (result == 0) ++degeneracy_counter;
            return result;
        }
        Oriented_side operator() (const Weighted_point &p0,
                                  const Weighted_point &p1,
                                  const Weighted_point &p2,
                                  const Weighted_point &p3,
                                  const Weighted_point &p) const
        {
            Oriented_side result = P3::operator()(p0, p1, p2, p3, p);
            if (result == 0) ++degeneracy_counter;
            return result;
        }
    };

    Power_side_of_oriented_power_sphere_3 power_side_of_oriented_power_sphere_3_object() const
    { return Power_side_of_oriented_power_sphere_3(); }
};

// Explicit instantiation of the whole class :
template class CGAL::Regular_triangulation_3<traits>;

typedef CGAL::Regular_triangulation_3<traits>                 Cls;

typedef K::Point_3 Point;
typedef K::Weighted_point_3 Weighted_point;

typedef Cls::Vertex_handle                         Vertex_handle;

// We don't want to use compare_xyz because it thinks two weighted points
// located at the same place with different weights are identical.
struct less_xyzw
{
    bool operator() (const Weighted_point &p,
                     const Weighted_point &q) const
    {
        if (p.x() < q.x()) return true;
        if (p.x() > q.x()) return false;
        if (p.y() < q.y()) return true;
        if (p.y() > q.y()) return false;
        if (p.z() < q.z()) return true;
        if (p.z() > q.z()) return false;
        if (p.weight() < q.weight()) return true;
        return false;
    }
};

typedef std::set<Weighted_point,less_xyzw> point_set;
typedef point_set::iterator set_iterator;

// Base class for a weighted point generator.
class point_iterator
{
    Weighted_point _wp;
    int _i;
public:
    point_iterator (int i = 0) : _i (i) {}

    void operator++ () { --_i; }
    const Weighted_point &operator* () const { return _wp; }
    bool operator== (const point_iterator &i) const { return _i == i._i; }
    bool operator!= (const point_iterator &i) const { return ! (*this == i); }

protected:
    void set_point (int x, int y, int z, int w)
    {
        _wp = Weighted_point (Point (x, y, z), w);
    }
};

static boost::minstd_rand randgen;

// point_iterator_x generates points randomly on a grid (thus making lots of
// degenerate cases), staying in dimension x.
struct point_iterator_0 : public point_iterator
{
    point_iterator_0 () {}
    point_iterator_0 (int i) : point_iterator(i + 1) { ++*this; }

    void operator++ ()
    {
        int w = randgen() % 10;
        set_point (0, 0, 0, w);
        point_iterator::operator++();
    }
};

struct point_iterator_1 : public point_iterator
{
    point_iterator_1 () {}
    point_iterator_1 (int i) : point_iterator(i + 1) { ++*this; }

    void operator++ ()
    {
        int x = randgen() % 10;
        int w = randgen() % 10;
        set_point (x, 0, 0, w);
        point_iterator::operator++();
    }
};

struct point_iterator_2 : public point_iterator
{
    point_iterator_2 () {}
    point_iterator_2 (int i) : point_iterator(i + 1) { ++*this; }

    void operator++ ()
    {
        int x = randgen() % 10;
        int y = randgen() % 10;
        int w = randgen() % 10;
        set_point (x, y, 0, w);
        point_iterator::operator++();
    }
};

struct point_iterator_3 : public point_iterator
{
    point_iterator_3 () {}
    point_iterator_3 (int i) : point_iterator(i + 1) { ++*this; }

    void operator++ ()
    {
        int x = randgen() % 10;
        int y = randgen() % 10;
        int z = randgen() % 10;
        int w = randgen() % 10;
        set_point (x, y, z, w);
        point_iterator::operator++();
    }
};

class point_reader
{
    std::istream *in;
    Weighted_point wp;
    int nb;
public:
    point_reader () : in (0), wp(), nb(0) {}
    point_reader (std::istream &is) : in(&is)
    {
        if (*in >> nb) {
            ++nb;
            ++*this;
        } else
            nb = 0;
    }
    point_reader &operator++ ()
    {
        if (nb > 0) {
            --nb;
            if (nb > 0) *in >> wp;
        }
        return *this;
    }
    bool operator== (const point_reader& p) const { return nb == p.nb; }
    bool operator!= (const point_reader& p) const { return nb != p.nb; }
    const Weighted_point &operator*() const { return wp; }
};

// Inserts number points in the triangulation and the multiset, using PI as the
// point generator.
template < class PI >
void insert (Cls &T, point_set &points, int number)
{
    int i = 1;
    for (PI pi (number), pend; pi != pend; ++pi, ++i) {
        points.insert (*pi);
        T.insert (*pi);
        std::cout << "\r number of inserted points: " << i << std::flush;
    }
    std::cout << std::endl;
    assert(T.is_valid());
    std::cout << " number of vertices: " << T.number_of_vertices()
        << std::endl;

    std::cout << " number of degeneracies: " << degeneracy_counter << std::endl;
    degeneracy_counter = 0;
}

// Removes number points from T, and removes each one from points also.
// Checks that each point of T that is removed exists in points.
void remove (Cls &T, point_set &points, int number)
{
    for (int i = 1; !points.empty(); ++i)
    {
        number--;
        assert(T.number_of_vertices() != 0);
        Vertex_handle v = T.finite_vertices_begin();
        set_iterator pos = points.find (v->point());
        assert(pos != points.end());
        T.remove (v);
        points.erase (pos);
        std::cout << "\r number of removed points:  " << i << std::flush;
    }
    std::cout << std::endl;

    assert(number >= 0);
    assert(T.number_of_vertices() == 0);
    assert(T.is_valid());
    assert(points.empty());
    std::cout << " number of degeneracies: " << degeneracy_counter << std::endl;
    degeneracy_counter = 0;
}

// Adds p which is supposed to increase the dimension of T from dim to dim+1,
// then removes it.
void dim_jump (Cls &T, const Point &p, int dim)
{
    assert(T.dimension() == dim);

    Vertex_handle v = T.insert (Weighted_point (p, 0));
    assert(T.is_valid());
    assert(v != Vertex_handle());
    assert(T.dimension() == dim + 1);

    T.remove (v);
    assert(T.is_valid());
    assert(T.dimension() == dim);
}

bool test_case (std::istream &is)
{
    point_reader pi (is), pend;
    if (pi == pend) return false;

    point_set points;
    Cls T;
    int number = 0;

    do {
        ++number;
        points.insert (*pi);
        T.insert (*pi);
    } while (++pi != pend);
    assert(T.is_valid());

    for (int i = 0; !points.empty(); ++i) {
        assert(T.number_of_vertices() != 0);
        Vertex_handle v = T.finite_vertices_begin();
        set_iterator pos = points.find (v->point());
        assert(pos != points.end());
        T.remove (v);
        points.erase(pos);
    }
    assert(T.number_of_vertices() == 0);
    assert(points.empty());

    return true;
}

int main(int argc, char **argv)
{
    // Test various data sets that crashed the code at some point in the past.
    // File format is:
    // number of points of first data set
    // point
    // ...
    // number of points of second data set
    // point
    // ...
    {
        std::ifstream fin ("data/regular_remove_3");
        assert(fin);
        std:: cout << " test `data/regular_remove_3'" << std::endl;
        while (test_case (fin))
            // semicolon
            ;
    }

    // Hardcoded seeds so that the test-suite is deterministic.
    boost::int32_t seed0 = 42, seed1 = 43, seed2 = 42, seed3 = 42;

    // You can also pass seeds on the command line.
    if (argc > 1) { std::istringstream iss(argv[1]); iss >>seed0; }
    if (argc > 2) { std::istringstream iss(argv[2]); iss >>seed1; }
    if (argc > 3) { std::istringstream iss(argv[3]); iss >>seed2; }
    if (argc > 4) { std::istringstream iss(argv[4]); iss >>seed3; }

    Cls T;
    point_set points;

    degeneracy_counter = 0;

    std::cout << " test dimension 0" << std::endl;
    randgen.seed(seed0);

    insert<point_iterator_0> (T, points, 10);
    dim_jump (T, Point (0, 0, 1), 0);
    remove (T, points, 10);

    std::cout << " test dimension 1" << std::endl;
    randgen.seed(seed1);

    insert<point_iterator_1> (T, points, 20);
    dim_jump (T, Point (0, 0, 1), 1);
    remove (T, points, 20);

    std::cout << " test dimension 2" << std::endl;
    randgen.seed(seed2);

    insert<point_iterator_2> (T, points, 100);
    dim_jump (T, Point (0, 0, 1), 2);
    remove (T, points, 100);

    std::cout << " test dimension 3" << std::endl;
    randgen.seed(seed3);

    insert<point_iterator_3> (T, points, 500);
    assert(T.dimension() == 3);
    remove (T, points, 500);

    std::cout << " quit" << std::endl;

    return 0;
}


#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <nlohmann/json.hpp>
#include <chrono>
#include "oful.h"
#include "contlinrep.h"
#include "utils.h"
#include "gzip.h"
#include "bandit.h"

using json = nlohmann::json;
using namespace std;
using namespace Eigen;

int PREC = 4;
int EVERY = 1;

int main()
{
    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    srand (time(NULL));
    // rng.seed(10000); // warm it up
    int n_runs = 50, T = 20000;
    double delta = 0.01;
    double reg_val = 1.;
    double noise_std = 0.2;
    double bonus_scale = 1;
    bool adaptive_ci = true;

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    vec2double regrets(n_runs), pseudo_regrets(n_runs);
    #pragma omp parallel for
    for (int i = 0; i < n_runs; ++i)
    {
        ContToy1_phi1 lrep = ContToy1_phi1(noise_std, seeds[i]);
        OFUL<std::vector<double>> localg(lrep, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(lrep, localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "OFULCont1_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "OFULCont1_pseudoregrets.csv.gz", EVERY, PREC);

    #pragma omp parallel for
    for (int i = 0; i < n_runs; ++i)
    {
        ContToy1_phi2 lrep = ContToy1_phi2(noise_std, seeds[i]);
        OFUL<std::vector<double>> localg(lrep, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(lrep, localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "OFULCont2_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "OFULCont2_pseudoregrets.csv.gz", EVERY, PREC);

    return 0;
}


#ifndef STAN_MATH_PRIM_MAT_PROB_DIRICHLET_LOG_HPP
#define STAN_MATH_PRIM_MAT_PROB_DIRICHLET_LOG_HPP

#include <stan/math/prim/mat/prob/dirichlet_lpmf.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>

namespace stan {
  namespace math {

    /**
     * The log of the Dirichlet density for the given theta and
     * a vector of prior sample sizes, alpha.
     * Each element of alpha must be greater than 0.
     * Each element of theta must be greater than or 0.
     * Theta sums to 1.
     *
     * @deprecated use dirichlet_lpmf
     *
     * @param theta A scalar vector.
     * @param alpha Prior sample sizes.
     * @return The log of the Dirichlet density.
     * @throw std::domain_error if any element of alpha is less than
     * or equal to 0.
     * @throw std::domain_error if any element of theta is less than 0.
     * @throw std::domain_error if the sum of theta is not 1.
     * @tparam T_prob Type of scalar.
     * @tparam T_prior_sample_size Type of prior sample sizes.
     */
    template <bool propto,
              typename T_prob, typename T_prior_sample_size>
    typename boost::math::tools::promote_args<T_prob, T_prior_sample_size>::type
    dirichlet_log(const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta,
                  const Eigen::Matrix<T_prior_sample_size, Eigen::Dynamic, 1>&
                  alpha) {
      return dirichlet_lpmf<propto, T_prob, T_prior_sample_size>(theta, alpha);
    }

    /**
     * @deprecated use dirichlet_lpmf
     */
    template <typename T_prob, typename T_prior_sample_size>
    inline
    typename boost::math::tools::promote_args<T_prob, T_prior_sample_size>::type
    dirichlet_log(const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta,
                  const Eigen::Matrix<T_prior_sample_size, Eigen::Dynamic, 1>&
                  alpha) {
      return dirichlet_lpmf<T_prob, T_prior_sample_size>(theta, alpha);
    }

  }
}
#endif


// system includes -----------------------------------------------
#include <mpi.h>
#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include "base/exceptions.hpp"
#include "base/logger.hpp"
#include "base/pcout.hpp"
#include "export/output_handler.hpp"
#ifdef USE_OMP
#include <omp.h>
#endif

// own includes --------------------------------------------------
#include "base/coeff_array.hpp"
#include "base/eigen2hdf.hpp"
#include "base/import_coeffs.hpp"
#include "base/init.hpp"
#include "base/numbers.hpp"
#include "fft/fft2_r2c.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"
#ifdef FTCG
#include "ridgelet/rc_linearize.hpp"
#endif
#include "operators/operators.hpp"
#include "solver/cg.hpp"
#include "solver/ridgelet_solver.hpp"
// boltzmann includes --------------------------------------------
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/collision/collision_tensor_galerkin.hpp"
#include "spectral/polar_to_nodal.hpp"

using namespace std;
namespace po = boost::program_options;

typedef boltzmann::SpectralBasisFactoryKS basis_factory_t;
typedef basis_factory_t::basis_type basis_type;
typedef RT<double, RidgeletFrame, FFTr2c<PlannerR2C> > RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

const double Lx = 1;
const double Ly = 1;

class BTE
{
 public:
  BTE(const po::variables_map& vm)
  {
    unsigned int Jx = vm["Jx"].as<unsigned int>();
    unsigned int Jy = vm["Jy"].as<unsigned int>();
    unsigned int rho_x = vm["rx"].as<unsigned int>();
    unsigned int rho_y = vm["ry"].as<unsigned int>();

    // make sure that Lx = 2**j
    rt_frame = RidgeletFrame(Jx - 1, Jy - 1, rho_x, rho_y);

    load_spectral_basis("spectral_basis.desc");

    K = spectral::get_max_k(spectral_basis) + 1;
    Nx = rt_frame.Nx();  // #cols
    Ny = rt_frame.Ny();  // #rows
    Lx = Nx / 2;
    Ly = Ny / 2;
    dimX = Lx * Ly;  // num physical DoFs
    dimV = K * K;    // num DoFs in spectral basis (nodal)

    xmap = Map(dimX);
    vmap = Map(dimV);

    ASSERT(Nx > 0);
    ASSERT(Ny > 0);
    ASSERT(K > 0);
    ASSERT(dimV > 0);
    ASSERT(dimX > 0);
  }

 private:
  void load_spectral_basis(const std::string& fname)
  {
    basis_factory_t::create(spectral_basis, fname);
  }

 public:
  RidgeletFrame rt_frame;
  basis_type spectral_basis;
  unsigned int Nx = 0;
  unsigned int Ny = 0;
  unsigned int Lx = 0;
  unsigned int Ly = 0;
  unsigned int K = 0;
  unsigned int dimV = 0;
  unsigned int dimX = 0;

  Map xmap;
  Map vmap;
};

struct bprogram_input
{
  static unsigned int Jx, Jy, rho_x, rho_y;
  static double dt;
  static unsigned int ntsteps;
  static unsigned int obuf, of;
  static int dump_freq;
  static double knudsen;
#ifdef FTCG
  static double rttre;
#else
  static double cg_tol;
  static int cg_maxit;
#endif

  static void print()
  {
#ifdef FTCG
    ASSERT(rttre <= 1);
#endif
    pcout << setw(20) << "Jx"
          << ": " << Jx << "\n"
          << setw(20) << "Jy"
          << ": " << Jy << "\n"
          << setw(20) << "rho_x"
          << ": " << rho_x << "\n"
          << setw(20) << "rho_y"
          << ": " << rho_y << "\n"
          << setw(20) << "dt"
          << ": " << dt << "\n"
          << "\n";
#ifndef FTCG
    pcout << setw(20) << "cg_tol"
          << ": " << cg_tol << "\n"
          << setw(20) << "cg_maxit"
          << ": " << cg_maxit << "\n";
#endif
    pcout << setw(20) << "obuf:"
          << ":" << obuf << "\n";
  }
};

unsigned int bprogram_input::Jx, bprogram_input::Jy, bprogram_input::rho_x, bprogram_input::rho_y;
unsigned int bprogram_input::ntsteps, bprogram_input::obuf, bprogram_input::of;
int bprogram_input::dump_freq;
double bprogram_input::dt, bprogram_input::knudsen;
#ifdef FTCG
double bprogram_input::rttre;
#else
double bprogram_input::cg_tol;
int bprogram_input::cg_maxit;
#endif

int main(int argc, char* argv[])
{
  int mpi_provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_SERIALIZED, &mpi_provided);
  ASSERT(mpi_provided = MPI_THREAD_SERIALIZED);

  int pid = -1, nprocs = -1;
  MPI_Comm_rank(MPI_COMM_WORLD, &pid);
  MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
  ASSERT(pid >= 0);
  ASSERT(nprocs > 0);

  pcout << "SOURCE::INFO::GIT_BRANCHNAME " << GIT_BNAME << "\n";
  pcout << "SOURCE::INFO::GIT_SHA1       " << GIT_SHA1 << "\n";
#ifdef FTCG
  pcout << "use FFT CG solver"
        << "\n";
#endif

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&bprogram_input::Jx)->default_value(3), "Jx (corresponds to Lx=2**(Jx+1)*rx)")
      ("Jy,j", po::value<unsigned int>(&bprogram_input::Jy)->default_value(3), "Jy (corresponds to Ly=2**(Jy+1)*ry)")
      ("rx,x", po::value<unsigned int>(&bprogram_input::rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&bprogram_input::rho_y)->default_value(1), "rho_x")
      ("f0", po::value<string>()->default_value(""), "initial distribution, dset='coeffs' (Polar basis)")
      ("init", po::value<string>()->required(), "initial distribution as matrix of size L x N (nodal basis of total size N)")
      ("dt,t", po::value<double>(&bprogram_input::dt)->default_value(0.001), "timesteps")
      ("nt,n", po::value<unsigned int>(&bprogram_input::ntsteps)->default_value(1000), "num timesteps")
      ("kn", po::value<double>(&bprogram_input::knudsen)->default_value(1.0), "Knudsen number")
      ("obuf", po::value<unsigned int>(&bprogram_input::obuf)->default_value(1), "buffer num timesteps")
      ("of", po::value<unsigned int>(&bprogram_input::of)->default_value(1), "output frequency")
      ("dump", po::value<int>(&bprogram_input::dump_freq)->default_value(-1), "dump full solution vector every n timesteps")
#ifdef FTCG
      ("rttre", po::value<double>(&bprogram_input::rttre)->default_value(1), "keep rttre rt-coefficients in every timestep")
#else
      ("cg_maxit", po::value<int>(&bprogram_input::cg_maxit)->default_value(100), "cg max. iterations")
      ("cg_tol", po::value<double>(&bprogram_input::cg_tol)->default_value(1e-6), "cg rel. tol.")
#endif
      ("solverlog", "log convergence details of cg solver")
      ("no-collision", "disable collision")
      ("export-dofs", "export grid information");
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);
  } catch (std::exception& e) {
    if (vm.count("help"))
      pcout << options << "\n";
    else
      pcout << e.what() << "\n";
    return 1;
  }

  // write parameters to stdout
  bprogram_input::print();

  if (vm["f0"].as<std::string>() != "" && vm["init"].as<std::string>() != "") {
    cout << "cannot specify --f0 and --init at the same time"
         << "\n";
    MPI_Finalize();
    return 1;
  }

  if (vm.count("help")) {
    std::cout << options << "\n";
    MPI_Finalize();
    return 0;
  }

  BTE main_class(vm);

  auto& rf = main_class.rt_frame;
  const int Nx = main_class.Nx;   // #cols
  const int Ny = main_class.Ny;   // #rows
  const int L = main_class.dimX;  // num physical DoFs
  const int K = main_class.K;
  // const int N = main_class.dimV; // num DoFs in spectral basis (nodal)
  // Nx is 2 time Lx
  pcout << "Nx: " << Nx << "\n";
  pcout << "Ny: " << Ny << "\n";

  if (vm.count("export-dofs")) {
    if (pid == 0) {
      hid_t file = H5Fcreate("grid.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
      Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx + 1, 0, Lx).segment(0, Nx);
      Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny + 1, 0, Ly).segment(0, Ny);
      eigen2hdf::save(file, "xi", xi);
      eigen2hdf::save(file, "xi", yi);
      H5Fclose(file);
    }
    MPI_Finalize();
    return 0;
  }

  RT_t rt(rf);
  FFTr2c<PlannerR2C> fft;
  init_fftw(fft, FFTW_MEASURE, rf);
  typedef RidgeletCellArray<rt_coeff_t> rca_t;

  // ======================================
  // create rte_map / vector
  // transport problem is distributed in V
  CoeffArray<double> rte_vector(main_class.vmap, main_class.dimX);
  // collision problem is distributed in X
  CoeffArray<double> bte_vector(main_class.xmap, main_class.dimV);

  // ========================================
  // ========= Polar Spectral setup =========
  // ========================================
  auto& spectral_basis = main_class.spectral_basis;
  const int Np = spectral_basis.n_dofs();
  boltzmann::Polar2Nodal<basis_type> p2n;
  p2n.init(spectral_basis, 1.0);

  // ------------------------------------------------------------
  // initialize coefficients at t=0
  // ------------------------------------------------------------
  load_coeffs_from_file(rte_vector, vm["init"].as<std::string>(), "coeffs");
  bte_vector = rte_vector.transpose();

  // load collision tensor
  boltzmann::CollisionTensorGalerkin collision_operator(spectral_basis);
  if (!vm.count("no-collision")) collision_operator.read_hdf5("collision_tensor.h5");
  pcout << "done loading collision tensor\n";

  std::vector<double> qi(K);
  boltzmann::gauss_hermite_roots(qi, K);

  OutputHandler output_handler(spectral_basis,
                               main_class.xmap,
                               p2n,
                               bprogram_input::of,         // output frequency for moments
                               bprogram_input::dump_freq,  // output frequency for full solution
                               bprogram_input::obuf);
  output_handler.compute(bte_vector, 0, 0.);

// ======================================== //
// ===========  time STEPPING ============= //
// ======================================== //
#ifdef FTCG
  RCLinearize rcl(rf);
  unsigned int nc = rcl.size();
  std::vector<int> active_set(nc, 0);
#endif

#pragma omp parallel
  {
    // --- thread local variables --- //
    double current_time = 0;
    auto v_ldofs = main_class.vmap.lsize();
    auto x_ldofs = main_class.xmap.lsize();
    Eigen::VectorXd cp(Np);
    Eigen::VectorXd cp_out(Np);
    Eigen::MatrixXd cn(K, K);
    array_t buf_rte(Ny / 2,
                    Nx / 2);
    // variables transport
    complex_array_t Bh(Ny, Nx);
    complex_array_t Fh(Ny, Nx);
    complex_array_t Fh_cut(Ny / 2, Nx / 2);
    rca_t f_rc(main_class.rt_frame);
    rca_t b_rc(main_class.rt_frame);
    double* rte_data = rte_vector.data();

#ifdef FTCG
    std::vector<double> lin_f_rc(rcl.size());
#endif

    for (unsigned int ii = 1; ii <= bprogram_input::ntsteps; ++ii) {
#pragma omp single
      pcout << "timestep " << ii << "\n";

// ========== solve transport problem==========
#pragma omp for schedule(static)
      for (int ijv = 0; ijv < v_ldofs; ++ijv) {
        Eigen::Map<array_t> rte_view(rte_data + ijv * L, Ny / 2, Nx / 2);
        buf_rte = rte_view;
        fft.ft(Fh_cut, buf_rte, false);
        Fh.setZero();
        ftcut(Fh, Ny / 2, Ny / 2) = Fh_cut;
        hf_zero(Fh);
        const int jv = main_class.vmap.GID(ijv);  // global V-index

        // polar2nodal assumes col-major storage
        const double vx = qi[jv / K];
        const double vy = qi[jv % K];

        // transport operator
        TransportOperator T(vx, vy, Lx, Ly, Nx, Ny, bprogram_input::dt);
#ifdef FTCG
        T.apply_bckwrd_euler(Fh);
        if (bprogram_input::rttre < 1) {
          rt.rt(f_rc.coeffs(), Fh);
          lin_f_rc = rcl.linearize(f_rc.coeffs());
          double tre = std::max(rcl.get_threshold(lin_f_rc, bprogram_input::rttre), 1e-12);
          // apply threshold
          rcl.threshold(f_rc, tre);

          // update active set
          for (unsigned int i = 0; i < rcl.size(); ++i) {
            // synchronization is not needed!
            if (std::abs(lin_f_rc[i]) > tre) active_set[i] = 1;
          }
        }
#else
        AhAOp AhA(vx, vy, Lx, Ly, Nx, Ny, bprogram_input::dt);
        // * solve with ridgelets * //
        PTransportOp<RT_t> A(rt, AhA, vx, vy);
        // init x0
        // initialize starting value for cg
        rt.rt(f_rc.coeffs(), Fh);
        // create rhs
        rt.rt(b_rc.coeffs(), Bh);
        RidgeletSolver<rt_coeff_t> rt_solver(rf, vx, vy);
        rt_solver.solve(f_rc, A, b_rc, bprogram_input::cg_tol, bprogram_input::cg_maxit);
#endif
#ifdef FTCG
        if (bprogram_input::rttre < 1) {
          rt.irt(Fh, f_rc.coeffs());  // go back to Fourier domain
        }
#else
        rt.irt(Fh, f_rc.coeffs());  // go back to Fourier domain
#endif

        Fh_cut = ftcut(Fh, Ny / 2, Nx / 2);
        fft.ift(buf_rte, Fh_cut);  // go back to Spatial domain
        rte_view = buf_rte;
      }                            // end for velocities, OMP => implied omp barrier
#pragma omp single
      {
// reduce active set to proc 0 and print statistics...
#ifdef FTCG
        std::vector<int> active_set_root(rcl.size(), 0);
        int ierr = MPI_Reduce(active_set.data(),
                              active_set_root.data(),
                              rcl.size(),
                              MPI_INT,
                              MPI_LOR,
                              0,
                              MPI_COMM_WORLD);
        BOOST_ASSERT(ierr == 0);
        if (pid == 0) {
          active_set = active_set_root;
          int ac = 0;
          for (unsigned q = 0; q < active_set.size(); ++q) {
            if (active_set[q]) ac++;
          }
          std::printf("ACTIVE_SIZE::frame=%d\t%d\n", ii, ac);
        }
#endif
        // communicate coefficients
        bte_vector = rte_vector.transpose();
      }
#pragma omp for schedule(static)
      for (int ijx = 0; ijx < x_ldofs; ++ijx) {
        // 1. transform to polar basis
        Eigen::Map<Eigen::MatrixXd> cn(bte_vector.get(ijx).data(), K, K);
        p2n.to_polar(cp, cn);
        if (vm.count("no-collision")) {
          // do nothing
        } else {
          // 2. apply Q
          collision_operator.apply(cp_out.data(), cp.data());
          // explicit euler
          cp_out = cp_out * bprogram_input::dt / bprogram_input::knudsen + cp;
          collision_operator.project(cp_out.data(), cp.data());
          // 3. transform to lagrange basis
          p2n.to_nodal(cn, cp_out);
          // output
        }
      }
      current_time += bprogram_input::dt;

#pragma omp master
      {
        output_handler.compute(bte_vector, ii, current_time);
        rte_vector = bte_vector.transpose();
      }
#pragma omp barrier
    }  // end timesteps
#ifdef FTCG
#pragma omp single
    {
      if (pid == 0) {
        // write active_set to disk...
        std::ofstream fout("ac.coeff.final.data");
        for (unsigned int i = 0; i < active_set.size(); ++i) {
          fout << active_set[i] << std::endl;
        }
        fout.close();
      }
    }
#endif
  }  // end parallel region

  MPI_Finalize();
  return 0;
}


#ifndef PYTHONIC_INCLUDE_NUMPY_TAN_HPP
#define PYTHONIC_INCLUDE_NUMPY_TAN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/tan.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME tan
#define NUMPY_NARY_FUNC_SYM boost::simd::tan
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/dict.h"
#include "dynet/expr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

using namespace std;
using namespace dynet;

using namespace std;
using namespace dynet;
using namespace dynet::expr;

/*
debug
void Print (const vector<float>& v){
  //vector<int> v;
  for (int i=0; i<v.size();i++){
    cout << v[i] << endl;
  }
}*/

/*vector<string> read_lines(string file_name)
{
	vector<string> lines;
	ifstream myfile (file_name);
	string line;
	while (getline(myfile, line))
	{
		lines.push_back(line);
	}
}*/

int main(int argc, char** argv) {
	dynet::initialize(argc, argv); // need to add this apparently ??

	dynet::Dict d;
	//int start_sent 	= d.convert("<s>");
  	//int end_sent 	= d.convert("</s>");

  	// Read data
	vector<vector<int>> training;

	cout << "READING DATA" << endl;
	ifstream in("/home/slouvan/dynet/examples/example-data/fin-words.txt");
	string line;
	while(getline(in, line)) {
      training.push_back(read_sentence(line, &d));
      cout<< line << endl;
    }

	unsigned hidden_dim 		= 50;
	unsigned layer_input_dim 	= 10;
	unsigned VOCAB_SIZE 		= d.size();

	// Define parameters
	Model model;
	LookupParameter p_c = model.add_lookup_parameters(VOCAB_SIZE, {layer_input_dim});
	Parameter 		p_W = model.add_parameters({hidden_dim, layer_input_dim});
	Parameter 		p_U	= model.add_parameters({hidden_dim, hidden_dim});
	Parameter 		p_B = model.add_parameters({hidden_dim});
	Parameter 		p_V = model.add_parameters({VOCAB_SIZE, hidden_dim});

	// Add parameters to the graph
	ComputationGraph cg;
  	Expression W = parameter(cg, p_W);
  	Expression U = parameter(cg, p_U);
  	Expression b = parameter(cg, p_B);
  	Expression V = parameter(cg, p_V);

  	vector<Expression> h_t; // store previous hidden state

  	SimpleSGDTrainer trainer(&model, 0.1);

  	// TRAINING

  	for (unsigned i = 0; i < training.size(); ++i) {
  		vector<int> current_sent = training[i];

  		// for each training instance build the sequence
  		vector<Expression> h_t_1;
  		Expression h0; // how to init the first one?
  		for (unsigned t = 0; t < current_sent.size(); ++t)
  		{
  			Expression x_t = lookup(cg, p_c, current_sent[t]);	// input
  			Expression h_t;

  			if (t == 0) // the first time step
  			{
  				h_t = tanh(W * x_t + U * h0 + b);
  			}
  			else
  			{
  				h_t = tanh(W * x_t + U * h_t_1[t-1] + b);
  			}

  			// compute and sum error here

  			h_t_1.push_back(h_t); // store the hidden state
  		}

  		// Expression loss =
  		// cg.forward(loss);
  		// cg.backward(loss);
  		// trainer.update();

  	}
	return 0;
}


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

#include <boost/simd/ieee/functions/ldexp.hpp>
#include <boost/simd/include/functions/scalar/shift_left.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/limitexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/constant/constant_value.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/include/constants/minexponent.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( ldexp_, tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_< integer_<A0> >)
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return (a1>0)?(a0<<a1):(a0>>a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( ldexp_, tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_< floating_<A0> >)
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    typedef typename dispatch::meta::as_integer<A0>::type iA0;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      iA0 e =  a1;
      A0 f = One<A0>();
#ifndef BOOST_SIMD_NO_DENORMALS
      if (BOOST_UNLIKELY((e < meta::constant_value<tag::Minexponent, A0>::value)))
      {
        e -= Minexponent<A0>();
        f = Smallestposval<A0>();
      }
#endif
      iA0 b = (e == Limitexponent<A0>());
      f += A0(b);
      e -= b;
      e += Maxexponent<A0>();
      e = shl(e, Nbmantissabits<A0>());
      return a0*bitwise_cast<A0>(e)*f;
    }
  };
} } }

#endif


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

#include <boost/simd/arithmetic/functions/divround.hpp>
#include <boost/simd/include/functions/simd/round.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/touints.hpp>
#include <boost/simd/include/functions/simd/toints.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/groups.hpp>
#include <boost/simd/sdk/meta/is_upgradable.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF          ( divround_, tag::cpu_, (A0)(X)
                                       , (simd::meta::is_upgradable_as_real_on_ext<A0, X>)
                                       , ((simd_< int_<A0>, X>))
                                         ((simd_< int_<A0>, X>))
                                       )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h );
      boost::simd::split(a1, a1l, a1h );
      ivtype d0 = toints(divround(tofloat(a0l), tofloat(a1l)));
      ivtype d1 = toints(divround(tofloat(a0h), tofloat(a1h)));
      return groups(d0, d1);
    }
  };

 BOOST_DISPATCH_IMPLEMENT_IF          ( divround_, tag::cpu_, (A0)(X)
                                      , (simd::meta::is_upgradable_as_real_on_ext<A0, X>)
                                      , ((simd_< uint_<A0>, X>))
                                        ((simd_< uint_<A0>, X>))
                                      )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h );
      boost::simd::split(a1, a1l, a1h );
      ivtype d0 = touints(divround(tofloat(a0l), tofloat(a1l)));
      ivtype d1 = touints(divround(tofloat(a0h), tofloat(a1h)));
      return groups(d0, d1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( divround_, tag::cpu_, (A0)(X)
                                    , ((simd_<ints8_<A0>,X>))
                                      ((simd_<ints8_<A0>,X>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h);
      boost::simd::split(a1, a1l, a1h);
      ivtype d0 = divround(a0l, a1l);
      ivtype d1 = divround(a0h, a1h);
      return groups(d0, d1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( divround_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                      ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return simd::round(a0/a1);
    }
  };
} } }

#endif


// Copyright (c) 2021 The Dash Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <test/util/setup_common.h>

#include <chainparams.h>
#include <consensus/validation.h>
#include <miner.h>
#include <script/interpreter.h>
#include <validation.h>

#include <boost/test/unit_test.hpp>

const auto deployment_id = Consensus::DEPLOYMENT_DIP0020;
constexpr int window{100}, th_start{80}, th_end{60};

static constexpr int threshold(int attempt)
{
    // An implementation of VersionBitsConditionChecker::Threshold()
    int threshold_calc = th_start - attempt * attempt * window / 100 / 5;
    if (threshold_calc < th_end) {
        return th_end;
    }
    return threshold_calc;
}

struct TestChainDATSetup : public TestChainSetup
{
    TestChainDATSetup() : TestChainSetup(window - 2) {}

    void signal(int num_blocks, bool expected_lockin)
    {
        const auto& consensus_params = Params().GetConsensus();
        // Mine non-signalling blocks
        gArgs.ForceSetArg("-blockversion", "536870912");
        for (int i = 0; i < window - num_blocks; ++i) {
            CreateAndProcessBlock({}, coinbaseKey);
        }
        gArgs.ForceRemoveArg("-blockversion");
        if (num_blocks > 0) {
            // Mine signalling blocks
            for (int i = 0; i < num_blocks; ++i) {
                CreateAndProcessBlock({}, coinbaseKey);
            }
        }
        LOCK(cs_main);
        if (expected_lockin) {
            BOOST_CHECK_EQUAL(VersionBitsTipState(consensus_params, deployment_id), ThresholdState::LOCKED_IN);
        } else {
            BOOST_CHECK_EQUAL(VersionBitsTipState(consensus_params, deployment_id), ThresholdState::STARTED);
        }
    }

    void test(int activation_index, bool check_activation_at_min)
    {
        const auto& consensus_params = Params().GetConsensus();
        CScript coinbasePubKey = CScript() <<  ToByteVector(coinbaseKey.GetPubKey()) << OP_CHECKSIG;

        {
            LOCK(cs_main);
            BOOST_CHECK_EQUAL(::ChainActive().Height(), window - 2);
            BOOST_CHECK_EQUAL(VersionBitsTipState(consensus_params, deployment_id), ThresholdState::DEFINED);
        }

        CreateAndProcessBlock({}, coinbaseKey);

        {
            LOCK(cs_main);
            // Advance from DEFINED to STARTED at height = window - 1
            BOOST_CHECK_EQUAL(::ChainActive().Height(), window - 1);
            BOOST_CHECK_EQUAL(VersionBitsTipState(consensus_params, deployment_id), ThresholdState::STARTED);
            BOOST_CHECK_EQUAL(VersionBitsTipStatistics(consensus_params, deployment_id).threshold, threshold(0));
            // Next block should be signaling by default
            const auto pblocktemplate = BlockAssembler(*m_node.mempool, Params()).CreateNewBlock(coinbasePubKey);
            const uint32_t bitmask = ((uint32_t)1) << consensus_params.vDeployments[deployment_id].bit;
            BOOST_CHECK_EQUAL(::ChainActive().Tip()->nVersion & bitmask, 0);
            BOOST_CHECK_EQUAL(pblocktemplate->block.nVersion & bitmask, bitmask);
        }

        // Reach activation_index level
        for (int i = 0; i < activation_index; ++i) {
            signal(threshold(i) - 1, false); // 1 block short

            {
                // Still STARTED but with a (potentially) new threshold
                LOCK(cs_main);
                BOOST_CHECK_EQUAL(::ChainActive().Height(), window * (i + 2) - 1);
                BOOST_CHECK_EQUAL(VersionBitsTipState(consensus_params, deployment_id), ThresholdState::STARTED);
                const auto vbts = VersionBitsTipStatistics(consensus_params, deployment_id);
                BOOST_CHECK_EQUAL(vbts.threshold, threshold(i + 1));
                BOOST_CHECK(vbts.threshold <= th_start);
                BOOST_CHECK(vbts.threshold >= th_end);
            }
        }
        if (LOCK(cs_main); check_activation_at_min) {
            BOOST_CHECK_EQUAL(VersionBitsTipStatistics(consensus_params, deployment_id).threshold, th_end);
        } else {
            BOOST_CHECK(VersionBitsTipStatistics(consensus_params, deployment_id).threshold > th_end);
        }

        // activate
        signal(threshold(activation_index), true);
        for (int i = 0; i < window; ++i) {
            CreateAndProcessBlock({}, coinbaseKey);
        }
        {
            LOCK(cs_main);
            BOOST_CHECK_EQUAL(VersionBitsTipState(consensus_params, deployment_id), ThresholdState::ACTIVE);
        }

    }
};

BOOST_AUTO_TEST_SUITE(dynamic_activation_thresholds_tests)

#define TEST(INDEX, activate_at_min_level)  BOOST_FIXTURE_TEST_CASE(activate_at_##INDEX##_level, TestChainDATSetup) \
{ \
    test(INDEX, activate_at_min_level); \
}

TEST(1, false)
TEST(2, false)
TEST(3, false)
TEST(4, false)
TEST(5, false)
TEST(6, false)
TEST(7, false)
TEST(8, false)
TEST(9, false)
TEST(10, true)
TEST(11, true)
TEST(12, true)

BOOST_AUTO_TEST_SUITE_END()


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2018.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Eugen Netz $
// $Authors: Eugen Netz $
// --------------------------------------------------------------------------

#include <OpenMS/MATH/STATISTICS/CumulativeBinomial.h>
#include <boost/math/special_functions/binomial.hpp>

#include <iostream>
#include <numeric>

namespace OpenMS
{

  namespace Math
  {

    double CumulativeBinomial::compute(Size n, Size k, double p)
    {
      double p_cumul = 0.0;
      if (p < 1e-99) return static_cast<double>(k == 0);
      if (1 - p < 1e-99) return static_cast<double>(k != n);
      if (k > n)  return 1.0;

      for (Size j = 0; j < k; ++j)
      {
        double coeff = 0;

        try
        {
          coeff = boost::math::binomial_coefficient<double>(static_cast<unsigned int>(n), static_cast<unsigned int>(j));
        }
        catch (std::overflow_error const& /*e*/)
        {
          // not sure if a warning is appropriate here, since if it happens, it will happen very often for the same spectrum and flood the stdout
//          std::cout << "Warning: Binomial coefficient for match-odds score has overflowed! Setting value to the maximal double value." << std::endl;
//          std::cout << "binomial_coefficient was called with N = " << n << " and k = " << j << std::endl;
          coeff = std::numeric_limits<double>::max();
        }

        p_cumul += coeff * pow(p,  static_cast<int>(j)) * pow((1-p), static_cast<int>((n-j)));
      }

      // A result of p_cumul >= 1.0 does not make sense theoretically, but it might reach 1.0 because of insufficient precision,
      // solved by using largest value smaller than 1.0
      if (p_cumul >= 1.0)
      {
        // TODO: C11 change to nexttoward
//        p_cumul = nexttoward(1.0, 0.0);
        p_cumul = 1.0 - std::numeric_limits<double>::epsilon();
      }

      return p_cumul;
    }
  } // namespace Math

} // namespace OpenMS


#include <dlib/opencv.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <chrono>
#include <iostream>
#include <iomanip>
#include <dlib/assert.h>
#include <dlib/cmd_line_parser.h>
#include <dlib/dir_nav.h>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <map>

namespace chrono = std::chrono;
using fseconds = chrono::duration<float>;

// Neural network definition for face detection
template <long num_filters, typename SUBNET> using con5d = dlib::con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = dlib::con<num_filters,5,5,1,1,SUBNET>;

template <typename SUBNET> using downsampler  = dlib::relu<dlib::affine<con5d<32, dlib::relu<dlib::affine<con5d<32, dlib::relu<dlib::affine<con5d<16,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = dlib::relu<dlib::affine<con5<45,SUBNET>>>;

using net_type = dlib::loss_mmod<dlib::con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<dlib::input_rgb_image_pyramid<dlib::pyramid_down<6>>>>>>>>;

// Neural network definition for face recognition (ResNet50)
template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = dlib::add_prev1<block<N,BN,1,dlib::tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = dlib::add_prev2<dlib::avg_pool<2,2,2,2,dlib::skip1<dlib::tag2<block<N,BN,2,dlib::tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<dlib::con<N,3,3,1,1,dlib::relu<BN<dlib::con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = dlib::relu<residual<block,N,dlib::affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = dlib::relu<residual_down<block,N,dlib::affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

using anet_type = dlib::loss_metric<dlib::fc_no_bias<128,dlib::avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            dlib::max_pool<3,3,2,2,dlib::relu<dlib::affine<dlib::con<32,7,7,2,2,
                            dlib::input_rgb_image_sized<150>
                            >>>>>>>>>>>>;

class FaceDetector
{
    public:
        FaceDetector()
        {
            this->light_detector = dlib::get_frontal_face_detector();
            this->detector = &FaceDetector::light_detect;
        }
        FaceDetector(const std::string net_path)
        {
            dlib::deserialize(net_path) >> this->dnn_detector;
            this->detector = &FaceDetector::dnn_detect;
        }

        std::vector<dlib::rectangle> detect(dlib::matrix<dlib::rgb_pixel> img)
        {
            return (this->*detector)(img);
        }
    private:
        typedef std::vector<dlib::rectangle> (FaceDetector::*DetectorPtr) (dlib::matrix<dlib::rgb_pixel>);
        DetectorPtr detector;
        net_type dnn_detector;
        dlib::frontal_face_detector light_detector ;
        std::vector<dlib::rectangle> light_detect(dlib::matrix<dlib::rgb_pixel> img)
        {
            return this->light_detector(img);
        }
        std::vector<dlib::rectangle> dnn_detect(dlib::matrix<dlib::rgb_pixel> img)
        {
            std::vector<dlib::rectangle> dets;
            auto mmod_rects = this->dnn_detector(img);
            for (const auto &r : mmod_rects)
            {
                dets.push_back(r);
            }
            return dets;
        }
};

class FaceAligner
{
    public:
        FaceAligner(const std::string model_path)
        {
            dlib::deserialize(model_path) >> this->pose_model;
        };
        dlib::full_object_detection align(dlib::matrix<dlib::rgb_pixel> img, dlib::rectangle det)
        {
            return this->pose_model(img, det);
        }
    private:
        dlib::shape_predictor pose_model;
};

int main(int argc, char** argv)
{
    dlib::command_line_parser parser;
    parser.add_option("input", "Path to video file to process (defaults to webcam)", 1);
    parser.add_option("mirror", "Mirror mode (left-right flip)");
    parser.add_option("light", "Use a lighter detection model");
    parser.add_option("threshold", "Face recognition threshold (default: 0.5)", 1);
    parser.add_option("enroll-dir", "Enrollment directory (default: enrollment)", 1);
    parser.add_option("pyramid-levels", "Pyramid levels for the face detector (default: 1)", 1);
    parser.add_option("scale-factor", "Scaling factor for the input image (default: 1.0)", 1);
    parser.add_option("fps", "Force the frames per second for the webcam", 1);
    parser.add_option("help","Display this help message.");
    parser.parse(argc, argv);

    if (parser.option("help"))
    {
        std::cout << "Usage: " << argv[0] << " [options] <list.txt>" << std::endl;
        parser.print_options();
        return EXIT_SUCCESS;
    }

    try
    {
        double threshold = get_option(parser, "threshold", 0.5);
        std::string enroll_dir = get_option(parser, "enroll-dir", "enrollment");
        int pyramid_levels = get_option(parser, "pyramid-levels", 1);
        double scale_factor = get_option(parser, "scale-factor", 1.0);

        std::string video_path;
        cv::VideoCapture vid_src;
        if(parser.option("input"))
        {
            video_path = parser.option("input").argument();
            cv::VideoCapture file(video_path);
            vid_src = file;
        }
        else
        {
            cv::VideoCapture cap(0);
            if (parser.option("fps")) {
                int fps = atoi(parser.option("fps").argument().c_str());
                cap.set(cv::CAP_PROP_FPS, fps);
            }
            vid_src = cap;
        }

        // open the webcam
        if (!vid_src.isOpened())
        {
            std::cerr << "Unable to connect to camera" << std::endl;
            return EXIT_FAILURE;
        }

        // Initialize the face detector
        FaceDetector face_detector;
        if (!parser.option("light"))
        {
            face_detector = FaceDetector("models/mmod_human_face_detector.dat");
        }

        // Initialize the face aligner
        FaceAligner face_aligner("models/shape_predictor_5_face_landmarks.dat");

        // Load the neural network for face recognition
        anet_type anet;
        dlib::deserialize("models/dlib_face_recognition_resnet_model_v1.dat") >> anet;

        // A mapping between face_descriptors and indentities
        std::map<dlib::matrix<float, 0, 1>, std::string> enr_map;
        // ----------------- ENROLLMENT -----------------
        {
            auto t0 = chrono::steady_clock::now();
            std::cout << "Scanning '" << enroll_dir << "' directory and generating face descriptors." << std::endl;
            dlib::directory root(enroll_dir);
            auto files = get_files_in_directory_tree(root, dlib::match_endings(".jpg .JPG .png .PNG"), 1);
            std::vector<std::string> names;
            std::vector<dlib::matrix<dlib::rgb_pixel>> enr_imgs;
            std::vector<dlib::full_object_detection> enr_shapes;
            for (const auto& f : files)
            {
                dlib::matrix<dlib::rgb_pixel> enr_img;
                load_image(enr_img, f.full_name());
                names.push_back(get_parent_directory(f).name());
                enr_imgs.push_back(enr_img);
            }
            // Detect faces on enrollment images
            for (const auto& enr_img : enr_imgs)
            {
                const auto& dets = face_detector.detect(enr_img);
                // Align and store detected faces
                for (const auto& det : dets)
                {
                    enr_shapes.push_back(face_aligner.align(enr_img, det));
                }
            }
            // Make sure we found as many faces as enrollment images
            DLIB_CASSERT(names.size() == enr_shapes.size());
            std::vector<dlib::matrix<dlib::rgb_pixel>> enr_faces;
            for (size_t i = 0; i < enr_shapes.size(); i++)
            {
                dlib::matrix<dlib::rgb_pixel> face_chip;
                extract_image_chip(enr_imgs[i], get_face_chip_details(enr_shapes[i], 150, 0.25), face_chip);
                enr_faces.push_back(std::move(face_chip));
            }
            std::vector<dlib::matrix<float, 0, 1>> face_descriptors = anet(enr_faces);
            DLIB_CASSERT(names.size() == face_descriptors.size());
            for (size_t i = 0; i < names.size(); i++)
            {
                enr_map[face_descriptors[i]] = names[i];
            }
            auto t1 = chrono::steady_clock::now();
            std::cout << std::fixed << std::setprecision(3) <<
                "Computed " << face_descriptors.size() << " face descriptors in " <<
                chrono::duration_cast<fseconds>(t1 - t0).count() << " seconds" << std::endl;
        }
        // ----------------------------------------------

        // create the display windows
        dlib::image_window win, det_win;
        win.set_title("Webcam");
        det_win.set_title("Face detections");

        // Grab and process frames until the main window is closed by the user.
        while(!win.is_closed())
        {
            auto t0 = chrono::steady_clock::now();
            // Grab a frame
            cv::Mat cv_tmp;
            if (!vid_src.read(cv_tmp))
            {
                break;
            }
            // Convert the OpenCV BRG image into a Dlib RGB image
            cv::Mat cv_tmp_rgb;
            cv::cvtColor(cv_tmp, cv_tmp_rgb, cv::COLOR_BGR2RGB);
            dlib::cv_image<dlib::rgb_pixel> cv_img(cv_tmp_rgb);

            // Handle the mirroring
            dlib::matrix<dlib::rgb_pixel> mir_img, img;
            if (parser.option("mirror"))
            {
                dlib::flip_image_left_right(cv_img, mir_img);
                dlib::assign_image(img, mir_img);
            }
            else
            {
                dlib::assign_image(img, cv_img);
            }

            // Handle scaling
            if (scale_factor != 1.0)
            {
                dlib::resize_image(scale_factor, img);
            }

            // vector to store all face landmarks
            std::vector<dlib::full_object_detection> shapes;
            // vector to store all aligned faces
            std::vector<dlib::matrix<dlib::rgb_pixel>> faces;

            int cur_pyr_lvl = 1;
            while (cur_pyr_lvl < pyramid_levels)
            {
                pyramid_up(img);
                cur_pyr_lvl++;
            }

            // detect faces in current frame
            auto dets = face_detector.detect(img);
            // store alignment information for each face
            for (const auto& det : dets)
            {
                shapes.push_back(face_aligner.align(img, det));
            }

            // align detected faces
            for (const auto& shape : shapes)
            {
                dlib::matrix<dlib::rgb_pixel> face_chip;
                extract_image_chip(img, get_face_chip_details(shape, 150, 0.25), face_chip);
                faces.push_back(std::move(face_chip));
            }

            std::vector<dlib::matrix<float, 0, 1>> face_descriptors = anet(faces);

            for (size_t i = 0; i < face_descriptors.size(); i++)
            {
                for (const auto& entry : enr_map)
                {
                    if (length(face_descriptors[i] - entry.first) < threshold)
                    {
                        cv::Mat cv_face = dlib::toMat(faces[i]);
                        cv::putText(cv_face, entry.second, cv::Point(10, 140), cv::FONT_HERSHEY_DUPLEX, 0.7, cv::Scalar(255, 102, 0), 1);
                        break;
                    }
                }
            }

            det_win.set_image(tile_images(faces));
            win.clear_overlay();
            win.set_image(img);
            win.add_overlay(render_face_detections(shapes));
            auto t1 = chrono::steady_clock::now();
            auto real_fps = 1.0 / chrono::duration_cast<fseconds>(t1 - t0).count();
            std::cout << std::fixed << std::setprecision(3) <<
                "Detected faces: " << shapes.size() << "\tFPS: " << real_fps << "    \r" << std::flush;
        }
        std::cout << std::endl;
    }
    catch(std::exception& e)
    {
        std::cout << e.what() << std::endl;
        return EXIT_FAILURE;
    }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_PIO2_MEDIUM_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_PIO2_MEDIUM_HPP_INCLUDED

#include <nt2/toolbox/trigonometric/functions/rem_pio2_medium.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/toolbox/constant/include/constants/three.hpp>
#include <nt2/include/functions/simd/round.hpp>
#include <nt2/include/functions/simd/fast_toint.hpp>
#include <nt2/include/functions/simd/bitwise_and.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_medium_, boost::simd::tag::simd_,
                      (A0)(X),
                      ((simd_ < floating_<A0>,X > ))
                    )
  {
    typedef typename meta::as_integer<A0>::type            itype;
    typedef boost::fusion::tuple<A0,A0,itype>        result_type;

    inline result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::fusion::at_c<2>(res) =
      nt2::rem_pio2_medium(a0,
                           boost::fusion::at_c<0>(res),
                           boost::fusion::at_c<1>(res)
                          );
      return res;
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementation when real
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_medium_, boost::simd::tag::simd_,
                      (A0)(X),
                      ((simd_ < floating_<A0>, X > ))
                      ((simd_ < floating_<A0>, X > ))
                      ((simd_ < floating_<A0>, X > ))
                    )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& t, A0 & xr, A0& xc) const
    {
      A0 fn = nt2::round(t*Invpio_2<A0>());
      A0 r  = t-fn*Pio2_1<A0>();
      A0 w  = fn*Pio2_1t<A0>();
      A0 t2 = r;
      w  = fn*Pio2_2<A0>();
      r  = t2-w;
      w  = fn*Pio2_2t<A0>()-((t2-r)-w);
      t2 = r;
      w  = fn*Pio2_3<A0>();
      r  = t2-w;
      w  = fn*Pio2_3t<A0>()-((t2-r)-w);
      xr = r-w;
      xc = (r-xr)-w;
      return  b_and(fast_toint(fn), Three<result_type>());
    }
  };
} }
#endif