text:
//
// execution/mapping.hpp
// ~~~~~~~~~~~~~~~~~~~~~
//
// Copyright (c) 2003-2022 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_ASIO_EXECUTION_MAPPING_HPP
#define BOOST_ASIO_EXECUTION_MAPPING_HPP

#if defined(_MSC_VER) && (_MSC_VER >= 1200)
# pragma once
#endif // defined(_MSC_VER) && (_MSC_VER >= 1200)

#include <boost/asio/detail/config.hpp>
#include <boost/asio/detail/type_traits.hpp>
#include <boost/asio/execution/executor.hpp>
#include <boost/asio/execution/scheduler.hpp>
#include <boost/asio/execution/sender.hpp>
#include <boost/asio/is_applicable_property.hpp>
#include <boost/asio/query.hpp>
#include <boost/asio/traits/query_free.hpp>
#include <boost/asio/traits/query_member.hpp>
#include <boost/asio/traits/query_static_constexpr_member.hpp>
#include <boost/asio/traits/static_query.hpp>
#include <boost/asio/traits/static_require.hpp>

#include <boost/asio/detail/push_options.hpp>

namespace boost {
namespace asio {

#if defined(GENERATING_DOCUMENTATION)

namespace execution {

/// A property to describe what guarantees an executor makes about the mapping
/// of execution agents on to threads of execution.
struct mapping_t
{
  /// The mapping_t property applies to executors, senders, and schedulers.
  template <typename T>
  static constexpr bool is_applicable_property_v =
    is_executor_v<T> || is_sender_v<T> || is_scheduler_v<T>;

  /// The top-level mapping_t property cannot be required.
  static constexpr bool is_requirable = false;

  /// The top-level mapping_t property cannot be preferred.
  static constexpr bool is_preferable = false;

  /// The type returned by queries against an @c any_executor.
  typedef mapping_t polymorphic_query_result_type;

  /// A sub-property that indicates that execution agents are mapped on to
  /// threads of execution.
  struct thread_t
  {
    /// The mapping_t::thread_t property applies to executors, senders, and
    /// schedulers.
    template <typename T>
    static constexpr bool is_applicable_property_v =
      is_executor_v<T> || is_sender_v<T> || is_scheduler_v<T>;

    /// The mapping_t::thread_t property can be required.
    static constexpr bool is_requirable = true;

    /// The mapping_t::thread_t property can be preferred.
    static constexpr bool is_preferable = true;

    /// The type returned by queries against an @c any_executor.
    typedef mapping_t polymorphic_query_result_type;

    /// Default constructor.
    constexpr thread_t();

    /// Get the value associated with a property object.
    /**
     * @returns thread_t();
     */
    static constexpr mapping_t value();
  };

  /// A sub-property that indicates that execution agents are mapped on to
  /// new threads of execution.
  struct new_thread_t
  {
    /// The mapping_t::new_thread_t property applies to executors, senders, and
    /// schedulers.
    template <typename T>
    static constexpr bool is_applicable_property_v =
      is_executor_v<T> || is_sender_v<T> || is_scheduler_v<T>;

    /// The mapping_t::new_thread_t property can be required.
    static constexpr bool is_requirable = true;

    /// The mapping_t::new_thread_t property can be preferred.
    static constexpr bool is_preferable = true;

    /// The type returned by queries against an @c any_executor.
    typedef mapping_t polymorphic_query_result_type;

    /// Default constructor.
    constexpr new_thread_t();

    /// Get the value associated with a property object.
    /**
     * @returns new_thread_t();
     */
    static constexpr mapping_t value();
  };

  /// A sub-property that indicates that the mapping of execution agents is
  /// implementation-defined.
  struct other_t
  {
    /// The mapping_t::other_t property applies to executors, senders, and
    /// schedulers.
    template <typename T>
    static constexpr bool is_applicable_property_v =
      is_executor_v<T> || is_sender_v<T> || is_scheduler_v<T>;

    /// The mapping_t::other_t property can be required.
    static constexpr bool is_requirable = true;

    /// The mapping_t::other_t property can be preferred.
    static constexpr bool is_preferable = true;

    /// The type returned by queries against an @c any_executor.
    typedef mapping_t polymorphic_query_result_type;

    /// Default constructor.
    constexpr other_t();

    /// Get the value associated with a property object.
    /**
     * @returns other_t();
     */
    static constexpr mapping_t value();
  };

  /// A special value used for accessing the mapping_t::thread_t property.
  static constexpr thread_t thread;

  /// A special value used for accessing the mapping_t::new_thread_t property.
  static constexpr new_thread_t new_thread;

  /// A special value used for accessing the mapping_t::other_t property.
  static constexpr other_t other;

  /// Default constructor.
  constexpr mapping_t();

  /// Construct from a sub-property value.
  constexpr mapping_t(thread_t);

  /// Construct from a sub-property value.
  constexpr mapping_t(new_thread_t);

  /// Construct from a sub-property value.
  constexpr mapping_t(other_t);

  /// Compare property values for equality.
  friend constexpr bool operator==(
      const mapping_t& a, const mapping_t& b) noexcept;

  /// Compare property values for inequality.
  friend constexpr bool operator!=(
      const mapping_t& a, const mapping_t& b) noexcept;
};

/// A special value used for accessing the mapping_t property.
constexpr mapping_t mapping;

} // namespace execution

#else // defined(GENERATING_DOCUMENTATION)

namespace execution {
namespace detail {
namespace mapping {

template <int I> struct thread_t;
template <int I> struct new_thread_t;
template <int I> struct other_t;

} // namespace mapping

template <int I = 0>
struct mapping_t
{
#if defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)
  template <typename T>
  BOOST_ASIO_STATIC_CONSTEXPR(bool,
    is_applicable_property_v = (
      is_executor<T>::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_sender<T>
          >::type::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_scheduler<T>
          >::type::value));
#endif // defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)

  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_requirable = false);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_preferable = false);
  typedef mapping_t polymorphic_query_result_type;

  typedef detail::mapping::thread_t<I> thread_t;
  typedef detail::mapping::new_thread_t<I> new_thread_t;
  typedef detail::mapping::other_t<I> other_t;

  BOOST_ASIO_CONSTEXPR mapping_t()
    : value_(-1)
  {
  }

  BOOST_ASIO_CONSTEXPR mapping_t(thread_t)
    : value_(0)
  {
  }

  BOOST_ASIO_CONSTEXPR mapping_t(new_thread_t)
    : value_(1)
  {
  }

  BOOST_ASIO_CONSTEXPR mapping_t(other_t)
    : value_(2)
  {
  }

  template <typename T>
  struct proxy
  {
#if defined(BOOST_ASIO_HAS_DEDUCED_QUERY_MEMBER_TRAIT)
    struct type
    {
      template <typename P>
      auto query(BOOST_ASIO_MOVE_ARG(P) p) const
        noexcept(
          noexcept(
            declval<typename conditional<true, T, P>::type>().query(
              BOOST_ASIO_MOVE_CAST(P)(p))
          )
        )
        -> decltype(
          declval<typename conditional<true, T, P>::type>().query(
            BOOST_ASIO_MOVE_CAST(P)(p))
        );
    };
#else // defined(BOOST_ASIO_HAS_DEDUCED_QUERY_MEMBER_TRAIT)
    typedef T type;
#endif // defined(BOOST_ASIO_HAS_DEDUCED_QUERY_MEMBER_TRAIT)
  };

  template <typename T>
  struct static_proxy
  {
#if defined(BOOST_ASIO_HAS_DEDUCED_QUERY_STATIC_CONSTEXPR_MEMBER_TRAIT)
    struct type
    {
      template <typename P>
      static constexpr auto query(BOOST_ASIO_MOVE_ARG(P) p)
        noexcept(
          noexcept(
            conditional<true, T, P>::type::query(BOOST_ASIO_MOVE_CAST(P)(p))
          )
        )
        -> decltype(
          conditional<true, T, P>::type::query(BOOST_ASIO_MOVE_CAST(P)(p))
        )
      {
        return T::query(BOOST_ASIO_MOVE_CAST(P)(p));
      }
    };
#else // defined(BOOST_ASIO_HAS_DEDUCED_QUERY_STATIC_CONSTEXPR_MEMBER_TRAIT)
    typedef T type;
#endif // defined(BOOST_ASIO_HAS_DEDUCED_QUERY_STATIC_CONSTEXPR_MEMBER_TRAIT)
  };

  template <typename T>
  struct query_member :
    traits::query_member<typename proxy<T>::type, mapping_t> {};

  template <typename T>
  struct query_static_constexpr_member :
    traits::query_static_constexpr_member<
      typename static_proxy<T>::type, mapping_t> {};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
  template <typename T>
  static BOOST_ASIO_CONSTEXPR
  typename query_static_constexpr_member<T>::result_type
  static_query()
    BOOST_ASIO_NOEXCEPT_IF((
      query_static_constexpr_member<T>::is_noexcept))
  {
    return query_static_constexpr_member<T>::value();
  }

  template <typename T>
  static BOOST_ASIO_CONSTEXPR
  typename traits::static_query<T, thread_t>::result_type
  static_query(
      typename enable_if<
        !query_static_constexpr_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        !query_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        traits::static_query<T, thread_t>::is_valid
      >::type* = 0) BOOST_ASIO_NOEXCEPT
  {
    return traits::static_query<T, thread_t>::value();
  }

  template <typename T>
  static BOOST_ASIO_CONSTEXPR
  typename traits::static_query<T, new_thread_t>::result_type
  static_query(
      typename enable_if<
        !query_static_constexpr_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        !query_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        !traits::static_query<T, thread_t>::is_valid
      >::type* = 0,
      typename enable_if<
        traits::static_query<T, new_thread_t>::is_valid
      >::type* = 0) BOOST_ASIO_NOEXCEPT
  {
    return traits::static_query<T, new_thread_t>::value();
  }

