// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ASTRONOMY_COORDINATE_SKY_POINT_HPP
#define BOOST_ASTRONOMY_COORDINATE_SKY_POINT_HPP

#include <string>
#include <type_traits>
#include <cmath>

#include <boost/units/io.hpp>
#include <boost/static_assert.hpp>

#include <boost/astronomy/coordinate/frame.hpp>
#include <boost/astronomy/detail/is_base_template_of.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

/*!sky_point is used to represent a point(coordinate) in the sky*/
template <typename CoordinateSystem>
struct sky_point
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_frame_of
            <boost::astronomy::coordinate::base_frame, CoordinateSystem>::value),
            "Template argument is expected to be a frame class");
    ///@endcond

protected:
    CoordinateSystem point;

public:
    typedef CoordinateSystem system;

    //constructors

    //!default constructor
    sky_point() {}

    //!create point with the given coordinates
    sky_point(CoordinateSystem const& object) : point(object) {}

    //!create point with providing representation and differential class object
    template <typename Representation, typename Differential>
    sky_point
    (
        Representation const& representation_data,
        Differential const& differential_data
    )
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_representation, Representation>::value),
            "argument type is expected to be a representation class");

        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_differential, Differential>::value),
            "argument type is expected to be a differential class");

        point = CoordinateSystem(representation_data, differential_data);
    }

    //!create point with direct values of representation and differential
    sky_point
    (
        typename CoordinateSystem::representation::quantity1 const& lat,
        typename CoordinateSystem::representation::quantity2 const& lon,
        typename CoordinateSystem::representation::quantity3 const& distance,
        typename CoordinateSystem::differential::quantity1 const& pm_lat,
        typename CoordinateSystem::differential::quantity2 const& pm_lon_coslat,
        typename CoordinateSystem::differential::quantity3 const& radian_velocity
    )
    {
        point = CoordinateSystem(lat, lon, distance, pm_lat, pm_lon_coslat,
                    radian_velocity);
    }

    template<class OtherCoordinateSystem>
    sky_point(sky_point<OtherCoordinateSystem> const& object);

    //!constructing from direct value of representation
    sky_point
    (
        typename CoordinateSystem::representation::quantity1 const& lat,
        typename CoordinateSystem::representation::quantity2 const& lon,
        typename CoordinateSystem::representation::quantity3 const& distance
    )
    {
        point = CoordinateSystem(lat, lon, distance);
    }

    //constructing from name of object if available in the calatoge
    sky_point(std::string const& name);

    std::string get_constillation();

    sky_point<CoordinateSystem> from_name(std::string const& name);

    //!angular separation between two coordinates in radians
    bu::quantity<bu::si::plane_angle> separation(sky_point<CoordinateSystem> const&
        object) const
    {
        return this->point.get_angular_separation(object.get_point());
    }

    //!returns positional angle in the radian
    bu::quantity<bu::si::plane_angle> positional_angle(sky_point<CoordinateSystem>
        const& object) const
    {
        auto p1 = make_spherical_representation(this->point.get_data());
        auto p2 = make_spherical_representation(object.get_point().get_data());

        auto diff = p2.get_lon() - p1.get_lon();

        double temp_p1 = static_cast<bu::quantity<bu::si::plane_angle,
            typename CoordinateSystem::representation::type>>(p1.get_lat()).value();
        double temp_p2 = static_cast<bu::quantity<bu::si::plane_angle,
            typename CoordinateSystem::representation::type>>(p2.get_lat()).value();
        double temp_diff = static_cast<bu::quantity<bu::si::plane_angle,
            typename CoordinateSystem::representation::type>>(diff).value();

        double coslat = std::cos(temp_p2);

        double x = std::sin(temp_p2) * std::cos(temp_p1) -
            coslat * std::sin(temp_p1) * std::cos(temp_diff);
        double y = std::sin(temp_diff) * coslat;

        return bu::quantity<bu::si::plane_angle>::from_value(std::atan2(x, y));
    }

    //!returns true if both coordinate systems are same else returns false
    template<class OtherCoordinateSystem>
    bool is_equivalent_system(sky_point<OtherCoordinateSystem> const& object)
    {
        return std::is_same<CoordinateSystem, OtherCoordinateSystem>::value;
    }

    template<class OtherCoordinateSystem>
    sky_point<OtherCoordinateSystem> transform_to();

    //!returns the point
    CoordinateSystem get_point() const
    {
        return this->point;
    }

    //!sets the point with given object
    void set_point(CoordinateSystem const& otherPoint)
    {
        this->point = otherPoint;
    }

}; //sky_point

}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_SKY_POINT_HPP


#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <cereal/access.hpp>
#include <cereal/archives/portable_binary.hpp>
#include <cereal/cereal.hpp>
#include <cereal/types/base_class.hpp>
#include <cereal/types/complex.hpp>
#include <cereal/types/memory.hpp>
#include <cereal/types/polymorphic.hpp>
#include <cereal/types/tuple.hpp>
#include <cereal/types/vector.hpp>
#include <complex>
#include <csim/type.hpp>
typedef std::complex<double> CPPCTYPE;
typedef Eigen::VectorXcd ComplexVector;

// In order to use matrix raw-data without reordering, we use RowMajor as
// default.
typedef Eigen::Matrix<CPPCTYPE, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    ComplexMatrix;
typedef Eigen::SparseMatrix<CPPCTYPE> SparseComplexMatrix;

// reference:
// https://stackoverflow.com/questions/22884216/serializing-eigenmatrix-using-cereal-library

namespace cereal {
template <class Archive>
void save(Archive& ar, const ComplexMatrix& m) {
    size_t rows = m.rows();
    size_t cols = m.cols();
    ar(rows);
    ar(cols);
    ar(binary_data(
        m.data(), static_cast<std::size_t>(rows * cols * sizeof(CPPCTYPE))));
}

template <class Archive>
void load(Archive& ar, ComplexMatrix& m) {
    size_t rows;
    size_t cols;
    ar(rows);
    ar(cols);

    m.resize(rows, cols);
    ar(binary_data(
        m.data(), static_cast<std::size_t>(rows * cols * sizeof(CPPCTYPE))));
}

template <class Archive>
void save(Archive& ar, const ComplexVector& m) {
    size_t rows = m.rows();
    size_t cols = m.cols();
    ar(rows);
    ar(cols);
    ar(binary_data(
        m.data(), static_cast<std::size_t>(rows * cols * sizeof(CPPCTYPE))));
}

template <class Archive>
void load(Archive& ar, ComplexVector& m) {
    size_t rows;
    size_t cols;
    ar(rows);
    ar(cols);

    m.resize(rows, cols);
    ar(binary_data(
        m.data(), static_cast<std::size_t>(rows * cols * sizeof(CPPCTYPE))));
}

template <class Archive>
void save(Archive& ar, const SparseComplexMatrix& m) {
    Eigen::Index rows = m.rows();
    Eigen::Index cols = m.cols();
    ar(rows);
    ar(cols);
    std::vector<std::tuple<Eigen::Index, Eigen::Index, CPPCTYPE>> TripletList;
    for (Eigen::Index k = 0; k < m.outerSize(); ++k) {
        for (Eigen::SparseMatrix<CPPCTYPE>::InnerIterator it(m, k); it; ++it) {
            TripletList.push_back(
                std::tuple<Eigen::Index, Eigen::Index, CPPCTYPE>(
                    it.row(), it.col(), it.value()));
        }
    }
    ar(TripletList);
}

template <class Archive>
void load(Archive& ar, SparseComplexMatrix& m) {
    Eigen::Index rows;
    Eigen::Index cols;
    ar(rows);
    ar(cols);

    m.resize(rows, cols);
    std::vector<std::tuple<Eigen::Index, Eigen::Index, CPPCTYPE>> TripletList;
    ar(TripletList);
    std::vector<Eigen::Triplet<CPPCTYPE>> Triplets;
    for (size_t i = 0; i < TripletList.size(); ++i) {
        Triplets.push_back(
            Eigen::Triplet<CPPCTYPE>((int)std::get<0>(TripletList[i]),
                (int)std::get<1>(TripletList[i]), std::get<2>(TripletList[i])));
    }
    m.setFromTriplets(Triplets.begin(), Triplets.end());
}

template <class Archive>
void save(Archive& ar, const Eigen::Triplet<CPPCTYPE>& m) {
    ar(m.row());
    ar(m.col());
    ar(m.value());
}

template <class Archive>
void load(Archive& ar, Eigen::Triplet<CPPCTYPE>& m) {
    int row, col, value;
    ar(row, col, value);
    m = Eigen::Triplet<CPPCTYPE>(row, col, value);
}

}  // namespace cereal

#ifdef __GNUC__
#if __GNUC__ >= 8
using namespace std::complex_literals;
#endif
#endif

#ifdef _MSC_VER
inline static FILE* popen(const char* command, const char* mode) {
    return _popen(command, mode);
}
inline static void pclose(FILE* fp) { _pclose(fp); }
#define strncasecmp _strnicmp
#define strcasecmp _stricmp
#endif

#if defined(__MINGW32__) || defined(_MSC_VER)
#define DllExport __declspec(dllexport)
#else
#define DllExport __attribute__((visibility("default")))
#endif


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include parser_def.hpp directly!"
#endif

#pragma once

#include "ast.hpp"
#include "ast_adapted.hpp"
#include "math.hpp"
#include "parser.hpp"

#include <boost/spirit/include/phoenix.hpp>
#define BOOST_SPIRIT_NO_PREDEFINED_TERMINALS
#include <boost/spirit/include/qi.hpp>

#include <cmath>
#include <iostream>
#include <limits>
#include <sstream>
#include <string>

