#ifndef CM_ROS_CONVERSION_HELPER_HPP
#define CM_ROS_CONVERSION_HELPER_HPP

// Ros
#include <geometry_msgs/Quaternion.h>

// Libraries
#include <boost/cstdfloat.hpp>
#include <Eigen/Dense>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl_ros/transforms.h>

namespace cm
{

using float64_t = boost::float64_t; ///< Alias for 64 bit float

/// @brief Class for integrating callback data to a given time
class RosConversionHelper
{
public:
    /// @brief Converts ros pc2 to pcl pointcloud
    /// @param pc2 PointCloud2 msg
    /// @return pcl PC
    static pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud2toPCL(const sensor_msgs::PointCloud2::ConstPtr& pc2);

    /// @brief Extracts a rotation matrix from quaternion
    /// @param q Quaternion message to extract yaw from
    /// @return Rotation Matrix
    static Eigen::Matrix3d quaternionMsgToRotationMatrix(const geometry_msgs::Quaternion& q);

    /// @brief Extracts a rotation matrix from quaternion, ignores yaw
    /// @param q Quaternion message to extract yaw from
    /// @return Rotation Matrix
    static Eigen::Matrix3d quaternionMsgToRotationMatrixIgnoreYaw(const geometry_msgs::Quaternion& q);
};

} // namespace cm

#endif // CM_ROS_CONVERSION_HELPER_HPP


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE vector_copy

#include <algorithm>
#include <boost/predef.h>
#include <random>

#if BOOST_COMP_GNUC
extern "C"
{
#    include "quadmath.h"
}
#endif

#include <boost/multiprecision/cpp_int.hpp>
#if BOOST_COMP_GNUC
#    include <boost/multiprecision/float128.hpp>
#endif
#include <boost/test/unit_test.hpp>
#include "vector/vector.hpp"

BOOST_AUTO_TEST_CASE(test_vector_cache_friendly_copy_1)
{
    std::vector<uint64_t> mat2(50);
    std::iota(mat2.begin(), mat2.end(), 0);

    auto mat1 = mat2;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(mat2), std::end(mat2), rng);

    // BOOST_FAIL( "Test is not ready yet" );
    BOOST_REQUIRE_MESSAGE(mat1 != mat2, "mat1 == mat2"
                                            << " instead: "
                                            << "mat1 != mat2");
    my::vector::cache_friendly_copy<uint64_t>(mat1, mat2);
    BOOST_REQUIRE_EQUAL_COLLECTIONS(mat2.begin(), mat2.end(), mat1.begin(), mat1.end());
}

BOOST_AUTO_TEST_CASE(test_vector_cache_friendly_copy_2)
{
    std::vector<uint64_t> mat2(200);
    std::iota(mat2.begin(), mat2.end(), 0);

    auto mat1 = mat2;
    auto rng = std::default_random_engine {};
    std::shuffle(std::begin(mat2), std::end(mat2), rng);

    // BOOST_FAIL( "Test is not ready yet" );
    BOOST_REQUIRE_MESSAGE(mat1 != mat2, "mat1 == mat2"
                                            << " instead: "
                                            << "mat1 != mat2");
    my::vector::cache_friendly_copy<uint64_t>(mat1, mat2);
    BOOST_REQUIRE_EQUAL_COLLECTIONS(mat2.begin(), mat2.end(), mat1.begin(), mat1.end());
}

BOOST_AUTO_TEST_CASE(test_vector_cache_friendly_copy_2d_1)
{
    auto &&mat1 = my::vector::generate_matrix<uint64_t>(50, 50, 0);
    auto &&mat2 = my::vector::generate_matrix<uint64_t>(50, 50, 42);

    // BOOST_FAIL( "Test is not ready yet" );
    BOOST_REQUIRE_MESSAGE(mat1 != mat2, "mat1 == mat2"
                                            << " instead: "
                                            << "mat1 != mat2");
    my::vector::cache_friendly_copy<uint64_t>(mat1, mat2);
    BOOST_REQUIRE_MESSAGE(mat1 == mat2, "mat1 != mat2"
                                            << " instead: "
                                            << "mat1 == mat2");
}

BOOST_AUTO_TEST_CASE(test_vector_cache_friendly_copy_2d_2)
{
    auto &&mat1 = my::vector::generate_matrix<uint64_t>(50, 50, 0);
    auto &&mat2 = my::vector::generate_matrix<uint64_t>(50, 50, 42);

    // BOOST_FAIL( "Test is not ready yet" );
    BOOST_REQUIRE_MESSAGE(mat1 != mat2, "mat1 == mat2"
                                            << " instead: "
                                            << "mat1 != mat2");
    my::vector::cache_friendly_copy<uint64_t>(mat1, mat2);
    // BOOST_REQUIRE_EQUAL_COLLECTIONS(mat2.begin(), mat2.end(),mat1.begin(), mat1.end());
    BOOST_REQUIRE_MESSAGE(mat1 == mat2, "mat1 != mat2"
                                            << " instead: "
                                            << "mat1 == mat2");
}
/*
BOOST_AUTO_TEST_CASE(test_vector_std_copy_2d_1)
{
    auto &&mat1 = my::vector::generate_matrix<uint64_t>(50, 50, 0);
    auto &&mat2 = my::vector::generate_matrix<uint64_t>(50, 50, 42);

    // BOOST_FAIL( "Test is not ready yet" );
    BOOST_REQUIRE_MESSAGE(mat1 != mat2, "mat1 == mat2"
                                            << " instead: "
                                            << "mat1 != mat2");
    my::vector::std_copy<uint64_t>(mat1, mat2);
    BOOST_REQUIRE_MESSAGE(mat1 == mat2, "mat1 != mat2"
                                            << " instead: "
                                            << "mat1 == mat2");
}
*/