  template <typename T>
  static BOOST_ASIO_CONSTEXPR
  typename traits::static_query<T, other_t>::result_type
  static_query(
      typename enable_if<
        !query_static_constexpr_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        !query_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        !traits::static_query<T, thread_t>::is_valid
      >::type* = 0,
      typename enable_if<
        !traits::static_query<T, new_thread_t>::is_valid
      >::type* = 0,
      typename enable_if<
        traits::static_query<T, other_t>::is_valid
      >::type* = 0) BOOST_ASIO_NOEXCEPT
  {
    return traits::static_query<T, other_t>::value();
  }

  template <typename E, typename T = decltype(mapping_t::static_query<E>())>
  static BOOST_ASIO_CONSTEXPR const T static_query_v
    = mapping_t::static_query<E>();
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

  friend BOOST_ASIO_CONSTEXPR bool operator==(
      const mapping_t& a, const mapping_t& b)
  {
    return a.value_ == b.value_;
  }

  friend BOOST_ASIO_CONSTEXPR bool operator!=(
      const mapping_t& a, const mapping_t& b)
  {
    return a.value_ != b.value_;
  }

  struct convertible_from_mapping_t
  {
    BOOST_ASIO_CONSTEXPR convertible_from_mapping_t(mapping_t) {}
  };

  template <typename Executor>
  friend BOOST_ASIO_CONSTEXPR mapping_t query(
      const Executor& ex, convertible_from_mapping_t,
      typename enable_if<
        can_query<const Executor&, thread_t>::value
      >::type* = 0)
#if !defined(__clang__) // Clang crashes if noexcept is used here.
#if defined(BOOST_ASIO_MSVC) // Visual C++ wants the type to be qualified.
    BOOST_ASIO_NOEXCEPT_IF((
      is_nothrow_query<const Executor&, mapping_t<>::thread_t>::value))
#else // defined(BOOST_ASIO_MSVC)
    BOOST_ASIO_NOEXCEPT_IF((
      is_nothrow_query<const Executor&, thread_t>::value))
#endif // defined(BOOST_ASIO_MSVC)
#endif // !defined(__clang__)
  {
    return boost::asio::query(ex, thread_t());
  }

  template <typename Executor>
  friend BOOST_ASIO_CONSTEXPR mapping_t query(
      const Executor& ex, convertible_from_mapping_t,
      typename enable_if<
        !can_query<const Executor&, thread_t>::value
      >::type* = 0,
      typename enable_if<
        can_query<const Executor&, new_thread_t>::value
      >::type* = 0)
#if !defined(__clang__) // Clang crashes if noexcept is used here.
#if defined(BOOST_ASIO_MSVC) // Visual C++ wants the type to be qualified.
    BOOST_ASIO_NOEXCEPT_IF((
      is_nothrow_query<const Executor&, mapping_t<>::new_thread_t>::value))
#else // defined(BOOST_ASIO_MSVC)
    BOOST_ASIO_NOEXCEPT_IF((
      is_nothrow_query<const Executor&, new_thread_t>::value))
#endif // defined(BOOST_ASIO_MSVC)
#endif // !defined(__clang__)
  {
    return boost::asio::query(ex, new_thread_t());
  }

  template <typename Executor>
  friend BOOST_ASIO_CONSTEXPR mapping_t query(
      const Executor& ex, convertible_from_mapping_t,
      typename enable_if<
        !can_query<const Executor&, thread_t>::value
      >::type* = 0,
      typename enable_if<
        !can_query<const Executor&, new_thread_t>::value
      >::type* = 0,
      typename enable_if<
        can_query<const Executor&, other_t>::value
      >::type* = 0)
#if !defined(__clang__) // Clang crashes if noexcept is used here.
#if defined(BOOST_ASIO_MSVC) // Visual C++ wants the type to be qualified.
    BOOST_ASIO_NOEXCEPT_IF((
      is_nothrow_query<const Executor&, mapping_t<>::other_t>::value))
#else // defined(BOOST_ASIO_MSVC)
    BOOST_ASIO_NOEXCEPT_IF((
      is_nothrow_query<const Executor&, other_t>::value))
#endif // defined(BOOST_ASIO_MSVC)
#endif // !defined(__clang__)
  {
    return boost::asio::query(ex, other_t());
  }

  BOOST_ASIO_STATIC_CONSTEXPR_DEFAULT_INIT(thread_t, thread);
  BOOST_ASIO_STATIC_CONSTEXPR_DEFAULT_INIT(new_thread_t, new_thread);
  BOOST_ASIO_STATIC_CONSTEXPR_DEFAULT_INIT(other_t, other);

#if !defined(BOOST_ASIO_HAS_CONSTEXPR)
  static const mapping_t instance;
#endif // !defined(BOOST_ASIO_HAS_CONSTEXPR)

private:
  int value_;
};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
template <int I> template <typename E, typename T>
const T mapping_t<I>::static_query_v;
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

#if !defined(BOOST_ASIO_HAS_CONSTEXPR)
template <int I>
const mapping_t<I> mapping_t<I>::instance;
#endif

template <int I>
const typename mapping_t<I>::thread_t mapping_t<I>::thread;

template <int I>
const typename mapping_t<I>::new_thread_t mapping_t<I>::new_thread;

template <int I>
const typename mapping_t<I>::other_t mapping_t<I>::other;

namespace mapping {

template <int I = 0>
struct thread_t
{
#if defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)
  template <typename T>
  BOOST_ASIO_STATIC_CONSTEXPR(bool,
    is_applicable_property_v = (
      is_executor<T>::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_sender<T>
          >::type::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_scheduler<T>
          >::type::value));
#endif // defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)

  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_requirable = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_preferable = true);
  typedef mapping_t<I> polymorphic_query_result_type;

  BOOST_ASIO_CONSTEXPR thread_t()
  {
  }

  template <typename T>
  struct query_member :
    traits::query_member<
      typename mapping_t<I>::template proxy<T>::type, thread_t> {};

  template <typename T>
  struct query_static_constexpr_member :
    traits::query_static_constexpr_member<
      typename mapping_t<I>::template static_proxy<T>::type, thread_t> {};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
  template <typename T>
  static BOOST_ASIO_CONSTEXPR
  typename query_static_constexpr_member<T>::result_type
  static_query()
    BOOST_ASIO_NOEXCEPT_IF((
      query_static_constexpr_member<T>::is_noexcept))
  {
    return query_static_constexpr_member<T>::value();
  }

  template <typename T>
  static BOOST_ASIO_CONSTEXPR thread_t static_query(
      typename enable_if<
        !query_static_constexpr_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        !query_member<T>::is_valid
      >::type* = 0,
      typename enable_if<
        !traits::query_free<T, thread_t>::is_valid
      >::type* = 0,
      typename enable_if<
        !can_query<T, new_thread_t<I> >::value
      >::type* = 0,
      typename enable_if<
        !can_query<T, other_t<I> >::value
      >::type* = 0) BOOST_ASIO_NOEXCEPT
  {
    return thread_t();
  }

  template <typename E, typename T = decltype(thread_t::static_query<E>())>
  static BOOST_ASIO_CONSTEXPR const T static_query_v
    = thread_t::static_query<E>();
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

  static BOOST_ASIO_CONSTEXPR mapping_t<I> value()
  {
    return thread_t();
  }

  friend BOOST_ASIO_CONSTEXPR bool operator==(
      const thread_t&, const thread_t&)
  {
    return true;
  }

  friend BOOST_ASIO_CONSTEXPR bool operator!=(
      const thread_t&, const thread_t&)
  {
    return false;
  }
};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
template <int I> template <typename E, typename T>
const T thread_t<I>::static_query_v;
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

template <int I = 0>
struct new_thread_t
{
#if defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)
  template <typename T>
  BOOST_ASIO_STATIC_CONSTEXPR(bool,
    is_applicable_property_v = (
      is_executor<T>::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_sender<T>
          >::type::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_scheduler<T>
          >::type::value));
#endif // defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)

  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_requirable = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_preferable = true);
  typedef mapping_t<I> polymorphic_query_result_type;

  BOOST_ASIO_CONSTEXPR new_thread_t()
  {
  }

  template <typename T>
  struct query_member :
    traits::query_member<
      typename mapping_t<I>::template proxy<T>::type, new_thread_t> {};

  template <typename T>
  struct query_static_constexpr_member :
    traits::query_static_constexpr_member<
      typename mapping_t<I>::template static_proxy<T>::type, new_thread_t> {};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
  template <typename T>
  static BOOST_ASIO_CONSTEXPR
  typename query_static_constexpr_member<T>::result_type
  static_query()
    BOOST_ASIO_NOEXCEPT_IF((
      query_static_constexpr_member<T>::is_noexcept))
  {
    return query_static_constexpr_member<T>::value();
  }

  template <typename E, typename T = decltype(new_thread_t::static_query<E>())>
  static BOOST_ASIO_CONSTEXPR const T static_query_v
    = new_thread_t::static_query<E>();
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

  static BOOST_ASIO_CONSTEXPR mapping_t<I> value()
  {
    return new_thread_t();
  }

  friend BOOST_ASIO_CONSTEXPR bool operator==(
      const new_thread_t&, const new_thread_t&)
  {
    return true;
  }

  friend BOOST_ASIO_CONSTEXPR bool operator!=(
      const new_thread_t&, const new_thread_t&)
  {
    return false;
  }
};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
template <int I> template <typename E, typename T>
const T new_thread_t<I>::static_query_v;
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

template <int I>
struct other_t
{
#if defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)
  template <typename T>
  BOOST_ASIO_STATIC_CONSTEXPR(bool,
    is_applicable_property_v = (
      is_executor<T>::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_sender<T>
          >::type::value
        || conditional<
            is_executor<T>::value,
            false_type,
            is_scheduler<T>
          >::type::value));
#endif // defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)

  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_requirable = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_preferable = true);
  typedef mapping_t<I> polymorphic_query_result_type;

