<commit_before>//  (C) Copyright Gennadiy Rozental 2001-2012.
//  (C) Copyright Beman Dawes 2001.
//  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/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : defines abstract monitor interfaces and implements execution exception
//  The original Boost Test Library included an implementation detail function
//  named catch_exceptions() which caught otherwise uncaught C++ exceptions.
//  It was derived from an existing test framework by Beman Dawes.  The
//  intent was to expand later to catch other detectable but platform dependent
//  error events like Unix signals or Windows structured C exceptions.
//
//  Requests from early adopters of the Boost Test Library included
//  configurable levels of error message detail, elimination of templates,
//  separation of error reporting, and making the catch_exceptions() facilities
//  available as a public interface.  Support for unit testing also stretched
//  the function based design.  Implementation within the header became less
//  attractive due to the need to include many huge system dependent headers,
//  although still preferable in certain cases.
//
//  All those issues have been addressed by introducing the class-based
//  design presented here.
// ***************************************************************************

#ifndef BOOST_TEST_EXECUTION_MONITOR_HPP_071894GER
#define BOOST_TEST_EXECUTION_MONITOR_HPP_071894GER

// Boost.Test
#include <boost/test/detail/global_typedef.hpp>
#include <boost/test/detail/fwd_decl.hpp>
#include <boost/test/utils/class_properties.hpp>

// Boost
#include <boost/shared_ptr.hpp>
#include <boost/scoped_array.hpp>
#include <boost/type.hpp>
#include <boost/cstdlib.hpp>
#include <boost/function/function0.hpp>

#include <boost/test/detail/suppress_warnings.hpp>

#ifdef BOOST_SEH_BASED_SIGNAL_HANDLING

// for the FP constants and control routines
#include <float.h>

#ifndef EM_INVALID
#define EM_INVALID _EM_INVALID
#endif

#ifndef EM_DENORMAL
#define EM_DENORMAL _EM_DENORMAL
#endif

#ifndef EM_ZERODIVIDE
#define EM_ZERODIVIDE _EM_ZERODIVIDE
#endif

#ifndef EM_OVERFLOW
#define EM_OVERFLOW _EM_OVERFLOW
#endif

#ifndef EM_UNDERFLOW
#define EM_UNDERFLOW _EM_UNDERFLOW
#endif

#ifndef MCW_EM
#define MCW_EM _MCW_EM
#endif

#else // based on ISO C standard

#if !defined(BOOST_NO_FENV_H)
  #include <boost/detail/fenv.hpp>
#endif

#endif

//____________________________________________________________________________//

namespace boost {

// ************************************************************************** //
// **************        detail::translator_holder_base        ************** //
// ************************************************************************** //

namespace detail {

class translator_holder_base;
typedef boost::shared_ptr<translator_holder_base> translator_holder_base_ptr;

class BOOST_TEST_DECL translator_holder_base {
protected:
    typedef boost::unit_test::const_string const_string;
public:
    // Constructor
    translator_holder_base( translator_holder_base_ptr next, const_string tag )
    : m_next( next )
    , m_tag( std::string() + tag )
    {
    }

    // Destructor
    virtual     ~translator_holder_base() {}

    // translator holder interface
    // invokes the function F inside the try/catch guarding against specific exception
    virtual int operator()( boost::function<int ()> const& F ) = 0;

    // erases specific translator holder from the chain
    translator_holder_base_ptr erase( translator_holder_base_ptr this_, const_string tag )
    {
        if( m_next )
            m_next = m_next->erase( m_next, tag );

        return m_tag == tag ? m_next : this_;
    }
#ifndef BOOST_NO_RTTI
    virtual translator_holder_base_ptr erase( translator_holder_base_ptr this_, std::type_info const& ) = 0;
    template<typename ExceptionType>
    translator_holder_base_ptr erase( translator_holder_base_ptr this_, boost::type<ExceptionType>* = 0 )
    {
        if( m_next )
            m_next = m_next->erase<ExceptionType>( m_next );

        return erase( this_, typeid(ExceptionType) );
    }
#endif

protected:
    // Data members
    translator_holder_base_ptr  m_next;
    std::string                 m_tag;
};

} // namespace detail

// ************************************************************************** //
// **************              execution_exception             ************** //
// ************************************************************************** //

//  design rationale: fear of being out (or nearly out) of memory.

class BOOST_TEST_DECL execution_exception {
    typedef boost::unit_test::const_string const_string;
public:
    enum error_code {
        //  These values are sometimes used as program return codes.
        //  The particular values have been chosen to avoid conflicts with
        //  commonly used program return codes: values < 100 are often user
        //  assigned, values > 255 are sometimes used to report system errors.
        //  Gaps in values allow for orderly expansion.

        no_error               = 0,   // for completeness only; never returned
        user_error             = 200, // user reported non-fatal error
        cpp_exception_error    = 205, // see note (1) below
        system_error           = 210, // see note (2) below
        timeout_error          = 215, // only detectable on certain platforms
        user_fatal_error       = 220, // user reported fatal error
        system_fatal_error     = 225  // see note (2) below

        //  Note 1: Only uncaught C++ exceptions are treated as errors.
        //  If the application catches a C++ exception, it will never reach
        //  the execution_monitor.

        //  Note 2: These errors include Unix signals and Windows structured
        //  exceptions.  They are often initiated by hardware traps.
        //
        //  The implementation decides what is a fatal_system_exception and what is
        //  just a system_exception.  Fatal errors are so likely to have corrupted
        //  machine state (like a stack overflow or addressing exception) that it
        //  is unreasonable to continue execution.
    };

    struct BOOST_TEST_DECL location {
        explicit    location( char const* file_name = 0, size_t line_num = 0, char const* func = 0 );

        const_string    m_file_name;
        size_t          m_line_num;
        const_string    m_function;
    };

    // Constructor
    execution_exception( error_code ec_, const_string what_msg_, location const& location_ ); // max length 256 inc '\0'

    // Access methods
    error_code      code() const    { return m_error_code; }
    const_string    what() const    { return m_what; }
    location const& where() const   { return m_location; }

private:
    // Data members
    error_code      m_error_code;
    const_string    m_what;
    location        m_location;
}; // execution_exception

// ************************************************************************** //
// **************               execution_monitor              ************** //
// ************************************************************************** //

class BOOST_TEST_DECL execution_monitor {
    typedef boost::unit_test::const_string const_string;
public:
    // Constructor
    execution_monitor();

    // Public properties

    //  The p_catch_system_errors parameter specifies whether the monitor should
    //  try to catch system errors/exceptions that would cause program to crash
    //  in regular case
    unit_test::readwrite_property<bool> p_catch_system_errors;

    //  The p_auto_start_dbg parameter specifies whether the monitor should
    //  try to attach debugger in case of caught system error
    unit_test::readwrite_property<bool> p_auto_start_dbg;

    //  The p_timeout parameter specifies the seconds that elapse before
    //  a timer_error occurs.  May be ignored on some platforms.
    unit_test::readwrite_property<int>  p_timeout;

    //  The p_use_alt_stack parameter specifies whether the monitor should
    //  use alternative stack for the signal catching
    unit_test::readwrite_property<bool> p_use_alt_stack;

    //  The p_detect_fp_exceptions parameter specifies whether the monitor should
    //  try to detect hardware floating point exceptions (!= 0), and which specific exception to catch
    unit_test::readwrite_property<unsigned> p_detect_fp_exceptions;

    int         execute( boost::function<int ()> const& F );
    //  Returns:  Value returned by function call F().
    //
    //  Effects:  Calls executes supplied function F inside a try/catch block which also may
    //  include other unspecified platform dependent error detection code.
    //
    //  Throws: execution_exception on an uncaught C++ exception,
    //  a hardware or software signal, trap, or other exception.
    //
    //  Note: execute() doesn't consider it an error for F to return a non-zero value.

    void         vexecute( boost::function<void ()> const& F );
    //  Effects:  Same as above, but returns nothing

    // register custom (user supplied) exception translator
    template<typename ExceptionType, typename ExceptionTranslator>
    void        register_exception_translator( ExceptionTranslator const& tr, const_string tag = const_string(), boost::type<ExceptionType>* = 0 );

    // erase custom exception translator
    void        erase_exception_translator( const_string tag )
    {
        m_custom_translators = m_custom_translators->erase( m_custom_translators, tag );
    }
    template<typename ExceptionType>
    void        erase_exception_translator( boost::type<ExceptionType>* = 0 )
    {
        m_custom_translators = m_custom_translators->template erase<ExceptionType>( m_custom_translators );
    }

private:
    // implementation helpers
    int         catch_signals( boost::function<int ()> const& F );

    // Data members
    detail::translator_holder_base_ptr  m_custom_translators;
    boost::scoped_array<char>           m_alt_stack;
}; // execution_monitor

// ************************************************************************** //
// **************          detail::translator_holder           ************** //
// ************************************************************************** //

namespace detail {

template<typename ExceptionType, typename ExceptionTranslator>
class translator_holder : public translator_holder_base
{
public:
    explicit    translator_holder( ExceptionTranslator const& tr, translator_holder_base_ptr& next, const_string tag = const_string() )
    : translator_holder_base( next, tag ), m_translator( tr ) {}

    // translator holder interface
    virtual int operator()( boost::function<int ()> const& F )
    {
        try {
            return m_next ? (*m_next)( F ) : F();
        } catch( ExceptionType const& e ) {
            m_translator( e );
            return boost::exit_exception_failure;
        }
    }
#ifndef BOOST_NO_RTTI
    virtual translator_holder_base_ptr erase( translator_holder_base_ptr this_, std::type_info const& ti )
    {
        return ti == typeid(ExceptionType) ? m_next : this_;
    }
#endif

private:
    // Data members
    ExceptionTranslator m_translator;
};

} // namespace detail

template<typename ExceptionType, typename ExceptionTranslator>
void
execution_monitor::register_exception_translator( ExceptionTranslator const& tr, const_string tag, boost::type<ExceptionType>* )
{
    m_custom_translators.reset(
        new detail::translator_holder<ExceptionType,ExceptionTranslator>( tr, m_custom_translators, tag ) );
}

// ************************************************************************** //
// **************               execution_aborted              ************** //
// ************************************************************************** //

struct execution_aborted {};

// ************************************************************************** //
// **************                  system_error                ************** //
// ************************************************************************** //

class system_error {
public:
    // Constructor
    explicit    system_error( char const* exp );

    unit_test::readonly_property<long>          p_errno;
    unit_test::readonly_property<char const*>   p_failed_exp;
};

#define BOOST_TEST_SYS_ASSERT( exp ) if( (exp) ) ; else throw ::boost::system_error( BOOST_STRINGIZE( exp ) )

// ************************************************************************** //
// **************Floating point exception management interface ************** //
// ************************************************************************** //

namespace fpe {

enum masks {
    BOOST_FPE_OFF       = 0,

#ifdef BOOST_SEH_BASED_SIGNAL_HANDLING
    BOOST_FPE_DIVBYZERO = EM_ZERODIVIDE,
    BOOST_FPE_INEXACT   = EM_INEXACT,
    BOOST_FPE_INVALID   = EM_INVALID,
    BOOST_FPE_OVERFLOW  = EM_OVERFLOW,
    BOOST_FPE_UNDERFLOW = EM_UNDERFLOW|EM_DENORMAL,