/*
 *
 * Copyright (c) 2002, 2003 Kresimir Fresl, Toon Knapen and Karl Meerbergen
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Authors assume no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_TNT_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_TNT_H

// Roldan Pozo's TNT (Template Numerical Toolkit)
// see: http://math.nist.gov/tnt/index.html

#include <boost/numeric/bindings/traits/config.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#include <boost/numeric/bindings/traits/traits.hpp>
#include <tnt/tnt_array1d.h>
#include <tnt/tnt_fortran_array1d.h>
#include <tnt/tnt_array2d.h>
#include <tnt/tnt_fortran_array2d.h>

namespace boost { namespace numeric { namespace bindings { namespace traits {

  // TNT::Array1D<>
  template <typename T, typename V>
  struct vector_detail_traits<TNT::Array1D<T>, V>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         TNT::Array1D<T>, typename boost::remove_const<V>::type
       >::value) );
#endif

    typedef TNT::Array1D<T> identifier_type;
    typedef V vector_type;
    typedef T value_type;
    typedef typename detail::generate_const<V,T>::type* pointer;

    static pointer storage (vector_type& v) { return &v[0]; }
    static int size (vector_type& v) { return v.dim(); }
    static int stride (vector_type& v) { return 1; }
  };

  // TNT::Fortran_Array1D<>
  template <typename T, typename V>
  struct vector_detail_traits<TNT::Fortran_Array1D<T>, V>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         TNT::Fortran_Array1D<T>, typename boost::remove_const<V>::type
       >::value) );
#endif

    typedef TNT::Fortran_Array1D<T> identifier_type;
    typedef V vector_type;
    typedef T value_type;
    typedef typename detail::generate_const<V,T>::type* pointer;

    static pointer storage (vector_type& v) { return &v(1); }
    static int size (vector_type& v) { return v.dim(); }
    static int stride (vector_type& v) { return 1; }
  };

  // TNT::Array2D<>
  template <typename T, typename M>
  struct matrix_detail_traits<TNT::Array2D<T>, M>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         TNT::Array2D<T>, typename boost::remove_const<M>::type
       >::value) );
#endif

    typedef TNT::Array2D<T> identifier_type;
    typedef M matrix_type;
    typedef general_t matrix_structure;
    typedef row_major_t ordering_type;

    typedef T value_type;
    typedef typename detail::generate_const<M,T>::type* pointer;

    static pointer storage (matrix_type& m) { return m[0]; }
    static int size1 (matrix_type& m) { return m.dim1(); }
    static int size2 (matrix_type& m) { return m.dim2(); }
    static int storage_size (matrix_type& m) {
      return size1 (m) * size2 (m);
    }
    static int leading_dimension (matrix_type& m) { return m.dim2(); }
  };

  // TNT::Fortran_Array2D<>
  template <typename T, typename M>
  struct matrix_detail_traits<TNT::Fortran_Array2D<T>, M> {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         TNT::Fortran_Array2D<T>, typename boost::remove_const<M>::type
       >::value) );
#endif

    typedef TNT::Fortran_Array2D<T> identifier_type;
    typedef M matrix_type;
    typedef general_t matrix_structure;
    typedef column_major_t ordering_type;

    typedef T value_type;
    typedef typename detail::generate_const<M,T>::type* pointer;

    static pointer storage (matrix_type& m) { return &m(1, 1); }
    static int size1 (matrix_type& m) { return m.dim1(); }
    static int size2 (matrix_type& m) { return m.dim2(); }
    static int storage_size (matrix_type& m) {
      return size1 (m) * size2 (m);
    }
    static int leading_dimension (matrix_type& m) { return m.dim1(); }
  };

}}}}

#else // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#error with your compiler TNT cannot be used in bindings

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_TNT_H


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_STEIN_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_STEIN_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for stein is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t stein( const fortran_int_t n, const float* d,
        const float* e, const fortran_int_t m, const float* w,
        const fortran_int_t* iblock, const fortran_int_t* isplit, float* z,
        const fortran_int_t ldz, float* work, fortran_int_t* iwork,
        fortran_int_t* ifail ) {
    fortran_int_t info(0);
    LAPACK_SSTEIN( &n, d, e, &m, w, iblock, isplit, z, &ldz, work, iwork,
            ifail, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t stein( const fortran_int_t n, const double* d,
        const double* e, const fortran_int_t m, const double* w,
        const fortran_int_t* iblock, const fortran_int_t* isplit, double* z,
        const fortran_int_t ldz, double* work, fortran_int_t* iwork,
        fortran_int_t* ifail ) {
    fortran_int_t info(0);
    LAPACK_DSTEIN( &n, d, e, &m, w, iblock, isplit, z, &ldz, work, iwork,
            ifail, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
inline std::ptrdiff_t stein( const fortran_int_t n, const float* d,
        const float* e, const fortran_int_t m, const float* w,
        const fortran_int_t* iblock, const fortran_int_t* isplit,
        std::complex<float>* z, const fortran_int_t ldz, float* work,
        fortran_int_t* iwork, fortran_int_t* ifail ) {
    fortran_int_t info(0);
    LAPACK_CSTEIN( &n, d, e, &m, w, iblock, isplit, z, &ldz, work, iwork,
            ifail, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
inline std::ptrdiff_t stein( const fortran_int_t n, const double* d,
        const double* e, const fortran_int_t m, const double* w,
        const fortran_int_t* iblock, const fortran_int_t* isplit,
        std::complex<double>* z, const fortran_int_t ldz, double* work,
        fortran_int_t* iwork, fortran_int_t* ifail ) {
    fortran_int_t info(0);
    LAPACK_ZSTEIN( &n, d, e, &m, w, iblock, isplit, z, &ldz, work, iwork,
            ifail, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to stein.
//
template< typename Value, typename Enable = void >
struct stein_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct stein_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorD, typename VectorE, typename VectorW,
            typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
            typename VectorIFAIL, typename WORK, typename IWORK >
    static std::ptrdiff_t invoke( const fortran_int_t n, const VectorD& d,
            const VectorE& e, const fortran_int_t m, const VectorW& w,
            const VectorIBLOCK& iblock, const VectorISPLIT& isplit,
            MatrixZ& z, VectorIFAIL& ifail, detail::workspace2< WORK,
            IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixZ >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorW >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixZ >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorIBLOCK >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorISPLIT >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorIBLOCK >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorIFAIL >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixZ >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorIFAIL >::value) );
        BOOST_ASSERT( bindings::size(d) >= n );
        BOOST_ASSERT( bindings::size(e) >= n-1 );
        BOOST_ASSERT( bindings::size(isplit) >= n );
        BOOST_ASSERT( bindings::size(w) >= n );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( n ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( n ));
        BOOST_ASSERT( bindings::size_minor(z) == 1 ||
                bindings::stride_minor(z) == 1 );
        BOOST_ASSERT( bindings::stride_major(z) >= std::max< std::ptrdiff_t >(1,
                n) );
        BOOST_ASSERT( n >= 0 );
        return detail::stein( n, bindings::begin_value(d),
                bindings::begin_value(e), m, bindings::begin_value(w),
                bindings::begin_value(iblock), bindings::begin_value(isplit),
                bindings::begin_value(z), bindings::stride_major(z),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())),
                bindings::begin_value(ifail) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename VectorD, typename VectorE, typename VectorW,
            typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
            typename VectorIFAIL >
    static std::ptrdiff_t invoke( const fortran_int_t n, const VectorD& d,
            const VectorE& e, const fortran_int_t m, const VectorW& w,
            const VectorIBLOCK& iblock, const VectorISPLIT& isplit,
            MatrixZ& z, VectorIFAIL& ifail, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< real_type > tmp_work( min_size_work( n ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( n ) );
        return invoke( n, d, e, m, w, iblock, isplit, z, ifail,
                workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename VectorD, typename VectorE, typename VectorW,
            typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
            typename VectorIFAIL >
    static std::ptrdiff_t invoke( const fortran_int_t n, const VectorD& d,
            const VectorE& e, const fortran_int_t m, const VectorW& w,
            const VectorIBLOCK& iblock, const VectorISPLIT& isplit,
            MatrixZ& z, VectorIFAIL& ifail, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        return invoke( n, d, e, m, w, iblock, isplit, z, ifail,
                minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return 5*n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t n ) {
        return n;
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct stein_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorD, typename VectorE, typename VectorW,
            typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
            typename VectorIFAIL, typename WORK, typename IWORK >
    static std::ptrdiff_t invoke( const fortran_int_t n, const VectorD& d,
            const VectorE& e, const fortran_int_t m, const VectorW& w,
            const VectorIBLOCK& iblock, const VectorISPLIT& isplit,
            MatrixZ& z, VectorIFAIL& ifail, detail::workspace2< WORK,
            IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixZ >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorD >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorW >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorIBLOCK >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorISPLIT >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorIBLOCK >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorIFAIL >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixZ >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorIFAIL >::value) );
        BOOST_ASSERT( bindings::size(d) >= n );
        BOOST_ASSERT( bindings::size(e) >= n-1 );
        BOOST_ASSERT( bindings::size(isplit) >= n );
        BOOST_ASSERT( bindings::size(w) >= n );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( n ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( n ));
        BOOST_ASSERT( bindings::size_minor(z) == 1 ||
                bindings::stride_minor(z) == 1 );
        BOOST_ASSERT( bindings::stride_major(z) >= std::max< std::ptrdiff_t >(1,
                n) );
        BOOST_ASSERT( n >= 0 );
        return detail::stein( n, bindings::begin_value(d),
                bindings::begin_value(e), m, bindings::begin_value(w),
                bindings::begin_value(iblock), bindings::begin_value(isplit),
                bindings::begin_value(z), bindings::stride_major(z),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())),
                bindings::begin_value(ifail) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename VectorD, typename VectorE, typename VectorW,
            typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
            typename VectorIFAIL >
    static std::ptrdiff_t invoke( const fortran_int_t n, const VectorD& d,
            const VectorE& e, const fortran_int_t m, const VectorW& w,
            const VectorIBLOCK& iblock, const VectorISPLIT& isplit,
            MatrixZ& z, VectorIFAIL& ifail, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< real_type > tmp_work( min_size_work( n ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( n ) );
        return invoke( n, d, e, m, w, iblock, isplit, z, ifail,
                workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename VectorD, typename VectorE, typename VectorW,
            typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
            typename VectorIFAIL >
    static std::ptrdiff_t invoke( const fortran_int_t n, const VectorD& d,
            const VectorE& e, const fortran_int_t m, const VectorW& w,
            const VectorIBLOCK& iblock, const VectorISPLIT& isplit,
            MatrixZ& z, VectorIFAIL& ifail, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        return invoke( n, d, e, m, w, iblock, isplit, z, ifail,
                minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return 5*n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t n ) {
        return n;
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the stein_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for stein. Its overload differs for
// * User-defined workspace
//
template< typename VectorD, typename VectorE, typename VectorW,
        typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
        typename VectorIFAIL, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
stein( const fortran_int_t n, const VectorD& d, const VectorE& e,
        const fortran_int_t m, const VectorW& w,
        const VectorIBLOCK& iblock, const VectorISPLIT& isplit, MatrixZ& z,
        VectorIFAIL& ifail, Workspace work ) {
    return stein_impl< typename bindings::value_type<
            MatrixZ >::type >::invoke( n, d, e, m, w, iblock, isplit, z,
            ifail, work );
}

//
// Overloaded function for stein. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename VectorD, typename VectorE, typename VectorW,
        typename VectorIBLOCK, typename VectorISPLIT, typename MatrixZ,
        typename VectorIFAIL >
inline typename boost::disable_if< detail::is_workspace< VectorIFAIL >,
        std::ptrdiff_t >::type
stein( const fortran_int_t n, const VectorD& d, const VectorE& e,
        const fortran_int_t m, const VectorW& w,
        const VectorIBLOCK& iblock, const VectorISPLIT& isplit, MatrixZ& z,
        VectorIFAIL& ifail ) {
    return stein_impl< typename bindings::value_type<
            MatrixZ >::type >::invoke( n, d, e, m, w, iblock, isplit, z,
            ifail, optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


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

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#include <iostream>
#include <cctype>
#include <boost/detail/lightweight_test.hpp>
#include <boost/spirit/home/support/char_set/range_run.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/integer_traits.hpp>
#if defined(_MSC_VER) && _MSC_VER < 1700
# pragma warning(disable: 4127) // conditional expression is constant
#endif
#include <boost/random.hpp>

#if defined(_MSC_VER)
# pragma warning(disable: 4127) // conditional expression is constant
# pragma warning(disable: 4800) // 'int' : forcing value to bool 'true' or 'false' warning
#endif

template <typename Char>
void acid_test()
{
    if (sizeof(Char) == sizeof(unsigned))
        return; // don't do this test if we have a Char that's very big.
                // the smaller chars will suffice for testing.

    using boost::spirit::support::detail::range_run;
    using boost::spirit::support::detail::range;

    typedef boost::integer_traits<Char> integer_traits;
    Char const const_min = integer_traits::const_min;
    Char const const_max = integer_traits::const_max;
    unsigned bit_set_size = unsigned(const_max)-unsigned(const_min)+1;
    int const test_size = 1000;

    boost::mt19937 rng;
    Char min = const_min;
    Char max = const_max;
    boost::uniform_int<Char> char_(min, max);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<Char> >
       gen(rng, char_);
    boost::uniform_int<Char> _1of10(1, 10);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<Char> >
       on_or_off(rng, _1of10);

    range_run<Char> rr;
    boost::dynamic_bitset<unsigned> bset(bit_set_size);

    for (int i = 0; i < test_size; ++i)
    {
        range<Char> r = range<Char>(gen(), gen());
        if (r.first > r.last)
            std::swap(r.first, r.last);

        bool set = on_or_off() != 1;
        if (set)
            rr.set(r);
        else
            rr.clear(r);
        for (int j = r.first; j <= int(r.last); ++j)
            bset[j-const_min] = set;
    }

    for (int i = const_min; i <= int(const_max); ++i)
    {
        BOOST_TEST(rr.test(static_cast<Char>(i)) == bset[i-const_min]);
    }
}

int
main()
{
    using boost::spirit::support::detail::range_run;
    using boost::spirit::support::detail::range;

    {
        range_run<char> rr;
        rr.set(range<char>('a', 'a'));
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST((c == 'a') == rr.test(c));
        }
    }
    {
        range_run<char> rr;
        rr.set(range<char>('a', 'z'));
        rr.set(range<char>('A', 'Z'));
        rr.clear(range<char>('A', 'Z'));
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST(bool(std::islower(c)) == rr.test(c));
        }
    }
    {
        range_run<char> rr;
        rr.set(range<char>(0, 0));
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST((c == 0) == rr.test(c));
        }
        rr.set(range<char>(0, 50));
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST(((c >= 0) && (c <= 50)) == rr.test(c));
        }
    }
    {
        range_run<unsigned char> rr;
        rr.set(range<unsigned char>(255, 255));
        for (unsigned char c = 0; c < 255; ++c)
        {
            BOOST_TEST((c == 255) == rr.test(c));
        }
        rr.set(range<unsigned char>(250, 255));
        for (unsigned char c = 0; c < 255; ++c)
        {
            BOOST_TEST((c >= 250) == rr.test(c));
        }
    }
    {
        range_run<char> rr;
        rr.set(range<char>('a', 'z'));
        rr.set(range<char>('A', 'Z'));
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST(bool(std::isalpha(c)) == rr.test(c));
        }
    }
    {
        range_run<char> rr;
        rr.set(range<char>('a', 'z'));
        rr.set(range<char>('A', 'Z'));
        rr.clear(range<char>('J', 'j'));
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST((bool(std::isalpha(c)) && (c < 'J' || c > 'j')) == rr.test(c));
        }
    }
    {
        range_run<char> rr;
        rr.set(range<char>(3, 3));
        rr.set(range<char>(1, 5));
        BOOST_TEST(rr.test(5));
    }
    {
        range_run<char> rr;
        for (char c = 0; c < 127; ++c)
        {
            if (c & 1)
            {
                rr.set(range<char>(c, c));
            }
        }
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST(bool((c & 1)) == rr.test(c));
        }
        rr.clear(range<char>(90, 105));
        for (char c = 0; c < 127; ++c)
        {
            BOOST_TEST((bool((c & 1)) && (c < 90 || c > 105)) == rr.test(c));
        }
    }
    {
        range_run<char> rr;
        rr.set(range<char>('c', 'e'));
        rr.set(range<char>('g', 'i'));
        rr.set(range<char>('d', 'k'));
        for (char c = 'a'; c <= 'm'; ++c)
        {
            BOOST_TEST((c >= 'c' && c <= 'k') == rr.test(c));
        }
    }
    {
        typedef boost::integer_traits<char> traits;
        char const const_min = traits::const_min;
        range_run<char> rr;
        rr.set(range<char>(const_min, const_min+16));
        rr.clear(range<char>(const_min, const_min+8));
        BOOST_TEST(!rr.test(const_min));
        BOOST_TEST(!rr.test(const_min+8));
        BOOST_TEST(rr.test(const_min+9));
        BOOST_TEST(rr.test(const_min+16));
        BOOST_TEST(!rr.test(const_min+17));
    }
    {
        acid_test<char>();
        acid_test<signed char>();
        acid_test<unsigned char>();
        acid_test<wchar_t>();
        acid_test<short>();
        acid_test<signed short>();
        acid_test<unsigned short>();
    }

    return boost::report_errors();
}


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

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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_GEOMETRY_TO_SEGMENT_OR_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_GEOMETRY_TO_SEGMENT_OR_BOX_HPP

#include <iterator>

#include <boost/range.hpp>

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

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

#include <boost/geometry/iterators/point_iterator.hpp>
#include <boost/geometry/iterators/segment_iterator.hpp>

#include <boost/geometry/algorithms/dispatch/distance.hpp>

#include <boost/geometry/algorithms/detail/closest_feature/geometry_to_range.hpp>
#include <boost/geometry/algorithms/detail/closest_feature/point_to_range.hpp>

#include <boost/geometry/algorithms/detail/distance/is_comparable.hpp>

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

// closure of segment or box point range
template
<
    typename SegmentOrBox,
    typename Tag = typename tag<SegmentOrBox>::type
>
struct segment_or_box_point_range_closure
    : not_implemented<SegmentOrBox>
{};

template <typename Segment>
struct segment_or_box_point_range_closure<Segment, segment_tag>
{
    static const closure_selector value = closed;
};

template <typename Box>
struct segment_or_box_point_range_closure<Box, box_tag>
{
    static const closure_selector value = open;
};

template
<
    typename Geometry,
    typename SegmentOrBox,
    typename Strategy,
    typename Tag = typename tag<Geometry>::type
>
class geometry_to_segment_or_box
{
private:
    typedef typename point_type<SegmentOrBox>::type segment_or_box_point;

    typedef typename strategy::distance::services::comparable_type
       <
           Strategy
       >::type comparable_strategy;

    typedef detail::closest_feature::point_to_point_range
        <
            typename point_type<Geometry>::type,
            std::vector<segment_or_box_point>,
            segment_or_box_point_range_closure<SegmentOrBox>::value,
            comparable_strategy
        > point_to_point_range;

    typedef detail::closest_feature::geometry_to_range geometry_to_range;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy,
            typename point_type<Geometry>::type,
            segment_or_box_point
        >::type comparable_return_type;

    // assign the new minimum value for an iterator of the point range
    // of a segment or a box
    template
    <
        typename SegOrBox,
        typename SegOrBoxTag = typename tag<SegOrBox>::type
    >
    struct assign_new_min_iterator
        : not_implemented<SegOrBox>
    {};

    template <typename Segment>
    struct assign_new_min_iterator<Segment, segment_tag>
    {
        template <typename Iterator>
        static inline void apply(Iterator&, Iterator)
        {
        }
    };

    template <typename Box>
    struct assign_new_min_iterator<Box, box_tag>
    {
        template <typename Iterator>
        static inline void apply(Iterator& it_min, Iterator it)
        {
            it_min = it;
        }
    };

    // assign the points of a segment or a box to a range
    template
    <
        typename SegOrBox,
        typename PointRange,
        typename SegOrBoxTag = typename tag<SegOrBox>::type
    >
    struct assign_segment_or_box_points
    {};

    template <typename Segment, typename PointRange>
    struct assign_segment_or_box_points<Segment, PointRange, segment_tag>
    {
        static inline void apply(Segment const& segment, PointRange& range)
        {
            detail::assign_point_from_index<0>(segment, range[0]);
            detail::assign_point_from_index<1>(segment, range[1]);
        }
    };

    template <typename Box, typename PointRange>
    struct assign_segment_or_box_points<Box, PointRange, box_tag>
    {
        static inline void apply(Box const& box, PointRange& range)
        {
            detail::assign_box_corners_oriented<true>(box, range);
        }
    };

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy,
            typename point_type<Geometry>::type,
            segment_or_box_point
        >::type return_type;

    static inline return_type apply(Geometry const& geometry,
                                    SegmentOrBox const& segment_or_box,
                                    Strategy const& strategy,
                                    bool check_intersection = true)
    {
        typedef geometry::point_iterator<Geometry const> point_iterator_type;
        typedef geometry::segment_iterator
            <
                Geometry const
            > segment_iterator_type;

        typedef typename std::vector
            <
                segment_or_box_point
            >::const_iterator seg_or_box_iterator_type;

        typedef assign_new_min_iterator<SegmentOrBox> assign_new_value;

        if (check_intersection
            && geometry::intersects(geometry, segment_or_box))
        {
            return 0;
        }

        comparable_strategy cstrategy =
            strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        // get all points of the segment or the box
        std::vector<segment_or_box_point>
            seg_or_box_points(geometry::num_points(segment_or_box));

        assign_segment_or_box_points
            <
                SegmentOrBox,
                std::vector<segment_or_box_point>
            >::apply(segment_or_box, seg_or_box_points);

        // consider all distances of the points in the geometry to the
        // segment or box
        comparable_return_type cd_min1(0);
        point_iterator_type pit_min;
        seg_or_box_iterator_type it_min1 = seg_or_box_points.begin();
        seg_or_box_iterator_type it_min2 = ++seg_or_box_points.begin();
        bool first = true;

        for (point_iterator_type pit = points_begin(geometry);
             pit != points_end(geometry); ++pit, first = false)
        {
            comparable_return_type cd;
            std::pair
                <
                    seg_or_box_iterator_type, seg_or_box_iterator_type
                > it_pair
                = point_to_point_range::apply(*pit,
                                              seg_or_box_points.begin(),
                                              seg_or_box_points.end(),
                                              cstrategy,
                                              cd);

            if (first || cd < cd_min1)
            {
                cd_min1 = cd;
                pit_min = pit;
                assign_new_value::apply(it_min1, it_pair.first);
                assign_new_value::apply(it_min2, it_pair.second);
            }
        }

        // consider all distances of the points in the segment or box to the
        // segments of the geometry
        comparable_return_type cd_min2(0);
        segment_iterator_type sit_min;
        typename std::vector<segment_or_box_point>::const_iterator it_min;

        first = true;
        for (typename std::vector<segment_or_box_point>::const_iterator it
                 = seg_or_box_points.begin();
             it != seg_or_box_points.end(); ++it, first = false)
        {
            comparable_return_type cd;
            segment_iterator_type sit
                = geometry_to_range::apply(*it,
                                           segments_begin(geometry),
                                           segments_end(geometry),
                                           cstrategy,
                                           cd);

            if (first || cd < cd_min2)
            {
                cd_min2 = cd;
                it_min = it;
                sit_min = sit;
            }
        }

        if (BOOST_GEOMETRY_CONDITION(is_comparable<Strategy>::value))
        {
            return (std::min)(cd_min1, cd_min2);
        }

        if (cd_min1 < cd_min2)
        {
            return strategy.apply(*pit_min, *it_min1, *it_min2);
        }
        else
        {
            return dispatch::distance
                <
                    segment_or_box_point,
                    typename std::iterator_traits
                        <
                            segment_iterator_type
                        >::value_type,
                    Strategy
                >::apply(*it_min, *sit_min, strategy);
        }
    }

    static inline return_type
    apply(SegmentOrBox const& segment_or_box, Geometry const& geometry,
          Strategy const& strategy, bool check_intersection = true)
    {
        return apply(geometry, segment_or_box, strategy, check_intersection);
    }
};

template <typename MultiPoint, typename SegmentOrBox, typename Strategy>
class geometry_to_segment_or_box
    <
        MultiPoint, SegmentOrBox, Strategy, multi_point_tag
    >
{
private:
    typedef detail::closest_feature::geometry_to_range base_type;

    typedef typename boost::range_iterator
        <
            MultiPoint const
        >::type iterator_type;

    typedef detail::closest_feature::geometry_to_range geometry_to_range;

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy,
            typename point_type<SegmentOrBox>::type,
            typename point_type<MultiPoint>::type
        >::type return_type;

    static inline return_type apply(MultiPoint const& multipoint,
                                    SegmentOrBox const& segment_or_box,
                                    Strategy const& strategy)
    {
        namespace sds = strategy::distance::services;

        typename sds::return_type
            <
                typename sds::comparable_type<Strategy>::type,
                typename point_type<SegmentOrBox>::type,
                typename point_type<MultiPoint>::type
            >::type cd_min;

        iterator_type it_min
            = geometry_to_range::apply(segment_or_box,
                                       boost::begin(multipoint),
                                       boost::end(multipoint),
                                       sds::get_comparable
                                           <
                                               Strategy
                                           >::apply(strategy),
                                       cd_min);

        return
            is_comparable<Strategy>::value
            ?
            cd_min
            :
            dispatch::distance
                <
                    typename point_type<MultiPoint>::type,
                    SegmentOrBox,
                    Strategy
                >::apply(*it_min, segment_or_box, strategy);
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Linear, typename Segment, typename Strategy>
struct distance
    <
        Linear, Segment, Strategy, linear_tag, segment_tag,
        strategy_tag_distance_point_segment, false
    > : detail::distance::geometry_to_segment_or_box<Linear, Segment, Strategy>
{};

template <typename Areal, typename Segment, typename Strategy>
struct distance
    <
        Areal, Segment, Strategy, areal_tag, segment_tag,
        strategy_tag_distance_point_segment, false
    > : detail::distance::geometry_to_segment_or_box<Areal, Segment, Strategy>
{};

template <typename Segment, typename Areal, typename Strategy>
struct distance
    <
        Segment, Areal, Strategy, segment_tag, areal_tag,
        strategy_tag_distance_point_segment, false
    > : detail::distance::geometry_to_segment_or_box<Areal, Segment, Strategy>
{};

template <typename Linear, typename Box, typename Strategy>
struct distance
    <
        Linear, Box, Strategy, linear_tag, box_tag,
        strategy_tag_distance_point_segment, false
    > : detail::distance::geometry_to_segment_or_box
        <
            Linear, Box, Strategy
        >
{};

template <typename Areal, typename Box, typename Strategy>
struct distance
    <
        Areal, Box, Strategy, areal_tag, box_tag,
        strategy_tag_distance_point_segment, false
    > : detail::distance::geometry_to_segment_or_box<Areal, Box, Strategy>
{};

template <typename MultiPoint, typename Segment, typename Strategy>
struct distance
    <
        MultiPoint, Segment, Strategy,
        multi_point_tag, segment_tag,
        strategy_tag_distance_point_segment, false
    > : detail::distance::geometry_to_segment_or_box
        <
            MultiPoint, Segment, Strategy
        >
{};

template <typename MultiPoint, typename Box, typename Strategy>
struct distance
    <
        MultiPoint, Box, Strategy,
        multi_point_tag, box_tag,
        strategy_tag_distance_point_box, false
    > : detail::distance::geometry_to_segment_or_box
        <
            MultiPoint, Box, Strategy
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_GEOMETRY_TO_SEGMENT_OR_BOX_HPP


#include <iostream>
#include <functional>
#include <chrono>
#include <thread>

#include <Eigen/Eigen>

#include <pangolin/pangolin.h>
#include <SceneGraph/SceneGraph.h>

using namespace std;

template<typename T>
void setRandomImageData(T* imageArray, int width, int height, int channels){
  for(int i = 0 ; i < channels*width*height;i++) {
    imageArray[i] = std::numeric_limits<T>::max() * ((float)rand()/RAND_MAX);
  }
}

// Draw in pixel units (center of image pixels)
struct ExampleDrawSomethingInPixelCoords
{
    void doTest( void )
    {
        pangolin::GlState glstate;
        glstate.glShadeModel(GL_FLAT);
        GLfloat verts[] = {0, 0, 0,20, 20, 20};
        GLfloat colors[] = {1,0,0,1, 0,1,0,1, 0,0,1,1};
        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_COLOR_ARRAY);
        glVertexPointer(2, GL_FLOAT, 0, verts);
        glColorPointer(4, GL_FLOAT, 0, colors);
        glDrawArrays(GL_TRIANGLES, 0, 3);
        glDisableClientState(GL_VERTEX_ARRAY);
        glDisableClientState(GL_COLOR_ARRAY);
    }

    void doTest2( void )
    {
        GLfloat verts[] = {20, 20, 20,0, 0, 0};
        GLfloat colors[] = {1,0,0,1, 0,1,0,1, 0,0,1,1};
        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_COLOR_ARRAY);
        glVertexPointer(2, GL_FLOAT, 0, verts);
        glColorPointer(4, GL_FLOAT, 0, colors);
        glDrawArrays(GL_TRIANGLES, 0, 3);
        glDisableClientState(GL_VERTEX_ARRAY);
        glDisableClientState(GL_COLOR_ARRAY);
    }

    void operator()(pangolin::View&) {
        glColor3f(1,1,1);
        pangolin::glDrawRectPerimeter(10,10, 40,40);
        doTest();
        doTest2();
    }
};

void GlobalKeyHook(std::string str)
{
    cout << str << endl;
}

int main( int /*argc*/, char** /*argv[]*/ )
{
    // Create OpenGL window in single line thanks to GLUT
    pangolin::CreateWindowAndBind("Main",640*2,480);
    SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
    glClearColor(0, 0, 0, 0);

    // Scenegraph to hold GLObjects and relative transformations
    SceneGraph::GLSceneGraph glGraph;

    SceneGraph::GLLight light(10,10,-100);
    glGraph.AddChild(&light);

    // Define grid object
    SceneGraph::GLGrid glGrid( 50, 2.0, true );
    glGraph.AddChild(&glGrid);

    // Define axis object, and set its pose
    SceneGraph::GLAxis glAxis;
    glAxis.SetPose(-1,-2,-1, 0, 0, M_PI/4);
    glAxis.SetScale(0.25);
    glGraph.AddChild(&glAxis);

    // Define 3D spiral using a line strip object
    SceneGraph::GLCachedPrimitives glSpiral(GL_LINE_STRIP, SceneGraph::GLColor(1.0f,0.7f,0.2f));
    for(double t=0; t < 10*M_PI; t+= M_PI/50) {
        glSpiral.AddVertex(Eigen::Vector3d(cos(t)+2, sin(t)+2, -0.1*t) );
    }
    glGraph.AddChild(&glSpiral);

    SceneGraph::GLWireSphere sphere(5);
    sphere.SetPose(5, 6, -7, M_PI / 3, M_PI / 3, M_PI / 3);
    glGraph.AddChild(&sphere);

#ifndef HAVE_GLES
    // Define 3D floating text object
    SceneGraph::GLText glText3d("3D Floating Text", -1, 1, -1);
    glGraph.AddChild(&glText3d);
#endif

    // Synthetic random image for demonstration
    const int w = 64;
    const int h = 48;
    unsigned char uImage[w*h*3];
    setRandomImageData(uImage,w,h,3);

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState stacks3d(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,0.1,1000),
        pangolin::ModelViewLookAt(0,-2,-4, 0,1,0, pangolin::AxisNegZ)
    );

    // Pangolin abstracts the OpenGL viewport as a View.
    // Here we get a reference to the default 'base' view.
    pangolin::View& container = pangolin::DisplayBase();

    // We define a new view which will reside within the container.
    pangolin::View view3d;

    // We set the views location on screen and add a handler which will
    // let user input update the model_view matrix (stacks3d) and feed through
    // to our scenegraph
    view3d.SetBounds(0.0, 1.0, 0.0, 1.0/2.0, 640.0f/480.0f)
          .SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d,pangolin::AxisNegZ))
          .SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));

    // We define a special type of view which will accept image data
    // to display and set its bounds on screen.
    SceneGraph::ImageView viewImage(true,false);
    viewImage.SetBounds(0.0, 1.0, 1.0/2.0, 1.0, (double)w/h);
    viewImage.SetDrawFunction(ExampleDrawSomethingInPixelCoords());

    // Add our views as children to the base container.
    container.AddDisplay(view3d);
    container.AddDisplay(viewImage);

    // Demonstration of how we can register a keyboard hook to trigger a method
    pangolin::RegisterKeyPressCallback( pangolin::PANGO_CTRL + 'r', std::bind(GlobalKeyHook, "You Pushed ctrl-r!" ) );