  BOOST_ASIO_CONSTEXPR other_t()
  {
  }

  template <typename T>
  struct query_member :
    traits::query_member<
      typename mapping_t<I>::template proxy<T>::type, other_t> {};

  template <typename T>
  struct query_static_constexpr_member :
    traits::query_static_constexpr_member<
      typename mapping_t<I>::template static_proxy<T>::type, other_t> {};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
  template <typename T>
  static BOOST_ASIO_CONSTEXPR
  typename query_static_constexpr_member<T>::result_type
  static_query()
    BOOST_ASIO_NOEXCEPT_IF((
      query_static_constexpr_member<T>::is_noexcept))
  {
    return query_static_constexpr_member<T>::value();
  }

  template <typename E, typename T = decltype(other_t::static_query<E>())>
  static BOOST_ASIO_CONSTEXPR const T static_query_v
    = other_t::static_query<E>();
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

  static BOOST_ASIO_CONSTEXPR mapping_t<I> value()
  {
    return other_t();
  }

  friend BOOST_ASIO_CONSTEXPR bool operator==(
      const other_t&, const other_t&)
  {
    return true;
  }

  friend BOOST_ASIO_CONSTEXPR bool operator!=(
      const other_t&, const other_t&)
  {
    return false;
  }
};

#if defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)
template <int I> template <typename E, typename T>
const T other_t<I>::static_query_v;
#endif // defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   && defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

} // namespace mapping
} // namespace detail

typedef detail::mapping_t<> mapping_t;

#if defined(BOOST_ASIO_HAS_CONSTEXPR) || defined(GENERATING_DOCUMENTATION)
constexpr mapping_t mapping;
#else // defined(BOOST_ASIO_HAS_CONSTEXPR) || defined(GENERATING_DOCUMENTATION)
namespace { static const mapping_t& mapping = mapping_t::instance; }
#endif

} // namespace execution

#if !defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)

template <typename T>
struct is_applicable_property<T, execution::mapping_t>
  : integral_constant<bool,
      execution::is_executor<T>::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_sender<T>
          >::type::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_scheduler<T>
          >::type::value>
{
};

template <typename T>
struct is_applicable_property<T, execution::mapping_t::thread_t>
  : integral_constant<bool,
      execution::is_executor<T>::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_sender<T>
          >::type::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_scheduler<T>
          >::type::value>
{
};

template <typename T>
struct is_applicable_property<T, execution::mapping_t::new_thread_t>
  : integral_constant<bool,
      execution::is_executor<T>::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_sender<T>
          >::type::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_scheduler<T>
          >::type::value>
{
};

template <typename T>
struct is_applicable_property<T, execution::mapping_t::other_t>
  : integral_constant<bool,
      execution::is_executor<T>::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_sender<T>
          >::type::value
        || conditional<
            execution::is_executor<T>::value,
            false_type,
            execution::is_scheduler<T>
          >::type::value>
{
};

#endif // !defined(BOOST_ASIO_HAS_VARIABLE_TEMPLATES)

namespace traits {

#if !defined(BOOST_ASIO_HAS_DEDUCED_QUERY_FREE_TRAIT)

template <typename T>
struct query_free_default<T, execution::mapping_t,
  typename enable_if<
    can_query<T, execution::mapping_t::thread_t>::value
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept =
    (is_nothrow_query<T, execution::mapping_t::thread_t>::value));

  typedef execution::mapping_t result_type;
};

template <typename T>
struct query_free_default<T, execution::mapping_t,
  typename enable_if<
    !can_query<T, execution::mapping_t::thread_t>::value
      && can_query<T, execution::mapping_t::new_thread_t>::value
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept =
    (is_nothrow_query<T, execution::mapping_t::new_thread_t>::value));

  typedef execution::mapping_t result_type;
};

template <typename T>
struct query_free_default<T, execution::mapping_t,
  typename enable_if<
    !can_query<T, execution::mapping_t::thread_t>::value
      && !can_query<T, execution::mapping_t::new_thread_t>::value
      && can_query<T, execution::mapping_t::other_t>::value
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept =
    (is_nothrow_query<T, execution::mapping_t::other_t>::value));

  typedef execution::mapping_t result_type;
};

#endif // !defined(BOOST_ASIO_HAS_DEDUCED_QUERY_FREE_TRAIT)

#if !defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT) \
  || !defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

template <typename T>
struct static_query<T, execution::mapping_t,
  typename enable_if<
    execution::detail::mapping_t<0>::
      query_static_constexpr_member<T>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef typename execution::detail::mapping_t<0>::
    query_static_constexpr_member<T>::result_type result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return execution::detail::mapping_t<0>::
      query_static_constexpr_member<T>::value();
  }
};

template <typename T>
struct static_query<T, execution::mapping_t,
  typename enable_if<
    !execution::detail::mapping_t<0>::
        query_static_constexpr_member<T>::is_valid
      && !execution::detail::mapping_t<0>::
        query_member<T>::is_valid
      && traits::static_query<T, execution::mapping_t::thread_t>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef typename traits::static_query<T,
    execution::mapping_t::thread_t>::result_type result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return traits::static_query<T, execution::mapping_t::thread_t>::value();
  }
};

template <typename T>
struct static_query<T, execution::mapping_t,
  typename enable_if<
    !execution::detail::mapping_t<0>::
        query_static_constexpr_member<T>::is_valid
      && !execution::detail::mapping_t<0>::
        query_member<T>::is_valid
      && !traits::static_query<T, execution::mapping_t::thread_t>::is_valid
      && traits::static_query<T, execution::mapping_t::new_thread_t>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef typename traits::static_query<T,
    execution::mapping_t::new_thread_t>::result_type result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return traits::static_query<T, execution::mapping_t::new_thread_t>::value();
  }
};

template <typename T>
struct static_query<T, execution::mapping_t,
  typename enable_if<
    !execution::detail::mapping_t<0>::
        query_static_constexpr_member<T>::is_valid
      && !execution::detail::mapping_t<0>::
        query_member<T>::is_valid
      && !traits::static_query<T, execution::mapping_t::thread_t>::is_valid
      && !traits::static_query<T, execution::mapping_t::new_thread_t>::is_valid
      && traits::static_query<T, execution::mapping_t::other_t>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef typename traits::static_query<T,
    execution::mapping_t::other_t>::result_type result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return traits::static_query<T, execution::mapping_t::other_t>::value();
  }
};

template <typename T>
struct static_query<T, execution::mapping_t::thread_t,
  typename enable_if<
    execution::detail::mapping::thread_t<0>::
      query_static_constexpr_member<T>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef typename execution::detail::mapping::thread_t<0>::
    query_static_constexpr_member<T>::result_type result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return execution::detail::mapping::thread_t<0>::
      query_static_constexpr_member<T>::value();
  }
};

template <typename T>
struct static_query<T, execution::mapping_t::thread_t,
  typename enable_if<
    !execution::detail::mapping::thread_t<0>::
        query_static_constexpr_member<T>::is_valid
      && !execution::detail::mapping::thread_t<0>::
        query_member<T>::is_valid
      && !traits::query_free<T, execution::mapping_t::thread_t>::is_valid
      && !can_query<T, execution::mapping_t::new_thread_t>::value
      && !can_query<T, execution::mapping_t::other_t>::value
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef execution::mapping_t::thread_t result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return result_type();
  }
};

template <typename T>
struct static_query<T, execution::mapping_t::new_thread_t,
  typename enable_if<
    execution::detail::mapping::new_thread_t<0>::
      query_static_constexpr_member<T>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef typename execution::detail::mapping::new_thread_t<0>::
    query_static_constexpr_member<T>::result_type result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return execution::detail::mapping::new_thread_t<0>::
      query_static_constexpr_member<T>::value();
  }
};

template <typename T>
struct static_query<T, execution::mapping_t::other_t,
  typename enable_if<
    execution::detail::mapping::other_t<0>::
      query_static_constexpr_member<T>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid = true);
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_noexcept = true);

  typedef typename execution::detail::mapping::other_t<0>::
    query_static_constexpr_member<T>::result_type result_type;

  static BOOST_ASIO_CONSTEXPR result_type value()
  {
    return execution::detail::mapping::other_t<0>::
      query_static_constexpr_member<T>::value();
  }
};

#endif // !defined(BOOST_ASIO_HAS_DEDUCED_STATIC_QUERY_TRAIT)
       //   || !defined(BOOST_ASIO_HAS_SFINAE_VARIABLE_TEMPLATES)

#if !defined(BOOST_ASIO_HAS_DEDUCED_STATIC_REQUIRE_TRAIT)

template <typename T>
struct static_require<T, execution::mapping_t::thread_t,
  typename enable_if<
    static_query<T, execution::mapping_t::thread_t>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid =
    (is_same<typename static_query<T,
      execution::mapping_t::thread_t>::result_type,
        execution::mapping_t::thread_t>::value));
};

template <typename T>
struct static_require<T, execution::mapping_t::new_thread_t,
  typename enable_if<
    static_query<T, execution::mapping_t::new_thread_t>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid =
    (is_same<typename static_query<T,
      execution::mapping_t::new_thread_t>::result_type,
        execution::mapping_t::new_thread_t>::value));
};

template <typename T>
struct static_require<T, execution::mapping_t::other_t,
  typename enable_if<
    static_query<T, execution::mapping_t::other_t>::is_valid
  >::type>
{
  BOOST_ASIO_STATIC_CONSTEXPR(bool, is_valid =
    (is_same<typename static_query<T,
      execution::mapping_t::other_t>::result_type,
        execution::mapping_t::other_t>::value));
};