    BOOST_FPE_ALL       = MCW_EM,
#elif defined(BOOST_NO_FENV_H) || defined(BOOST_CLANG)
    BOOST_FPE_ALL       = 1,
#else
    BOOST_FPE_DIVBYZERO = FE_DIVBYZERO,
    BOOST_FPE_INEXACT   = FE_INEXACT,
    BOOST_FPE_INVALID   = FE_INVALID,
    BOOST_FPE_OVERFLOW  = FE_OVERFLOW,
    BOOST_FPE_UNDERFLOW = FE_UNDERFLOW,

    BOOST_FPE_ALL       = FE_ALL_EXCEPT,
#endif
    BOOST_FPE_INV       = BOOST_FPE_ALL+1
};

//____________________________________________________________________________//

// return the previous set of enabled exceptions when successful, and BOOST_FPE_INV otherwise
unsigned BOOST_TEST_DECL enable( unsigned mask );
unsigned BOOST_TEST_DECL disable( unsigned mask );

//____________________________________________________________________________//

} // namespace fpe
}  // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif

<commit_msg>More correct fix for Boost.Test and RTTI-off<commit_after>//  (C) Copyright Gennadiy Rozental 2001-2012.
//  (C) Copyright Beman Dawes 2001.
//  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/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : defines abstract monitor interfaces and implements execution exception
//  The original Boost Test Library included an implementation detail function
//  named catch_exceptions() which caught otherwise uncaught C++ exceptions.
//  It was derived from an existing test framework by Beman Dawes.  The
//  intent was to expand later to catch other detectable but platform dependent
//  error events like Unix signals or Windows structured C exceptions.
//
//  Requests from early adopters of the Boost Test Library included
//  configurable levels of error message detail, elimination of templates,
//  separation of error reporting, and making the catch_exceptions() facilities
//  available as a public interface.  Support for unit testing also stretched
//  the function based design.  Implementation within the header became less
//  attractive due to the need to include many huge system dependent headers,
//  although still preferable in certain cases.
//
//  All those issues have been addressed by introducing the class-based
//  design presented here.
// ***************************************************************************

#ifndef BOOST_TEST_EXECUTION_MONITOR_HPP_071894GER
#define BOOST_TEST_EXECUTION_MONITOR_HPP_071894GER

// Boost.Test
#include <boost/test/detail/global_typedef.hpp>
#include <boost/test/detail/fwd_decl.hpp>
#include <boost/test/utils/class_properties.hpp>

// Boost
#include <boost/shared_ptr.hpp>
#include <boost/scoped_array.hpp>
#include <boost/type.hpp>
#include <boost/cstdlib.hpp>
#include <boost/function/function0.hpp>

#include <boost/test/detail/suppress_warnings.hpp>

#ifdef BOOST_SEH_BASED_SIGNAL_HANDLING

// for the FP constants and control routines
#include <float.h>

#ifndef EM_INVALID
#define EM_INVALID _EM_INVALID
#endif

#ifndef EM_DENORMAL
#define EM_DENORMAL _EM_DENORMAL
#endif

#ifndef EM_ZERODIVIDE
#define EM_ZERODIVIDE _EM_ZERODIVIDE
#endif

#ifndef EM_OVERFLOW
#define EM_OVERFLOW _EM_OVERFLOW
#endif

#ifndef EM_UNDERFLOW
#define EM_UNDERFLOW _EM_UNDERFLOW
#endif

#ifndef MCW_EM
#define MCW_EM _MCW_EM
#endif

#else // based on ISO C standard

#if !defined(BOOST_NO_FENV_H)
  #include <boost/detail/fenv.hpp>
#endif

#endif

//____________________________________________________________________________//

namespace boost {

// ************************************************************************** //
// **************        detail::translator_holder_base        ************** //
// ************************************************************************** //

namespace detail {

class translator_holder_base;
typedef boost::shared_ptr<translator_holder_base> translator_holder_base_ptr;

class BOOST_TEST_DECL translator_holder_base {
protected:
    typedef boost::unit_test::const_string const_string;
public:
    // Constructor
    translator_holder_base( translator_holder_base_ptr next, const_string tag )
    : m_next( next )
    , m_tag( std::string() + tag )
    {
    }

    // Destructor
    virtual     ~translator_holder_base() {}

    // translator holder interface
    // invokes the function F inside the try/catch guarding against specific exception
    virtual int operator()( boost::function<int ()> const& F ) = 0;

    // erases specific translator holder from the chain
    translator_holder_base_ptr erase( translator_holder_base_ptr this_, const_string tag )
    {
        if( m_next )
            m_next = m_next->erase( m_next, tag );

        return m_tag == tag ? m_next : this_;
    }
#ifndef BOOST_NO_RTTI
    virtual translator_holder_base_ptr erase( translator_holder_base_ptr this_, std::type_info const& ) = 0;
    template<typename ExceptionType>
    translator_holder_base_ptr erase( translator_holder_base_ptr this_, boost::type<ExceptionType>* = 0 )
    {
        if( m_next )
            m_next = m_next->erase<ExceptionType>( m_next );

        return erase( this_, typeid(ExceptionType) );
    }
#endif

protected:
    // Data members
    translator_holder_base_ptr  m_next;
    std::string                 m_tag;
};

} // namespace detail

// ************************************************************************** //
// **************              execution_exception             ************** //
// ************************************************************************** //

//  design rationale: fear of being out (or nearly out) of memory.

class BOOST_TEST_DECL execution_exception {
    typedef boost::unit_test::const_string const_string;
public:
    enum error_code {
        //  These values are sometimes used as program return codes.
        //  The particular values have been chosen to avoid conflicts with
        //  commonly used program return codes: values < 100 are often user
        //  assigned, values > 255 are sometimes used to report system errors.
        //  Gaps in values allow for orderly expansion.

        no_error               = 0,   // for completeness only; never returned
        user_error             = 200, // user reported non-fatal error
        cpp_exception_error    = 205, // see note (1) below
        system_error           = 210, // see note (2) below
        timeout_error          = 215, // only detectable on certain platforms
        user_fatal_error       = 220, // user reported fatal error
        system_fatal_error     = 225  // see note (2) below

        //  Note 1: Only uncaught C++ exceptions are treated as errors.
        //  If the application catches a C++ exception, it will never reach
        //  the execution_monitor.

        //  Note 2: These errors include Unix signals and Windows structured
        //  exceptions.  They are often initiated by hardware traps.
        //
        //  The implementation decides what is a fatal_system_exception and what is
        //  just a system_exception.  Fatal errors are so likely to have corrupted
        //  machine state (like a stack overflow or addressing exception) that it
        //  is unreasonable to continue execution.
    };

    struct BOOST_TEST_DECL location {
        explicit    location( char const* file_name = 0, size_t line_num = 0, char const* func = 0 );

        const_string    m_file_name;
        size_t          m_line_num;
        const_string    m_function;
    };

    // Constructor
    execution_exception( error_code ec_, const_string what_msg_, location const& location_ ); // max length 256 inc '\0'

    // Access methods
    error_code      code() const    { return m_error_code; }
    const_string    what() const    { return m_what; }
    location const& where() const   { return m_location; }

private:
    // Data members
    error_code      m_error_code;
    const_string    m_what;
    location        m_location;
}; // execution_exception

// ************************************************************************** //
// **************               execution_monitor              ************** //
// ************************************************************************** //

class BOOST_TEST_DECL execution_monitor {
    typedef boost::unit_test::const_string const_string;
public:
    // Constructor
    execution_monitor();

    // Public properties

    //  The p_catch_system_errors parameter specifies whether the monitor should
    //  try to catch system errors/exceptions that would cause program to crash
    //  in regular case
    unit_test::readwrite_property<bool> p_catch_system_errors;

    //  The p_auto_start_dbg parameter specifies whether the monitor should
    //  try to attach debugger in case of caught system error
    unit_test::readwrite_property<bool> p_auto_start_dbg;

    //  The p_timeout parameter specifies the seconds that elapse before
    //  a timer_error occurs.  May be ignored on some platforms.
    unit_test::readwrite_property<int>  p_timeout;

    //  The p_use_alt_stack parameter specifies whether the monitor should
    //  use alternative stack for the signal catching
    unit_test::readwrite_property<bool> p_use_alt_stack;

    //  The p_detect_fp_exceptions parameter specifies whether the monitor should
    //  try to detect hardware floating point exceptions (!= 0), and which specific exception to catch
    unit_test::readwrite_property<unsigned> p_detect_fp_exceptions;

    int         execute( boost::function<int ()> const& F );
    //  Returns:  Value returned by function call F().
    //
    //  Effects:  Calls executes supplied function F inside a try/catch block which also may
    //  include other unspecified platform dependent error detection code.
    //
    //  Throws: execution_exception on an uncaught C++ exception,
    //  a hardware or software signal, trap, or other exception.
    //
    //  Note: execute() doesn't consider it an error for F to return a non-zero value.

    void         vexecute( boost::function<void ()> const& F );
    //  Effects:  Same as above, but returns nothing

    // register custom (user supplied) exception translator
    template<typename ExceptionType, typename ExceptionTranslator>
    void        register_exception_translator( ExceptionTranslator const& tr, const_string tag = const_string(), boost::type<ExceptionType>* = 0 );

    // erase custom exception translator
    void        erase_exception_translator( const_string tag )
    {
        m_custom_translators = m_custom_translators->erase( m_custom_translators, tag );
    }
#ifndef BOOST_NO_RTTI
    template<typename ExceptionType>
    void        erase_exception_translator( boost::type<ExceptionType>* = 0 )
    {
        m_custom_translators = m_custom_translators->erase<ExceptionType>( m_custom_translators );
    }
#endif

private:
    // implementation helpers
    int         catch_signals( boost::function<int ()> const& F );

    // Data members
    detail::translator_holder_base_ptr  m_custom_translators;
    boost::scoped_array<char>           m_alt_stack;
}; // execution_monitor

// ************************************************************************** //
// **************          detail::translator_holder           ************** //
// ************************************************************************** //

namespace detail {

template<typename ExceptionType, typename ExceptionTranslator>
class translator_holder : public translator_holder_base
{
public:
    explicit    translator_holder( ExceptionTranslator const& tr, translator_holder_base_ptr& next, const_string tag = const_string() )
    : translator_holder_base( next, tag ), m_translator( tr ) {}