//    // Default hooks for exiting (Esc) and fullscreen (tab).
    while( !pangolin::ShouldQuit() )
    {
        // Clear whole screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // These calls can safely be made outside of the OpenGL thread.
        setRandomImageData(uImage,w,h,3);
        viewImage.SetImage(uImage, w,h, GL_RGB, GL_RGB, GL_UNSIGNED_BYTE);

        // Swap frames and Process Events
        pangolin::FinishFrame();

        // Pause for 1/60th of a second.
        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 60));
    }

    return 0;
}


// Modified from NetKet source for YANNQ Project
// <Chae-Yeun Park>(chae.yeun.park@gmail.com)
// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <complex>
#include <fstream>
#include <random>
#include <vector>

#include <Eigen/Dense>
#include "nlohmann/json.hpp"

namespace yannq {
/**
  Abstract class for Neural Network layer.
*/

template<typename T>
class AbstractLayer {
public:
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	/**
	  Member function returning the name of the layer.
	  @return Name of Layer.
	  */
	virtual std::string name() const = 0;

	/**
	  Member function returning the number of variational parameters.
	  @return Number of variational parameters in the Layer.
	  */
	virtual uint32_t paramDim() const = 0;

	virtual uint32_t fanIn() { return 0; }
	virtual uint32_t fanOut() { return 0; }