namespace matheval {

namespace qi = boost::spirit::qi;

namespace parser {

template <typename Iterator>
grammar<Iterator>::grammar() : grammar::base_type(expression) {
    qi::_2_type _2;
    qi::_3_type _3;
    qi::_4_type _4;

    qi::alnum_type alnum;
    qi::alpha_type alpha;
    qi::double_type double_;
    qi::lexeme_type lexeme;
    qi::raw_type raw;

    // clang-format off

    constant.add
        ("e"      , boost::math::constants::e<double>())
        ("epsilon", std::numeric_limits<double>::epsilon())
        ("phi"    , boost::math::constants::phi<double>())
        ("pi"     , boost::math::constants::pi<double>())
        ;

    ufunc.add
        ("abs"  , static_cast<double (*)(double)>(&std::abs))
        ("acos" , static_cast<double (*)(double)>(&std::acos))
        ("asin" , static_cast<double (*)(double)>(&std::asin))
        ("atan" , static_cast<double (*)(double)>(&std::atan))
        ("ceil" , static_cast<double (*)(double)>(&std::ceil))
        ("cos"  , static_cast<double (*)(double)>(&std::cos))
        ("cosh" , static_cast<double (*)(double)>(&std::cosh))
        ("deg"  , static_cast<double (*)(double)>(&math::deg))
        ("exp"  , static_cast<double (*)(double)>(&std::exp))
        ("floor", static_cast<double (*)(double)>(&std::floor))
        ("isinf", static_cast<double (*)(double)>(&math::isinf))
        ("isnan", static_cast<double (*)(double)>(&math::isnan))
        ("log"  , static_cast<double (*)(double)>(&std::log))
        ("log10", static_cast<double (*)(double)>(&std::log10))
        ("rad"  , static_cast<double (*)(double)>(&math::rad))
        ("sgn"  , static_cast<double (*)(double)>(&math::sgn))
        ("sin"  , static_cast<double (*)(double)>(&std::sin))
        ("sinh" , static_cast<double (*)(double)>(&std::sinh))
        ("sqrt" , static_cast<double (*)(double)>(&std::sqrt))
        ("tan"  , static_cast<double (*)(double)>(&std::tan))
        ("tanh" , static_cast<double (*)(double)>(&std::tanh))
        ;

    bfunc.add
        ("atan2", static_cast<double (*)(double, double)>(&std::atan2))
        ("pow"  , static_cast<double (*)(double, double)>(&std::pow))
        ;

    unary_op.add
        ("+", static_cast<double (*)(double)>(&math::plus))
        ("-", static_cast<double (*)(double)>(&math::minus))
        ("!", static_cast<double (*)(double)>(&math::unary_not))
        ;

    additive_op.add
        ("+", static_cast<double (*)(double, double)>(&math::plus))
        ("-", static_cast<double (*)(double, double)>(&math::minus))
        ;

    multiplicative_op.add
        ("*", static_cast<double (*)(double, double)>(&math::multiplies))
        ("/", static_cast<double (*)(double, double)>(&math::divides))
        ("%", static_cast<double (*)(double, double)>(&std::fmod))
        ;

    logical_op.add
        ("&&", static_cast<double (*)(double, double)>(&math::logical_and))
        ("||", static_cast<double (*)(double, double)>(&math::logical_or))
        ;

    relational_op.add
        ("<" , static_cast<double (*)(double, double)>(&math::less))
        ("<=", static_cast<double (*)(double, double)>(&math::less_equals))
        (">" , static_cast<double (*)(double, double)>(&math::greater))
        (">=", static_cast<double (*)(double, double)>(&math::greater_equals))
        ;

    equality_op.add
        ("==", static_cast<double (*)(double, double)>(&math::equals))
        ("!=", static_cast<double (*)(double, double)>(&math::not_equals))
        ;

    power.add
        ("**", static_cast<double (*)(double, double)>(&std::pow))
        ;

    expression =
        logical.alias()
        ;

    logical =
        equality >> *(logical_op > equality)
        ;

    equality =
        relational >> *(equality_op > relational)
        ;

    relational =
        additive >> *(relational_op > additive)
        ;

    additive =
        multiplicative >> *(additive_op > multiplicative)
        ;

    multiplicative =
        factor >> *(multiplicative_op > factor)
        ;

    factor =
        primary >> *( power > factor )
        ;

    unary =
        ufunc > '(' > expression > ')'
        ;

    binary =
        bfunc > '(' > expression > ',' > expression > ')'
        ;

    variable =
        raw[lexeme[alpha >> *(alnum | '_')]]
        ;

    primary =
          double_
        | ('(' > expression > ')')
        | (unary_op > primary)
        | binary
        | unary
        | constant
        | variable
        ;

    // clang-format on

    expression.name("expression");
    logical.name("logical");
    equality.name("equality");
    relational.name("relational");
    additive.name("additive");
    multiplicative.name("multiplicative");
    factor.name("factor");
    variable.name("variable");
    primary.name("primary");
    unary.name("unary");
    binary.name("binary");

    // typedef boost::phoenix::function<error_handler<Iterator> >
    // error_handler_function; qi::on_error<qi::fail>(expression,
    //        error_handler_function(error_handler<Iterator>())(
    //            "Error! Expecting ", qi::_4, qi::_3));
    qi::on_error<qi::fail>(
        expression,
        boost::phoenix::bind(boost::phoenix::ref(err_handler), _3, _2, _4));
}

} // namespace parser

} // namespace matheval


#include <ros/ros.h>
#include <Eigen/Eigen>

#include <prometheus_drl/ugv_state.h>

#include "printf_utils.h"

prometheus_drl::ugv_state ugv_state;
Eigen::MatrixXd obstacle_matrix;
Eigen::MatrixXd agent_matrix;
Eigen::MatrixXd goal_matrix;
Eigen::MatrixXd nei_goal_matrix;

void state_cb(const prometheus_drl::ugv_stateConstPtr& msg)
{
    ugv_state = *msg;

    obstacle_matrix.resize(ugv_state.matrix_size,ugv_state.matrix_size);
    agent_matrix.resize(ugv_state.matrix_size,ugv_state.matrix_size);
    goal_matrix.resize(ugv_state.matrix_size,ugv_state.matrix_size);
    nei_goal_matrix.resize(ugv_state.matrix_size,ugv_state.matrix_size);

    for(int i = 0; i < ugv_state.matrix_size; i++)
    {
        for(int j = 0; j < ugv_state.matrix_size; j++)
        {
            obstacle_matrix(i,j) = ugv_state.obstacle_matrix[i*ugv_state.matrix_size+j];
            agent_matrix(i,j) = ugv_state.agent_matrix[i*ugv_state.matrix_size+j];
            goal_matrix(i,j) = ugv_state.goal_matrix[i*ugv_state.matrix_size+j];
            nei_goal_matrix(i,j) = ugv_state.nei_goal_matrix[i*ugv_state.matrix_size+j];
        }
    }
}

void printf_cb(const ros::TimerEvent &e)
{
    cout << GREEN << "obstacle_matrix: " << TAIL <<endl;
    cout << GREEN <<  obstacle_matrix  << TAIL <<endl;
    cout << GREEN << "agent_matrix: " << TAIL <<endl;
    cout << GREEN <<  agent_matrix  << TAIL <<endl;
    cout << GREEN << "goal_matrix: " << TAIL <<endl;
    cout << GREEN <<  goal_matrix  << TAIL <<endl;
    cout << GREEN << "nei_goal_matrix: " << TAIL <<endl;
    cout << GREEN <<  nei_goal_matrix  << TAIL <<endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "fake_odom_node");
    ros::NodeHandle nh("~");

    string ugv_name = "/ugv" + std::to_string(1);

    ros::Subscriber ugv_state_sub = nh.subscribe<prometheus_drl::ugv_state>(ugv_name + "/ugv_state", 1, state_cb);

    ros::Timer printf_timer = nh.createTimer(ros::Duration(5.0), printf_cb);

    ros::spin();

    return 0;
}


//  Distributed under the Boost Software License, Version 1.0.
//  Copyright 2011 Vicente J. Botet Escriba
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/chrono/chrono_io.hpp>
//#include <boost/chrono/io/duration_units.hpp>
#include <sstream>
#include <boost/detail/lightweight_test.hpp>

template<typename D>
void test_good(const char* str, D res)
{
  std::istringstream in(str);
  D d(0);
  in >> d;
  BOOST_TEST(in.eof());
  BOOST_TEST(!in.fail());
  BOOST_TEST(d == res);
  std::cout << str << " " << res << " " << d << std::endl;
}

template<typename DFail>
void test_fail(const char* str, DFail res)
{
  {
    std::istringstream in(str);
    DFail d = DFail::zero();
    in >> d;
    BOOST_TEST(in.fail());
    BOOST_TEST(d == DFail::zero());
    std::cout << str << " " << res << " " << d << std::endl;
  }
}

template<typename D>
void test_not_eof(const char* str, D res)
{
  {
    std::istringstream in(str);
    D d = D::zero();
    in >> d;
    BOOST_TEST(!in.eof());
    BOOST_TEST(d == res);
    std::cout << str << " " << res << " " << d << std::endl;
  }
}
int main()
{
  using namespace boost::chrono;
  using namespace boost;

  test_good("5000", 5000);

  std::cerr << __FILE__ << "[" << __LINE__ << "]"<< std::endl;
  test_good("5000 hours", hours(5000));
  std::cerr << __FILE__ << "[" << __LINE__ << "]"<< std::endl;
  test_good("5000 minutes", minutes(5000));
  test_good("5000 seconds", seconds(5000));
  test_fail("1.0 second", seconds(1));

  test_good("1.0 second", duration<float,ratio<1> >(1));
  /* BUG with DURATION_GET
../../../boost/math/common_factor_rt.hpp: In function 'RingType boost::math::detail::gcd_euclidean(RingType, RingType) [with RingType = long double]':
../../../boost/math/common_factor_rt.hpp:122:   instantiated from 'IntegerType boost::math::detail::gcd_integer(const IntegerType&, const IntegerType&) [with IntegerType = long double]'
../../../boost/math/common_factor_rt.hpp:240:   instantiated from 'T boost::math::detail::gcd_optimal_evaluator_helper_t<T, true, true>::operator()(const T&, const T&) [with T = long double]'
../../../boost/math/common_factor_rt.hpp:290:   instantiated from 'T boost::math::detail::gcd_optimal_evaluator<T>::operator()(const T&, const T&) [with T = long double]'
../../../boost/math/common_factor_rt.hpp:442:   instantiated from 'T boost::math::detail::gcd_optimal(const T&, const T&) [with T = long double]'
../../../boost/math/common_factor_rt.hpp:473:   instantiated from 'typename boost::math::gcd_evaluator<IntegerType>::result_type boost::math::gcd_evaluator<IntegerType>::operator()(const IntegerType&, const IntegerType&) const [with IntegerType = long double]'
../../../boost/math/common_factor_rt.hpp:505:   instantiated from 'IntegerType boost::math::gcd(const IntegerType&, const IntegerType&) [with IntegerType = long double]'
../../../boost/chrono/io/duration_get.hpp:239:   instantiated from 'InputIterator boost::chrono::duration_get<CharT, InputIterator>::get(InputIterator, InputIterator, std::ios_base&, std::_Ios_Iostate&, boost::chrono::duration<Rep2, Period2>&, const CharT*, const CharT*) const [with Rep = double, Period = boost::ratio<1l, 1l>, CharT = char, InputIterator = std::istreambuf_iterator<char, std::char_traits<char> >]'
../../../boost/chrono/io/duration_get.hpp:294:   instantiated from 'InputIterator boost::chrono::duration_get<CharT, InputIterator>::get(InputIterator, InputIterator, std::ios_base&, std::_Ios_Iostate&, boost::chrono::duration<Rep2, Period2>&) const [with Rep = double, Period = boost::ratio<1l, 1l>, CharT = char, InputIterator = std::istreambuf_iterator<char, std::char_traits<char> >]'
../../../boost/chrono/io/duration_io.hpp:593:   instantiated from 'std::basic_istream<_CharT, _Traits>& boost::chrono::operator>>(std::basic_istream<_CharT, _Traits>&, boost::chrono::duration<Rep2, Period2>&) [with CharT = char, Traits = std::char_traits<char>, Rep = double, Period = boost::ratio<1l, 1l>]'
io/duration_input.cpp:15:   instantiated from 'void test_good(const char*, D) [with D = boost::chrono::duration<double, boost::ratio<1l, 1l> >]'
io/duration_input.cpp:52:   instantiated from here
../../../boost/math/common_factor_rt.hpp:102: error: invalid operands of types 'long double' and 'long double' to binary 'operator%'
../../../boost/math/common_factor_rt.hpp:102: error:   in evaluation of 'operator%=(long double, long double)'
../../../boost/math/common_factor_rt.hpp:106: error: invalid operands of types 'long double' and 'long double' to binary 'operator%'
../../../boost/math/common_factor_rt.hpp:106: error:   in evaluation of 'operator%=(long double, long double)'
   *
   */
  test_good("1 second", seconds(1));
  test_not_eof("1 second ", seconds(1));
  test_not_eof("1 seconde", seconds(1));
  test_good("1 seconds", seconds(1));
  test_good("0 seconds", seconds(0));
  test_good("-1 seconds", seconds(-1));
  test_good("5000 milliseconds", milliseconds(5000));
  test_good("5000 microseconds", microseconds(5000));
  test_good("5000 nanoseconds", nanoseconds(5000));
  test_good("5000 deciseconds", duration<boost::int_least64_t, deci> (5000));
  test_good("5000 [1/30]seconds", duration<boost::int_least64_t, ratio<1, 30> > (5000));
  test_good("5000 [1/30]second", duration<boost::int_least64_t, ratio<1, 30> > (5000));
  test_good("5000 h", hours(5000));
#if BOOST_CHRONO_VERSION==2
  test_good("5000 min", minutes(5000));
#else
  test_good("5000 m", minutes(5000));
#endif
  test_good("5000 s", seconds(5000));
  test_good("5000 ms", milliseconds(5000));
  test_good("5000 ns", nanoseconds(5000));
  test_good("5000 ds", duration<boost::int_least64_t, deci> (5000));
  test_good("5000 [1/30]s", duration<boost::int_least64_t, ratio<1, 30> > (5000));
  test_not_eof("5000 [1/30]ss", duration<boost::int_least64_t, ratio<1, 30> > (5000));
  std::cout << __LINE__<< "*****" << std::endl;
  test_good("5000 milliseconds", seconds(5));
  test_good("5000 millisecond", seconds(5));
  test_good("5 milliseconds", nanoseconds(5000000));
  std::cout << __LINE__<< "*****" << std::endl;
  test_good("4000 ms", seconds(4));
  std::cout << __LINE__<< "*****" << std::endl;
  test_fail("3001 ms", seconds(3));
  std::cout << __LINE__<< "*****" << std::endl;
  test_fail("3001 ", milliseconds(3001));
  std::cout << __LINE__<< "*****" << std::endl;
  test_fail("one ms", milliseconds(1));
  test_fail("5000 millisecon", seconds(5));
  test_not_eof("3001 ms ", milliseconds(3001));

  return boost::report_errors();

}