#endif // !defined(BOOST_ASIO_HAS_DEDUCED_STATIC_REQUIRE_TRAIT)

} // namespace traits

#endif // defined(GENERATING_DOCUMENTATION)

} // namespace asio
} // namespace boost

#include <boost/asio/detail/pop_options.hpp>

#endif // BOOST_ASIO_EXECUTION_MAPPING_HPP


text:
#include "pyglobals.hpp"
#include <slideio/slideio.hpp>
#include <slideio/imagetools/imagetools.hpp>
#include <boost/format.hpp>
#include <opencv2/core/mat.hpp>

#include "slideio/core/cvtools.hpp"

namespace py = pybind11;

std::shared_ptr<PySlide> pyOpenSlide(const std::string& path, const std::string& driver)
{
    std::shared_ptr<slideio::Slide> slide = slideio::openSlide(path, driver);
    if(!slide)
    {
        throw std::runtime_error(
            (boost::format("Cannot open file \"%1%\" with driver \"%2%\"") % path % driver).str());
    }
    PySlide* pySlide = new PySlide(slide);
    std::shared_ptr<PySlide> wrapper(pySlide);
    return wrapper;
}

std::vector<std::string> pyGetDriverIDs()
{
    return slideio::getDriverIDs();
}
static slideio::DataType typeFromNumpyType(const pybind11::dtype& type)
{
    if (type.is(py::detail::npy_format_descriptor<uint8_t>::dtype()))
    {
    return slideio::DataType::DT_Byte;
    }
    else if(type.is(py::detail::npy_format_descriptor<int8_t>::dtype()))
    {
        return slideio::DataType::DT_Int8;
    }
    else if(type.is(py::detail::npy_format_descriptor<int16_t>::dtype()))
    {
        return slideio::DataType::DT_Int16;
    }
    else if (type.is(py::detail::npy_format_descriptor<uint16_t>::dtype()))
    {
        return slideio::DataType::DT_UInt16;
    }
    else if (type.is(py::detail::npy_format_descriptor<int32_t>::dtype()))
    {
        return slideio::DataType::DT_Int32;
    }
    else if (type.is(py::detail::npy_format_descriptor<float>::dtype()))
    {
        return slideio::DataType::DT_Float32;
    }
    else if (type.is(py::detail::npy_format_descriptor<double>::dtype()))
    {
        return slideio::DataType::DT_Float64;
    }
    throw std::runtime_error("Cannot convert numpy data type to internal type");
}

static cv::Mat fromNumpy2Mat(py::array& np_array)
{
    slideio::DataType dt = typeFromNumpyType(np_array.dtype());
    int cvType = slideio::CVTools::toOpencvType(dt);
    size_t dims = np_array.ndim();
    if(dims>3) {
        throw std::runtime_error("Only 2D images are supported.");
    }
    int width = (int)np_array.shape(0);
    int height = (int)np_array.shape(1);
    int channels = 1;
    if(dims>2) {
        channels = (int)np_array.shape(2);
    }
    cv::Mat mat(height, width, CV_MAKETYPE(cvType, channels), np_array.mutable_data());
    return mat;
}

double pyCompareImages(py::array& left, py::array& right)
{
    cv::Mat leftMat = fromNumpy2Mat(left);
    cv::Mat rightMat = fromNumpy2Mat(right);
    double sim = slideio::ImageTools::computeSimilarity(leftMat, rightMat);
    return sim;
}

ext: cpp

max_stars_repo_name: Booritas/slideio

max_issues_repo_name: Booritas/slideio

max_forks_repo_name: Booritas/slideio


text:
//  Copyright (c) 2001-2011 Hartmut Kaiser
//  Copyright (c) 2001-2011 Joel de Guzman
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(SPIRIT_KARMA_SEQUENCE_FEB_28_2007_0247PM)
#define SPIRIT_KARMA_SEQUENCE_FEB_28_2007_0247PM

#if defined(_MSC_VER)
#pragma once
#endif

#include <boost/spirit/home/karma/domain.hpp>
#include <boost/spirit/home/karma/generator.hpp>
#include <boost/spirit/home/karma/meta_compiler.hpp>
#include <boost/spirit/home/karma/detail/fail_function.hpp>
#include <boost/spirit/home/karma/detail/pass_container.hpp>
#include <boost/spirit/home/karma/detail/get_stricttag.hpp>
#include <boost/spirit/home/support/info.hpp>
#include <boost/spirit/home/support/detail/what_function.hpp>
#include <boost/spirit/home/karma/detail/attributes.hpp>
#include <boost/spirit/home/karma/detail/indirect_iterator.hpp>
#include <boost/spirit/home/support/algorithm/any_if.hpp>
#include <boost/spirit/home/support/unused.hpp>
#include <boost/spirit/home/support/sequence_base_id.hpp>
#include <boost/spirit/home/support/has_semantic_action.hpp>
#include <boost/spirit/home/support/handles_container.hpp>
#include <boost/spirit/home/support/attributes.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/bitor.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/fusion/include/transform.hpp>
#include <boost/mpl/accumulate.hpp>

///////////////////////////////////////////////////////////////////////////////
namespace boost { namespace spirit
{
    ///////////////////////////////////////////////////////////////////////////
    // Enablers
    ///////////////////////////////////////////////////////////////////////////
    template <>
    struct use_operator<karma::domain, proto::tag::shift_left> // enables <<
      : mpl::true_ {};

    template <>
    struct flatten_tree<karma::domain, proto::tag::shift_left> // flattens <<
      : mpl::true_ {};
}}

///////////////////////////////////////////////////////////////////////////////
namespace boost { namespace spirit { namespace traits
{
    // specialization for sequences
    template <typename Elements>
    struct sequence_properties
    {
        struct element_properties
        {
            template <typename T>
            struct result;

            template <typename F, typename Element>
            struct result<F(Element)>
            {
                typedef properties_of<Element> type;
            };

            // never called, but needed for decltype-based result_of (C++0x)
            template <typename Element>
            typename result<element_properties(Element)>::type
            operator()(Element&) const;
        };

        typedef typename mpl::accumulate<
            typename fusion::result_of::transform<
                Elements, element_properties>::type
          , mpl::int_<karma::generator_properties::no_properties>
          , mpl::bitor_<mpl::_2, mpl::_1>
        >::type type;
    };
}}}

///////////////////////////////////////////////////////////////////////////////
namespace boost { namespace spirit { namespace karma
{
    template <typename Elements, typename Strict, typename Derived>
    struct base_sequence : nary_generator<Derived>
    {
        typedef typename traits::sequence_properties<Elements>::type properties;

        base_sequence(Elements const& elements)
          : elements(elements) {}

        typedef Elements elements_type;
        struct sequence_base_id;

        template <typename Context, typename Iterator = unused_type>
        struct attribute
        {
            // Put all the element attributes in a tuple
            typedef typename traits::build_attribute_sequence<
                Elements, Context, traits::sequence_attribute_transform
              , Iterator, karma::domain
            >::type all_attributes;

            // Now, build a fusion vector over the attributes. Note
            // that build_fusion_vector 1) removes all unused attributes
            // and 2) may return unused_type if all elements have
            // unused_type(s).
            typedef typename
                traits::build_fusion_vector<all_attributes>::type
            type_;

            // Finally, strip single element vectors into its
            // naked form: vector1<T> --> T
            typedef typename
                traits::strip_single_element_vector<type_>::type
            type;
        };

        // standard case. Attribute is a fusion tuple
        template <
            typename OutputIterator, typename Context, typename Delimiter
          , typename Attribute, typename Pred1, typename Pred2>
        bool generate_impl(OutputIterator& sink, Context& ctx
          , Delimiter const& d, Attribute& attr_, Pred1, Pred2) const
        {
            typedef detail::fail_function<
                OutputIterator, Context, Delimiter> fail_function;
            typedef traits::attribute_not_unused<Context> predicate;

            // wrap the attribute in a tuple if it is not a tuple or if the
            // attribute of this sequence is a single element tuple
            typedef typename attribute<Context>::type_ attr_type_;
            typename traits::wrap_if_not_tuple<Attribute
              , typename mpl::and_<
                    traits::one_element_sequence<attr_type_>
                  , mpl::not_<traits::one_element_sequence<Attribute> >
                >::type
            >::type attr(attr_);

            // return false if *any* of the generators fail
            bool r = spirit::any_if(elements, attr
                          , fail_function(sink, ctx, d), predicate());

            typedef typename traits::attribute_size<Attribute>::type size_type;

            // fail generating if sequences have not the same (logical) length
            return !r && (!Strict::value ||
                // This ignores container element count (which is not good),
                // but allows valid attributes to succeed. This will lead to
                // false positives (failing generators, even if they shouldn't)
                // if the embedded component is restricting the number of
                // container elements it consumes (i.e. repeat). This solution
                // is not optimal but much better than letting _all_ repetitive
                // components fail.
                Pred1::value ||
                size_type(traits::sequence_size<attr_type_>::value) == traits::size(attr_));
        }

        // Special case when Attribute is an stl container and the sequence's
        // attribute is not a one element sequence
        template <
            typename OutputIterator, typename Context, typename Delimiter
          , typename Attribute>
        bool generate_impl(OutputIterator& sink, Context& ctx
          , Delimiter const& d, Attribute const& attr_
          , mpl::true_, mpl::false_) const
        {
            // return false if *any* of the generators fail
            typedef detail::fail_function<
                OutputIterator, Context, Delimiter> fail_function;

            typedef typename traits::container_iterator<
                typename add_const<Attribute>::type
            >::type iterator_type;

            typedef
                typename traits::make_indirect_iterator<iterator_type>::type
            indirect_iterator_type;
            typedef detail::pass_container<
              fail_function, Attribute, indirect_iterator_type, mpl::true_>
            pass_container;

            iterator_type begin = traits::begin(attr_);
            iterator_type end = traits::end(attr_);

            pass_container pass(fail_function(sink, ctx, d),
                indirect_iterator_type(begin), indirect_iterator_type(end));
            bool r = fusion::any(elements, pass);

            // fail generating if sequences have not the same (logical) length
            return !r && (!Strict::value || begin == end);
        }