	virtual uint32_t outputDim(uint32_t inputDim) const = 0;

	/**
	  Member function writing the current set of parameters in the machine.
	  @param pars is where the layer parameters are written into.
	  @param start_idx is the index of the vector pars to start writing from.
	  */
	virtual Vector getParams() const = 0;

	/**
	  Member function setting the current set of parameters in the layer.
	  @param pars is where the layer parameters are to be read from.
	  @param start_idx is the index of the vector pars to start reading from.
	  */
	virtual void setParams(VectorConstRef pars) = 0;

	virtual void updateParams(VectorConstRef pars) { (void)pars; return ; }

	/**
	  Member function to feedforward through the layer.
	  @param input a constant reference to the input to the layer
	  @param output reference to the output vector. Must have the size of outputDim.
	  */
	virtual void forward(const VectorConstRef& input, VectorRef output) = 0;

	/**
	  Member function to perform backpropagation to compute derivates.
	  Internal stride of din and der must be 1.

	  @param prev_layer_output a constant reference to the output from previous
	  layer.
	  @param this_layer_output a constant reference to the output from the current
	  layer.
	  @param next_layer_data a constant reference to the derivative dL/dA where A is
	  the activations of the current layer and L is the the final output of the
	  Machine: L = log(psi(v))
	  @param din a constant reference to the derivative of the input from the
	  current layer.
	  @param der a constant reference to the derivatives wrt to the parameters in
	  the machine.
	*/
	virtual void backprop(const VectorConstRef& prev_layer_output,
			const VectorConstRef& this_layer_output,
			const VectorConstRef& dout,
			VectorRef din,
			VectorRef der) = 0;