/***************************************************************************
*   Copyright 2012 - 2013 Advanced Micro Devices, Inc.
*
*   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.
***************************************************************************/

/******************************************************************************
 *  Benchmark Bolt Functions
 *****************************************************************************/
#include <iostream>
#include "bolt/cl/functional.h"
#include "bolt/cl/device_vector.h"
#include "bolt/cl/generate.h"
#include "bolt/cl/binary_search.h"
#include "bolt/cl/copy.h"
#include "bolt/cl/count.h"
#include "bolt/cl/fill.h"
#include "bolt/cl/max_element.h"
#include "bolt/cl/min_element.h"
#include "bolt/cl/merge.h"
#include "bolt/cl/transform.h"
#include "bolt/cl/scan.h"
#include "bolt/cl/sort.h"
#include "bolt/cl/reduce.h"
#include "bolt/cl/sort_by_key.h"
#include "bolt/cl/stablesort.h"
#include "bolt/cl/reduce_by_key.h"
#include "bolt/cl/stablesort_by_key.h"
#include "bolt/cl/transform_scan.h"
#include "bolt/cl/scan_by_key.h"
#include "bolt/cl/gather.h"
#include "bolt/cl/scatter.h"

#include <fstream>
#include <vector>
#include <bolt/unicode.h>
#include <algorithm>
#include <iomanip>
#include "bolt/unicode.h"
#include "bolt/countof.h"
#include <boost/program_options.hpp>
#include<random>

//#define BOLT_PROFILER_ENABLED
#define BOLT_BENCH_DEVICE_VECTOR_FLAGS CL_MEM_READ_WRITE

#include "bolt/AsyncProfiler.h"
#include "bolt/statisticalTimer.h"

#if defined(_WIN32)
AsyncProfiler aProfiler("default");
#endif
const std::streamsize colWidth = 26;

//#define DATA_TYPE float
//BOLT_CREATE_DEFINE(Bolt_DATA_TYPE,DATA_TYPE,float);

#ifndef DATA_TYPE
#define DATA_TYPE unsigned int
BOLT_CREATE_DEFINE(Bolt_DATA_TYPE,DATA_TYPE,unsigned int);
#endif // !DATA_TYPE

// function generator:
unsigned int RandomNumber()
{
    std::default_random_engine gen;
    std::uniform_int_distribution<unsigned int> distr(10,1<<31);
    unsigned int dice_roll = distr(gen);  // generates number in the range 10..1<<31
    return (dice_roll);
}

/******************************************************************************
 *  Functions Enumerated
 *****************************************************************************/

static const size_t FList = 23;

enum functionType {
    f_binarytransform,
    f_binarysearch,
    f_copy,
    f_count,
    f_fill,
    f_generate,
    f_innerproduct,
    f_maxelement,
    f_minelement,
    f_merge,
    f_reduce,
    f_reducebykey,
    f_scan,
    f_scanbykey,
    f_sort,
    f_sortbykey,
    f_stablesort,
    f_stablesortbykey,
    f_transformreduce,
    f_transformscan,
    f_unarytransform,
    f_gather,
    f_scatter

};
static char *functionNames[] = {
"binarytransform",
"binarysearch",
"copy",
"count",
"fill",
"generate",
"innerproduct",
"maxelement",
"minelement",
"merge",
"reduce",
"reducebykey",
"scan",
"scanbykey",
"sort",
"sortbykey",
"stablesort",
"stablesortbykey",
"transformreduce",
"transformscan",
"unarytransform",
"gather",
"scatter"

};

/******************************************************************************
 *  Data Types Enumerated
 *****************************************************************************/
enum dataType {
    t_int,
    t_vec2,
    t_vec4,
    t_vec8
};
static char *dataTypeNames[] = {
    "int1",
    "vec2",
    "vec4",
    "vec8"
};

namespace po = boost::program_options;
using namespace std;
/******************************************************************************
 *  User Defined Data Types - vec2,4,8
 *****************************************************************************/

BOLT_FUNCTOR(vec2,
struct vec2
{
    DATA_TYPE a, b;
    vec2  operator =(const DATA_TYPE inp)
    {
        vec2 tmp;
        a = b = tmp.a = tmp.b = inp;
        return tmp;
    }
    bool operator==(const vec2& rhs) const
    {
        bool l_equal = true;
        l_equal = ( a == rhs.a ) ? l_equal : false;
        l_equal = ( b == rhs.b ) ? l_equal : false;
        return l_equal;
    }
  //friend ostream& operator<<(ostream& os, const vec2& dt);
};
);

  ostream& operator<<(ostream& os, const vec2& dt)
    {
        os<<dt.a<<" "<<dt.b;
        return os;
    }

BOLT_CREATE_TYPENAME( bolt::cl::device_vector< vec2 >::iterator );
BOLT_CREATE_CLCODE( bolt::cl::device_vector< vec2 >::iterator, bolt::cl::deviceVectorIteratorTemplate );
BOLT_TEMPLATE_REGISTER_NEW_TYPE( bolt::cl::detail::CountIfEqual, DATA_TYPE, vec2 );

BOLT_FUNCTOR(vec4,
struct vec4
{
    DATA_TYPE a, b, c, d;
    vec4  operator =(const DATA_TYPE inp)
    {
        vec4 tmp;
        tmp.a = tmp.b = tmp.c = tmp.d = a = b = c=d=inp;
        return tmp;
    }
    bool operator==(const vec4& rhs) const
    {
        bool l_equal = true;
        l_equal = ( a == rhs.a ) ? l_equal : false;
        l_equal = ( b == rhs.b ) ? l_equal : false;
        l_equal = ( c == rhs.c ) ? l_equal : false;
        l_equal = ( d == rhs.d ) ? l_equal : false;
        return l_equal;
    }
   // friend ostream& operator<<(ostream& os, const vec4& dt);
};
);

    ostream& operator<<(ostream& os, const vec4& dt)
    {
        os<<dt.a<<" "<<dt.b<<" "<<dt.c<<" "<<dt.d;
        return os;
    }

BOLT_CREATE_TYPENAME( bolt::cl::device_vector< vec4 >::iterator );
BOLT_CREATE_CLCODE( bolt::cl::device_vector< vec4 >::iterator, bolt::cl::deviceVectorIteratorTemplate );

BOLT_TEMPLATE_REGISTER_NEW_TYPE( bolt::cl::detail::CountIfEqual, DATA_TYPE, vec4 );

BOLT_FUNCTOR(vec8,
struct vec8
{
    DATA_TYPE a, b, c, d, e, f, g, h;

    vec8  operator =(const DATA_TYPE inp)
    {
        a = b = c=d=e=f=g=h=inp;

        vec8 tmp;
        tmp.a = tmp.b = tmp.c = tmp.d = a = b = c=d=e=f=g=h=inp;
        tmp.e = tmp.f = tmp.g = tmp.h = inp;
        return tmp;

    }

    bool operator==(const vec8& rhs) const
    {
        bool l_equal = true;
        l_equal = ( a == rhs.a ) ? l_equal : false;
        l_equal = ( b == rhs.b ) ? l_equal : false;
        l_equal = ( c == rhs.c ) ? l_equal : false;
        l_equal = ( d == rhs.d ) ? l_equal : false;
        l_equal = ( e == rhs.e ) ? l_equal : false;
        l_equal = ( f == rhs.f ) ? l_equal : false;
        l_equal = ( g == rhs.g ) ? l_equal : false;
        l_equal = ( h == rhs.h ) ? l_equal : false;
        return l_equal;
    }
   // friend ostream& operator<<(ostream& os, const vec8& dt);

};
);
    ostream& operator<<(ostream& os, const vec8& dt)
    {
        os<<dt.a<<" "<<dt.b<<" "<<dt.c<<" "<<dt.d<<" "<<dt.e<<" "<<dt.f<<" "<<dt.g<<" "<<dt.h;
        return os;
    }

BOLT_CREATE_TYPENAME( bolt::cl::device_vector< vec8 >::iterator );
BOLT_CREATE_CLCODE( bolt::cl::device_vector< vec8 >::iterator, bolt::cl::deviceVectorIteratorTemplate );
BOLT_TEMPLATE_REGISTER_NEW_TYPE( bolt::cl::detail::CountIfEqual, DATA_TYPE, vec8 );
BOLT_TEMPLATE_REGISTER_NEW_TYPE( bolt::cl::less, DATA_TYPE, vec8 );
/******************************************************************************
 *  User Defined Binary Functions - vec2,4,8plus
 *****************************************************************************/

BOLT_FUNCTOR(vec2plus,
struct vec2plus
{
    vec2 operator()(const vec2 &lhs, const vec2 &rhs) const
    {
        vec2 l_result;
        l_result.a = lhs.a+rhs.a;
        l_result.b = lhs.b+rhs.b;
        return l_result;
    };
};
);

BOLT_FUNCTOR(vec4plus,
struct vec4plus
{
    vec4 operator()(const vec4 &lhs, const vec4 &rhs) const
    {
        vec4 l_result;
        l_result.a = lhs.a+rhs.a;
        l_result.b = lhs.b+rhs.b;
        l_result.c = lhs.c+rhs.c;
        l_result.d = lhs.d+rhs.d;
        return l_result;
    };
};
);

BOLT_FUNCTOR(vec8plus,
struct vec8plus
{
    vec8 operator()(const vec8 &lhs, const vec8 &rhs) const
    {
        vec8 l_result;
        l_result.a = lhs.a+rhs.a;
        l_result.b = lhs.b+rhs.b;
        l_result.c = lhs.c+rhs.c;
        l_result.d = lhs.d+rhs.d;
        l_result.e = lhs.e+rhs.e;
        l_result.f = lhs.f+rhs.f;
        l_result.g = lhs.g+rhs.g;
        l_result.h = lhs.h+rhs.h;
        return l_result;
    };
};
);