    // translator holder interface
    virtual int operator()( boost::function<int ()> const& F )
    {
        try {
            return m_next ? (*m_next)( F ) : F();
        } catch( ExceptionType const& e ) {
            m_translator( e );
            return boost::exit_exception_failure;
        }
    }
#ifndef BOOST_NO_RTTI
    virtual translator_holder_base_ptr erase( translator_holder_base_ptr this_, std::type_info const& ti )
    {
        return ti == typeid(ExceptionType) ? m_next : this_;
    }
#endif

private:
    // Data members
    ExceptionTranslator m_translator;
};

} // namespace detail

template<typename ExceptionType, typename ExceptionTranslator>
void
execution_monitor::register_exception_translator( ExceptionTranslator const& tr, const_string tag, boost::type<ExceptionType>* )
{
    m_custom_translators.reset(
        new detail::translator_holder<ExceptionType,ExceptionTranslator>( tr, m_custom_translators, tag ) );
}

// ************************************************************************** //
// **************               execution_aborted              ************** //
// ************************************************************************** //

struct execution_aborted {};

// ************************************************************************** //
// **************                  system_error                ************** //
// ************************************************************************** //

class system_error {
public:
    // Constructor
    explicit    system_error( char const* exp );

    unit_test::readonly_property<long>          p_errno;
    unit_test::readonly_property<char const*>   p_failed_exp;
};

#define BOOST_TEST_SYS_ASSERT( exp ) if( (exp) ) ; else throw ::boost::system_error( BOOST_STRINGIZE( exp ) )

// ************************************************************************** //
// **************Floating point exception management interface ************** //
// ************************************************************************** //

namespace fpe {

enum masks {
    BOOST_FPE_OFF       = 0,

#ifdef BOOST_SEH_BASED_SIGNAL_HANDLING
    BOOST_FPE_DIVBYZERO = EM_ZERODIVIDE,
    BOOST_FPE_INEXACT   = EM_INEXACT,
    BOOST_FPE_INVALID   = EM_INVALID,
    BOOST_FPE_OVERFLOW  = EM_OVERFLOW,
    BOOST_FPE_UNDERFLOW = EM_UNDERFLOW|EM_DENORMAL,

    BOOST_FPE_ALL       = MCW_EM,
#elif defined(BOOST_NO_FENV_H) || defined(BOOST_CLANG)
    BOOST_FPE_ALL       = 1,
#else
    BOOST_FPE_DIVBYZERO = FE_DIVBYZERO,
    BOOST_FPE_INEXACT   = FE_INEXACT,
    BOOST_FPE_INVALID   = FE_INVALID,
    BOOST_FPE_OVERFLOW  = FE_OVERFLOW,
    BOOST_FPE_UNDERFLOW = FE_UNDERFLOW,

    BOOST_FPE_ALL       = FE_ALL_EXCEPT,
#endif
    BOOST_FPE_INV       = BOOST_FPE_ALL+1
};

//____________________________________________________________________________//

// return the previous set of enabled exceptions when successful, and BOOST_FPE_INV otherwise
unsigned BOOST_TEST_DECL enable( unsigned mask );
unsigned BOOST_TEST_DECL disable( unsigned mask );

//____________________________________________________________________________//

} // namespace fpe
}  // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif

<|endoftext|>


<commit_before>#pragma once

#include <cilantro/data_containers.hpp>

namespace cilantro {
    struct IndexAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = 0};

        inline IndexAccumulator() {}

        inline IndexAccumulator(size_t i) : indices(1,i) {}

        inline IndexAccumulator& mergeWith(const IndexAccumulator &other) {
            indices.insert(indices.end(), other.indices.begin(), other.indices.end());
            return *this;
        }

        std::vector<size_t> indices;
    };

    class IndexAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef IndexAccumulator Accumulator;

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(i);
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.indices.emplace_back(i);
            return accum;
        }
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointSumAccumulator(size_t dim = 0) : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)), pointCount(0) {}

        inline PointSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point)
                : pointSum(point), pointCount(1)
        {}

        inline PointSumAccumulator& mergeWith(const PointSumAccumulator &other) {
            pointSum += other.pointSum;
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points)
                : points_(points)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointNormalSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointNormalSumAccumulator(size_t dim = 0)
                : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  normalSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  pointCount(0)
        {}

        inline PointNormalSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point,
                                         const Eigen::Ref<const Vector<ScalarT,EigenDim>> &normal)
                : pointSum(point), normalSum(normal), pointCount(1)
        {}

        inline PointNormalSumAccumulator& mergeWith(const PointNormalSumAccumulator &other) {
            pointSum += other.pointSum;
            normalSum += other.normalSum;
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        Vector<ScalarT,EigenDim> normalSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointNormalSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointNormalSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointNormalSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points,
                                              const ConstVectorSetMatrixMap<ScalarT,EigenDim> &normals)
                : points_(points), normals_(normals)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i), normals_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            if (accum.normalSum.dot(normals_.col(i)) < (ScalarT)0.0) {
                accum.normalSum -= normals_.col(i);
            } else {
                accum.normalSum += normals_.col(i);
            }
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
        ConstVectorSetMatrixMap<ScalarT,EigenDim> normals_;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointColorSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointColorSumAccumulator(size_t dim = 0)
                : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  colorSum(Vector<float,3>::Zero()),
                  pointCount(0)
        {}

        inline PointColorSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point,
                                        const Eigen::Ref<const Vector<float,3>> &color)
                : pointSum(point), colorSum(color), pointCount(1)
        {}

        inline PointColorSumAccumulator& mergeWith(const PointColorSumAccumulator &other) {
            pointSum += other.pointSum;
            colorSum += other.colorSum;
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        Vector<float,3> colorSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointColorSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointColorSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointColorSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points,
                                             const ConstVectorSetMatrixMap<float,3> &colors)
                : points_(points), colors_(colors)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i), colors_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            accum.colorSum += colors_.col(i);
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
        ConstVectorSetMatrixMap<float,3> colors_;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointNormalColorSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointNormalColorSumAccumulator(size_t dim = 0)
                : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  normalSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  colorSum(Vector<float,3>::Zero()),
                  pointCount(0)
        {}

        inline PointNormalColorSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point,
                                              const Eigen::Ref<const Vector<ScalarT,EigenDim>> &normal,
                                              const Eigen::Ref<const Vector<float,3>> &color)
                : pointSum(point), normalSum(normal), colorSum(color), pointCount(1)
        {}

        inline PointNormalColorSumAccumulator& mergeWith(const PointNormalColorSumAccumulator &other) {
            pointSum += other.pointSum;
            normalSum += other.normalSum;
            colorSum += other.colorSum;
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        Vector<ScalarT,EigenDim> normalSum;
        Vector<float,3> colorSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointNormalColorSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointNormalColorSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointNormalColorSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points,
                                                   const ConstVectorSetMatrixMap<ScalarT,EigenDim> &normals,
                                                   const ConstVectorSetMatrixMap<float,3> &colors)
                : points_(points), normals_(normals), colors_(colors)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i), normals_.col(i), colors_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            if (accum.normalSum.dot(normals_.col(i)) < (ScalarT)0.0) {
                accum.normalSum -= normals_.col(i);
            } else {
                accum.normalSum += normals_.col(i);
            }
            accum.colorSum += colors_.col(i);
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
        ConstVectorSetMatrixMap<ScalarT,EigenDim> normals_;
        ConstVectorSetMatrixMap<float,3> colors_;
    };
}
<commit_msg>Fixed discrepancy in enforcing normal consistency in parallel grid accumulator<commit_after>#pragma once

#include <cilantro/data_containers.hpp>

namespace cilantro {
    struct IndexAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = 0};

        inline IndexAccumulator() {}

        inline IndexAccumulator(size_t i) : indices(1,i) {}

        inline IndexAccumulator& mergeWith(const IndexAccumulator &other) {
            indices.insert(indices.end(), other.indices.begin(), other.indices.end());
            return *this;
        }

        std::vector<size_t> indices;
    };

    class IndexAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef IndexAccumulator Accumulator;

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(i);
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.indices.emplace_back(i);
            return accum;
        }
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointSumAccumulator(size_t dim = 0) : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)), pointCount(0) {}

        inline PointSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point)
                : pointSum(point), pointCount(1)
        {}

        inline PointSumAccumulator& mergeWith(const PointSumAccumulator &other) {
            pointSum += other.pointSum;
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points)
                : points_(points)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointNormalSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointNormalSumAccumulator(size_t dim = 0)
                : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  normalSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  pointCount(0)
        {}

        inline PointNormalSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point,
                                         const Eigen::Ref<const Vector<ScalarT,EigenDim>> &normal)
                : pointSum(point), normalSum(normal), pointCount(1)
        {}

        inline PointNormalSumAccumulator& mergeWith(const PointNormalSumAccumulator &other) {
            pointSum += other.pointSum;
            if (normalSum.dot(other.normalSum) < (ScalarT)0.0) {
                normalSum -= other.normalSum;
            } else {
                normalSum += other.normalSum;
            }
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        Vector<ScalarT,EigenDim> normalSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointNormalSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointNormalSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointNormalSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points,
                                              const ConstVectorSetMatrixMap<ScalarT,EigenDim> &normals)
                : points_(points), normals_(normals)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i), normals_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            if (accum.normalSum.dot(normals_.col(i)) < (ScalarT)0.0) {
                accum.normalSum -= normals_.col(i);
            } else {
                accum.normalSum += normals_.col(i);
            }
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
        ConstVectorSetMatrixMap<ScalarT,EigenDim> normals_;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointColorSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointColorSumAccumulator(size_t dim = 0)
                : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  colorSum(Vector<float,3>::Zero()),
                  pointCount(0)
        {}

        inline PointColorSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point,
                                        const Eigen::Ref<const Vector<float,3>> &color)
                : pointSum(point), colorSum(color), pointCount(1)
        {}

        inline PointColorSumAccumulator& mergeWith(const PointColorSumAccumulator &other) {
            pointSum += other.pointSum;
            colorSum += other.colorSum;
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        Vector<float,3> colorSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointColorSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointColorSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointColorSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points,
                                             const ConstVectorSetMatrixMap<float,3> &colors)
                : points_(points), colors_(colors)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i), colors_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            accum.colorSum += colors_.col(i);
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
        ConstVectorSetMatrixMap<float,3> colors_;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    struct PointNormalColorSumAccumulator {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum {EigenAlign = (EigenDim != Eigen::Dynamic) && (sizeof(Vector<ScalarT,EigenDim>) % 16 == 0)};

        inline PointNormalColorSumAccumulator(size_t dim = 0)
                : pointSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  normalSum(Vector<ScalarT,EigenDim>::Zero(dim,1)),
                  colorSum(Vector<float,3>::Zero()),
                  pointCount(0)
        {}

        inline PointNormalColorSumAccumulator(const Eigen::Ref<const Vector<ScalarT,EigenDim>> &point,
                                              const Eigen::Ref<const Vector<ScalarT,EigenDim>> &normal,
                                              const Eigen::Ref<const Vector<float,3>> &color)
                : pointSum(point), normalSum(normal), colorSum(color), pointCount(1)
        {}

        inline PointNormalColorSumAccumulator& mergeWith(const PointNormalColorSumAccumulator &other) {
            pointSum += other.pointSum;
            if (normalSum.dot(other.normalSum) < (ScalarT)0.0) {
                normalSum -= other.normalSum;
            } else {
                normalSum += other.normalSum;
            }
            colorSum += other.colorSum;
            pointCount += other.pointCount;
            return *this;
        }

        Vector<ScalarT,EigenDim> pointSum;
        Vector<ScalarT,EigenDim> normalSum;
        Vector<float,3> colorSum;
        size_t pointCount;
    };

    template <typename ScalarT, ptrdiff_t EigenDim>
    class PointNormalColorSumAccumulatorProxy {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointNormalColorSumAccumulator<ScalarT,EigenDim> Accumulator;

        inline PointNormalColorSumAccumulatorProxy(const ConstVectorSetMatrixMap<ScalarT,EigenDim> &points,
                                                   const ConstVectorSetMatrixMap<ScalarT,EigenDim> &normals,
                                                   const ConstVectorSetMatrixMap<float,3> &colors)
                : points_(points), normals_(normals), colors_(colors)
        {}

        inline Accumulator buildAccumulator(size_t i) const {
            return Accumulator(points_.col(i), normals_.col(i), colors_.col(i));
        }

        inline Accumulator& addToAccumulator(Accumulator& accum, size_t i) const {
            accum.pointSum += points_.col(i);
            if (accum.normalSum.dot(normals_.col(i)) < (ScalarT)0.0) {
                accum.normalSum -= normals_.col(i);
            } else {
                accum.normalSum += normals_.col(i);
            }
            accum.colorSum += colors_.col(i);
            accum.pointCount++;
            return accum;
        }

    private:
        ConstVectorSetMatrixMap<ScalarT,EigenDim> points_;
        ConstVectorSetMatrixMap<ScalarT,EigenDim> normals_;
        ConstVectorSetMatrixMap<float,3> colors_;
    };
}
<|endoftext|>