	virtual nlohmann::json desc() const = 0;

	/**
	  destructor
	  */
	virtual ~AbstractLayer() {}
};
}  // namespace yannq


// (-*- C++ -*- header)

/*
 *
 * Copyright (c) Kresimir Fresl 2003
 * Copyright (c) Georg Baum 2004
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_SUPERLU_OVERLOADS_H
#define BOOST_NUMERIC_BINDINGS_SUPERLU_OVERLOADS_H

#include <cassert>
#include <complex>
#include <boost/numeric/bindings/superlu/superlu_decl.hpp>

namespace boost { namespace numeric { namespace bindings {

  namespace superlu { namespace detail {

    void gssv (float, superlu_options_t* options, SuperMatrix* A, int* perm_c, int* perm_r,
               SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      sgssv (options, A, perm_c, perm_r, L, U, B, stat, info);
    }

    void gssv (double, superlu_options_t* options, SuperMatrix* A, int* perm_c, int* perm_r,
               SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      dgssv (options, A, perm_c, perm_r, L, U, B, stat, info);
    }

    void gssv (std::complex<float>, superlu_options_t* options, SuperMatrix* A, int* perm_c, int* perm_r,
               SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      cgssv (options, A, perm_c, perm_r, L, U, B, stat, info);
    }

    void gssv (std::complex<double>, superlu_options_t* options, SuperMatrix* A, int* perm_c, int* perm_r,
               SuperMatrix* L, SuperMatrix* U, SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      zgssv (options, A, perm_c, perm_r, L, U, B, stat, info);
    }

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

    inline
    void Create_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                float* nzval, int* rowind, int* colptr,
                                Stype_t stype = SLU_NC,
                                Dtype_t dtype = SLU_S,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NC);
      assert (dtype == SLU_S);
      assert (mtype == SLU_GE);
      sCreate_CompCol_Matrix (A, m, n, nnz, nzval, rowind, colptr,
                              SLU_NC, SLU_S, SLU_GE);
    }

    inline
    void Create_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                float* nzval, int* colind, int* rowptr,
                                Stype_t stype = SLU_NR,
                                Dtype_t dtype = SLU_S,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NR);
      assert (dtype == SLU_S);
      assert (mtype == SLU_GE);
      sCreate_CompRow_Matrix (A, m, n, nnz, nzval, colind, rowptr,
                              SLU_NR, SLU_S, SLU_GE);
    }

    inline
    void Create_Dense_Matrix (SuperMatrix* A, int m, int n,
                              float* vals, int ld_vals,
                              Stype_t stype = SLU_DN,
                              Dtype_t dtype = SLU_S,
                              Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_DN);
      assert (dtype == SLU_S);
      assert (mtype == SLU_GE);
      sCreate_Dense_Matrix (A, m, n, vals, ld_vals, SLU_DN, SLU_S, SLU_GE);
    }

    inline
    void Create_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                double* nzval, int* rowind, int* colptr,
                                Stype_t stype = SLU_NC,
                                Dtype_t dtype = SLU_D,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NC);
      assert (dtype == SLU_D);
      assert (mtype == SLU_GE);
      dCreate_CompCol_Matrix (A, m, n, nnz, nzval, rowind, colptr,
                              SLU_NC, SLU_D, SLU_GE);
    }

    inline
    void Create_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                double* nzval, int* colind, int* rowptr,
                                Stype_t stype = SLU_NR,
                                Dtype_t dtype = SLU_D,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NR);
      assert (dtype == SLU_D);
      assert (mtype == SLU_GE);
      dCreate_CompRow_Matrix (A, m, n, nnz, nzval, colind, rowptr,
                              SLU_NR, SLU_D, SLU_GE);
    }

    inline
    void Create_Dense_Matrix (SuperMatrix* A, int m, int n,
                              double* vals, int ld_vals,
                              Stype_t stype = SLU_DN,
                              Dtype_t dtype = SLU_D,
                              Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_DN);
      assert (dtype == SLU_D);
      assert (mtype == SLU_GE);
      dCreate_Dense_Matrix (A, m, n, vals, ld_vals, SLU_DN, SLU_D, SLU_GE);
    }

    inline
    void Create_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                std::complex<float>* nzval,
                                int* rowind, int* colptr,
                                Stype_t stype = SLU_NC,
                                Dtype_t dtype = SLU_C,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NC);
      assert (dtype == SLU_C);
      assert (mtype == SLU_GE);
      cCreate_CompCol_Matrix (A, m, n, nnz,
                              reinterpret_cast<flt_complex*> (nzval),
                              rowind, colptr,
                              SLU_NC, SLU_C, SLU_GE);
    }

    inline
    void Create_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                std::complex<float>* nzval,
                                int* colind, int* rowptr,
                                Stype_t stype = SLU_NR,
                                Dtype_t dtype = SLU_C,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NR);
      assert (dtype == SLU_C);
      assert (mtype == SLU_GE);
      cCreate_CompRow_Matrix (A, m, n, nnz,
                              reinterpret_cast<flt_complex*> (nzval),
                              colind, rowptr,
                              SLU_NR, SLU_C, SLU_GE);
    }

    inline
    void Create_Dense_Matrix (SuperMatrix* A, int m, int n,
                              std::complex<float>* vals, int ld_vals,
                              Stype_t stype = SLU_DN,
                              Dtype_t dtype = SLU_C,
                              Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_DN);
      assert (dtype == SLU_C);
      assert (mtype == SLU_GE);
      cCreate_Dense_Matrix (A, m, n,
                            reinterpret_cast<flt_complex*> (vals),
                            ld_vals, SLU_DN, SLU_C, SLU_GE);
    }

    inline
    void Create_CompCol_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                std::complex<double>* nzval,
                                int* rowind, int* colptr,
                                Stype_t stype = SLU_NC,
                                Dtype_t dtype = SLU_Z,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NC);
      assert (dtype == SLU_Z);
      assert (mtype == SLU_GE);
      zCreate_CompCol_Matrix (A, m, n, nnz,
                              reinterpret_cast<dbl_complex*> (nzval),
                              rowind, colptr,
                              SLU_NC, SLU_Z, SLU_GE);
    }

    inline
    void Create_CompRow_Matrix (SuperMatrix* A, int m, int n, int nnz,
                                std::complex<double>* nzval,
                                int* colind, int* rowptr,
                                Stype_t stype = SLU_NR,
                                Dtype_t dtype = SLU_Z,
                                Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_NR);
      assert (dtype == SLU_Z);
      assert (mtype == SLU_GE);
      zCreate_CompRow_Matrix (A, m, n, nnz,
                              reinterpret_cast<dbl_complex*> (nzval),
                              colind, rowptr,
                              SLU_NR, SLU_Z, SLU_GE);
    }

    inline
    void Create_Dense_Matrix (SuperMatrix* A, int m, int n,
                              std::complex<double>* vals, int ld_vals,
                              Stype_t stype = SLU_DN,
                              Dtype_t dtype = SLU_Z,
                              Mtype_t mtype = SLU_GE)
    {
      assert (stype == SLU_DN);
      assert (dtype == SLU_Z);
      assert (mtype == SLU_GE);
      zCreate_Dense_Matrix (A, m, n,
                            reinterpret_cast<dbl_complex*> (vals),
                            ld_vals, SLU_DN, SLU_Z, SLU_GE);
    }

    template<typename T> inline
    void Create_Comp_Matrix(traits::column_major_t, SuperMatrix* A, int m, int n,
                            int nnz, T* nzval, int* colptr, int* rowind)
    {
      Create_CompCol_Matrix (A, m, n, nnz, nzval, rowind, colptr);
    }

    template<typename T> inline
    void Create_Comp_Matrix(traits::row_major_t, SuperMatrix* A, int m, int n,
                            int nnz, T* nzval, int* rowptr, int* colind)
    {
      Create_CompRow_Matrix (A, m, n, nnz, nzval, colind, rowptr);
    }

    inline
    void gstrf (float, superlu_options_t* options, SuperMatrix* A, float drop_tol,
                int relax, int panel_size, int* etree, void* work, int lwork,
                int* perm_c, int* perm_r, SuperMatrix* L, SuperMatrix* U,
                SuperLUStat_t *stat, int *info)
    {
      sgstrf(options, A, drop_tol, relax, panel_size, etree, work, lwork, perm_c, perm_r, L, U, stat, info);
    }

    inline
    void gstrf (double, superlu_options_t* options, SuperMatrix* A, double drop_tol,
                int relax, int panel_size, int* etree, void* work, int lwork,
                int* perm_c, int* perm_r, SuperMatrix* L, SuperMatrix* U,
                SuperLUStat_t *stat, int *info)
    {
      dgstrf(options, A, drop_tol, relax, panel_size, etree, work, lwork, perm_c, perm_r, L, U, stat, info);
    }

    inline
    void gstrf (std::complex<float>, superlu_options_t* options, SuperMatrix* A, float drop_tol,
                int relax, int panel_size, int* etree, void* work, int lwork,
                int* perm_c, int* perm_r, SuperMatrix* L, SuperMatrix* U,
                SuperLUStat_t *stat, int *info)
    {
      cgstrf(options, A, drop_tol, relax, panel_size, etree, work, lwork, perm_c, perm_r, L, U, stat, info);
    }

    inline
    void gstrf (std::complex<double>, superlu_options_t* options, SuperMatrix* A, double drop_tol,
                int relax, int panel_size, int* etree, void* work, int lwork,
                int* perm_c, int* perm_r, SuperMatrix* L, SuperMatrix* U,
                SuperLUStat_t *stat, int *info)
    {
      zgstrf(options, A, drop_tol, relax, panel_size, etree, work, lwork, perm_c, perm_r, L, U, stat, info);
    }

    inline
    void gstrs (float, trans_t trans, SuperMatrix* L, SuperMatrix* U, int* perm_c, int* perm_r,
                SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      sgstrs(trans, L, U, perm_c, perm_r, B, stat, info);
    }

    inline
    void gstrs (double, trans_t trans, SuperMatrix* L, SuperMatrix* U, int* perm_c, int* perm_r,
                SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      dgstrs(trans, L, U, perm_c, perm_r, B, stat, info);
    }

    inline
    void gstrs (std::complex<float>, trans_t trans, SuperMatrix* L, SuperMatrix* U, int* perm_c, int* perm_r,
                SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      cgstrs(trans, L, U, perm_c, perm_r, B, stat, info);
    }

    inline
    void gstrs (std::complex<double>, trans_t trans, SuperMatrix* L, SuperMatrix* U, int* perm_c, int* perm_r,
                SuperMatrix* B, SuperLUStat_t* stat, int* info)
    {
      zgstrs(trans, L, U, perm_c, perm_r, B, stat, info);
    }

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

    void Print_CompCol_Matrix (float, char* what, SuperMatrix* A) {
      sPrint_CompCol_Matrix (what, A);
    }
    void Print_Dense_Matrix (float, char* what, SuperMatrix* A) {
      sPrint_Dense_Matrix (what, A);
    }
    void Print_SuperNode_Matrix (float, char* what, SuperMatrix* A) {
      sPrint_SuperNode_Matrix (what, A);
    }

    void Print_CompCol_Matrix (double, char* what, SuperMatrix* A) {
      dPrint_CompCol_Matrix (what, A);
    }
    void Print_Dense_Matrix (double, char* what, SuperMatrix* A) {
      dPrint_Dense_Matrix (what, A);
    }
    void Print_SuperNode_Matrix (double, char* what, SuperMatrix* A) {
      dPrint_SuperNode_Matrix (what, A);
    }

    void Print_CompCol_Matrix (std::complex<float>,
                               char* what, SuperMatrix* A)
    {
      cPrint_CompCol_Matrix (what, A);
    }
    void Print_Dense_Matrix (std::complex<float>,
                             char* what, SuperMatrix* A)
    {
      cPrint_Dense_Matrix (what, A);
    }
    void Print_SuperNode_Matrix (std::complex<float>,
                                 char* what, SuperMatrix* A)
    {
      cPrint_SuperNode_Matrix (what, A);
    }

    void Print_CompCol_Matrix (std::complex<double>,
                               char* what, SuperMatrix* A)
    {
      zPrint_CompCol_Matrix (what, A);
    }
    void Print_Dense_Matrix (std::complex<double>,
                             char* what, SuperMatrix* A)
    {
      zPrint_Dense_Matrix (what, A);
    }
    void Print_SuperNode_Matrix (std::complex<double>,
                                 char* what, SuperMatrix* A)
    {
      zPrint_SuperNode_Matrix (what, A);
    }
    template<typename T> inline
    void Print_Comp_Matrix (traits::column_major_t, T t,
                            char* what, SuperMatrix* A)
    {
      Print_CompCol_Matrix (t, what, A);
    }
    template<typename T>
    void Print_CompRow_Matrix(T, char *what, SuperMatrix *A)
    {
      NRformat     *Astore;
      register int i,n;
      T            *dp;

      printf("\nCompRow matrix %s:\n", what);
      printf("Stype %d, Dtype %d, Mtype %d\n", A->Stype,A->Dtype,A->Mtype);
      n = A->nrow;
      Astore = (NRformat *) A->Store;
      dp = (T *) Astore->nzval;
      printf("nrow %d, ncol %d, nnz %d\n", A->nrow,A->ncol,Astore->nnz);
      printf("nzval: ");
      for (i = 0; i < Astore->rowptr[n]; ++i) printf("%f  ", dp[i]);
      printf("\ncolind: ");
      for (i = 0; i < Astore->rowptr[n]; ++i) printf("%d  ", Astore->colind[i]);
      printf("\nrowptr: ");
      for (i = 0; i <= n; ++i) printf("%d  ", Astore->rowptr[i]);
      printf("\n");
      fflush(stdout);
    }
    template<typename T>
    void Print_CompRow_Matrix (std::complex<T>, char* what, SuperMatrix* A)
    {
      NRformat     *Astore;
      register int i,n;
      T            *dp;

      printf("\nCompRow matrix %s:\n", what);
      printf("Stype %d, Dtype %d, Mtype %d\n", A->Stype,A->Dtype,A->Mtype);
      n = A->nrow;
      Astore = (NRformat *) A->Store;
      dp = (T *) Astore->nzval;
      printf("nrow %d, ncol %d, nnz %d\n", A->nrow,A->ncol,Astore->nnz);
      printf("nzval: ");
      for (i = 0; i < 2*Astore->rowptr[n]; ++i) printf("%f  ", dp[i]);
      printf("\ncolind: ");
      for (i = 0; i < Astore->rowptr[n]; ++i) printf("%d  ", Astore->colind[i]);
      printf("\nrowptr: ");
      for (i = 0; i <= n; ++i) printf("%d  ", Astore->rowptr[i]);
      printf("\n");
      fflush(stdout);
    }
    template<typename T> inline
    void Print_Comp_Matrix (traits::row_major_t, T t,
                            char* what, SuperMatrix* A)
    {
      Print_CompRow_Matrix (t, what, A);
    }
  }}

}}}

#endif


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing le capabilities

    Returns True<result_type>() or False<result_type>() depending on whether x is
    less than or equal to y or not.

    Infix notation can be used with operator '<='.

    This is a convenient alias of @ref is_less_equal
  **/
  const boost::dispatch::functor<tag::le_> le = {};
} }
#endif