        // main generate function. Dispatches to generate_impl depending
        // on the Attribute type.
        template <
            typename OutputIterator, typename Context, typename Delimiter
          , typename Attribute>
        bool generate(OutputIterator& sink, Context& ctx, Delimiter const& d
          , Attribute const& attr) const
        {
            typedef typename traits::is_container<Attribute>::type
                is_container;

            typedef typename attribute<Context>::type_ attr_type_;
            typedef typename traits::one_element_sequence<attr_type_>::type
                is_one_element_sequence;

            return generate_impl(sink, ctx, d, attr, is_container()
              , is_one_element_sequence());
        }

        template <typename Context>
        info what(Context& context) const
        {
            info result("sequence");
            fusion::for_each(elements,
                spirit::detail::what_function<Context>(result, context));
            return result;
        }

        Elements elements;
    };

    template <typename Elements>
    struct sequence
      : base_sequence<Elements, mpl::false_, sequence<Elements> >
    {
        typedef base_sequence<Elements, mpl::false_, sequence> base_sequence_;

        sequence(Elements const& subject)
          : base_sequence_(subject) {}
    };

    template <typename Elements>
    struct strict_sequence
      : base_sequence<Elements, mpl::true_, strict_sequence<Elements> >
    {
        typedef base_sequence<Elements, mpl::true_, strict_sequence>
            base_sequence_;

        strict_sequence(Elements const& subject)
          : base_sequence_(subject) {}
    };

    ///////////////////////////////////////////////////////////////////////////
    // Generator generators: make_xxx function (objects)
    ///////////////////////////////////////////////////////////////////////////
    namespace detail
    {
        template <typename Elements, bool strict_mode = false>
        struct make_sequence
          : make_nary_composite<Elements, sequence>
        {};

        template <typename Elements>
        struct make_sequence<Elements, true>
          : make_nary_composite<Elements, strict_sequence>
        {};
    }

    template <typename Elements, typename Modifiers>
    struct make_composite<proto::tag::shift_left, Elements, Modifiers>
      : detail::make_sequence<Elements, detail::get_stricttag<Modifiers>::value>
    {};

    ///////////////////////////////////////////////////////////////////////////
    // Helper template allowing to get the required container type for a rule
    // attribute, which is part of a sequence.
    template <typename Iterator>
    struct make_sequence_iterator_range
    {
        typedef iterator_range<detail::indirect_iterator<Iterator> > type;
    };
}}}

namespace boost { namespace spirit { namespace traits
{
    ///////////////////////////////////////////////////////////////////////////
    template <typename Elements>
    struct has_semantic_action<karma::sequence<Elements> >
      : nary_has_semantic_action<Elements> {};

    template <typename Elements>
    struct has_semantic_action<karma::strict_sequence<Elements> >
      : nary_has_semantic_action<Elements> {};

    ///////////////////////////////////////////////////////////////////////////
    template <typename Elements, typename Attribute, typename Context
      , typename Iterator>
    struct handles_container<karma::sequence<Elements>, Attribute, Context
          , Iterator>
      : mpl::true_ {};

    template <typename Elements, typename Attribute, typename Context
      , typename Iterator>
    struct handles_container<karma::strict_sequence<Elements>, Attribute
          , Context, Iterator>
      : mpl::true_ {};
}}}

#endif

ext: hpp

max_stars_repo_name: dyzmapl/BumpTop

max_issues_repo_name: dyzmapl/BumpTop

max_forks_repo_name: dyzmapl/BumpTop


text: #include <boost/config/compiler/xlcpp.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*

   Copyright [2008] [Trevor Hogan]

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

   CODE PORTED FROM THE ORIGINAL GHOST PROJECT: http://ghost.pwner.org/

*/

#include "ghost.h"
#include "gameslot.h"

//
// CGameSlot
//

CGameSlot :: CGameSlot( BYTEARRAY &n ) : m_PID( 0 ), m_DownloadStatus( 255 ), m_SlotStatus( SLOTSTATUS_OPEN ), m_Computer( 0 ), m_Team( 0 ), m_Colour( 1 ), m_Race( SLOTRACE_RANDOM ), m_ComputerType( SLOTCOMP_NORMAL ), m_Handicap( 100 )
{
	if( n.size( ) >= 7 )
	{
		m_PID = n[0];
		m_DownloadStatus = n[1];
		m_SlotStatus = n[2];
		m_Computer = n[3];
		m_Team = n[4];
		m_Colour = n[5];
		m_Race = n[6];

		if( n.size( ) >= 8 )
			m_ComputerType = n[7];

		if( n.size( ) >= 9 )
			m_Handicap = n[8];
	}
}

CGameSlot :: CGameSlot( unsigned char nPID, unsigned char nDownloadStatus, unsigned char nSlotStatus, unsigned char nComputer, unsigned char nTeam, unsigned char nColour, unsigned char nRace, unsigned char nComputerType, unsigned char nHandicap )
    : m_PID( nPID ), m_DownloadStatus( nDownloadStatus ), m_SlotStatus( nSlotStatus ), m_Computer( nComputer ), m_Team( nTeam ), m_Colour( nColour ), m_Race( nRace ), m_ComputerType( nComputerType ), m_Handicap( nHandicap )
{

}

CGameSlot :: ~CGameSlot( )
{

}

BYTEARRAY CGameSlot :: GetByteArray( ) const
{
	BYTEARRAY b;
	b.push_back( m_PID );
	b.push_back( m_DownloadStatus );
	b.push_back( m_SlotStatus );
	b.push_back( m_Computer );
	b.push_back( m_Team );
	b.push_back( m_Colour );
	b.push_back( m_Race );
	b.push_back( m_ComputerType );
	b.push_back( m_Handicap );
	return b;
}

#include <boost/python.hpp>

void CGameSlot :: RegisterPythonClass( )
{
	using namespace boost::python;

	class_<CGameSlot>("gameSlot", no_init)
		.def_readonly("PID", &CGameSlot::m_PID)
		.def_readonly("downloadStatus", &CGameSlot::m_DownloadStatus)
		.def_readonly("slotStatus", &CGameSlot::m_SlotStatus)
		.def_readonly("computer", &CGameSlot::m_Computer)
		.def_readonly("team", &CGameSlot::m_Team)
		.def_readonly("colour", &CGameSlot::m_Colour)
		.def_readonly("race", &CGameSlot::m_Race)
		.def_readonly("computerType", &CGameSlot::m_ComputerType)
		.def_readonly("handicap", &CGameSlot::m_Handicap)

		.def("getPID", &CGameSlot::GetPID)
		.def("getDownloadStatus", &CGameSlot::GetDownloadStatus)
		.def("getSlotStatus", &CGameSlot::GetSlotStatus)
		.def("getComputer", &CGameSlot::GetComputer)
		.def("getTeam", &CGameSlot::GetTeam)
		.def("getColour", &CGameSlot::GetColour)
		.def("getRace", &CGameSlot::GetRace)
		.def("getComputerType", &CGameSlot::GetComputerType)
		.def("getHandicap", &CGameSlot::GetHandicap)

		.def("setPID", &CGameSlot::SetPID)
		.def("setDownloadStatus", &CGameSlot::SetDownloadStatus)
		.def("setSlotStatus", &CGameSlot::SetSlotStatus)
		.def("setComputer", &CGameSlot::SetComputer)
		.def("setTeam", &CGameSlot::SetTeam)
		.def("setColour", &CGameSlot::SetColour)
		.def("setRace", &CGameSlot::SetRace)
		.def("setComputerType", &CGameSlot::SetComputerType)
		.def("setHandicap", &CGameSlot::SetHandicap)

		.def("getByteArray", &CGameSlot::GetByteArray)
	;
}


text:
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>

#include <cctbx/covariance/covariance.h>

namespace cctbx { namespace covariance {
  namespace boost_python {