<commit_before>/**
 * \file
 * \brief StaticRawFifoQueue class header
 *
 * \author Copyright (C) 2015 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * \date 2015-10-21
 */

#ifndef INCLUDE_DISTORTOS_STATICRAWFIFOQUEUE_HPP_
#define INCLUDE_DISTORTOS_STATICRAWFIFOQUEUE_HPP_

#include "RawFifoQueue.hpp"

#include "distortos/memory/dummyDeleter.hpp"

namespace distortos
{

/**
 * \brief StaticRawFifoQueue class is a wrapper for RawFifoQueue that also provides automatic storage for queue's
 * contents.
 *
 * \note Objects of this class can be safely casted to (const) reference to RawFifoQueue.
 *
 * \param T is the type of data in queue
 * \param QueueSize is the maximum number of elements in queue
 */

template<typename T, size_t QueueSize>
class StaticRawFifoQueue
{
public:

	/**
	 * \brief StaticRawFifoQueue's constructor
	 */

	explicit StaticRawFifoQueue() :
			rawFifoQueue_{{storage_.data(), memory::dummyDeleter}, sizeof(*storage_.data()), storage_.size()}
	{

	}

	/**
	 * \brief conversion to RawFifoQueue&
	 *
	 * \return reference to internal RawFifoQueue object
	 */

	operator RawFifoQueue&()
	{
		return rawFifoQueue_;
	}

	/**
	 * \brief conversion to const RawFifoQueue&
	 *
	 * \return const reference to internal RawFifoQueue object
	 */

	operator const RawFifoQueue&() const
	{
		return rawFifoQueue_;
	}

	/**
	 * \brief Wrapper for RawFifoQueue::pop(void*, size_t)
	 */