#include <boost/simd/function/scalar/is_less_equal.hpp>
#include <boost/simd/function/simd/le.hpp>

#endif


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


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

#include <boost/test/unit_test.hpp>

#include "Werk/Utility/Counter.hpp"

BOOST_AUTO_TEST_SUITE(CounterTest)

BOOST_AUTO_TEST_CASE(TestAdd)
{
	werk::Counter<> c;
	BOOST_REQUIRE_EQUAL(c.value(), 0);

	c.increment();
	BOOST_REQUIRE_EQUAL(c.value(), 1);

	c.increment();
	BOOST_REQUIRE_EQUAL(c.value(), 2);

	c.reset();
	BOOST_REQUIRE_EQUAL(c.value(), 0);
}

BOOST_AUTO_TEST_CASE(TestMultiply)
{
	werk::Counter<uint64_t, 7, 3, werk::CounterMode::MULTIPLY> c;
	BOOST_REQUIRE_EQUAL(c.value(), 7);

	c.increment();
	BOOST_REQUIRE_EQUAL(c.value(), 21);

	c.increment();
	BOOST_REQUIRE_EQUAL(c.value(), 63);

	c.reset();
	BOOST_REQUIRE_EQUAL(c.value(), 7);
}

BOOST_AUTO_TEST_CASE(TestShift)
{
	werk::Counter<uint64_t, 1, 1, werk::CounterMode::LSHIFT> c;
	BOOST_REQUIRE_EQUAL(c.value(), 1);

	c.increment();
	BOOST_REQUIRE_EQUAL(c.value(), 2);

	c.increment();
	BOOST_REQUIRE_EQUAL(c.value(), 4);

	c.reset();
	BOOST_REQUIRE_EQUAL(c.value(), 1);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * \ file GainSwellFilter.cpp
 */

#include <ATK/Dynamic/GainSwellFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <gtest/gtest.h>

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

constexpr gsl::index PROCESSSIZE = 64;

TEST(GainSwellFilter, softness_test)
{
  ATK::GainFilter<ATK::GainSwellFilter<double>> filter;
  filter.set_softness(0.5);
  ASSERT_EQ(filter.get_softness(), 0.5);
}

TEST(GainSwellFilter, softness_range_test)
{
  ATK::GainFilter<ATK::GainSwellFilter<double>> filter;
  ASSERT_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

TEST(GainSwellFilter, const_1_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1, outdata[i], 0.1);
  }
}