    void wrap_covariance_matrix() {
      using namespace boost::python;

      def("extract_covariance_matrix_for_sites", (
        af::versa<double, af::packed_u_accessor>(*)(
        af::const_ref<std::size_t> const &,
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        extract_covariance_matrix_for_sites,
        (arg("i_seqs"), arg("matrix"), arg("parameter_map")));
      def("extract_covariance_matrix_for_u_aniso", (
        af::versa<double, af::packed_u_accessor>(*)(
        std::size_t,
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        extract_covariance_matrix_for_u_aniso,
        (arg("i_seq"), arg("matrix"), arg("parameter_map")));
      def("variance_for_u_iso", (
        double(*)(
        std::size_t,
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        variance_for_u_iso,
        (arg("i_seq"), arg("matrix"), arg("parameter_map")));
      def("orthogonalize_covariance_matrix", (
        af::versa<double, af::packed_u_accessor>(*)(
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::uctbx::unit_cell const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        orthogonalize_covariance_matrix,
        (arg("matrix"), arg("unit_cell"), arg("parameter_map")));
      def("covariance_orthogonalization_matrix", (
       scitbx::sparse::matrix<double>(*)(
        cctbx::uctbx::unit_cell const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        covariance_orthogonalization_matrix,
        (arg("unit_cell"), arg("parameter_map")));
    }

    void init_module()
    {
      wrap_covariance_matrix();
    }

}}} // namespace cctbx::covariance::boost_python

BOOST_PYTHON_MODULE(cctbx_covariance_ext)
{
  cctbx::covariance::boost_python::init_module();
}


text:
/**

Copyright (c) 2016, Heller Florian, Stöckle Patrick, Wittenbeck Valerij
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <ros/ros.h>
#include <asr_msgs/AsrObject.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>
#include <posedetection_msgs/Object6DPose.h>
#include <string>
#include <iostream>
#include <fstream>
#include <boost/thread.hpp>

class DataTracker {
public:
        ros::NodeHandle nh;

        // Subscriptions
        ros::Subscriber ptu_sub;
        ros::Subscriber fob_sub;
        ros::Subscriber cbd_sub;

        std::string ptu_topic, fob_topic, cbd_topic, output_file_path;

        double pan, tilt;
        geometry_msgs::Pose fob_pose;
        geometry_msgs::Pose cbd_pose;

        int data_counter;

        void ptuCB(const sensor_msgs::JointState& ptu_msg) {
            ROS_DEBUG("ptu Message recieved");
            pan = ptu_msg.position[0];
            tilt = ptu_msg.position[1];
        }

        void fobCB(const asr_msgs::AsrObject& fob_msg) {
            if (!fob_msg.identifier.empty() && strcmp(fob_msg.identifier.c_str(), "tracker_left_raw") == 0) {
                ROS_DEBUG("fob message recieved");
                geometry_msgs::PoseWithCovariance current_pose_with_c = fob_msg.sampledPoses.back();
                fob_pose = current_pose_with_c.pose;
            }
        }
        void cbdCB(const geometry_msgs::PoseStamped& cbd_msg) {
            ROS_DEBUG("cbd message recieved");
            ROS_INFO("cbd message recieved");
            cbd_pose = cbd_msg.pose;
        }

        DataTracker(ros::NodeHandle nh){
            this->nh = nh;
            nh.getParam("ptu_topic", ptu_topic);
            nh.getParam("fob_topic", fob_topic);
            nh.getParam("cbd_topic", cbd_topic);
            nh.getParam("output_file_path", output_file_path);

            ptu_sub = nh.subscribe(ptu_topic, 1, &DataTracker::ptuCB, this);
            fob_sub = nh.subscribe(fob_topic, 1, &DataTracker::fobCB, this);
            cbd_sub = nh.subscribe(cbd_topic, 1, &DataTracker::cbdCB, this);
            data_counter = 0;
            boost::thread trackerThread(spin, this);
            ros::spin();
        }
        static void spin(DataTracker* obj){
            ROS_INFO("Capturing started.");
            ROS_INFO("Enter 'e' to stop capturing.");
            while (std::cin.get() != 'e'){
               obj->write();
            }
            ROS_INFO("Capturing stopped.");
        }

        void write(){
            std::stringstream ss;
            ss << pan << "; " << tilt << "; ";
            ss << fob_pose.position.x << "; " << fob_pose.position.y << "; " << fob_pose.position.z << "; ";
            ss << fob_pose.orientation.w << "; " << fob_pose.orientation.x << "; " << fob_pose.orientation.y << "; " << fob_pose.orientation.z << "; ";
            ss << cbd_pose.position.x << "; " << cbd_pose.position.y << "; " << cbd_pose.position.z << std::endl;
            ROS_INFO("%s", ss.str().c_str());
            std::ofstream dataFile;
            dataFile.open (output_file_path.c_str(),std::ios::app);
            dataFile << ss.str();
            dataFile.close();
            ROS_INFO("data captured %d", data_counter);
            data_counter++;
        }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "calibration_tracker");

  ros::NodeHandle nh("~");
  new ::DataTracker(nh);
  //ros::spin();

  return 0;
}


text:
// ======================================================================
// \title  MVVislamImpl.cpp
// \author mereweth
// \brief  cpp file for MVVislam component implementation class
//
// \copyright
// Copyright 2009-2015, by the California Institute of Technology.
// ALL RIGHTS RESERVED.  United States Government Sponsorship
// acknowledged. Any commercial use must be negotiated with the Office
// of Technology Transfer at the California Institute of Technology.
//
// This software may be subject to U.S. export control laws and
// regulations.  By accepting this document, the user agrees to comply
// with all U.S. export laws and regulations.  User has the
// responsibility to obtain export licenses, or other export authority
// as may be required before exporting such information to foreign
// countries or providing access to foreign persons.
// ======================================================================

#include <SnapdragonFlight/MVVislam/MVVislamComponentImpl.hpp>
#include "Fw/Types/BasicTypes.hpp"

#include <Eigen/Geometry>
#include <math.h>

#include <Os/File.hpp>

//#define DEBUG_PRINT(x,...) printf(x,##__VA_ARGS__); fflush(stdout)
#define DEBUG_PRINT(x,...)

namespace SnapdragonFlight {

  // ----------------------------------------------------------------------
  // Construction, initialization, and destruction
  // ----------------------------------------------------------------------

  MVVislamComponentImpl ::
#if FW_OBJECT_NAMES == 1
    MVVislamComponentImpl(
        const char *const compName
    ) :
      MVVislamComponentBase(compName)
#else
      MVVislamImpl(void)
#endif
#ifdef BUILD_SDFLIGHT
      ,m_mvVISLAMPtr(NULL)
      ,m_camCfg()
#endif
      ,m_tbc()
      ,m_ombc()
      ,m_std0tbc()
      ,m_std0ombc()
      ,m_tba()

      ,m_readoutTime(0.0)
      ,m_camDelta(0.0)
      ,m_std0camDelta(0.0)

      ,m_accelMeasRange(0.0)
      ,m_gyroMeasRange(0.0)
      ,m_stdAccelMeasNoise(0.0)
      ,m_stdGyroMeasNoise(0.0)

      ,m_stdCamNoise(0.0)
      ,m_minStdPixelNoise(0.0)

      ,m_failHighPixelNoiseScaleFactor(0.0)
      ,m_logDepthBootstrap(0.0)
      ,m_useLogCameraHeight(false)
      ,m_logCameraHeightBootstrap(0.0)
      ,m_noInitWhenMoving(false)
      ,m_limitedIMUbWtrigger(0.0)
      ,m_staticMaskFilename("na")
      ,m_gpsImuTimeAlignment(0.0)
      ,m_mapping(false)
      ,m_initialized(false)
      ,m_activated(false)
      ,m_errorCode(0u)
      ,w_q_b(0.0, 0.0, 0.0, 1.0)
      ,x_b(0.0, 0.0, 0.0)
      ,m_tbDes(TB_NONE)
  {

  }

  void MVVislamComponentImpl ::
    init(
        const NATIVE_INT_TYPE queueDepth,
        const NATIVE_INT_TYPE instance
    )
  {
    MVVislamComponentBase::init(queueDepth, instance);
  }

  MVVislamComponentImpl ::
    ~MVVislamComponentImpl(void)
  {

  }

  void MVVislamComponentImpl ::
    setTBDes(TimeBase tbDes) {
      this->m_tbDes = tbDes;
  }

  void MVVislamComponentImpl ::
    preamble(void)
  {
      initHelper();
  }

    void MVVislamComponentImpl ::
      parameterUpdated(FwPrmIdType id)
    {
        DEBUG_PRINT("prm %d updated\n", id);
        Fw::ParamValid valid;

        switch (id) {
            // default params

            case PARAMID_MVVISLAM_INITWHENMOVING:
            {
                bool temp = paramGet_MVVISLAM_initWhenMoving(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_noInitWhenMoving = !temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_MAPPING:
            {
                bool temp = paramGet_MVVISLAM_mapping(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_mapping = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STATICMASKFILENAME:
            {
                Fw::ParamString temp = paramGet_MVVISLAM_staticMaskFilename(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_staticMaskFilename = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_LOGDEPTHBOOTSTRAP:
            {
                F32 temp = paramGet_MVVISLAM_logDepthBootstrap(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_logDepthBootstrap = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_USELOGCAMERAHEIGHT:
            {
                U8 temp = paramGet_MVVISLAM_useLogCameraHeight(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_useLogCameraHeight = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_LOGCAMERAHEIGHTBOOTSTRAP:
            {
                F32 temp = paramGet_MVVISLAM_logCameraHeightBootstrap(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_logCameraHeightBootstrap = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_UVOFFSET:
            {
                U32 temp = paramGet_MVVISLAM_uvOffset(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.uvOffset = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTIONMODEL:
            {
                U8 temp = paramGet_MVVISLAM_distortionModel(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortionModel = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_READOUTTIME:
            {
                F32 temp = paramGet_MVVISLAM_readoutTime(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_readoutTime = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STD0_CAMDELTA:
            {
                F32 temp = paramGet_MVVISLAM_std0_camDelta(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_std0camDelta = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STD0_TBC_X:
            {
                F32 temp = paramGet_MVVISLAM_std0_tbc_x(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_std0tbc[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STD0_TBC_Y:
            {
                F32 temp = paramGet_MVVISLAM_std0_tbc_y(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_std0tbc[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STD0_TBC_Z:
            {
                F32 temp = paramGet_MVVISLAM_std0_tbc_z(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_std0tbc[2] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STD0_OMBC_X:
            {
                F32 temp = paramGet_MVVISLAM_std0_ombc_x(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_std0ombc[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STD0_OMBC_Y:
            {
                F32 temp = paramGet_MVVISLAM_std0_ombc_y(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_std0ombc[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STD0_OMBC_Z:
            {
                F32 temp = paramGet_MVVISLAM_std0_ombc_z(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_std0ombc[2] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_TBA_X:
            {
                F32 temp = paramGet_MVVISLAM_tba_x(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_tba[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_TBA_Y:
            {
                F32 temp = paramGet_MVVISLAM_tba_y(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_tba[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_TBA_Z:
            {
                F32 temp = paramGet_MVVISLAM_tba_z(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_tba[2] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_PIXELWIDTH:
            {
                U32 temp = paramGet_MVVISLAM_pixelWidth(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.pixelWidth = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_PIXELHEIGHT:
            {
                U32 temp = paramGet_MVVISLAM_pixelHeight(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.pixelHeight = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_MEMORYSTRIDE:
            {
                U32 temp = paramGet_MVVISLAM_memoryStride(valid);
                if ((Fw::PARAM_VALID == valid) ||
                    (Fw::PARAM_DEFAULT == valid)) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.memoryStride = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;

            // imu <-> camera extrinsics and time alignment

            case PARAMID_MVVISLAM_CAMDELTA:
            {
                F32 temp = paramGet_MVVISLAM_camDelta(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camDelta = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_TBC_X:
            {
                F32 temp = paramGet_MVVISLAM_tbc_x(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_tbc[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_TBC_Y:
            {
                F32 temp = paramGet_MVVISLAM_tbc_y(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_tbc[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_TBC_Z:
            {
                F32 temp = paramGet_MVVISLAM_tbc_z(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_tbc[2] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_OMBC_X:
            {
                F32 temp = paramGet_MVVISLAM_ombc_x(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_ombc[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_OMBC_Y:
            {
                F32 temp = paramGet_MVVISLAM_ombc_y(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_ombc[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_OMBC_Z:
            {
                F32 temp = paramGet_MVVISLAM_ombc_z(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_ombc[2] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;

            // intrinsics

            case PARAMID_MVVISLAM_PRINCIPALPOINT_U:
            {
                F64 temp = paramGet_MVVISLAM_principalPoint_u(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.principalPoint[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_PRINCIPALPOINT_V:
            {
                F64 temp = paramGet_MVVISLAM_principalPoint_v(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.principalPoint[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_FOCALLENGTH_U:
            {
                F64 temp = paramGet_MVVISLAM_focalLength_u(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.focalLength[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_FOCALLENGTH_V:
            {
                F64 temp = paramGet_MVVISLAM_focalLength_v(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.focalLength[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;

            case PARAMID_MVVISLAM_DISTORTION_0:
            {
                F64 temp = paramGet_MVVISLAM_distortion_0(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[0] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_1:
            {
                F64 temp = paramGet_MVVISLAM_distortion_1(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[1] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_2:
            {
                F64 temp = paramGet_MVVISLAM_distortion_2(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[2] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_3:
            {
                F64 temp = paramGet_MVVISLAM_distortion_3(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[3] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_4:
            {
                F64 temp = paramGet_MVVISLAM_distortion_4(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[4] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_5:
            {
                F64 temp = paramGet_MVVISLAM_distortion_5(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[5] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_6:
            {
                F64 temp = paramGet_MVVISLAM_distortion_6(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[6] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_7:
            {
                F64 temp = paramGet_MVVISLAM_distortion_7(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_camCfg.distortion[7] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_8:
            {
                F64 temp = paramGet_MVVISLAM_distortion_8(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    // NOTE(Mereweth) - only 8 elements in array in header decl
                    //m_camCfg.distortion[8] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_DISTORTION_9:
            {
                F64 temp = paramGet_MVVISLAM_distortion_9(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    // NOTE(Mereweth) - only 8 elements in array in header decl
                    //m_camCfg.distortion[9] = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;

            // image sensor noise characteristics

            case PARAMID_MVVISLAM_STDCAMNOISE:
            {
                F32 temp = paramGet_MVVISLAM_stdCamNoise(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_stdCamNoise = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_MINSTDPIXELNOISE:
            {
                F32 temp = paramGet_MVVISLAM_minStdPixelNoise(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_minStdPixelNoise = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_FAILHIGHPIXELNOISESCALEFACTOR:
            {
                F32 temp = paramGet_MVVISLAM_failHighPixelNoiseScaleFactor(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_failHighPixelNoiseScaleFactor = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;

            // imu characteristics

            case PARAMID_MVVISLAM_ACCELMEASRANGE:
            {
                F32 temp = paramGet_MVVISLAM_accelMeasRange(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_accelMeasRange = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_GYROMEASRANGE:
            {
                F32 temp = paramGet_MVVISLAM_gyroMeasRange(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_gyroMeasRange = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STDACCELMEASNOISE:
            {
                F32 temp = paramGet_MVVISLAM_stdAccelMeasNoise(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_stdAccelMeasNoise = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_STDGYROMEASNOISE:
            {
                F32 temp = paramGet_MVVISLAM_stdGyroMeasNoise(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_stdGyroMeasNoise = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
            case PARAMID_MVVISLAM_LIMITEDIMUBWTRIGGER:
            {
                F32 temp = paramGet_MVVISLAM_limitedIMUbWtrigger(valid);
                if (Fw::PARAM_VALID == valid) {
#ifdef BUILD_SDFLIGHT
                    m_limitedIMUbWtrigger = temp;
#endif
                }
                else {
                    // TODO(mereweth) - issue EVR
                }
            }
                break;
        }
    }

    void MVVislamComponentImpl ::
      parametersLoaded()
    {
        for (U32 i = 0; i < __MAX_PARAMID; i++) {
            parameterUpdated(i);
        }
    }

  void MVVislamComponentImpl ::
    initHelper(void)
  {
#ifdef BUILD_SDFLIGHT
      this->m_mvVISLAMPtr =
        mvVISLAM_Initialize(&m_camCfg,
                            m_readoutTime,
                            m_tbc,
                            m_ombc,
                            m_camDelta,
                            m_std0tbc,
                            m_std0ombc,
                            m_std0camDelta,
                            m_accelMeasRange,
                            m_gyroMeasRange,
                            m_stdAccelMeasNoise,
                            m_stdGyroMeasNoise,
                            m_stdCamNoise,
                            m_minStdPixelNoise,
                            m_failHighPixelNoiseScaleFactor,
                            m_logDepthBootstrap,
                            m_useLogCameraHeight,
                            m_logCameraHeightBootstrap,
                            m_noInitWhenMoving,
                            m_limitedIMUbWtrigger,
                            m_staticMaskFilename.toChar(),
                            m_gpsImuTimeAlignment,
                            m_tba,
                            m_mapping
      );
      if (NULL != m_mvVISLAMPtr) {
          m_initialized = true;
      }
#endif //BUILD_SDFLIGHT
  }

  void MVVislamComponentImpl ::
    finalizer(void)
  {
      m_initialized = false;
#ifdef BUILD_SDFLIGHT
      if (NULL != m_mvVISLAMPtr) {
          mvVISLAM_Deinitialize(m_mvVISLAMPtr);
      }
#endif //BUILD_SDFLIGHT
  }

  // ----------------------------------------------------------------------
  // Handler implementations for user-defined typed input ports
  // ----------------------------------------------------------------------

  void MVVislamComponentImpl ::
    addImuHelper(ROS::sensor_msgs::ImuNoCov &imu)
  {
      ROS::std_msgs::Header h = imu.getheader();
      Fw::Time stamp = h.getstamp();

      // if port is not connected, default to no conversion
      Fw::Time convTime = stamp;

      if (this->isConnected_convertTime_OutputPort(0)) {
          bool success = false;
          convTime = this->convertTime_out(0, stamp, TB_WORKSTATION_TIME, 0, success);
          if (!success) {
              // TODO(Mereweth) - EVR
              return;
          }
      }

      h.setstamp(convTime);
      imu.setheader(h);

      const I64 usecHLOS = (I64) convTime.getSeconds() * 1000LL * 1000LL + (I64) convTime.getUSeconds();
      if (m_initialized && m_activated) {
#ifdef BUILD_SDFLIGHT
          mvVISLAM_AddAccel(m_mvVISLAMPtr, usecHLOS * 1000LL,
                            imu.getlinear_acceleration().getx(),
                            imu.getlinear_acceleration().gety(),
                            imu.getlinear_acceleration().getz());
          mvVISLAM_AddGyro(m_mvVISLAMPtr, usecHLOS * 1000LL,
                           imu.getangular_velocity().getx(),
                           imu.getangular_velocity().gety(),
                           imu.getangular_velocity().getz());
#endif //BUILD_SDFLIGHT
      }
  }

  void MVVislamComponentImpl ::
    Imu_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::sensor_msgs::ImuNoCov &imu
    )
  {
      addImuHelper(imu);
      if (isConnected_ImuFwd_OutputPort(0)) {
          ImuFwd_out(0, imu);
      }
  }

  void MVVislamComponentImpl ::
    BatchImu_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::mav_msgs::BatchImu &BatchImu
    )
  {
      NATIVE_INT_TYPE size = 0;
      ROS::sensor_msgs::ImuNoCov rwArray[100];
      const ROS::sensor_msgs::ImuNoCov* imuArray = BatchImu.getsamples(size);
      NATIVE_INT_TYPE setSize = FW_MIN(BatchImu.getsamples_count(), size);
      if (setSize > FW_NUM_ARRAY_ELEMENTS(rwArray)) {
          // TODO(mereweth) - EVR
          return;
      }
      for (U32 ii = 0; ii < setSize; ii++) {
          rwArray[ii] = imuArray[ii];
          addImuHelper(rwArray[ii]);
      }

      BatchImu.setsamples(rwArray, setSize);

      if (isConnected_BatchImuFwd_OutputPort(0)) {
          BatchImuFwd_out(0, BatchImu);
      }
  }

  void MVVislamComponentImpl ::
    ImageIn_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::sensor_msgs::Image &Image
    )
  {
      Fw::Buffer data = Image.getdata();
      if (m_initialized && m_activated) {
#ifdef BUILD_SDFLIGHT
          Fw::Time hlosTime = Image.getheader().getstamp();
          I64 usecHLOS = hlosTime.getSeconds() * 1000LL * 1000LL + hlosTime.getUSeconds();
          mvVISLAM_AddImage(m_mvVISLAMPtr, usecHLOS * 1000LL, (U8*) (data.getdata()));
          mvVISLAMPose vio_pose = mvVISLAM_GetPose(m_mvVISLAMPtr);

          /*
            - \b 0:  Reset: cov not pos definite
            - \b 1:  Reset: IMU exceeded range
            - \b 2:  Reset: IMU bandwidth too low
            - \b 3:  Reset: not stationary at initialization
            - \b 4:  Reset: no features for x seconds
            - \b 5:  Reset: insufficient constraints from features
            - \b 6:  Reset: failed to add new features
            - \b 7:  Reset: exceeded instant velocity uncertainty
            - \b 8:  Reset: exceeded velocity uncertainty over window
            - \b 10:  Dropped IMU samples
            - \b 11:  Intrinsic camera cal questionable
            - \b 12:  Insufficient number of good features to initialize
            - \b 13:  Dropped camera frame
            - \b 14:  Dropped GPS velocity sample
            - \b 15:  Sensor measurements with uninitialized time stamps or uninitialized uncertainty (set to 0)
          */

          DEBUG_PRINT("tbc %0.4f, %0.4f, %0.4f\n", vio_pose.tbc[0], vio_pose.tbc[1], vio_pose.tbc[2]);
          DEBUG_PRINT("Rbc\n[%0.4f, %0.4f, %0.4f\n%0.4f, %0.4f, %0.4f\n%0.4f, %0.4f, %0.4f\n]\n",
                      vio_pose.Rbc[0][0], vio_pose.Rbc[0][1], vio_pose.Rbc[0][2],
                      vio_pose.Rbc[1][0], vio_pose.Rbc[1][1], vio_pose.Rbc[1][2],
                      vio_pose.Rbc[2][0], vio_pose.Rbc[2][1], vio_pose.Rbc[2][2]);
          DEBUG_PRINT("aAccInv\n[%0.4f, %0.4f, %0.4f\n%0.4f, %0.4f, %0.4f\n%0.4f, %0.4f, %0.4f\n]\n",
                      vio_pose.aAccInv[0][0], vio_pose.aAccInv[0][1], vio_pose.aAccInv[0][2],
                      vio_pose.aAccInv[1][0], vio_pose.aAccInv[1][1], vio_pose.aAccInv[1][2],
                      vio_pose.aAccInv[2][0], vio_pose.aAccInv[2][1], vio_pose.aAccInv[2][2]);
          DEBUG_PRINT("aGyrInv\n[%0.4f, %0.4f, %0.4f\n%0.4f, %0.4f, %0.4f\n%0.4f, %0.4f, %0.4f\n]\n",
                      vio_pose.aGyrInv[0][0], vio_pose.aGyrInv[0][1], vio_pose.aGyrInv[0][2],
                      vio_pose.aGyrInv[1][0], vio_pose.aGyrInv[1][1], vio_pose.aGyrInv[1][2],
                      vio_pose.aGyrInv[2][0], vio_pose.aGyrInv[2][1], vio_pose.aGyrInv[2][2]);

          //TODO(mereweth) - move these after transform
          this->x_b.setx(vio_pose.bodyPose.matrix[0][3]);
          this->x_b.sety(vio_pose.bodyPose.matrix[1][3]);
          this->x_b.setz(vio_pose.bodyPose.matrix[2][3]);

          this->m_errorCode = vio_pose.errorCode;
          if ((!this->m_errorCode) &&
              (MV_TRACKING_STATE_HIGH_QUALITY == vio_pose.poseQuality)) {
              using namespace Eigen;
              using namespace ROS::std_msgs;
              using namespace ROS::mav_msgs;
              using namespace ROS::geometry_msgs;
              Vector3f grav(vio_pose.gravity[0], vio_pose.gravity[1], vio_pose.gravity[2]);
              grav.normalize();
              Transform<float,3,Affine> odom_to_imu(AngleAxisf(3.14159, Vector3f::UnitY())
                                                    * AngleAxisf(acos(grav.dot(Vector3f::UnitZ())),
                                                                 grav.cross(Vector3f::UnitZ())));

              Transform<float,3,Affine> imu_start_to_imu;
              imu_start_to_imu.matrix() << vio_pose.bodyPose.matrix[0][0],
                vio_pose.bodyPose.matrix[0][1],
                vio_pose.bodyPose.matrix[0][2],
                vio_pose.bodyPose.matrix[0][3],
                vio_pose.bodyPose.matrix[1][0],
                vio_pose.bodyPose.matrix[1][1],
                vio_pose.bodyPose.matrix[1][2],
                vio_pose.bodyPose.matrix[1][3],
                vio_pose.bodyPose.matrix[2][0],
                vio_pose.bodyPose.matrix[2][1],
                vio_pose.bodyPose.matrix[2][2],
                vio_pose.bodyPose.matrix[2][3],
                0.0, 0.0, 0.0, 1.0;
              odom_to_imu = odom_to_imu * imu_start_to_imu;

              // NOTE(Mereweth) - velocity is expressed in MV spatial, so this is correct to get
              // to body
              Vector3f vel(vio_pose.velocity[0], vio_pose.velocity[1], vio_pose.velocity[2]);
              vel = imu_start_to_imu.rotation() * vel;

              const Quaternionf odom_to_imu_q(odom_to_imu.rotation());
              const Vector3f odom_to_imu_v(odom_to_imu.translation());

              // if port is not connected, default to no conversion
              Fw::Time convTime = hlosTime;

              if (this->isConnected_convertTime_OutputPort(0)) {
                  bool success = false;
                  convTime = this->convertTime_out(0, hlosTime, this->m_tbDes, 0, success);
                  if (!success) {
                      // TODO(Mereweth) - EVR
                      return;
                  }
              }

              ImuStateUpdateNoCov update(
                  Header(Image.getheader().getseq(),
                         convTime,
                         0),
                  0, // TODO(mereweth) - child id
                  Pose(Point(odom_to_imu_v(0),
                             odom_to_imu_v(1),
                             odom_to_imu_v(2)),
                       ROS::geometry_msgs::Quaternion(odom_to_imu_q.x(),
                                                      odom_to_imu_q.y(),
                                                      odom_to_imu_q.z(),
                                                      odom_to_imu_q.w())),
                  Twist(Vector3(vel(0), vel(1), vel(2)),
                        Vector3(vio_pose.angularVelocity[0],
                                vio_pose.angularVelocity[1],
                                vio_pose.angularVelocity[2])),
                  Vector3(vio_pose.wBias[0],
                          vio_pose.wBias[1],
                          vio_pose.wBias[2]),
                  Vector3(vio_pose.aBias[0],
                          vio_pose.aBias[1],
                          vio_pose.aBias[2])
              ); // end ImuStateUpdate constructor

              for (int i = 0; i < NUM_IMUSTATEUPDATE_OUTPUT_PORTS; i++) {
                  if (isConnected_ImuStateUpdate_OutputPort(i)) {
                      ImuStateUpdate_out(i, update);
                  }
              }
          } // if !errorCode

          int num_points = mvVISLAM_HasUpdatedPointCloud(m_mvVISLAMPtr);
          //std::vector<mvVISLAMMapPoint> map_points(num_points, {0});
          //int num_received = mvVISLAM_GetPointCloud(m_mvVISLAMPtr, map_points.data(), num_points);
          //PublishMapPoints(map_points, msg->header.seq, msg->header.stamp);

#endif //BUILD_SDFLIGHT
      }

      ImageBufferReturn_out(0, data);
  }

  void MVVislamComponentImpl ::
    sched_handler(
        const NATIVE_INT_TYPE portNum,
        NATIVE_UINT_TYPE context
    )
  {
      tlmWrite_MVVISLAM_ErrorCode(m_errorCode);
      tlmWrite_MVVISLAM_x_b(x_b);
  }

  void MVVislamComponentImpl ::
    pingIn_handler(
        const NATIVE_INT_TYPE portNum,
        U32 key
    )
  {
      pingOut_out(0, key);
  }

  // ----------------------------------------------------------------------
  // Command handler implementations
  // ----------------------------------------------------------------------

  void MVVislamComponentImpl ::
    MVVISLAM_Reinit_cmdHandler(
        const FwOpcodeType opCode,
        const U32 cmdSeq
    )
  {
#ifdef BUILD_SDFLIGHT
      if (NULL != m_mvVISLAMPtr) {
          mvVISLAM_Deinitialize(m_mvVISLAMPtr);
      }
#endif //BUILD_SDFLIGHT
      m_initialized = false;
      initHelper();
      if (m_initialized) {
          this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_OK);
      }
      else {
          this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_EXECUTION_ERROR);
      }
  }

  void MVVislamComponentImpl ::
    MVVISLAM_Activate_cmdHandler(
        const FwOpcodeType opCode,
        const U32 cmdSeq,
        bool enable
    )
  {
      if (enable) {
          if (m_initialized) {
              m_activated = true;
              this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_OK);
          }
          else {
              this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_EXECUTION_ERROR);
          }
      }
      else {
          m_activated = false;
    	  this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_OK);
      }
  }

  void MVVislamComponentImpl ::
    MVVISLAM_Reset_cmdHandler(
        const FwOpcodeType opCode,
        const U32 cmdSeq
    )
  {
      if (m_initialized) {
#ifdef BUILD_SDFLIGHT
      	  mvVISLAM_Reset(m_mvVISLAMPtr, true);
#endif //BUILD_SDFLIGHT
          this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_OK);
      }
      else {
	      this->cmdResponse_out(opCode, cmdSeq, Fw::COMMAND_EXECUTION_ERROR);
      }
  }

} // end namespace SnapdragonFlight

max_stars_repo_name: genemerewether/fprime

max_issues_repo_name: genemerewether/fprime

max_forks_repo_name: genemerewether/fprime