/******************************************************************************
 *  User Defined Unary Functions vec2,4,8square
 *****************************************************************************/

BOLT_FUNCTOR(vec2square,
struct vec2square
{
    vec2 operator()(const vec2 &rhs) const
    {
        vec2 l_result;
        l_result.a = rhs.a*rhs.a;
        l_result.b = rhs.b*rhs.b;
        return l_result;
    };
};
);

BOLT_FUNCTOR(vec4square,
struct vec4square
{
    vec4 operator()(const vec4 &rhs) const
    {
        vec4 l_result;
        l_result.a = rhs.a*rhs.a;
        l_result.b = rhs.b*rhs.b;
        l_result.c = rhs.c*rhs.c;
        l_result.d = rhs.d*rhs.d;
        return l_result;
    };
};
);

BOLT_FUNCTOR(vec8square,
struct vec8square
{
    vec8 operator()(const vec8 &rhs) const
    {
        vec8 l_result;
        l_result.a = rhs.a*rhs.a;
        l_result.b = rhs.b*rhs.b;
        l_result.c = rhs.c*rhs.c;
        l_result.d = rhs.d*rhs.d;
        l_result.e = rhs.e*rhs.e;
        l_result.f = rhs.f*rhs.f;
        l_result.g = rhs.g*rhs.g;
        l_result.h = rhs.h*rhs.h;
        return l_result;
    };
};
);

/******************************************************************************
 *  User Defined Binary Predicates equal
 *****************************************************************************/

BOLT_FUNCTOR(vec2equal,
struct vec2equal
{
    bool operator()(const vec2 &lhs, const vec2 &rhs) const
    {
        return lhs == rhs;
    };
};
);

BOLT_FUNCTOR(vec4equal,
struct vec4equal
{
    bool operator()(const vec4 &lhs, const vec4 &rhs) const
    {
        return lhs == rhs;
    };
};
);

BOLT_FUNCTOR(vec8equal,
struct vec8equal
{
    bool operator()(const vec8 &lhs, const vec8 &rhs) const
    {
        return lhs == rhs;
    };
};
);

/******************************************************************************
 *  User Defined Binary Predicates less than
 *****************************************************************************/

BOLT_FUNCTOR(vec2less,
struct vec2less
{
    bool operator()(const vec2 &lhs, const vec2 &rhs) const
    {
        if (lhs.a < rhs.a) return true;
        if (lhs.b < rhs.b) return true;
        return false;
    };
};
);

BOLT_FUNCTOR(vec4less,
struct vec4less
{
    bool operator()(const vec4 &lhs, const vec4 &rhs) const
    {
        if (lhs.a < rhs.a) return true;
        if (lhs.b < rhs.b) return true;
        if (lhs.c < rhs.c) return true;
        if (lhs.d < rhs.d) return true;
        return false;
    };
};
);

BOLT_FUNCTOR(vec8less,
struct vec8less
{
    bool operator()(const vec8 &lhs, const vec8 &rhs) const
    {
        if (lhs.a < rhs.a) return true;
        if (lhs.b < rhs.b) return true;
        if (lhs.c < rhs.c) return true;
        if (lhs.d < rhs.d) return true;
        if (lhs.e < rhs.e) return true;
        if (lhs.f < rhs.f) return true;
        if (lhs.g < rhs.g) return true;
        if (lhs.h < rhs.h) return true;
        return false;
    };
};
);

/******************************************************************************
 *  User Defined Binary Predicates vec2,4,8square
 *****************************************************************************/

BOLT_FUNCTOR(intgen,
struct intgen
{
    DATA_TYPE operator()() const
    {
        DATA_TYPE v = 1;
        return v;
    };
};
);

BOLT_FUNCTOR(vec2gen,
struct vec2gen
{
    vec2 operator()() const
    {
        vec2 v = { 2, 3 };
        return v;
    };
};
);

BOLT_FUNCTOR(vec4gen,
struct vec4gen
{
    vec4 operator()() const
    {
        vec4 v = { 4, 5, 6, 7 };
        return v;
    };
};
);

BOLT_FUNCTOR(vec8gen,
struct vec8gen
{
    vec8 operator()() const
    {
        vec8 v = { 8, 9, 10, 11, 12, 13, 14, 15 };
        return v;
    };
};
);

/******************************************************************************
 *  Initializers
 *****************************************************************************/
vec2 v2init = { 1, 1 };
vec2 v2iden = { 0, 0 };
vec4 v4init = { 1, 1, 1, 1 };
vec4 v4iden = { 0, 0, 0, 0 };
vec8 v8init = { 1, 1, 1, 1, 1, 1, 1, 1 };
vec8 v8iden = { 0, 0, 0, 0, 0, 0, 0, 0 };

/******************************************************************************
 *
 *  Execute Function
 *
 *****************************************************************************/
functionType get_functionindex(std::string &fun)
{
    for(int i =0 ; i < FList; i++)
    {
        if(fun.compare(functionNames[i]) == 0)
            return (functionType)i;
    }
    std::cout<< "Specified Function not listed for Benchmar. exiting";
    exit(0);
}

/******************************************************************************
 *
 *  Execute Function Type
 *
 *****************************************************************************/
template<
    typename VectorType,
    typename Generator,
    typename UnaryFunction,
    typename BinaryFunction,
    typename BinaryPredEq,
    typename BinaryPredLt >
void executeFunctionType(
    bolt::cl::control& ctrl,
    VectorType &input1,
    VectorType &input2,
    VectorType &input3,
    VectorType &output,
    VectorType &output_merge,
    Generator generator,
    UnaryFunction unaryFunct,
    BinaryFunction binaryFunct,
    BinaryPredEq binaryPredEq,
    BinaryPredLt binaryPredLt,
    size_t function,
    size_t iterations,
    size_t siz
    )
{

    bolt::statTimer& myTimer = bolt::statTimer::getInstance( );
    myTimer.Reserve( 1, iterations );
    size_t testId	= myTimer.getUniqueID( _T( "test" ), 0 );

switch(function)
{

            case f_merge:
            {

            std::cout <<  functionNames[f_merge] << std::endl;

            bolt::cl::sort( ctrl, input1.begin( ), input1.end( ), binaryPredLt);
            bolt::cl::sort( ctrl, input2.begin( ), input2.end( ), binaryPredLt);

                for (size_t iter = 0; iter < iterations+1; iter++)
                {

                    myTimer.Start( testId );
                    bolt::cl::merge( ctrl,input1.begin( ),input1.end( ),input2.begin( ),input2.end( ),output_merge.begin( ),binaryPredLt);

                    myTimer.Stop( testId );
                }
            }
            break;

            case f_binarysearch:
            {

            bool tmp;

            typename VectorType::value_type val;

            std::cout <<  functionNames[f_binarysearch] << std::endl;

            bolt::cl::sort( ctrl, input1.begin( ), input1.end( ), binaryPredLt);

                for (size_t iter = 0; iter < iterations+1; iter++)
                {

                    int index = 0;
                    if(iter!=0)
                        index = rand()%iter;

                    val = input1[index];

                    myTimer.Start( testId );
                    tmp = bolt::cl::binary_search( ctrl,input1.begin( ),input1.end( ),val,binaryPredLt);

                    myTimer.Stop( testId );
                }
            }
            break;

             case f_gather:
            {

                std::cout <<  functionNames[f_gather] << std::endl;
                bolt::cl::device_vector<DATA_TYPE> Map(input1.size());
                for( int i=0; i < input1.size() ; i++ )
                   {
                        Map[i] = i;
                   }
                for (size_t iter = 0; iter < iterations+1; iter++)
                    {
                        myTimer.Start( testId );
                        bolt::cl::gather( ctrl,Map.begin( ), Map.end( ),input1.begin( ),output.begin());
                        myTimer.Stop( testId );
                    }
            }
            break;

             case f_scatter:
            {

                std::cout <<  functionNames[f_scatter] << std::endl;
                bolt::cl::device_vector<DATA_TYPE> Map(input1.size());
                for( int i=0; i < input1.size() ; i++ )
                   {
                        Map[i] = i;
                   }
                for (size_t iter = 0; iter < iterations+1; iter++)
                    {
                        myTimer.Start( testId );
                        bolt::cl::scatter( ctrl, input1.begin( ),input1.end( ), Map.begin(), output.begin());
                        myTimer.Stop( testId );
                    }
            }
            break;

            case f_transformreduce: // fill
            {

            typename VectorType::value_type tmp;
            tmp=0;
            std::cout <<  functionNames[f_transformreduce] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    tmp = bolt::cl::transform_reduce( ctrl,input1.begin( ), input1.end( ),
                                                       unaryFunct, tmp,
                                                       binaryFunct);
                    myTimer.Stop( testId );
                }
            }
            break;

            case f_stablesort: // fill
            {
            std::cout <<  functionNames[f_stablesort] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::stable_sort(ctrl, input1.begin(), input1.end(),binaryPredLt);
                    myTimer.Stop( testId );
                }
            }

             break;
            case f_stablesortbykey: // fill
            {
            std::cout <<  functionNames[f_stablesortbykey] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::stable_sort_by_key(ctrl, input1.begin(), input1.end(),input2.begin(),binaryPredLt);
                    myTimer.Stop( testId );
                }
            }

             break;

            case f_reducebykey: // fill
            {
            std::cout <<  functionNames[f_reducebykey] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                   bolt::cl::reduce_by_key(ctrl, input1.begin(), input1.end(),input2.begin(),input3.begin(),
                       output.begin(),binaryPredEq, binaryFunct);
                    myTimer.Stop( testId );
                }
            }
             break;

            case f_sort: // fill
            {
            std::cout <<  functionNames[f_sort] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                     bolt::cl::sort(ctrl, input1.begin(), input1.end(),binaryPredLt);
                    myTimer.Stop( testId );
                }
            }

             break;

            case f_sortbykey: // fill
            {
            std::cout <<  functionNames[f_sortbykey] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::sort_by_key( input1.begin(), input1.end(), input2.begin( ),binaryPredLt );
                    myTimer.Stop( testId );
                }
            }

             break;

            case f_reduce: // fill
            {
            typename VectorType::value_type tmp;
            tmp=0;
            std::cout <<  functionNames[f_reduce] << std::endl;

          /*  for(VectorType::iterator itr = input1.begin();   itr  !=input1.end(); itr++)
            {
                std::cout<<*itr<<std::endl;
            }*/
                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    tmp = bolt::cl::reduce(ctrl, input1.begin(), input1.end(),tmp,binaryFunct);
                    myTimer.Stop( testId );
                }
            }

             break;

        case f_maxelement: // fill
            {

            std::cout <<  functionNames[f_maxelement] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    typename VectorType::iterator itr = bolt::cl::max_element(ctrl, input1.begin(), input1.end(),binaryPredLt);
                    myTimer.Stop( testId );
                }
            }

             break;

        case f_minelement: // fill
            {

            std::cout <<  functionNames[f_minelement] << std::endl;

/*                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    typename VectorType::iterator itr = bolt::cl::min_element(ctrl, input1.begin(), input1.end(),binaryPredLt);
                    myTimer.Stop( testId );
                }
*/
            }

             break;

        case f_fill: // fill
            {

            typename VectorType::value_type tmp;

            std::cout <<  functionNames[f_count] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::fill(ctrl, input1.begin(), input1.end(),tmp);
                    myTimer.Stop( testId );
                }
            }
            break;

        case f_count: // Count
            {
             typename VectorType::value_type tmp;
            std::cout <<  functionNames[f_count] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::count(ctrl, input1.begin(), input1.end(),tmp);
                    myTimer.Stop( testId );
                }
            }
            break;

        case f_generate: // generate
            std::cout <<  functionNames[f_generate] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::generate(ctrl, input1.begin(), input1.end(), generator );
                    myTimer.Stop( testId );
                }
            break;

        case f_copy: // copy
            std::cout <<  functionNames[f_copy] << std::endl;
                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::copy(ctrl, input1.begin(), input1.end(), output.begin() );
                    myTimer.Stop( testId );
                }
            break;

        case f_unarytransform: // unary transform
            std::cout <<  functionNames[f_unarytransform] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::transform(ctrl, input1.begin(), input1.end(), output.begin(), unaryFunct );
                    myTimer.Stop( testId );
                }
            break;

        case f_binarytransform: // binary transform
            std::cout <<  functionNames[f_binarytransform] << std::endl;
                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::transform(ctrl, input1.begin(), input1.end(), input2.begin(), output.begin(), binaryFunct );
                    myTimer.Stop( testId );
                }
            break;

        case f_scan: // scan
            std::cout <<  functionNames[f_scan] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                     myTimer.Start( testId );
            bolt::cl::inclusive_scan(
                ctrl, input1.begin(), input1.end(), output.begin(), binaryFunct );
                        myTimer.Stop( testId );
                }
            break;

        case f_transformscan: // transform_scan
            std::cout <<  functionNames[f_transformscan] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::transform_inclusive_scan(
                    ctrl, input1.begin(), input1.end(), output.begin(), unaryFunct, binaryFunct );
                    myTimer.Stop( testId );
                }
            break;

        case f_scanbykey: // scan_by_key
            std::cout <<  functionNames[f_scanbykey] << std::endl;

                for (size_t iter = 0; iter < iterations+1; iter++)
                {
                    myTimer.Start( testId );
                    bolt::cl::inclusive_scan_by_key(
                    ctrl, input1.begin(), input1.end(), input2.begin(), output.begin(), binaryPredEq, binaryFunct );
                    myTimer.Stop( testId );
                }
            break;

        default:
            //std::cout << "Unsupported function=" << function << std::endl;
            break;
   } // switch

    size_t length = input1.size();
    double MKeys = length / ( 1024.0 * 1024.0 );
    size_t pruned = myTimer.pruneOutliers( 1.0 );
    double sortTime = myTimer.getAverageTime( testId );
    double testMB = MKeys*siz;
    double testGB = testMB/ 1024.0;

    bolt::tout << std::left;
    bolt::tout << std::setw( colWidth ) << _T( "Test profile: " ) << _T( "[" ) << iterations-pruned << _T( "] samples" ) << std::endl;
    bolt::tout << std::setw( colWidth ) << _T( "    Size (MKeys): " ) << MKeys << std::endl;
    bolt::tout << std::setw( colWidth ) << _T( "    Size (GB): " ) << testGB << std::endl;
    bolt::tout << std::setw( colWidth ) << _T( "    Time (s): " ) << sortTime << std::endl;
    bolt::tout << std::setw( colWidth ) << _T( "    Speed (GB/s): " ) << testGB / sortTime << std::endl;
    bolt::tout << std::setw( colWidth ) << _T( "    Speed (MKeys/s): " ) << MKeys / sortTime << std::endl;
    bolt::tout << std::endl;

}