TEST(GainSwellFilter, const_0_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(1,
        outdata[i],
        0.1); // if input is zero, we still need a gain of 1 to have a progression of 1 for values < threshold
  }
}

TEST(GainSwellFilter, const_1_threshold_05_ratio_2_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.490775019, outdata[i], 0.1);
  }
}

TEST(GainSwellFilter, const_1_threshold_05_ratio_4_test)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(0.240860119, outdata[i], 0.1);
  }
}


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

/**
 *  \file   heatEqn.cpp
 *  \date   Aug 4, 2015
 *  \author "James C. Sutherland"
 */

#include <iostream>
using std::cout;
using std::endl;

#include <spatialops/structured/FVStaggered.h>
#include <spatialops/structured/Grid.h>
#include <spatialops/util/TimeLogger.h>

namespace so = SpatialOps;
typedef so::SVolField   VolField;
typedef so::SSurfXField FaceField;

typedef so::BasicOpTypes<VolField>::GradX      GradX;
typedef so::BasicOpTypes<VolField>::InterpC2FX InterpX;
typedef so::BasicOpTypes<VolField>::DivX       DivX;

//-- boost includes ---//
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <expression/ExprLib.h>
#include <expression/dualtime/FixedPointBDFDualTimeIntegrator.h>
#include <expression/dualtime/VariableImplicitBDFDualTimeIntegrator.h>
#include <expression/BoundaryConditionExpression.h>

#include <test/simpleHeatEqn/RHSExpr.h>
#include <test/simpleHeatEqn/Flux.h>

namespace po = boost::program_options;
using SpatialOps::IntVec;

void setup_bcs( Expr::ExpressionFactory& factory,
                const Expr::Tag& tempTag,
                const so::Grid& grid )
{
  std::vector<IntVec> xminusPts, xplusPts;
  xminusPts.push_back( IntVec( 0,              0, 0 ) );
  xplusPts .push_back( IntVec( grid.extent(0), 0, 0 ) );

  const so::GhostData ghost( IntVec(1,0,0), IntVec(1,0,0) );
  const so::BoundaryCellInfo bc = so::BoundaryCellInfo::build<FaceField>(true,true,false,false,false,false);
  const so::MemoryWindow window( so::get_window_with_ghost( grid.extent(), ghost, bc ) );

  typedef Expr::ConstantBCOpExpression<VolField,so::XDIR>::Builder XBC;
  XBC::MaskPtr xminus( new XBC::MaskType( window, bc, ghost, xminusPts ) );
  XBC::MaskPtr xplus ( new XBC::MaskType( window, bc, ghost, xplusPts  ) );

# ifdef ENABLE_CUDA
  // Masks are created on CPU so we need to explicitly transfer them to GPU
  xminus->add_consumer( GPU_INDEX );
  xplus ->add_consumer( GPU_INDEX );
# endif

  // jcs note that we need to have a better way of setting BCs on the old state.
  const Expr::Tag tempTagOld( tempTag.name(),Expr::STATE_N );
  const Expr::Tag xmTag( "xmbc", Expr::STATE_NONE );
  const Expr::Tag xpTag( "xpbc", Expr::STATE_NONE );
  const Expr::Tag xmTagDT( "xmbc-dualtime", Expr::STATE_NONE );
  const Expr::Tag xpTagDT( "xpbc-dualtime", Expr::STATE_NONE );
  factory.register_expression( new XBC( xmTag,   xminus, so::DIRICHLET, so::MINUS_SIDE, 2.0 ) );
  factory.register_expression( new XBC( xpTag,   xplus , so::DIRICHLET, so::PLUS_SIDE,  2.0 ) );
  factory.register_expression( new XBC( xmTagDT, xminus, so::DIRICHLET, so::MINUS_SIDE, 2.0 ) );
  factory.register_expression( new XBC( xpTagDT, xplus , so::DIRICHLET, so::PLUS_SIDE,  2.0 ) );

  factory.attach_modifier_expression( xmTagDT, tempTag    );
  factory.attach_modifier_expression( xpTagDT, tempTag    );
  factory.attach_modifier_expression( xmTag,   tempTagOld );
  factory.attach_modifier_expression( xpTag,   tempTagOld );
}

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

int main( int iarg, char* carg[] )
{
  double dt, ds, tend;
  IntVec npts(10,1,1);
  so::DoubleVec length(1,1,1);

# ifdef ENABLE_THREADS
  int soThreadCount, exprThreadCount;
# endif

  // parse the command line options input describing the problem
  {
    po::options_description desc("Supported Options");
    desc.add_options()
      ( "help", "print help message" )
      ( "dt",   po::value<double>(&dt  )->default_value(1e-1), "time step size" )
      ( "tend", po::value<double>(&tend)->default_value(10), "end time" )
      ( "ds",po::value<double>(&ds)->default_value(0.05), "dual time step size" )
#     ifdef ENABLE_THREADS
      ( "tc", po::value<int>(&soThreadCount)->default_value(NTHREADS), "Number of threads for Nebo")
      ( "etc", po::value<int>(&exprThreadCount)->default_value(1), "Number of threads for ExprLib")
#     endif
      ( "nx", po::value<int>(&npts[0])->default_value(40), "nx" )
      ( "Lx", po::value<double>(&length[0])->default_value(10),"Length in x");

    po::variables_map args;
    try{
      po::store( po::parse_command_line(iarg,carg,desc), args );
      po::notify(args);
    }
    catch( std::exception& err ){
      cout << err.what()
          << "\nusage:\n" << desc << "\n";
      return 1;
    }
    if (args.count("help")) {
      cout << desc << "\n";
      return 1;
    }
  }

# ifdef ENABLE_THREADS
  cout << "ENABLE_THREADS is ON" << endl;
  SpatialOps::set_hard_thread_count( NTHREADS );
  SpatialOps::set_soft_thread_count( soThreadCount );
  Expr::set_hard_thread_count( NTHREADS );
  Expr::set_soft_thread_count( exprThreadCount );
  sleep(1);
# endif

  cout
#      ifdef ENABLE_THREADS
       << " SpatialOps NTHREADS (can set at runtime) = " << SpatialOps::get_soft_thread_count()
       << " out of " << SpatialOps::get_hard_thread_count() << endl
       << " ExprLib    NTHREADS (can set at runtime) = "
       << SpatialOps::ThreadPool::get_pool_size() << " out of "
       << SpatialOps::ThreadPool::get_pool_capacity() << endl
#      endif
       << endl;

  // build the spatial operators
  const so::Grid grid( npts, length );
  SpatialOps::OperatorDatabase sodb;
  so::build_stencils( grid, sodb );

  const Expr::Tag tempt   ( "Temperature",   Expr::STATE_NONE );
  const Expr::Tag xfluxt  ( "HeatFluxX",     Expr::STATE_NONE );
  const Expr::Tag dcoef   ( "ThermalDiff",   Expr::STATE_NONE );
  const Expr::Tag dtTag   ( "timestep",      Expr::STATE_NONE );
  const Expr::Tag dsTag   ( "dual-timestep", Expr::STATE_NONE );
  const Expr::Tag rhsTag  ( "rhs",           Expr::STATE_NONE );

  const unsigned bdfOrder = 1;
  Expr::DualTime::BDFDualTimeIntegrator* integrator = new Expr::DualTime::FixedPointBDFDualTimeIntegrator<VolField>( 0, "BasicDualTime", dtTag, dsTag, bdfOrder );

  Expr::ExpressionFactory& factory = integrator->factory();

  factory.register_expression( new Flux<GradX,InterpX>::Builder( xfluxt, tempt, dcoef ) );
  factory.register_expression( new RHSExpr<VolField>::Builder( rhsTag, xfluxt, Expr::Tag(), Expr::Tag() ) );
  factory.register_expression( new Expr::ConstantExpr<VolField>::Builder(dcoef,0.1) );

  typedef Expr::ConstantExpr<so::SingleValueField>::Builder ConstantSingleValue;
  integrator->set_physical_time_step_expression( new ConstantSingleValue(dtTag,dt) );
  integrator->set_dual_time_step_expression    ( new ConstantSingleValue(dsTag,ds) );

  //-- build a few coordinate fields
  const IntVec hasBC(true,true,true);
  const so::BoundaryCellInfo cellBCInfo = so::BoundaryCellInfo::build<VolField>(hasBC,hasBC);
  const so::GhostData cellGhosts(1);
  const so::MemoryWindow vwindow( so::get_window_with_ghost(npts,cellGhosts,cellBCInfo) );
  VolField xcoord( vwindow, cellBCInfo, cellGhosts, NULL );
  grid.set_coord<SpatialOps::XDIR>( xcoord );
# ifdef ENABLE_CUDA
  xcoord.add_device( GPU_INDEX );
# endif

  setup_bcs( factory, tempt, grid );

  Expr::FieldManagerList fml;

  try{
    const Expr::FieldAllocInfo ainfo( npts, 0, 0, false, false, false );

    integrator->add_variable<VolField>( tempt.name(), rhsTag );
    integrator->prepare_for_integration( fml, sodb, ainfo );
    integrator->request_timings(true);

    std::ofstream  fout("heatEqn.dot" );
    integrator->get_tree().write_tree( fout, false, true );
  }
  catch( std::exception& err ){
   std::cout << "ERROR allocating/binding fields" << std::endl
       << err.what() << std::endl;
   return -1;
  }

  //-- initialize the temperature
  {
    VolField& temp = fml.field_manager<VolField>().field_ref(Expr::Tag(tempt.name(),Expr::STATE_N));
    temp <<= sin( xcoord );
#   ifdef ENABLE_CUDA
    temp.validate_device( GPU_INDEX );
#   endif
    std::ofstream tprof("T0.dat");
    VolField::const_iterator it=temp.interior_begin();
    VolField::const_iterator ix=xcoord.interior_begin();
    for(; it!=temp.interior_end(); ++it, ++ix ){
      tprof << *ix << "\t" << *it << std::endl;
    }
  }

  try{

    const unsigned maxIterPerStep = 100;

    unsigned totalIter = 0;
    unsigned stepCount = 0;
    unsigned dualIter  = 0;
    bool     converged = false;

    double t = 0;
    while( t<tend ){

      dualIter = 0;
      integrator->begin_time_step();

      do{

        integrator->advance_dualtime( converged );

        dualIter++;
        totalIter++;

      } while( !converged && dualIter <= maxIterPerStep );

      integrator->end_time_step();
      stepCount++;
      t += dt;

    }

    {
#     ifdef ENABLE_CUDA
      VolField& temp = fml.field_ref<VolField>(tempt);
      temp.validate_device( GPU_INDEX );
#     else
      const VolField& temp = fml.field_ref<VolField>(tempt);
#     endif
      std::ofstream tprof("T.dat");
      VolField::const_iterator it=temp.interior_begin();
      VolField::const_iterator ix=xcoord.interior_begin();
      for(; it!=temp.interior_end(); ++it, ++ix ){
        tprof << *ix << "\t" << *it << std::endl;
      }
    }
    return 0;
  }
  catch( std::exception& err ){
    cout << "ERROR!" << endl
         << err.what() << endl;
  }
  return -1;

}