	int pop(void* const buffer, const size_t size)
	{
		return rawFifoQueue_.pop(buffer, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::pop(T&)
	 */

	template<typename U>
	int pop(U& buffer)
	{
		return rawFifoQueue_.pop(buffer);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::push(const void*, size_t)
	 */

	int push(const void* const data, const size_t size)
	{
		return rawFifoQueue_.push(data, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::push(const T&)
	 */

	template<typename U>
	int push(const U& data)
	{
		return rawFifoQueue_.push(data);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPop(void*, size_t)
	 */

	int tryPop(void* const buffer, const size_t size)
	{
		return rawFifoQueue_.tryPop(buffer, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPop(T&)
	 */

	template<typename U>
	int tryPop(U& buffer)
	{
		return rawFifoQueue_.tryPop(buffer);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPopFor(std::chrono::duration<Rep, Period>, void*, size_t)
	 */

	template<typename Rep, typename Period>
	int tryPopFor(const std::chrono::duration<Rep, Period> duration, void* const buffer, const size_t size)
	{
		return rawFifoQueue_.tryPopFor(duration, buffer, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPopFor(std::chrono::duration<Rep, Period>, T&)
	 */

	template<typename Rep, typename Period, typename U>
	int tryPopFor(const std::chrono::duration<Rep, Period> duration, U& buffer)
	{
		return rawFifoQueue_.tryPopFor(duration, buffer);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPopUntil(std::chrono::time_point<TickClock, Duration>, void*, size_t)
	 */

	template<typename Duration>
	int tryPopUntil(const std::chrono::time_point<TickClock, Duration> timePoint, void* const buffer, const size_t size)
	{
		return rawFifoQueue_.tryPopUntil(timePoint, buffer, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPopUntil(std::chrono::time_point<TickClock, Duration>, T&)
	 */

	template<typename Duration, typename U>
	int tryPopUntil(const std::chrono::time_point<TickClock, Duration> timePoint, U& buffer)
	{
		return rawFifoQueue_.tryPopUntil(timePoint, buffer);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPush(const void*, size_t)
	 */

	int tryPush(const void* const data, const size_t size)
	{
		return rawFifoQueue_.tryPush(data, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPush(const T&)
	 */

	template<typename U>
	int tryPush(const U& data)
	{
		return rawFifoQueue_.tryPush(data);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPushFor(std::chrono::duration<Rep, Period>, const void*, size_t)
	 */

	template<typename Rep, typename Period>
	int tryPushFor(const std::chrono::duration<Rep, Period> duration, const void* const data, const size_t size)
	{
		return rawFifoQueue_.tryPushFor(duration, data, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPushFor(std::chrono::duration<Rep, Period>, const T&)
	 */

	template<typename Rep, typename Period, typename U>
	int tryPushFor(const std::chrono::duration<Rep, Period> duration, const U& data)
	{
		return rawFifoQueue_.tryPushFor(duration, data);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPushUntil(std::chrono::time_point<TickClock, Duration>, const void*, size_t)
	 */

	template<typename Duration>
	int tryPushUntil(const std::chrono::time_point<TickClock, Duration> timePoint, const void* const data,
			const size_t size)
	{
		return rawFifoQueue_.tryPushUntil(timePoint, data, size);
	}

	/**
	 * \brief Wrapper for RawFifoQueue::tryPushUntil(std::chrono::time_point<TickClock, Duration>, const T&)
	 */

	template<typename Duration, typename U>
	int tryPushUntil(const std::chrono::time_point<TickClock, Duration> timePoint, const U& data)
	{
		return rawFifoQueue_.tryPushUntil(timePoint, data);
	}

private:

	/// storage for queue's contents
	std::array<typename std::aligned_storage<sizeof(T), alignof(T)>::type, QueueSize> storage_;

	/// internal RawFifoQueue object
	RawFifoQueue rawFifoQueue_;
};

/**
 * \brief StaticRawFifoQueueFromSize type alias is a variant of StaticRawFifoQueue which uses size of element (instead
 * of type) as template argument.
 *
 * \param ElementSize is the size of single queue element, bytes
 * \param QueueSize is the maximum number of elements in queue
 */

template<size_t ElementSize, size_t QueueSize>
using StaticRawFifoQueueFromSize =
		StaticRawFifoQueue<typename std::aligned_storage<ElementSize, ElementSize>::type, QueueSize>;

}	// namespace distortos

#endif	// INCLUDE_DISTORTOS_STATICRAWFIFOQUEUE_HPP_
<commit_msg>StaticRawFifoQueue: inherit from FifoQueue<commit_after>/**
 * \file
 * \brief StaticRawFifoQueue class header
 *
 * \author Copyright (C) 2015 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * \date 2015-10-21
 */

#ifndef INCLUDE_DISTORTOS_STATICRAWFIFOQUEUE_HPP_
#define INCLUDE_DISTORTOS_STATICRAWFIFOQUEUE_HPP_

#include "RawFifoQueue.hpp"

#include "distortos/memory/dummyDeleter.hpp"

namespace distortos
{

/**
 * \brief StaticRawFifoQueue class is a variant of RawFifoQueue that has automatic storage for queue's contents.
 *
 * \param T is the type of data in queue
 * \param QueueSize is the maximum number of elements in queue
 */

template<typename T, size_t QueueSize>
class StaticRawFifoQueue : public RawFifoQueue
{
public:

	/**
	 * \brief StaticRawFifoQueue's constructor
	 */

	explicit StaticRawFifoQueue() :
			RawFifoQueue{{storage_.data(), memory::dummyDeleter}, sizeof(*storage_.data()), storage_.size()}
	{

	}

private:

	/// storage for queue's contents
	std::array<typename std::aligned_storage<sizeof(T), alignof(T)>::type, QueueSize> storage_;
};

/**
 * \brief StaticRawFifoQueueFromSize type alias is a variant of StaticRawFifoQueue which uses size of element (instead
 * of type) as template argument.
 *
 * \param ElementSize is the size of single queue element, bytes
 * \param QueueSize is the maximum number of elements in queue
 */

template<size_t ElementSize, size_t QueueSize>
using StaticRawFifoQueueFromSize =
		StaticRawFifoQueue<typename std::aligned_storage<ElementSize, ElementSize>::type, QueueSize>;

}	// namespace distortos

#endif	// INCLUDE_DISTORTOS_STATICRAWFIFOQUEUE_HPP_
<|endoftext|>


<commit_before>#include <stdio.h>
#include <math.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdint.h>
#include <sys/mman.h>
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <sched.h>

#include <algorithm>
#include <assert.h>

//=============== Macro abstractions =================

#define SM2(x,y) ((x)+(y))
#define SM4(x,y,z,w) SM2(SM2(x,y),SM2(z,w))
#define SM8(a,b,c,d,e,f,g,h) SM2(SM4(a,b,c,d),SM4(e,f,g,h))

#if L1_PENALTY
#define OUTSIDE_THRESH_UPDATE updateL1
#else
#define OUTSIDE_THRESH_UPDATE solveDepressedCubic2
#endif

#define HOUSEHOLDER_ITER (10)
#if 4==V_LN
#define FV_SET1(x) {(x),(x),(x),(x)}
#else
#define FV_SET1(x) {(x),(x),(x),(x),(x),(x),(x),(x)}
#endif
#define FV_ZERO() FV_SET1(0.0f)

#define V_SZ (4*V_LN)
typedef float FV __attribute__((vector_size(V_SZ)));
typedef int BV __attribute__((vector_size(V_SZ)));

#if V_LN==8 /*kludgy way of detecting avx with its fma instruction*/
#define fma(a,b,c) (a)*(b)+(c)
#else
#define fma(a,b,c) (a)*(b)+(c)
#endif

#define FORCE_READ(p,o) (*((volatile FV*)(p+o)))
#define FORCE_WRITE(p,i,x) (*(volatile FV*)(p+i))=x

//================ Data structures =====================

struct ControlData {
    float initLambda,lamdaStep,lambdaLIm;
};

struct Memory {
    float *sharedParams;
    FV** original;
    FV* publishedPredictions[NO_CPUS];
    FV* parameterEstimates[NO_CPUS];
    FV*** perThread1;
    FV*** perThread2;
    FV*** perThread3;
    int32_t* syncFutexes;
    int ourIdx;
};

//================== inter-CPU data reading/writing ===============

/*
 *Add in to results the contributions from the otherUnits and multiply resulting value by multipliers.
 *otherUnits are read using FORCE_READ to get latest values wrtten by other CPUs.
 *results has already been initialized with a partal contribution.
 *Note that while some "partial update skew" is inevitable, we want to do this as fast as possible
 *to minimise the amount.
 *Doing 5 accumulations at once is the most possible before hitting register spills.
 */
void accSumOtherContributions(FV* results,FV* ourValues,FV** otherUnits,FV* multipliers,int noOtherUnits,int noBlks)
{
    assert(noOtherUnits==NO_CPUS);
    FV bigAcc0=FV_ZERO(),bigAcc1=FV_ZERO(),bigAcc2=FV_ZERO(),bigAcc3=FV_ZERO(),bigAcc4=FV_ZERO();
    int i=0;
    do{
        FV acc=ourValues[i];
        int unit=0;
        do{
            acc=acc+FORCE_READ(otherUnits[unit],i);
        }while (++unit<NO_CPUS-1);
        bigAcc0=fma(acc,multipliers[0],bigAcc0);
        bigAcc1=fma(acc,multipliers[1],bigAcc1);
        bigAcc2=fma(acc,multipliers[2],bigAcc2);
        bigAcc3=fma(acc,multipliers[3],bigAcc3);
        bigAcc4=fma(acc,multipliers[4],bigAcc4);
        multipliers+=6;

    }while(++i<noBlks);
    results[0]=bigAcc0;
    results[1]=bigAcc1;
    results[2]=bigAcc2;
    results[3]=bigAcc3;
    results[4]=bigAcc4;
}

/*
 *Write pre-prepared values into the published array for this unit.
 *Use FORCE_WRITE to ensure results are written immediately, and use
 *a loop to minimise time values from different passes exist in the cache.
 */
void writeContributionForThisUnit(FV* publish,FV *values,int noBlks)
{
    int i=0;
    do{
        FORCE_WRITE(publish,i,values[i]);
    }while(++i<noBlks);
}

//=========== Preparing "this CPU" problem components ============

/*
 *
 */
void updateContributions(FV* values,int noBlks)
{

}

/*
 *
 */
void prepareMultipliers(FV* multipliers,int noBlks)
{

}

//==================== update functions ======================

/*solve a vector of depressed cubics starting from a previous soln of x*/
//inline
void solveDepressedCubic(FV *result, FV p,FV q,FV x,float lambda,int householderIter)
{
    do{
        FV t1=3*x;
        FV t2=t1*x+p;
        FV t3=(x*x+p)*x+q;
        FV num=t3*t2;
        FV denom=t2*t2-t1*t3;
        x=x-num/denom;
/*        if(fabs((num/denom)/x)<=1e-6) break;*/
    }while(--householderIter>0);
    *result=x;
}

//inline
void solveDepressedCubic2(FV *result,FV p,FV q,FV x,float ambda,int householderIter)
{
    int l=0;
    do{
        FV t1=3*x;
        FV fderiv=t1*x+p;
        FV fderivSq=fderiv*fderiv;
        FV f=(x*x+p)*x+q;
        FV hlfffdderiv=f*t1;
        FV num=f*(fderivSq-hlfffdderiv);
        FV fSq=f*f;
        FV denom=fderiv*(fderivSq-2*hlfffdderiv)+fSq;
        x=x-num/denom;
    }while(++l<householderIter);
    *result=x;
}

//inline
void updateL1(FV *result,FV p,FV q,FV x,float lambda,int householderIter)
{
    BV gtLambda=x>=lambda;
    *result=gtLambda ? (x-lambda) : (x+lambda);
}

void formAndSolveUpdate(FV* result,FV *others,FV *corrections,FV *here,FV *params,float lambda,int noBlks,int householderIter)
{
    float thresh=lambda;
    do{
        FV t0=*result-*corrections;//we've kept some corrections here
        FV t1=*here/t0;
        FV t2=*params/t0;
//        BV toKeep=(t2>=thresh) | (t2<=-thresh);//record which should be moved to zero
        //get the update which is correct outside the threshold region
        OUTSIDE_THRESH_UPDATE(result,t1,t2,*params,thresh,householderIter);
        FV zeroes=FV_ZERO();
//        *result=toKeep ? (*result) : zeroes;
    }while(--noBlks>0);
}

//================= setting up data structures ===================

void* allocSharedArray(int no32BitWords)
{
    void *p=mmap(0,4*no32BitWords,PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS,0,0);
    return p;
}

template<class T>
T** add2DSuperstructure(void* raw,int noRows,int noCols)
{
    T* p=reinterpret_cast<T*>(raw);
    T** super=new T*[noRows];
    for(int i=0;i<noRows;++i){
        super[i]=&(p[i*noCols]);
    }
    return super;
}

template<class T>
T*** add3DSuperstructure(void* raw,int noRows,int noCols)
{
    T* p=reinterpret_cast<T*>(raw);
    T** super=new T*[noRows];
    for(int i=0;i<noRows;++i){
        super[i]=&(p[i*noCols]);
    }
    return &super;
}

void setupSharedMemoryAreas(Memory *mem)
{
    int r,c;
    mem->sharedParams=reinterpret_cast<float*>(allocSharedArray(1));
    mem->original=add2DSuperstructure<FV>(allocSharedArray(0),r,c);
    for(int i=0;i<NO_CPUS;++i){
        mem->publishedPredictions[i]=reinterpret_cast<FV*>(allocSharedArray(0));
        mem->parameterEstimates[i]=reinterpret_cast<FV*>(allocSharedArray(0));
    }
    mem->perThread1=0;
    mem->perThread2=0;
    mem->perThread3=0;
    mem->syncFutexes=reinterpret_cast<int*>(allocSharedArray(0));
    mem->syncFutexes[0]=NO_CPUS;
    mem->ourIdx=-1;
}

void figureLinearRegressionProblemDivision() {

}

//============== running processes on CPUs ================

void exportSolution(ControlData *control,Memory *mem)
{
    for(int i=0;i<0;++i){
        fprintf(stdout,"%u %g\n",i,(double)0.0f);
    }
    fflush(stdout);

}

void runAThread(Memory *mem)
{

}

void
fireUpForks(ControlData *control,Memory *mem)
{
    pid_t pids[NO_CPUS];
    pid_t parentPID=getpid();
    for(int i=0;i<NO_CPUS;++i){
        pid_t thisPID;
        if((thisPID=getpid())!=0){//we're the child
            //it's easier if our publication array is final pointer in array
            FV* t=mem->publishedPredictions[i];
            mem->publishedPredictions[i]=mem->publishedPredictions[NO_CPUS-1];
            mem->publishedPredictions[NO_CPUS-1]=t;
            mem->ourIdx=NO_CPUS-1;
            cpu_set_t *cpu_mask=CPU_ALLOC(NO_CPUS);
            CPU_ZERO(cpu_mask);
            CPU_SET(i,cpu_mask);
            int err=sched_setaffinity(0,sizeof(cpu_set_t),cpu_mask);
            CPU_FREE(cpu_mask);
            runAThread(mem);
            _exit(0);
        }
    }
    exportSolution(control,mem);
}

int main(int argc,char* argv[])
{
    ControlData ctrl;
    Memory mem;
    fireUpForks(&ctrl,&mem);

    return 0;
}
<commit_msg>More code added<commit_after>#include <stdio.h>
#include <math.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdint.h>
#include <sys/mman.h>
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <sched.h>

#include <algorithm>
#include <assert.h>

//=============== Macro abstractions =================

#define SM2(x,y) ((x)+(y))
#define SM4(x,y,z,w) SM2(SM2(x,y),SM2(z,w))
#define SM8(a,b,c,d,e,f,g,h) SM2(SM4(a,b,c,d),SM4(e,f,g,h))

#if L1_PENALTY
#define OUTSIDE_THRESH_UPDATE updateL1
#else
#define OUTSIDE_THRESH_UPDATE solveDepressedCubic2
#endif

#define HOUSEHOLDER_ITER (10)
#if 4==V_LN
#define FV_SET1(x) {(x),(x),(x),(x)}
#else
#define FV_SET1(x) {(x),(x),(x),(x),(x),(x),(x),(x)}
#endif
#define FV_ZERO() FV_SET1(0.0f)

#define V_SZ (4*V_LN)
typedef float FV __attribute__((vector_size(V_SZ)));
typedef int BV __attribute__((vector_size(V_SZ)));

#if V_LN==8 /*kludgy way of detecting avx with its fma instruction*/
#define fma(a,b,c) (a)*(b)+(c)
#else
#define fma(a,b,c) (a)*(b)+(c)
#endif

#define FORCE_READ(p,o) (*((volatile FV*)(p+o)))
#define FORCE_WRITE(p,i,x) (*(volatile FV*)(p+i))=x

//================ Data structures =====================

struct ControlData {
    float initLambda,lamdaStep,lambdaLIm;
};

struct Memory {
    float *sharedParams;
    FV** original;
    FV* publishedPredictions[NO_CPUS];
    FV* parameterEstimates[NO_CPUS];
    FV*** perThread1;
    FV*** perThread2;
    FV*** perThread3;
    int32_t* syncFutexes;
    int ourIdx;
    int noParams,noNonZero;
    int* nonzeroParams;//local to the core working on this
};

//================== inter-CPU data reading/writing ===============

/*
 *Add in to results the contributions from the otherUnits and multiply resulting value by multipliers.
 *otherUnits are read using FORCE_READ to get latest values wrtten by other CPUs.
 *results has already been initialized with a partal contribution.
 *Note that while some "partial update skew" is inevitable, we want to do this as fast as possible
 *to minimise the amount.
 *Doing 5 accumulations at once is the most possible before hitting register spills.
 */
void accSumOtherContributions(FV* results,FV* ourValues,FV** otherUnits,FV* multipliers,int noOtherUnits,int noBlks)
{
    assert(noOtherUnits==NO_CPUS);
    FV bigAcc0=FV_ZERO(),bigAcc1=FV_ZERO(),bigAcc2=FV_ZERO(),bigAcc3=FV_ZERO(),bigAcc4=FV_ZERO();
    int i=0;
    do{
        FV acc=ourValues[i];
        int unit=0;
        do{
            acc=acc+FORCE_READ(otherUnits[unit],i);
        }while (++unit<NO_CPUS-1);
        bigAcc0=fma(acc,multipliers[0],bigAcc0);
        bigAcc1=fma(acc,multipliers[1],bigAcc1);
        bigAcc2=fma(acc,multipliers[2],bigAcc2);
        bigAcc3=fma(acc,multipliers[3],bigAcc3);
        bigAcc4=fma(acc,multipliers[4],bigAcc4);
        multipliers+=6;

    }while(++i<noBlks);
    results[0]=bigAcc0;
    results[1]=bigAcc1;
    results[2]=bigAcc2;
    results[3]=bigAcc3;
    results[4]=bigAcc4;
}

/*
 *Write pre-prepared values into the published array for this unit.
 *Use FORCE_WRITE to ensure results are written immediately, and use
 *a loop to minimise time values from different passes exist in the cache.
 */
void writeContributionForThisUnit(FV* publish,FV *values,int noBlks)
{
    int i=0;
    do{
        FORCE_WRITE(publish,i,values[i]);
    }while(++i<noBlks);
}

//=========== Preparing "this CPU" problem components ============

/*
 *
 */
void updateContributions(FV* values,int noBlks)
{

}

/*
 *
 */
void prepareMultipliers(FV* multipliers,int noBlks)
{

}

//==================== update functions ======================

/*solve a vector of depressed cubics starting from a previous soln of x*/
//inline
void solveDepressedCubic(FV *result, FV p,FV q,FV x,float lambda,int householderIter)
{
    do{
        FV t1=3*x;
        FV t2=t1*x+p;
        FV t3=(x*x+p)*x+q;
        FV num=t3*t2;
        FV denom=t2*t2-t1*t3;
        x=x-num/denom;
/*        if(fabs((num/denom)/x)<=1e-6) break;*/
    }while(--householderIter>0);
    *result=x;
}

//inline
void solveDepressedCubic2(FV *result,FV p,FV q,FV x,float ambda,int householderIter)
{
    int l=0;
    do{
        FV t1=3*x;
        FV fderiv=t1*x+p;
        FV fderivSq=fderiv*fderiv;
        FV f=(x*x+p)*x+q;
        FV hlfffdderiv=f*t1;
        FV num=f*(fderivSq-hlfffdderiv);
        FV fSq=f*f;
        FV denom=fderiv*(fderivSq-2*hlfffdderiv)+fSq;
        x=x-num/denom;
    }while(++l<householderIter);
    *result=x;
}

//inline
void updateL1(FV *result,FV p,FV q,FV x,float lambda,int householderIter)
{
    BV gtLambda=x>=lambda;
    *result=gtLambda ? (x-lambda) : (x+lambda);
}

void formAndSolveUpdate(FV* result,FV *others,FV *corrections,FV *here,FV *params,float lambda,int noBlks,int householderIter)
{
    float thresh=lambda;
    do{
        FV t0=*result-*corrections;//we've kept some corrections here
        FV t1=*here/t0;
        FV t2=*params/t0;
        BV toKeep=(t2>=thresh) | (t2<=-thresh);//record which should be moved to zero
        //get the update which is correct outside the threshold region
        OUTSIDE_THRESH_UPDATE(result,t1,t2,*params,thresh,householderIter);
        FV zeroes=FV_ZERO();
        *result=toKeep ? (*result) : zeroes;
    }while(--noBlks>0);
}

void f()
{
    FV* result;
    formAndSolveUpdate(result,FV *others,FV *corrections,FV *here,FV *params,float lambda,int noBlks,int householderIter);
    //put the results back into the correct place
    float *arr=reinterpret_cast<float*>(result);
    int i;
    for(i=0;i<noBlks*FV_LN;++i){
        if(arr[i]==0.0f){
            //remove from non-zero bitmap
        }
    }
}

//================= setting up data structures ===================

void* allocSharedArray(int no32BitWords)
{
    void *p=mmap(0,4*no32BitWords,PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS,0,0);
    return p;
}

template<class T>
T** add2DSuperstructure(void* raw,int noRows,int noCols)
{
    T* p=reinterpret_cast<T*>(raw);
    T** super=new T*[noRows];
    for(int i=0;i<noRows;++i){
        super[i]=&(p[i*noCols]);
    }
    return super;
}

template<class T>
T*** add3DSuperstructure(void* raw,int noRows,int noCols)
{
    T* p=reinterpret_cast<T*>(raw);
    T** super=new T*[noRows];
    for(int i=0;i<noRows;++i){
        super[i]=&(p[i*noCols]);
    }
    return &super;
}

void setupSharedMemoryAreas(Memory *mem)
{
    int r,c;
    mem->sharedParams=reinterpret_cast<float*>(allocSharedArray(1));
    mem->original=add2DSuperstructure<FV>(allocSharedArray(0),r,c);
    for(int i=0;i<NO_CPUS;++i){
        mem->publishedPredictions[i]=reinterpret_cast<FV*>(allocSharedArray(0));
        mem->parameterEstimates[i]=reinterpret_cast<FV*>(allocSharedArray(0));
    }
    mem->perThread1=0;
    mem->perThread2=0;
    mem->perThread3=0;
    mem->syncFutexes=reinterpret_cast<int*>(allocSharedArray(0));
    mem->syncFutexes[0]=NO_CPUS;
    mem->ourIdx=-1;
}

void figureLinearRegressionProblemDivision() {

}

//============== running processes on CPUs ================

void exportSolution(ControlData *control,Memory *mem)
{
    for(int i=0;i<0;++i){
        fprintf(stdout,"%u %g\n",i,(double)0.0f);
    }
    fflush(stdout);

}

void runAThread(Memory *mem)
{

}

void
fireUpForks(ControlData *control,Memory *mem)
{
    pid_t pids[NO_CPUS];
    pid_t parentPID=getpid();
    for(int i=0;i<NO_CPUS;++i){
        pid_t thisPID;
        if((thisPID=getpid())!=0){//we're the child
            //it's easier if our publication array is final pointer in array
            FV* t=mem->publishedPredictions[i];
            mem->publishedPredictions[i]=mem->publishedPredictions[NO_CPUS-1];
            mem->publishedPredictions[NO_CPUS-1]=t;
            mem->ourIdx=NO_CPUS-1;
            cpu_set_t *cpu_mask=CPU_ALLOC(NO_CPUS);
            CPU_ZERO(cpu_mask);
            CPU_SET(i,cpu_mask);
            int err=sched_setaffinity(0,sizeof(cpu_set_t),cpu_mask);
            CPU_FREE(cpu_mask);
            runAThread(mem);
            _exit(0);
        }
    }
    exportSolution(control,mem);
}

int main(int argc,char* argv[])
{
    ControlData ctrl;
    Memory mem;
    fireUpForks(&ctrl,&mem);

    return 0;
}
<|endoftext|>


<commit_before>// Tests to drive abstract sharing analysis

#include "types.h"
#include "user.h"
#include "fcntl.h"
#include "mtrace.h"

static int cpu;

void
next()
{
  if (setaffinity(cpu) < 0) {
    cpu = 0;
    if (setaffinity(cpu) < 0)
      die("sys_setaffinity(%d) failed", cpu);
  }
  cpu++;
}

void
vmsharing(void)
{
  for (int i = 0; i < 8; i++) {
    next();

    volatile char *p = (char*)(0x40000UL + i * 4096);
    mtenable("xv6-mapsharing");
    if (map((void *) p, 4096) < 0)
      die("map failed");
    mtdisable("xv6-mapsharing");

    mtenable("xv6-mapsharing");
    if (unmap((void *) p, 4096) < 0)
      die("unmap failed");
    mtdisable("xv6-mapsharing");
  }
}

void
fssharing(void)
{
  // Note that we keep these files open; otherwise all of these
  // operations will share the abstract FD object and we won't get any
  // results.

  next();
  mtenable("xv6-fssharing");
  open("a", O_CREATE|O_RDWR);
  mtdisable("xv6-fssharing");

  next();
  mtenable("xv6-fssharing");
  open("b", O_CREATE|O_RDWR);
  mtdisable("xv6-fssharing");

  next();
  mtenable("xv6-fssharing");
  open("a", O_RDWR);
  mtdisable("xv6-fssharing");

  next();
  mtenable("xv6-fssharing");
  open("b", O_RDWR);
  mtdisable("xv6-fssharing");
}

int
main(int ac, char **av)
{
  if (ac == 2 && strcmp(av[1], "vm") == 0)
    vmsharing();
  else if (ac == 2 && strcmp(av[1], "fs") == 0)
    fssharing();
  else
    fprintf(1, "usage: %s vm|fs\n", av[0]);
}
<commit_msg>avoid kernel stack sharing by spawning distinct threads per core<commit_after>// Tests to drive abstract sharing analysis

#include "types.h"
#include "user.h"
#include "fcntl.h"
#include "mtrace.h"
#include "pthread.h"

static int cpu;
static pthread_barrier_t bar;
enum { ncore = 8 };

void
next()
{
  if (setaffinity(cpu) < 0) {
    cpu = 0;
    if (setaffinity(cpu) < 0)
      die("sys_setaffinity(%d) failed", cpu);
  }
  cpu++;
}

void*
vmsharing(void* arg)
{
  u64 i = (u64) arg;

  volatile char *p = (char*)(0x40000UL + i * 4096);
  mtenable("xv6-mapsharing");
  if (map((void *) p, 4096) < 0)
    die("map failed");
  mtdisable("xv6-mapsharing");

  mtenable("xv6-mapsharing");
  if (unmap((void *) p, 4096) < 0)
    die("unmap failed");
  mtdisable("xv6-mapsharing");

  return 0;
}

void*
fssharing(void* arg)
{
  u64 i = (u64) arg;

  // Note that we keep these files open; otherwise all of these
  // operations will share the abstract FD object and we won't get any
  // results.

  char filename[32];
  snprintf(filename, sizeof(filename), "f%d", i);

  mtenable("xv6-fssharing");
  open(filename, O_CREATE|O_RDWR);
  mtdisable("xv6-fssharing");

  pthread_barrier_wait(&bar);

  for (u64 j = 0; j < ncore; j++) {
    snprintf(filename, sizeof(filename), "f%d", j);
    mtenable("xv6-fssharing");
    open(filename, O_RDWR);
    mtdisable("xv6-fssharing");
  }
  return 0;
}

int
main(int ac, char **av)
{
  void* (*op)(void*) = 0;
  if (ac == 2 && strcmp(av[1], "vm") == 0)
    op = vmsharing;
  else if (ac == 2 && strcmp(av[1], "fs") == 0)
    op = fssharing;
  else
    fprintf(1, "usage: %s vm|fs\n", av[0]);

  if (op) {
    pthread_barrier_init(&bar, 0, ncore);
    for (u64 i = 0; i < ncore; i++) {
      next();
      pthread_t tid;
      pthread_create(&tid, 0, op, (void*) i);
    }
  }
}
<|endoftext|>


<commit_before><commit_msg>small fix<commit_after><|endoftext|>


<commit_before>// -*- mode:C++; tab-width:3; c-basic-offset:4; indent-tabs-mode:nil -*-

//Copyright: Universidad Carlos III de Madrid (C) 2016
//Authors: jgvictores, raulfdzbis, smorante

#include "CgdaIronFitnessFunction.hpp"

#define NTPOINTS 17
#define NFEATURES 3
#define NSQUARES 16

namespace teo
{

/************************************************************************/

void CgdaIronFitnessFunction::registerParameters(StateP state) {
	state->getRegistry()->registerEntry("function", (voidP) (new uint(1)), ECF::UINT);
}

/************************************************************************/

bool CgdaIronFitnessFunction::initialize(StateP state) {

    voidP sptr = state->getRegistry()->getEntry("function"); // get parameter value

	return true;
}

/************************************************************************/

FitnessP CgdaIronFitnessFunction::evaluate(IndividualP individual) {
    // Evaluation creates a new fitness object using a smart pointer
	// in our case, we try to minimize the function value, so we use FitnessMin fitness (for minimization problems)
	FitnessP fitness (new FitnessMin);

	// we define FloatingPoint as the only genotype (in the configuration file)
	FloatingPoint::FloatingPoint* gen = (FloatingPoint::FloatingPoint*) individual->getGenotype().get();

	// we implement the fitness function 'as is', without any translation
	// the number of variables is read from the genotype itself (size of 'realValue' vactor)

    double value =0;
    value= getCustomFitness(gen->realValue);
    fitness->setValue(value);
    return fitness;
}

/************************************************************************/

double CgdaIronFitnessFunction::getCustomFitness(vector <double> genPoints){

    //***********************Define the generalized feature trajectory**********************************************//

    std::vector<std::vector<double>> target;

    //This is sqFeatures

    //target.push_back(feature1T1,feature2T2,feature3T3...);

    //*****************************************OBSERVATION STEP*****************************************************//

    std::vector<double> observation;

    //READ FROM PsqFeatures

    //Obtain TEO state
    //P:yarp::os::Bottle cmd3,res3;
    //P:cmd3.addString("reset");
    //P:pRpcClient->write(cmd3,res3);

    //P:double percentage;
    //P:double Npaint=0;

    //P:for(int i=0;i<psqFeatures->size();i++)
    //P:{
    //P:    Npaint += psqFeatures->operator [](i);
    //P: }

    //Percentage of the wall painted before evolution
    //P:printf("EL NPAINT ES::::::: %f \n",Npaint);
    //P:percentage=(Npaint/NSQUARES)*100;
    //P:printf("EL PERCENTAGE ES::::::: %f \n",percentage);

    //***************************************LOCALIZATION STEP******************************************************//

    //Serch the timeStep where we are (LOCALIZATION STEP)
    int timeStep;
    double diff=1000000;
    for(int i=0;i<NTPOINTS;i++){ //We give priority to the elements at the last positions
        double aux_dist=0;
        for(int j=0;j<NFEATURES;j++){
            double aux_elem;
            aux_elem=observation[j]-target[i][j];
            aux_elem=aux_elem*aux_elem;
            aux_dist=aux_dist+aux_elem;
        }
        //printf("Percentage: %d \n",percentage);
        //printf("Target: %d \n", Const_target[i]);
        //P:diff_aux=percentage-Const_target[i];

        //P:diff_aux=diff_aux*diff_aux;
        //P: diff_aux=sqrt(diff_aux);

        aux_dist=sqrt(aux_dist);

        if(aux_dist<diff){
            timeStep=i;
            diff=aux_dist;
            //printf("i -> %d", i);
            //printf("Time Step %d \n",timeStep);

        }
    }

    //********************************SIMULATED EXECUTION STEP******************************************************//

    //Set new positions of the robot using dEncRaw
    std::vector<double> dEncRaw(6);  // NUM_MOTORS
    dEncRaw[0] = genPoints[0];  // simple
    dEncRaw[1] = -genPoints[1];  // simple
    dEncRaw[3] = genPoints[2];  // simple

    dEncRaw[4] = 45; //TODO:This must be changed for IRON

    //Actually move the robot
    mentalPositionControl->positionMove(dEncRaw.data());

    //********************************SECOND OBSERVATION STEP*******************************************************//

    observation.clear();

    //FORCE

    yarp::os::Bottle* b = pForcePort->read(false);
    if(!b)
    {
        printf("No force yet\n");
    }
    printf("El parámetro del sensor de fuerza es %s\n", b->toString().c_str());
    for(size_t i=0; i<b->size(); i++)
    {
        observation.push_back( b->get(i).asDouble() );
        //std(observation[i]);
    }

    //POSITION

    yarp::os::Bottle cmd,res;
    cmd.addString("world");
    cmd.addString("whereis");
    cmd.addString("tcp");
    cmd.addString("rightArm");
    pRpcClientWorld->write(cmd,res);
    /*printf("El parámetro de posicion es %s\n", res.toString().c_str());
    for(size_t i=0; i<res.size(); i++)
    {
        observation.push_back( res.get(i).asDouble() );
        //std(observation[i]);
    }*/
    yarp::os::Bottle* pos = res.get(0).asList();
    printf("El parámetro de posicion es %s\n", pos->toString().c_str());
    for(size_t i=0; i<pos->size(); i++)
    {
        observation.push_back( pos->get(i).asDouble() );
        //std(observation[i]);
    }

    //P:Npaint=0;
    //P:yarp::os::Time::delay(DEFAULT_DELAY_S);
    //P:yarp::os::Bottle cmd2,res2;
    //P:cmd2.addString("get");
    //P:pRpcClient->write(cmd2,res2);
    //P:for(int i=0;i<res2.size();i++)
    //P:{
    //P:    if ( res2.get(i).asInt() || psqFeatures->operator [](i) )  // logic OR;
    //P:        Npaint ++;
    //P:}

    //********************************FITNESS CALCULATION STEP******************************************************//

    //Fitness of the actual point= percentage of the wall painted

    //P:percentage=(Npaint/NSQUARES)*100;

    //The fit is the L2 norm between the current features, and the t+1 feature environment.
    double fit;
    printf("EL TIME STEP ES EL SIGUIENTE:: %d \n",timeStep);
    //P:printf("El percentage es:: %f", percentage);
    //P:printf("El Consta target es:: %f \n", Const_target[timeStep+1]);
    //fit=sqrt(pow((percentage-Const_target[timeStep+1]),2)); //The fit is the euclidean distance between current feature and t+1. Since 1 dimension euclidean distance equals difference.

    for(int i=0;i<NFEATURES;i++){
        double aux_fit,aux_elem;
        aux_elem=observation[i]-target[timeStep][i];
        aux_fit=aux_elem*aux_elem;
        fit=fit+aux_fit;
    }

    fit=sqrt(fit);

    //featureTrajectories->compare(attempVectforSimpleDiscrepancy,fit);

    cout << std::endl << " fit: " << fit << " ";

    //GOING BACK TO INIT POSITION
    std::vector<double> dEncRaw2(6,0);  // NUM_MOTORS
    //Actually move the robot
    mentalPositionControl->positionMove(dEncRaw2.data());

    return fit;
}

/************************************************************************/

void CgdaIronFitnessFunction::individualExecution(vector<double> results){

    //GOAL: Execute the obtained values to obtain the executed trajectory

    //********************************EXECUTION STEP****************************************************************//

    std::vector<double> dEncRaw(6);  // NUM_MOTORS

    dEncRaw[0] = results[0];  // simple
    dEncRaw[1] = -results[1];  // simple
    dEncRaw[3] = results[2];  // simple

    dEncRaw[4] = 45;

    mentalPositionControl->positionMove(dEncRaw.data());

    //********************************FINAL OBSERVATION STEP********************************************************//

    //Obtain the actual state of the feature environment.

    std::vector<double> observation;

    //FORCE

    yarp::os::Bottle* b = pForcePort->read(false);
    if(!b)
    {
        printf("No force yet\n");
    }
    printf("El parámetro del sensor de fuerza es %s\n", b->toString().c_str());
    for(size_t i=0; i<b->size(); i++)
    {
        observation.push_back( b->get(i).asDouble() );
        //std(observation[i]);
    }

    //POSITION

    yarp::os::Bottle* b = pForcePort->read(false);
    if(!b)
    {
        printf("No force yet\n");
    }
    printf("El parámetro del sensor de fuerza es %s\n", b->toString().c_str());
    for(size_t i=0; i<b->size(); i++)
    {
        observation.push_back( b->get(i).asDouble() );
        //std(observation[i]);
    }

    //POSITION

    yarp::os::Bottle cmd,res;
    cmd.addString("world");
    cmd.addString("whereis");
    cmd.addString("tcp");
    cmd.addString("rightArm");
    pRpcClientWorld->write(cmd,res);
    /*printf("El parámetro de posicion es %s\n", res.toString().c_str());
    for(size_t i=0; i<res.size(); i++)
    {
        observation.push_back( res.get(i).asDouble() );
        //std(observation[i]);
    }*/
    yarp::os::Bottle* pos = res.get(0).asList();
    printf("El parámetro de posicion es %s\n", pos->toString().c_str());
    for(size_t i=0; i<pos->size(); i++)
    {
        observation.push_back( pos->get(i).asDouble() );
        //std(observation[i]);
    }

    //P:yarp::os::Time::delay(DEFAULT_DELAY_S);
    //yarp::os::Bottle cmd,res;
    //P:cmd.addString("get");
    //P:pRpcClient->write(cmd,res);
    //P:printf("got: %s\n",res.toString().c_str());
    //P:for(int i=0;i<res.size();i++)
    //P:{
        //std::cout << "past: ";
        //std::cout << psqFeatures->operator [](i);
        //std::cout << " present: ";
        //P:psqFeatures->operator [](i) |= res.get(i).asInt();  // logic OR
        //std::cout << psqFeatures->operator [](i);
        //std::cout << std::endl;
    //P:}

    //********************************MEMORY UPDATE STEP*******************************************************************//

    std::ofstream myfile1;
    myfile1.open("memoryOET.txt", std::ios_base::out );
    if (myfile1.is_open()){
        for(int i=0;i<observation.size();i++)
        {
            myfile1<<observation[i];
            //myfile1<<"1 ";
            //myfile1<< psqFeatures->operator[](i) << " ";
            //P myfile1<< psqFeatures->operator [](i);
            //Pmyfile1<< " ";
            //std::cout<<psqFeatures->operator[](i) << " ";
            //Pstd::cout<< psqFeatures->operator [](i);
            //Pstd::cout<< " ";
        }
        std::cout<<std::endl;
        //myfile1<<bestInd[0]->fitness->getValue()<<std::endl; //Fitness
    }
    myfile1.close();

    //********************************RESET STEP*******************************************************************//
    //-- Move the mental robot to 0 so env can be reset
    std::vector<double> dEncRaw2(6,0);  // NUM_MOTORS
    mentalPositionControl->positionMove(dEncRaw2.data());
    //-- Reset the wall
    //P:yarp::os::Bottle cmd3,res3;
    //P:cmd3.addString("reset");
    //P:pRpcClient->write(cmd3,res3);

}

/************************************************************************/

} // namespace teo
<commit_msg>ready to start working<commit_after>// -*- mode:C++; tab-width:3; c-basic-offset:4; indent-tabs-mode:nil -*-

//Copyright: Universidad Carlos III de Madrid (C) 2016
//Authors: jgvictores, raulfdzbis, smorante

#include "CgdaIronFitnessFunction.hpp"

#define NTPOINTS 17
#define NFEATURES 6
#define NSQUARES 16

namespace teo
{

/************************************************************************/

void CgdaIronFitnessFunction::registerParameters(StateP state) {
	state->getRegistry()->registerEntry("function", (voidP) (new uint(1)), ECF::UINT);
}

/************************************************************************/

bool CgdaIronFitnessFunction::initialize(StateP state) {

    voidP sptr = state->getRegistry()->getEntry("function"); // get parameter value

	return true;
}

/************************************************************************/

FitnessP CgdaIronFitnessFunction::evaluate(IndividualP individual) {
    // Evaluation creates a new fitness object using a smart pointer
	// in our case, we try to minimize the function value, so we use FitnessMin fitness (for minimization problems)
	FitnessP fitness (new FitnessMin);

	// we define FloatingPoint as the only genotype (in the configuration file)
	FloatingPoint::FloatingPoint* gen = (FloatingPoint::FloatingPoint*) individual->getGenotype().get();

	// we implement the fitness function 'as is', without any translation
	// the number of variables is read from the genotype itself (size of 'realValue' vactor)

    double value =0;
    value= getCustomFitness(gen->realValue);
    fitness->setValue(value);
    return fitness;
}

/************************************************************************/

double CgdaIronFitnessFunction::getCustomFitness(vector <double> genPoints){

    //***********************Define the generalized feature trajectory**********************************************//

    std::vector<std::vector<double>> target;
    //double target[NFEATURES][NTPOINTS];

//    int a[3][4] = {
//       {0, 1, 2, 3} ,   /*  initializers for row indexed by 0 */
//       {4, 5, 6, 7} ,   /*  initializers for row indexed by 1 */
//       {8, 9, 10, 11}   /*  initializers for row indexed by 2 */
//    };

    //This is sqFeatures

    //target.push_back(feature1T1,feature2T2,feature3T3...);

    //*****************************************OBSERVATION STEP*****************************************************//

    std::vector<double> observation;

    //READ FROM PsqFeatures
    for(int i=0;i<psqFeatures->size();i++)
    {
        observation.push_back(psqFeatures->operator [](i));
    }

    //Obtain TEO state
    //P:yarp::os::Bottle cmd3,res3;
    //P:cmd3.addString("reset");
    //P:pRpcClient->write(cmd3,res3);

    //P:double percentage;
    //P:double Npaint=0;

    //P:for(int i=0;i<psqFeatures->size();i++)
    //P:{
    //P:    Npaint += psqFeatures->operator [](i);
    //P: }

    //Percentage of the wall painted before evolution
    //P:printf("EL NPAINT ES::::::: %f \n",Npaint);
    //P:percentage=(Npaint/NSQUARES)*100;
    //P:printf("EL PERCENTAGE ES::::::: %f \n",percentage);

    //***************************************LOCALIZATION STEP******************************************************//

    //Serch the timeStep where we are (LOCALIZATION STEP)
    int timeStep;
    double diff=1000000;
    for(int i=0;i<NFEATURES;i++){
        double aux_dist=0;
        double aux_elem;
        for(int j=0;j<NTPOINTS;j++){ //We give priority to the elements at the last positions
            aux_elem=observation[i]-target[i][j];
            aux_elem=aux_elem*aux_elem;
            aux_dist=aux_dist+aux_elem;
        }
        //printf("Percentage: %d \n",percentage);
        //printf("Target: %d \n", Const_target[i]);
        //P:diff_aux=percentage-Const_target[i];

        //P:diff_aux=diff_aux*diff_aux;
        //P: diff_aux=sqrt(diff_aux);

        aux_dist=sqrt(aux_dist);

        if(aux_dist<diff){
            timeStep=i;
            diff=aux_dist;
            //printf("i -> %d", i);
            //printf("Time Step %d \n",timeStep);

        }
    }

    //********************************SIMULATED EXECUTION STEP******************************************************//

    //Set new positions of the robot using dEncRaw
    std::vector<double> dEncRaw(6);  // NUM_MOTORS
    dEncRaw[0] = genPoints[0];  // simple
    dEncRaw[1] = -genPoints[1];  // simple
    dEncRaw[3] = genPoints[2];  // simple

    dEncRaw[4] = 45; //TODO:This must be changed for IRON

    //Actually move the robot
    mentalPositionControl->positionMove(dEncRaw.data());

    //********************************SECOND OBSERVATION STEP*******************************************************//

    observation.clear();

    //FORCE

    yarp::os::Bottle* b = pForcePort->read(false);
    if(!b)
    {
        printf("No force yet\n");
    }
    printf("El parámetro del sensor de fuerza es %s\n", b->toString().c_str());
    for(size_t i=0; i<b->size(); i++)
    {
        observation.push_back( b->get(i).asDouble() );
        //std(observation[i]);
    }

    //POSITION

    yarp::os::Bottle cmd,res;
    cmd.addString("world");
    cmd.addString("whereis");
    cmd.addString("tcp");
    cmd.addString("rightArm");
    pRpcClientWorld->write(cmd,res);
    /*printf("El parámetro de posicion es %s\n", res.toString().c_str());
    for(size_t i=0; i<res.size(); i++)
    {
        observation.push_back( res.get(i).asDouble() );
        //std(observation[i]);
    }*/
    yarp::os::Bottle* pos = res.get(0).asList();
    printf("El parámetro de posicion es %s\n", pos->toString().c_str());
    for(size_t i=0; i<pos->size(); i++)
    {
        observation.push_back( pos->get(i).asDouble() );
        //std(observation[i]);
    }

    //P:Npaint=0;
    //P:yarp::os::Time::delay(DEFAULT_DELAY_S);
    //P:yarp::os::Bottle cmd2,res2;
    //P:cmd2.addString("get");
    //P:pRpcClient->write(cmd2,res2);
    //P:for(int i=0;i<res2.size();i++)
    //P:{
    //P:    if ( res2.get(i).asInt() || psqFeatures->operator [](i) )  // logic OR;
    //P:        Npaint ++;
    //P:}

    //********************************FITNESS CALCULATION STEP******************************************************//

    //Fitness of the actual point= percentage of the wall painted

    //P:percentage=(Npaint/NSQUARES)*100;

    //The fit is the L2 norm between the current features, and the t+1 feature environment.
    double fit;
    printf("EL TIME STEP ES EL SIGUIENTE:: %d \n",timeStep);
    //P:printf("El percentage es:: %f", percentage);
    //P:printf("El Consta target es:: %f \n", Const_target[timeStep+1]);
    //fit=sqrt(pow((percentage-Const_target[timeStep+1]),2)); //The fit is the euclidean distance between current feature and t+1. Since 1 dimension euclidean distance equals difference.

    for(int i=0;i<NFEATURES;i++){
        double aux_fit,aux_elem;
        aux_elem=observation[i]-target[timeStep][i];
        aux_fit=aux_elem*aux_elem;
        fit=fit+aux_fit;
    }

    fit=sqrt(fit);

    //featureTrajectories->compare(attempVectforSimpleDiscrepancy,fit);

    cout << std::endl << " fit: " << fit << " ";

    //GOING BACK TO INIT POSITION
    std::vector<double> dEncRaw2(6,0);  // NUM_MOTORS
    //Actually move the robot
    mentalPositionControl->positionMove(dEncRaw2.data());

    return fit;
}

/************************************************************************/

void CgdaIronFitnessFunction::individualExecution(vector<double> results){

    //GOAL: Execute the obtained values to obtain the executed trajectory

    //********************************EXECUTION STEP****************************************************************//

    std::vector<double> dEncRaw(6);  // NUM_MOTORS

    dEncRaw[0] = results[0];  // simple
    dEncRaw[1] = -results[1];  // simple
    dEncRaw[3] = results[2];  // simple

    dEncRaw[4] = 45;

    mentalPositionControl->positionMove(dEncRaw.data());

    //********************************FINAL OBSERVATION STEP********************************************************//

    //Obtain the actual state of the feature environment.

    std::vector<double> observation;

    //FORCE

    yarp::os::Bottle* b = pForcePort->read(false);
    if(!b)
    {
        printf("No force yet\n");
    }
    printf("El parámetro del sensor de fuerza es %s\n", b->toString().c_str());
    for(size_t i=0; i<b->size(); i++)
    {
        observation.push_back( b->get(i).asDouble() );
        //std(observation[i]);
    }

    //POSITION

    yarp::os::Bottle cmd,res;
    cmd.addString("world");
    cmd.addString("whereis");
    cmd.addString("tcp");
    cmd.addString("rightArm");
    pRpcClientWorld->write(cmd,res);
    /*printf("El parámetro de posicion es %s\n", res.toString().c_str());
    for(size_t i=0; i<res.size(); i++)
    {
        observation.push_back( res.get(i).asDouble() );
        //std(observation[i]);
    }*/
    yarp::os::Bottle* pos = res.get(0).asList();
    printf("El parámetro de posicion es %s\n", pos->toString().c_str());
    for(size_t i=0; i<pos->size(); i++)
    {
        observation.push_back( pos->get(i).asDouble() );
        //std(observation[i]);
    }

    //P:yarp::os::Time::delay(DEFAULT_DELAY_S);
    //yarp::os::Bottle cmd,res;
    //P:cmd.addString("get");
    //P:pRpcClient->write(cmd,res);
    //P:printf("got: %s\n",res.toString().c_str());
    //P:for(int i=0;i<res.size();i++)
    //P:{
        //std::cout << "past: ";
        //std::cout << psqFeatures->operator [](i);
        //std::cout << " present: ";
        //P:psqFeatures->operator [](i) |= res.get(i).asInt();  // logic OR
        //std::cout << psqFeatures->operator [](i);
        //std::cout << std::endl;
    //P:}

    //********************************MEMORY UPDATE STEP*******************************************************************//

    std::ofstream myfile1;
    myfile1.open("memoryOET.txt", std::ios_base::out );
    if (myfile1.is_open()){
        for(int i=0;i<observation.size();i++)
        {
            myfile1<<observation[i];
            //myfile1<<"1 ";
            //myfile1<< psqFeatures->operator[](i) << " ";
            //P myfile1<< psqFeatures->operator [](i);
            //Pmyfile1<< " ";
            //std::cout<<psqFeatures->operator[](i) << " ";
            //Pstd::cout<< psqFeatures->operator [](i);
            //Pstd::cout<< " ";
        }
        std::cout<<std::endl;
        //myfile1<<bestInd[0]->fitness->getValue()<<std::endl; //Fitness
    }
    myfile1.close();

    //********************************RESET STEP*******************************************************************//
    //-- Move the mental robot to 0 so env can be reset
    std::vector<double> dEncRaw2(6,0);  // NUM_MOTORS
    mentalPositionControl->positionMove(dEncRaw2.data());
    //-- Reset the wall
    //P:yarp::os::Bottle cmd3,res3;
    //P:cmd3.addString("reset");
    //P:pRpcClient->write(cmd3,res3);

}

/************************************************************************/

} // namespace teo
<|endoftext|>