/******************************************************************************
 *
 *  Determine types
 *
 *****************************************************************************/
void executeFunction(
    bolt::cl::control& ctrl,
    size_t vecType,
    bool hostMemory,
    size_t length,
    size_t routine,
    size_t iterations )
{
    size_t siz;
    if (vecType == t_int)
    {
        intgen                  generator;
        bolt::cl::square<DATA_TYPE>   unaryFunct;
        bolt::cl::plus<DATA_TYPE>     binaryFunct;
        bolt::cl::equal_to<DATA_TYPE> binaryPredEq;
        bolt::cl::less<DATA_TYPE>     binaryPredLt;
        siz = sizeof(DATA_TYPE);

        std::vector<DATA_TYPE> input1(length);
        std::vector<DATA_TYPE> input2(length);
        std::vector<DATA_TYPE> input3(length);
        std::vector<DATA_TYPE> output(length);
        std::vector<DATA_TYPE> output_merge(length*2) ;

        std::generate(input1.begin(), input1.end(), RandomNumber);
        std::generate(input2.begin(), input2.end(), RandomNumber);
        std::generate(input3.begin(), input3.end(), RandomNumber);
        std::generate(output.begin(), output.end(), RandomNumber);
        std::generate(output_merge.begin(), output_merge.end(), RandomNumber);

        if (hostMemory) {

            executeFunctionType( ctrl, input1, input2, input3, output, output_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
        else
        {
            bolt::cl::device_vector<DATA_TYPE> binput1(input1.begin(), input1.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<DATA_TYPE> binput2(input2.begin(), input2.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<DATA_TYPE> binput3(input3.begin(), input3.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<DATA_TYPE> boutput(output.begin(), output.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<DATA_TYPE> boutput_merge(output.begin(), output.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            executeFunctionType( ctrl, binput1, binput2, binput3, boutput, boutput_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
    }
    else if (vecType == t_vec2)
    {
        vec2gen     generator;
        vec2square  unaryFunct;
        vec2plus    binaryFunct;
        vec2equal   binaryPredEq;
        vec2less    binaryPredLt;
        siz = sizeof(vec2);

        BOLT_ADD_DEPENDENCY(vec2, Bolt_DATA_TYPE);

        std::vector<vec2> input1(length);
        std::vector<vec2> input2(length);
        std::vector<vec2> input3(length);
        std::vector<vec2> output(length);
        std::vector<vec2> output_merge(length*2) ;

        std::generate(input1.begin(), input1.end(),RandomNumber);
        std::generate(input2.begin(), input2.end(),RandomNumber);
        std::generate(input3.begin(), input3.end(),RandomNumber);
        std::generate(output.begin(), output.end(),RandomNumber);
        std::generate(output_merge.begin(), output_merge.end(), RandomNumber);

        if (hostMemory) {

            executeFunctionType( ctrl, input1, input2, input3, output, output_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
        else
        {
            bolt::cl::device_vector<vec2> binput1(input1.begin(), input1.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec2> binput2(input2.begin(), input2.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec2> binput3(input3.begin(), input3.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec2> boutput(output.begin(), output.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec2> boutput_merge(output.begin(), output.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            executeFunctionType( ctrl, binput1, binput2,binput3, boutput, boutput_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
    }
    else if (vecType == t_vec4)
    {
        siz = sizeof(vec4);
        vec4gen     generator;
        vec4square  unaryFunct;
        vec4plus    binaryFunct;
        vec4equal   binaryPredEq;
        vec4less    binaryPredLt;

        std::vector<vec4> input1(length, v4init);
        std::vector<vec4> input2(length, v4init);
        std::vector<vec4> input3(length, v4init);
        std::vector<vec4> output(length, v4iden);
        std::vector<vec4> output_merge(length * 2, v4iden);
        BOLT_ADD_DEPENDENCY(vec4, Bolt_DATA_TYPE);
        std::generate(input1.begin(), input1.end(),RandomNumber);
        std::generate(input2.begin(), input2.end(),RandomNumber);
        std::generate(input3.begin(), input3.end(),RandomNumber);
        std::generate(output.begin(), output.end(),RandomNumber);
        std::generate(output_merge.begin(), output_merge.end(), RandomNumber);

        if (hostMemory) {

            executeFunctionType( ctrl, input1, input2, input3, output, output_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
        else
        {
            bolt::cl::device_vector<vec4> binput1(input1.begin(), input1.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec4> binput2(input2.begin(), input2.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec4> binput3(input3.begin(), input3.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec4> boutput(output.begin(), output.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec4> boutput_merge(output_merge.begin(), output_merge.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            executeFunctionType( ctrl, input1, input2, input3, output, output_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
    }
    else if (vecType == t_vec8)
    {
        vec8gen     generator;
        vec8square  unaryFunct;
        vec8plus    binaryFunct;
        vec8equal   binaryPredEq;
        vec8less    binaryPredLt;
       siz = sizeof(vec8);

        std::vector<vec8> input1(length, v8init);
        std::vector<vec8> input2(length, v8init);
        std::vector<vec8> input3(length, v8init);
        std::vector<vec8> output(length, v8iden);
        std::vector<vec8> output_merge(length*2, v8iden);
        BOLT_ADD_DEPENDENCY(vec8, Bolt_DATA_TYPE);
        std::generate(input1.begin(), input1.end(),RandomNumber);
        std::generate(input2.begin(), input2.end(),RandomNumber);
        std::generate(input3.begin(), input3.end(),RandomNumber);
        std::generate(output.begin(), output.end(),RandomNumber);
        std::generate(output_merge.begin(), output_merge.end(),RandomNumber);

        if (hostMemory) {

            executeFunctionType( ctrl, input1, input2, input3, output, output_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
        else
        {
            bolt::cl::device_vector<vec8> binput1(input1.begin(), input1.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec8> binput2(input2.begin(), input2.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec8> binput3(input3.begin(), input3.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec8> boutput(output.begin(), output.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            bolt::cl::device_vector<vec8> boutput_merge(output_merge.begin(), output_merge.end(), BOLT_BENCH_DEVICE_VECTOR_FLAGS,  ctrl);
            executeFunctionType( ctrl, input1, input2, input3, output, output_merge,
                generator, unaryFunct, binaryFunct, binaryPredEq, binaryPredLt, routine, iterations,siz);
        }
    }
    else
    {
        std::cerr << "Unsupported vecType=" << vecType << std::endl;
    }

}

/******************************************************************************
 *
 *  Main
 *
 *****************************************************************************/
int _tmain( int argc, _TCHAR* argv[] )
{
    cl_int err = CL_SUCCESS;
    cl_uint userPlatform    = 0;
    cl_uint userDevice      = 0;
    size_t iterations       = 10;
    size_t length           = 1<<4;
    size_t vecType          = 0;
    size_t runMode          = 0;
    size_t routine          = f_scan;
    size_t numThrowAway     = 0;
     std::string function_called=functionNames[routine] ;
    std::string filename    = "bench.xml";
    cl_device_type deviceType = CL_DEVICE_TYPE_DEFAULT;
    bool defaultDevice      = true;
    bool print_clInfo       = false;
    bool hostMemory         = true;

    /******************************************************************************
     * Parse Command-line Parameters
     ******************************************************************************/
    try
    {
        // Declare the supported options.
        po::options_description desc( "OpenCL Scan command line options" );
        desc.add_options()
            ( "help,h",			"produces this help message" )
            ( "version,v",		"Print queryable version information from the Bolt CL library" )
            ( "queryOpenCL,q",  "Print queryable platform and device info and return" )
            ( "gpu,g",          "Report only OpenCL GPU devices" )
            ( "cpu,c",          "Report only OpenCL CPU devices" )
            ( "all,a",          "Report all OpenCL devices" )
            ( "deviceMemory,D",   "Allocate vectors in device memory; default is host memory" )
            ( "platform,p",     po::value< cl_uint >( &userPlatform )->default_value( userPlatform ),
                "Specify the platform under test using the index reported by -q flag" )
            ( "device,d",       po::value< cl_uint >( &userDevice )->default_value( userDevice ),
                "Specify the device under test using the index reported by the -q flag.  "
                "Index is relative with respect to -g, -c or -a flags" )
            ( "length,l",       po::value< size_t >( &length )->default_value( length ),
                "Length of scan array" )
            ( "iterations,i",   po::value< size_t >( &iterations )->default_value( iterations ),
                "Number of samples in timing loop" )
            ( "vecType,t",      po::value< size_t >( &vecType )->default_value( vecType ),
                "Data Type to use: 0-(1 value), 1-(2 values), 2-(4 values), 3-(8 values)" )
            ( "runMode,m",      po::value< size_t >( &runMode )->default_value( runMode ),
                "Run Mode: 0-Auto, 1-SerialCPU, 2-MultiCoreCPU, 3-GPU" )
            ( "function,f",      po::value< std::string >( &function_called )->default_value( function_called ),
                "Number of samples in timing loop" )
            ( "filename",     po::value< std::string >( &filename )->default_value( filename ),
                "Name of output file" )
            ( "throw-away",   po::value< size_t >( &numThrowAway )->default_value( numThrowAway ),
                "Number of trials to skip averaging" )
            ;

        po::variables_map vm;

        if(argc <= 1)
        {
            std::cout << desc << std::endl;
            return 0;
        }
        po::store( po::parse_command_line( argc, argv, desc ), vm );
        po::notify( vm );

        routine = get_functionindex(function_called);

        if( vm.count( "version" ) )
        {
            cl_uint libMajor, libMinor, libPatch;
            bolt::cl::getVersion( libMajor, libMinor, libPatch );

            const int indent = countOf( "Bolt version: " );
            bolt::tout << std::left << std::setw( indent ) << _T( "Bolt version: " )
                << libMajor << _T( "." )
                << libMinor << _T( "." )
                << libPatch << std::endl;
        }

        if( vm.count( "help" ) )
        {
            //	This needs to be 'cout' as program-options does not support wcout yet
            std::cout << desc << std::endl;
            return 0;
        }

        if( vm.count( "queryOpenCL" ) )
        {
            print_clInfo = true;
        }

        if( vm.count( "gpu" ) )
        {
            deviceType	= CL_DEVICE_TYPE_GPU;
        }

        if( vm.count( "cpu" ) )
        {
            deviceType	= CL_DEVICE_TYPE_CPU;
        }

        if( vm.count( "all" ) )
        {
            deviceType	= CL_DEVICE_TYPE_ALL;
        }

        if( vm.count( "deviceMemory" ) )
        {
            hostMemory = false;
        }
    }
    catch( std::exception& e )
    {
        std::cout << _T( "Scan Benchmark error condition reported:" ) << std::endl << e.what() << std::endl;
        return 1;
    }

    /******************************************************************************
     * Initialize platforms and devices
     ******************************************************************************/
#if defined(_WIN32)
    aProfiler.throwAway( numThrowAway );
#endif
    bolt::cl::control ctrl = bolt::cl::control::getDefault();

    std::string strDeviceName;
    if (runMode == 1) // serial cpu
    {
        ctrl.setForceRunMode( bolt::cl::control::SerialCpu );
        strDeviceName = "Serial CPU";
    }
    else if (runMode == 2) // multicore cpu
    {
        ctrl.setForceRunMode( bolt::cl::control::MultiCoreCpu );
        strDeviceName = "MultiCore CPU";
    }
    else // gpu || automatic
    {
        // Platform vector contains all available platforms on system
        std::vector< ::cl::Platform > platforms;
        bolt::cl::V_OPENCL( ::cl::Platform::get( &platforms ), "Platform::get() failed" );
        if( print_clInfo )
        {
            bolt::cl::control::printPlatforms(true,deviceType);
           //std::for_each( platforms.begin( ), platforms.end( ), printPlatformFunctor( 0 ) );
            return 0;
        }

        // Device info
        ::cl::Context myContext = bolt::cl::control::getDefault( ).getContext( );
        std::vector< cl::Device > devices = myContext.getInfo< CL_CONTEXT_DEVICES >();
        //::cl::CommandQueue myQueue( myContext, devices.at( userDevice ) , CL_QUEUE_PROFILING_ENABLE);
        ::cl::CommandQueue myQueue( myContext, devices.at( userDevice ));

        //  Now that the device we want is selected and we have created our own cl::CommandQueue, set it as the
        //  default cl::CommandQueue for the Bolt API
        ctrl.setCommandQueue( myQueue );

        strDeviceName = ctrl.getDevice( ).getInfo< CL_DEVICE_NAME >( &err );
        bolt::cl::V_OPENCL( err, "Device::getInfo< CL_DEVICE_NAME > failed" );
    }
    std::cout << "Device: " << strDeviceName << std::endl;

    /******************************************************************************
     * Select then Execute Function
     ******************************************************************************/
    executeFunction(
        ctrl,
        vecType,
        hostMemory,
        length,
        routine,
        iterations + numThrowAway
        );

    /******************************************************************************
     * Print Results
     ******************************************************************************/
#if defined(_WIN32)
    aProfiler.end();
#endif
    std::ofstream outFile( filename.c_str() );

#if defined(_WIN32)
    aProfiler.writeSum( outFile );
#endif
    outFile.close();
    return 0;
}


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2017-2018.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/move for documentation.
//
//////////////////////////////////////////////////////////////////////////////

//! \file

#ifndef BOOST_MOVE_DETAIL_HEAP_SORT_HPP
#define BOOST_MOVE_DETAIL_HEAP_SORT_HPP

#ifndef BOOST_CONFIG_HPP
#  include <boost/config.hpp>
#endif
#
#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

#include <boost/move/detail/config_begin.hpp>
#include <boost/move/detail/workaround.hpp>
#include <boost/move/detail/iterator_traits.hpp>
#include <boost/move/algo/detail/is_sorted.hpp>
#include <boost/move/utility_core.hpp>

namespace boost {  namespace movelib{

template <class RandomAccessIterator, class Compare>
class heap_sort_helper
{
   typedef typename boost::movelib::iterator_traits<RandomAccessIterator>::size_type  size_type;
   typedef typename boost::movelib::iterator_traits<RandomAccessIterator>::value_type value_type;

   static void adjust_heap(RandomAccessIterator first, size_type hole_index, size_type const len, value_type &value, Compare comp)
   {
      size_type const top_index = hole_index;
      size_type second_child = 2 * (hole_index + 1);

      while (second_child < len) {
         if (comp(*(first + second_child), *(first + (second_child - 1))))
            second_child--;
         *(first + hole_index) = boost::move(*(first + second_child));
         hole_index = second_child;
         second_child = 2 * (second_child + 1);
      }
      if (second_child == len) {
         *(first + hole_index) = boost::move(*(first + (second_child - 1)));
         hole_index = second_child - 1;
      }

      {  //push_heap-like ending
         size_type parent = (hole_index - 1) / 2;
         while (hole_index > top_index && comp(*(first + parent), value)) {
            *(first + hole_index) = boost::move(*(first + parent));
            hole_index = parent;
            parent = (hole_index - 1) / 2;
         }
         *(first + hole_index) = boost::move(value);
      }
   }

   static void make_heap(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
   {
      size_type const len = size_type(last - first);
      if (len > 1) {
         size_type parent = len/2u - 1u;

         do {
            value_type v(boost::move(*(first + parent)));
            adjust_heap(first, parent, len, v, comp);
         }while (parent--);
      }
   }

   static void sort_heap(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
   {
      size_type len = size_type(last - first);
      while (len > 1) {
         //move biggest to the safe zone
         --last;
         value_type v(boost::move(*last));
         *last = boost::move(*first);
         adjust_heap(first, size_type(0), --len, v, comp);
      }
   }

   public:
   static void sort(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
   {
      make_heap(first, last, comp);
      sort_heap(first, last, comp);
      BOOST_ASSERT(boost::movelib::is_sorted(first, last, comp));
   }
};

template <class RandomAccessIterator, class Compare>
BOOST_MOVE_FORCEINLINE void heap_sort(RandomAccessIterator first, RandomAccessIterator last, Compare comp)
{
   heap_sort_helper<RandomAccessIterator, Compare>::sort(first, last, comp);
}

}} //namespace boost {  namespace movelib{

#include <boost/move/detail/config_end.hpp>

#endif //#ifndef BOOST_MOVE_DETAIL_HEAP_SORT_HPP


#include <dynamic_trees/parallel_euler_tour_tree/include/euler_tour_tree.hpp>
#include <dynamic_trees/parallel_euler_tour_tree/tests/simple_forest_connectivity.hpp>

#include <boost/functional/hash.hpp>
#include <cassert>
#include <random>
#include <utility>

#include <utilities/include/debug.hpp>
#include <utilities/include/hash_pair.hpp>

using EulerTourTree = parallel_euler_tour_tree::EulerTourTree;

constexpr int num_vertices{500};
constexpr int link_attempts_per_round{400};
constexpr int cut_ratio{3};
constexpr int num_rounds{10};

void CheckAllPairsConnectivity(
    const SimpleForestConnectivity& reference_solution,
    const EulerTourTree& ett) {
  for (int u = 0; u < num_vertices; u++) {
    for (int v = 0; v < num_vertices; v++) {
      assert(reference_solution.IsConnected(u, v) == ett.IsConnected(u, v));
    }
  }
}

int main() {
  std::mt19937 rng{};
  rng.seed(0);
  std::uniform_int_distribution<std::mt19937::result_type>
    vert_dist{0, num_vertices - 1};
  std::uniform_int_distribution<std::mt19937::result_type>
    coin{0, 1};

  SimpleForestConnectivity reference_solution{num_vertices};
  EulerTourTree ett{num_vertices};
  std::unordered_set<std::pair<int, int>, HashIntPairStruct> edges{};
  std::pair<int, int>* ett_input{
      pbbs::new_array_no_init<pair<int, int>>(num_vertices)};
  for (int i = 0; i < num_rounds; i++) {
    // Generate `link_attempts_per_round` edges randomly, keeping each one that
    // doesn't add a cycle into the forest. Then call `BatchLink` on all of
    // them.
    int input_len{0};
    for (int j = 0; j < link_attempts_per_round; j++) {
      const unsigned long u{vert_dist(rng)}, v{vert_dist(rng)};
      if (!reference_solution.IsConnected(u, v)) {
        reference_solution.Link(u, v);
        edges.emplace(u, v);
        ett_input[input_len++] = std::make_pair(u, v);
      }
    }
    ett.BatchLink(ett_input, input_len);
    CheckAllPairsConnectivity(reference_solution, ett);

    // Call `BatchCut` over each `cut_ratio`-th edge.
    input_len = 0;
    int cnt{0};
    for (auto e : edges) {
      if (++cnt % cut_ratio == 0) {
        ett_input[input_len++] = e;
      }
    }
    for (int j = 0; j < input_len; j++) {
      pair<int, int> cut{ett_input[j]};
      edges.erase(cut);
      if (coin(rng) == 1) {
        ett_input[j] = make_pair(cut.second, cut.first);
      }
      reference_solution.Cut(cut.first, cut.second);
    }
    ett.BatchCut(ett_input, input_len);
    CheckAllPairsConnectivity(reference_solution, ett);
  }
  pbbs::delete_array(ett_input, num_vertices);

  std::cout << "Test complete." << std::endl;
}


/* twoway_layer_test.cc
   Jeremy Barnes, 9 November 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

   Unit tests for the twoway layer class.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#undef NDEBUG

#include <boost/test/unit_test.hpp>
#include <boost/multi_array.hpp>
#include "jml/neural/twoway_layer.h"
#include "jml/utils/testing/serialize_reconstitute_include.h"
#include <boost/assign/list_of.hpp>
#include <limits>
#include "bprop_test.h"
#include "jml/arch/exception_handler.h"

using namespace ML;
using namespace ML::DB;
using namespace std;

using boost::unit_test::test_suite;

#if 1

BOOST_AUTO_TEST_CASE( test_serialize_reconstitute_twoway_layer )
{
    Thread_Context context;
    int ni = 2, no = 4;
    Twoway_Layer layer("test", ni, no, TF_TANH, MV_ZERO, context);

    // Test equality operator
    BOOST_CHECK_EQUAL(layer, layer);

    Twoway_Layer layer2 = layer;
    BOOST_CHECK_EQUAL(layer, layer2);

    layer2.forward.weights[0][0] -= 1.0;
    BOOST_CHECK(layer != layer2);

    BOOST_CHECK_EQUAL(layer.forward.weights.shape()[0], ni);
    BOOST_CHECK_EQUAL(layer.forward.weights.shape()[1], no);
    BOOST_CHECK_EQUAL(layer.forward.bias.size(), no);
    BOOST_CHECK_EQUAL(layer.forward.missing_replacements.size(), 0);
    BOOST_CHECK_EQUAL(layer.forward.missing_activations.num_elements(), 0);
    BOOST_CHECK_EQUAL(layer.ibias.size(), ni);
    BOOST_CHECK_EQUAL(layer.iscales.size(), ni);
    BOOST_CHECK_EQUAL(layer.oscales.size(), no);

    test_serialize_reconstitute(layer);
    test_poly_serialize_reconstitute<Layer>(layer);
}

BOOST_AUTO_TEST_CASE( test_dense_layer_none )
{
    Twoway_Layer layer("test", 2, 1, TF_IDENTITY, MV_NONE);
    layer.forward.weights[0][0] = 0.5;
    layer.forward.weights[1][0] = 2.0;
    layer.forward.bias[0] = 0.0;
    layer.iscales[0] = 1.0;
    layer.iscales[1] = 1.0;
    layer.oscales[0] = 1.0;
    layer.ibias[0] = 0.0;
    layer.ibias[1] = 0.0;

    distribution<float> input
        = boost::assign::list_of<float>(1.0)(-1.0);

    // Check that the basic functions work
    BOOST_REQUIRE_EQUAL(layer.apply(input).size(), 1);
    BOOST_CHECK_EQUAL(layer.apply(input)[0], -1.5);

    // Check the missing values throw an exception
    input[0] = numeric_limits<float>::quiet_NaN();
    {
        JML_TRACE_EXCEPTIONS(false);
        BOOST_CHECK_THROW(layer.apply(input), ML::Exception);
    }

    // Check that the wrong size throws an exception
    input.push_back(2.0);
    input[0] = 1.0;
    {
        JML_TRACE_EXCEPTIONS(false);
        BOOST_CHECK_THROW(layer.apply(input), ML::Exception);
    }

    input.pop_back();

    // Check that the bias works
    layer.forward.bias[0] = 1.0;
    BOOST_CHECK_EQUAL(layer.apply(input)[0], -0.5);

    // Check that there are parameters
    BOOST_CHECK_EQUAL(layer.parameters().parameter_count(), 8);

    // Check the info
    BOOST_CHECK_EQUAL(layer.inputs(), 2);
    BOOST_CHECK_EQUAL(layer.outputs(), 1);
    BOOST_CHECK_EQUAL(layer.name(), "test");

    // Check the copy constructor
    Twoway_Layer layer2 = layer;
    BOOST_CHECK_EQUAL(layer2, layer);
    BOOST_CHECK_EQUAL(layer2.parameters().parameter_count(), 8);

    // Check the assignment operator
    Twoway_Layer layer3;
    BOOST_CHECK(layer3 != layer);
    layer3 = layer;
    BOOST_CHECK_EQUAL(layer3, layer);
    BOOST_CHECK_EQUAL(layer3.parameters().parameter_count(), 8);

    // Make sure that the assignment operator didn't keep a reference
    layer3.forward.weights[0][0] = 5.0;
    BOOST_CHECK_EQUAL(layer.forward.weights[0][0], 0.5);
    BOOST_CHECK_EQUAL(layer3.forward.weights[0][0], 5.0);
    layer3.forward.weights[0][0] = 0.5;
    BOOST_CHECK_EQUAL(layer, layer3);

    // Check fprop (that it gives the same result as apply)
    distribution<float> applied
        = layer.apply(input);

    size_t temp_space_size = layer.fprop_temporary_space_required();

    float temp_space[temp_space_size];

    distribution<float> fproped(layer.outputs());
    layer.fprop(&input[0], temp_space, temp_space_size, &fproped[0]);

    BOOST_CHECK_EQUAL_COLLECTIONS(applied.begin(), applied.end(),
                                  fproped.begin(), fproped.end());

    // Check parameters
    Parameters_Copy<float> params(layer.parameters());
    distribution<float> & param_dist = params.values;

    BOOST_CHECK_EQUAL(param_dist.size(), 8);
    BOOST_CHECK_EQUAL(param_dist.at(0), 0.5);  // weight 0
    BOOST_CHECK_EQUAL(param_dist.at(1), 2.0);  // weight 1
    BOOST_CHECK_EQUAL(param_dist.at(2), 1.0);  // bias

    Thread_Context context;
    layer3.random_fill(-1.0, context);

    BOOST_CHECK(layer != layer3);

    layer3.parameters().set(params);

    BOOST_CHECK_EQUAL(layer, layer3);

    // Check backprop
    distribution<float> output_errors(1, 1.0);
    distribution<float> input_errors(layer.inputs());
    Parameters_Copy<float> gradient(layer.parameters());
    gradient.fill(0.0);

    layer.bprop(&input[0], &fproped[0], temp_space, temp_space_size,
                &output_errors[0], &input_errors[0], gradient, 1.0);

    BOOST_CHECK_EQUAL(input_errors.size(), layer.inputs());

    // Check the values of input errors.  It's easy since there's only one
    // weight that contributes to each input (since there's only one output).
    BOOST_CHECK_EQUAL(input_errors[0], layer.forward.weights[0][0]);
    BOOST_CHECK_EQUAL(input_errors[1], layer.forward.weights[1][0]);

    //cerr << "input_errors = " << input_errors << endl;

    // Check that example_weight scales the gradient
    Parameters_Copy<float> gradient2(layer.parameters());
    gradient2.fill(0.0);
    layer.bprop(&input[0], &fproped[0], temp_space, temp_space_size,
                &output_errors[0], &input_errors[0], gradient2, 2.0);

    //cerr << "gradient.values = " << gradient.values << endl;
    //cerr << "gradient2.values = " << gradient2.values << endl;

    distribution<float> gradient_times_2 = gradient.values * 2.0;

    BOOST_CHECK_EQUAL_COLLECTIONS(gradient2.values.begin(),
                                  gradient2.values.end(),
                                  gradient_times_2.begin(),
                                  gradient_times_2.end());

    // Check that subtracting the parameters from each other returns a zero
    // parameter vector
    layer3.parameters().update(layer3.parameters(), -1.0);

    Parameters_Copy<float> layer3_params(layer3);
    BOOST_CHECK_EQUAL(layer3_params.values.total(), 0.0);

    BOOST_CHECK_EQUAL(layer3.forward.weights[0][0], 0.0);
    BOOST_CHECK_EQUAL(layer3.forward.weights[1][0], 0.0);
    BOOST_CHECK_EQUAL(layer3.forward.bias[0], 0.0);

    layer3.parameters().set(layer.parameters());
    BOOST_CHECK_EQUAL(layer, layer3);

    layer3.parameters().fill(0.0);
    layer3.parameters().update(layer.parameters(), 1.0);
    BOOST_CHECK_EQUAL(layer, layer3);

    layer3.zero_fill();
    layer3.parameters().set(params);
    BOOST_CHECK_EQUAL(layer, layer3);

    Parameters_Copy<double> layer_params2(layer);
    BOOST_CHECK((params.values == layer_params2.values).all());

    // Check that on serialize and reconstitute, the params get properly
    // updated.
    {
        ostringstream stream_out;
        {
            DB::Store_Writer writer(stream_out);
            writer << layer;
        }

        istringstream stream_in(stream_out.str());
        DB::Store_Reader reader(stream_in);
        Twoway_Layer layer4;
        reader >> layer4;

        Parameters_Copy<float> params4(layer4.parameters());
        distribution<float> & param_dist4 = params4.values;

        BOOST_REQUIRE_EQUAL(param_dist4.size(), 8);
        BOOST_CHECK_EQUAL(param_dist4.at(0), 0.5);  // weight 0
        BOOST_CHECK_EQUAL(param_dist4.at(1), 2.0);  // weight 1
        BOOST_CHECK_EQUAL(param_dist4.at(2), 1.0);  // bias

        layer4.forward.weights[0][0] = 5.0;
        params4 = layer4.parameters();
        BOOST_CHECK_EQUAL(param_dist4[0], 5.0);
    }
}

BOOST_AUTO_TEST_CASE( test_bprop_identity_double_none )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_IDENTITY, MV_NONE, context);

    bprop_test<double>(layer, context);
    bprop_test_backward<double>(layer, context, 0.1);

    // We have to leave a big error margin due to numerical issues in the
    // (long) calculation
    // A better test would allow higher error magnitudes when the gradients
    // were small.  We should also, at some stage, do a sensitivity
    // analysis to figure out how to improve the numerical stability of the
    // algorithms.
    bprop_test_reconstruct<double>(layer, context, 5.0);
}

BOOST_AUTO_TEST_CASE( test_bprop_tanh_double_none )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_TANH, MV_NONE, context);

    bprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bprop_identity_float_none )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_IDENTITY, MV_NONE, context);

    bprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bprop_identity_float_zero )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_IDENTITY, MV_ZERO, context);

    bprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bprop_identity_double_zero )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_IDENTITY, MV_ZERO, context);

    bprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bprop_identity_float_input )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_IDENTITY, MV_INPUT, context);

    bprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bprop_identity_float_dense )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_IDENTITY, MV_DENSE, context);

    bprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bprop_identity_double_dense )
{
    Thread_Context context;
    Twoway_Layer layer("test", 20, 40, TF_IDENTITY, MV_DENSE, context);

    bprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bbprop_identity_double_none1 )
{
    Thread_Context context;
    context.seed(123);
    Twoway_Layer layer("test", 1, 1, TF_IDENTITY, MV_NONE, context);

    bbprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bbprop_identity_double_none2 )
{
    Thread_Context context;
    context.seed(123);
    Twoway_Layer layer("test", 5, 5, TF_IDENTITY, MV_NONE, context);

    bbprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bbprop_tanh_double_none1 )
{
    Thread_Context context;
    context.seed(123);
    Twoway_Layer layer("test", 1, 1, TF_TANH, MV_NONE, context);

    bbprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bbprop_tanh_double_none2 )
{
    Thread_Context context;
    context.seed(123);
    Twoway_Layer layer("test", 5, 5, TF_TANH, MV_NONE, context);

    bbprop_test<double>(layer, context);
}

BOOST_AUTO_TEST_CASE( test_bbprop_identity_double_none )
{
    Thread_Context context;
    Twoway_Layer layer("test", 5, 5, TF_IDENTITY, MV_NONE, context);

    cerr << "layer = " << layer << endl;

    cerr << endl << endl << "*** TESTING BBPROP FORWARD" << endl;
    bbprop_test<double>(layer, context);

    cerr << endl << endl << "*** TESTING BBPROP BACKWARD" << endl;
    bbprop_test_backward<double>(layer, context);

    cerr << endl << endl << "*** TESTING BBPROP RECONSTRUCTION" << endl;
    // We have to leave a big error margin due to numerical issues in the
    // (long) calculation
    bbprop_test_reconstruct<double>(layer, context, 3.0);
}
#endif

#if 1
BOOST_AUTO_TEST_CASE( test_bbprop_tanh_double_none )
{
    Thread_Context context;
    Twoway_Layer layer("test", 1, 1, TF_IDENTITY, MV_NONE, context);

    layer.forward.weights[0][0] = 1.0;
    layer.forward.bias[0] = 0.0;
    layer.ibias[0] = 0.0;

    cerr << "layer = " << layer << endl;

    cerr << endl << endl << "*** TESTING BBPROP FORWARD" << endl;
    bbprop_test<double>(layer, context);

    cerr << endl << endl << "*** TESTING BBPROP BACKWARD" << endl;
    bbprop_test_backward<double>(layer, context);

    cerr << endl << endl << "*** TESTING BBPROP RECONSTRUCTION" << endl;
    // We have to leave a big error margin due to numerical issues in the
    // (long) calculation
    bbprop_test_reconstruct<double>(layer, context, 3.0);
}

BOOST_AUTO_TEST_CASE( test_bbprop_tanh_double_none3 )
{
    Thread_Context context;
    Twoway_Layer layer("test", 1, 1, TF_TANH, MV_NONE, context);

    cerr << "layer = " << layer << endl;

    cerr << endl << endl << "*** TESTING BBPROP FORWARD" << endl;
    bbprop_test<double>(layer, context);

    cerr << endl << endl << "*** TESTING BBPROP BACKWARD" << endl;
    bbprop_test_backward<double>(layer, context);

    cerr << endl << endl << "*** TESTING BBPROP RECONSTRUCTION" << endl;
    // We have to leave a big error margin due to numerical issues in the
    // (long) calculation
    bbprop_test_reconstruct<double>(layer, context, 3.0);
}
#endif


#include <boost/compute/functional/geometry.hpp>


#ifndef PARAM_UTIL_HPP
#define PARAM_UTIL_HPP

#include <Eigen/Eigen>
#include <ros/ros.h>
#include <iostream>
#include <vector>

namespace param_util {

/**
 * @brief Attmeps to load a matrix from the ROS parameter server or throws a
 * runtime_error.
 *
 * @tparam nRow Number of rows
 * @tparam nCol Number of collumns
 * @param t_nh A ROS Node Handle
 * @param matrix_name Name of the matrix parameter
 * @return Eigen::Matrix<double, nRow, nCol>
 */
template<int nRow, int nCol>
Eigen::Matrix<double, nRow, nCol> loadMatrixOrThrow(ros::NodeHandle &t_nh,
  const std::string &matrix_name)
{
  Eigen::Matrix<double, nRow, nCol> matrix;

  std::vector<double> tmp_vec;
  bool success = t_nh.getParam(matrix_name, tmp_vec);
  if (!success) {
    ROS_FATAL_STREAM("Unable to get param: [" << matrix_name << "]. Throwing...");
    throw std::runtime_error("Parameter initialization failed.");
  }

  if (static_cast<int>(tmp_vec.size()) != nRow * nCol) {
    ROS_FATAL_STREAM("Param: [" << matrix_name << "] of size " << tmp_vec.size()
                                << ". Expected " << nRow << "x" << nCol << "!");
    throw std::runtime_error("Parameter initialization failed.");
  }

  // Matrix is the correct size, assign it
  try {
    for (int i = 0; i < nRow; i++) {
      for (int j = 0; j < nCol; j++) { matrix(i, j) = tmp_vec[i * nCol + j]; }
    }
  } catch (std::runtime_error &e) {
    ROS_FATAL_STREAM("Loading param: [" << matrix_name << "] failed.");
    throw e;
  }

  // Assume everything is correct from here
  ROS_INFO_STREAM("Got param [" << matrix_name << "]\n" << matrix);
  return matrix;
}

/**
 * @brief Attempt to load a dynamic matrix from the ROS parameter server or throw a
 * runtimer error.
 *
 * @param t_nh A ROS node handle.
 * @param matrix_name Name of the matrix parameter.
 * @param row_count Number of matrix rows.
 * @param col_count Number of matrix columns.
 * @return Eigen::MatrixXd
 */
Eigen::MatrixXd loadMatrixOrThrow(ros::NodeHandle &t_nh,
  const std::string &matrix_name,
  int row_count,
  int col_count);

/**
 * @brief This output operator overload is needed to corrcly print out std::vector<T>
 * parameter types in the getParamOrThrow(...) function.
 *
 * @tparam T
 * @param o
 * @param vec
 * @return std::ostream&
 */
template<class T>
std::ostream& operator<<(std::ostream& o, const std::vector<T>& vec)
{
  o << "[";
  for (const auto& el : vec) {
    o << el << ", ";
  }
  o << "]";
  return o;
}

/**
 * @brief Attempt to get a value from the ROS parameter server or throws a runtime_error.
 *
 * @tparam T Value Typename
 * @param t_nh A ROS node handle
 * @param t_paramName ROS Parameter name.
 * @param t_paramContainer Variable where the parameter value will be saved.
 */
template<class T>
void getParamOrThrow(ros::NodeHandle &t_nh,
  const std::string &t_paramName,
  T &t_paramContainer)
{
  bool gotParam = t_nh.getParam(t_paramName, t_paramContainer);
  ROS_INFO_STREAM("Got param [" << t_paramName << "] = " << t_paramContainer);
  if (!gotParam) {
    ROS_FATAL_STREAM("Unable to get param: [" << t_paramName << "]. Throwing...");
    throw std::runtime_error("Parameter initialization failed.");
  }
}

/**
 * @brief Attemps to return a value from the ROS parameter server or throws a
 * runtime_error.
 *
 * @tparam T Value Typename
 * @param nh A ROS node handle
 * @param param_name ROS parameter name
 * @return T
 */
template<class T> T getParamOrThrow(ros::NodeHandle &nh, std::string param_name)
{
  T param;
  getParamOrThrow(nh, param_name, param);
  return param;
}

}// namespace param_util
#endif /* PARAM_UTIL_HPP */


#include <boost/hana/unfold_right.hpp>


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef TEST6_H
#define TEST6_H

#include <iostream>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;

#include "common/init.hpp"

void test_matrix_vector ();
void test_matrix ();

#endif


// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2017 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2014 - 2017)
//   Rene Milk       (2014, 2016 - 2017)

#ifndef DUNE_GDT_TEST_SPACES_DG_HH
#define DUNE_GDT_TEST_SPACES_DG_HH

#include <boost/numeric/conversion/cast.hpp>

#include <dune/xt/common/print.hh>
#include <dune/xt/common/ranges.hh>

#include "base.hh"

template <class SpaceType>
class DG_Space : public SpaceBase<SpaceType>
{
};

template <class SpaceType>
struct P1Q1_DG_Space : public SpaceBase<SpaceType>
{
  typedef typename SpaceType::GridLayerType GridLayerType;
  using GridType = Dune::XT::Grid::extract_grid_t<typename SpaceType::GridLayerType>;
  static const size_t dimDomain = GridType::dimension;
  typedef typename GridType::ctype DomainFieldType;
  typedef Dune::FieldVector<DomainFieldType, dimDomain> DomainType;

  static std::vector<DomainFieldType> convert_vector(const DomainType& source)
  {
    std::vector<DomainFieldType> ret(dimDomain, DomainFieldType(0));
    for (size_t ii = 0; ii < dimDomain; ++ii)
      ret[ii] = source[ii];
    return ret;
  }

  void maps_correctly()
  {
    using namespace Dune::XT;
    // walk the grid to create a map of all vertices
    std::map<std::vector<DomainFieldType>, std::pair<std::set<size_t>, size_t>> vertex_to_indices_map;
    const auto entity_end_it = this->space_.grid_layer().template end<0>();
    for (auto entity_it = this->space_.grid_layer().template begin<0>(); entity_it != entity_end_it; ++entity_it) {
      const auto& entity = *entity_it;
      for (auto cc : Dune::XT::Common::value_range(entity.subEntities(dimDomain))) {
        const auto vertex = entity.template subEntity<dimDomain>(cc);
        const DomainType vertex_center = vertex.geometry().center();
        vertex_to_indices_map[convert_vector(vertex_center)].first = std::set<size_t>();
        ++vertex_to_indices_map[convert_vector(vertex_center)].second;
      }
    }
    // walk the grid again to find all DoF ids
    for (auto entity_it = this->space_.grid_layer().template begin<0>(); entity_it != entity_end_it; ++entity_it) {
      const auto& entity = *entity_it;
      const size_t num_vertices = entity.subEntities(dimDomain);
      const auto basis = this->space_.base_function_set(entity);
      EXPECT_EQ(basis.size(), num_vertices);
      for (size_t cc = 0; cc < num_vertices; ++cc) {
        const auto vertex = entity.template subEntity<dimDomain>(boost::numeric_cast<int>(cc));
        const DomainType vertex_center = vertex.geometry().center();
        // find the local basis function which corresponds to this vertex
        const auto basis_values = basis.evaluate(entity.geometry().local(vertex_center));
        EXPECT_EQ(basis_values.size(), num_vertices);
        size_t ones = 0;
        size_t zeros = 0;
        size_t failures = 0;
        size_t local_DoF_index = 0;
        for (size_t ii = 0; ii < basis.size(); ++ii) {
          if (Common::FloatCmp::eq(basis_values[ii][0], typename SpaceType::RangeFieldType(1))) {
            local_DoF_index = ii;
            ++ones;
          } else if (Common::FloatCmp::eq(basis_values[ii][0] + 1, typename SpaceType::RangeFieldType(1)))
            ++zeros;
          else
            ++failures;
        }
        if (ones != 1 || zeros != (basis.size() - 1) || failures > 0) {
          std::stringstream ss;
          ss << "ones = " << ones << ", zeros = " << zeros << ", failures = " << failures
             << ", num_vertices = " << num_vertices << ", entity " << this->space_.grid_layer().indexSet().index(entity)
             << ", vertex " << cc << ": [ " << vertex_center << "], ";
          Dune::XT::Common::print(basis_values, "basis_values", ss);
          EXPECT_TRUE(false) << ss.str();
        }
        // now we know that the local DoF index of this vertex is ii
        const size_t global_DoF_index = this->space_.mapper().mapToGlobal(entity, local_DoF_index);
        vertex_to_indices_map[convert_vector(vertex_center)].first.insert(global_DoF_index);
      }
    }
    // check that each vertex has the appropiate number of associated DoF ids and that the numbering is consecutive
    std::set<size_t> global_DoF_indices;
    for (const auto& entry : vertex_to_indices_map) {
      const auto vertex_ids = entry.second.first;
      for (auto vertex_ids_it = vertex_ids.begin(); vertex_ids_it != vertex_ids.end(); ++vertex_ids_it)
        global_DoF_indices.insert(*vertex_ids_it);
    }
    size_t count = 0;
    for (const auto& global_DoF_id : global_DoF_indices) {
      EXPECT_EQ(global_DoF_id, count);
      ++count;
    }
    for (const auto& entry : vertex_to_indices_map) {
      const auto vertex_ids = entry.second.first;
      size_t number_of_associated_DoF_ids = vertex_ids.size();
      size_t number_of_adjacent_entitys = entry.second.second;
      EXPECT_EQ(number_of_associated_DoF_ids, number_of_adjacent_entitys);
    }
  } // ... maps_correctly()
}; // struct P1Q1_DG_Space

#endif // DUNE_GDT_TEST_SPACES_DG_HH