/*===========================================================================*\
 *                                                                           *
 *                               CoMISo                                      *
 *      Copyright (C) 2008-2009 by Computer Graphics Group, RWTH Aachen      *
 *                           www.rwth-graphics.de                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 *  This file is part of CoMISo.                                             *
 *                                                                           *
 *  CoMISo is free software: you can redistribute it and/or modify           *
 *  it under the terms of the GNU General Public License as published by     *
 *  the Free Software Foundation, either version 3 of the License, or        *
 *  (at your option) any later version.                                      *
 *                                                                           *
 *  CoMISo is distributed in the hope that it will be useful,                *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            *
 *  GNU General Public License for more details.                             *
 *                                                                           *
 *  You should have received a copy of the GNU General Public License        *
 *  along with CoMISo.  If not, see <http://www.gnu.org/licenses/>.          *
 *                                                                           *
\*===========================================================================*/

#ifndef COMISO_Eigen_TOOLS_HH
#define COMISO_Eigen_TOOLS_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_EIGEN3_AVAILABLE

//== INCLUDES =================================================================

#include <iostream>
#include <vector>
#include <algorithm>
#include <limits>
#include <cmath>

#include <Eigen/Dense>
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>

#if COMISO_SUITESPARSE_AVAILABLE
#include <cholmod.h>
#endif

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO_EIGEN
{

/** \class EigenTools Eigen_Tools.hh

    A collection of helper functions for manipulating (Eigen) matrices.
*/

//== FUNCTION DEFINITION ======================================================

/// Get matrix data (CSC matrix format) from matrix
/** Used by Cholmod wrapper
 *  @param _mat matrix
 *  @param _c uplo parameter (l, L, u, U, c, C)
 *  @param _values values vector
 *  @param _rowind row indices
 *  @param _colptr column pointer  */
template<class MatrixT, class REALT, class INTT>
void get_ccs_symmetric_data( const MatrixT&      _mat,
                             const char          _c,
                             std::vector<REALT>& _values,
                             std::vector<INTT>&  _rowind,
                             std::vector<INTT>&  _colptr );

/// Inspect the matrix (print)
/** Prints useful matrix informations such as, dimension, symmetry, zero_rows, zero_cols, nnz, max, min, max_abs, min_abs, NAN, INF
  * @param _A matrix */
template<class MatrixT>
void inspect_matrix( const MatrixT& _A);

/** checks for symmetry
  * @param _A matrix
  * @return symmetric? (bool)*/
template<class MatrixT>
bool is_symmetric( const MatrixT& _A);

template< class Eigen_MatrixT, class IntT >
void permute( const Eigen_MatrixT& _QR, const std::vector< IntT>& _Pvec, Eigen_MatrixT& _A);

#if COMISO_SUITESPARSE_AVAILABLE

/// Eigen to Cholmod_sparse interface
template<class MatrixT>
void cholmod_to_eigen( const cholmod_sparse& _AC, MatrixT& _A);

template<class MatrixT>
void eigen_to_cholmod( const MatrixT&  _A,
                     cholmod_sparse* &_AC,
                     cholmod_common* _common,
                     int             _sparsity_type = 0,
                     bool            _long_int      = false);
#endif

// convert a gmm column-sparse matrix into an eigen sparse matrix
template<class GMM_MatrixT, class EIGEN_MatrixT>
void gmm_to_eigen( const GMM_MatrixT& _G, EIGEN_MatrixT& _E);

//=============================================================================
} // namespace COMISO_Eigen
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(COMISO_Eigen_TOOLS_C)
#define COMISO_Eigen_TOOLS_TEMPLATES
#include "Eigen_Tools.cc"
#endif

//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================//=============================================================================
#endif // Eigen_TOOLS_HH defined
//=============================================================================


#include "drake/geometry/render/vtk_util.h"

#include <Eigen/Dense>
#include <vtkNew.h>
#include <vtkPlaneSource.h>
#include <vtkSmartPointer.h>
#include <vtkTransform.h>

#include "drake/math/rotation_matrix.h"

namespace drake {
namespace geometry {
namespace render {
namespace vtk_util {

vtkSmartPointer<vtkPlaneSource> CreateSquarePlane(double size) {
  vtkSmartPointer<vtkPlaneSource> plane =
      vtkSmartPointer<vtkPlaneSource>::New();
  const double half_size = size * 0.5;
  plane->SetOrigin(-half_size, -half_size, 0.);
  plane->SetPoint1(-half_size, half_size, 0.);
  plane->SetPoint2(half_size, -half_size, 0.);
  plane->SetNormal(0., 0., 1.);
  plane->Update();

  return plane;
}

vtkSmartPointer<vtkTransform> ConvertToVtkTransform(
    const Eigen::Isometry3d& transform) {
  vtkNew<vtkMatrix4x4> vtk_mat;
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      vtk_mat->SetElement(i, j, transform.matrix()(i, j));
    }
  }

  vtkSmartPointer<vtkTransform> vtk_transform =
      vtkSmartPointer<vtkTransform>::New();
  vtk_transform->SetMatrix(vtk_mat.GetPointer());

  return vtk_transform;
}

vtkSmartPointer<vtkTransform> ConvertToVtkTransform(
    const math::RigidTransformd& transform) {
  vtkNew<vtkMatrix4x4> vtk_mat;
  for (int i = 0; i < 3; ++i) {
    const auto& row = transform.rotation().row(i);
    for (int j = 0; j < 3; ++j) {
      vtk_mat->SetElement(i, j, row(j));
    }
    vtk_mat->SetElement(i, 3, transform.translation()(i));
  }
  for (int j = 0; j < 3; ++j) {
    vtk_mat->SetElement(3, j, 0.0);
  }
  vtk_mat->SetElement(3, 3, 1.0);

  vtkSmartPointer<vtkTransform> vtk_transform =
      vtkSmartPointer<vtkTransform>::New();
  vtk_transform->SetMatrix(vtk_mat.GetPointer());

  return vtk_transform;
}

}  // namespace vtk_util
}  // namespace render
}  // namespace geometry
}  // namespace drake


/*
Copyright (c) by respective owners including Yahoo!, Microsoft, and
individual contributors. All rights reserved.  Released under a BSD (revised)
license as described in the file LICENSE.
*/

#include <boost/test/unit_test.hpp>

#include "ds_explore.h"

using namespace boost::unit_test;
using namespace std;
using namespace Microsoft::DecisionService;

BOOST_AUTO_TEST_SUITE( DecisionServiceExploreEpsilon )

BOOST_AUTO_TEST_CASE( EpsilonGreedy )
{
    EpsilonGreedyExplorer explorer(0.3f);

    vector<float> scores = { 1, 2, 3 };
    DecisionServicePredictorsSimple predictor(scores);
    PredictorContainer container(&predictor);
    ActionProbabilities actionProbs = explorer.explore(container);

    BOOST_CHECK_EQUAL(actionProbs.size(), 3 );

    // sorted by score
    BOOST_CHECK_EQUAL(actionProbs[0].action, 2);
    BOOST_CHECK_EQUAL(actionProbs[1].action, 1);
    BOOST_CHECK_EQUAL(actionProbs[2].action, 0);

    BOOST_CHECK_CLOSE(actionProbs[0].probability, 0.8, 0.00001);
    BOOST_CHECK_CLOSE(actionProbs[1].probability, 0.1, 0.00001);
    BOOST_CHECK_CLOSE(actionProbs[2].probability, 0.1, 0.00001);
}

BOOST_AUTO_TEST_CASE( EpsilonGreedyStable )
{
    EpsilonGreedyExplorer bag(0.3f);

    vector<float> scores = { 0, 1, 1 };
    DecisionServicePredictorsSimple predictor(scores);
    PredictorContainer container(&predictor);
    ActionProbabilities actionProbs = bag.explore(container);

    BOOST_CHECK_EQUAL(actionProbs.size(), 3);

    // sorted by score
    BOOST_CHECK_EQUAL(actionProbs[0].action, 1);
    BOOST_CHECK_EQUAL(actionProbs[1].action, 2);
    BOOST_CHECK_EQUAL(actionProbs[2].action, 0);

    BOOST_CHECK_CLOSE(actionProbs[0].probability, 0.8, 0.00001);
    BOOST_CHECK_CLOSE(actionProbs[1].probability, 0.1, 0.00001);
    BOOST_CHECK_CLOSE(actionProbs[2].probability, 0.1, 0.00001);
}

BOOST_AUTO_TEST_SUITE_END()