/*
 // Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
 // component of the Texas A&M University System.

 // All rights reserved.

 // The information and source code contained herein is the exclusive
 // property of TEES and may not be disclosed, examined or reproduced
 // in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_MAPFUNCS_LINEARIZATION_HPP
#define STAPL_MAPFUNCS_LINEARIZATION_HPP

#include <stapl/algorithms/functional.hpp>
#include <stapl/views/mapping_functions/mapping_functions.hpp>
#include <stapl/utility/tuple.hpp>
#include <stapl/utility/tuple/to_index.hpp>
#include <stapl/utility/tuple/ensure_tuple.hpp>
#include <stapl/utility/tuple/rearrange.hpp>
#include <stapl/utility/integer_sequence.hpp>

#include <boost/mpl/find_if.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/distance.hpp>

namespace stapl {

template<typename GID, typename Traversal>
struct nd_reverse_linearize;

template<typename GID, typename Traversal>
struct nd_linearize;

namespace detail {

//////////////////////////////////////////////////////////////////////
/// @brief Rearrange the contents of a tuple to be in the correct
///        traversal order.
///
/// For example, given the tuple (x, y, z) and the traversal <1, 2, 0>,
/// output the tuple (z, x, y).
///
/// @tparam Tuple Type of the run-time tuple of std::size_ts
/// @tparam Traversal Traversal ordering.
//////////////////////////////////////////////////////////////////////
template<typename Tuple, typename Traversal,
         typename Indices = make_index_sequence<tuple_size<Traversal>::value>>
struct traversal_order;

template<typename Tuple, typename... IntTypes, std::size_t... Indices>
struct traversal_order<Tuple, tuple<IntTypes...>, index_sequence<Indices...>>
{
  typedef boost::mpl::vector<IntTypes...>                  vector_t;

private:
  typedef tuple<
    typename boost::mpl::distance<
      typename boost::mpl::begin<vector_t>::type,
      typename boost::mpl::find_if<
        vector_t, type_constant_equal<Indices>>::type
    >::type...>                                            indices_t;

public:
  static Tuple apply(Tuple const& src)
  {
    return Tuple(get<tuple_element<Indices, indices_t>::type::value>(src)...);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for size_t
//////////////////////////////////////////////////////////////////////
template<typename... IntTypes, std::size_t... Indices>
struct traversal_order<std::size_t, tuple<IntTypes...>,
                       index_sequence<Indices...>>
{
  static std::size_t apply(std::size_t src)
  {
    return src;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Rearrange the contents of a tuple to be in the reverse
///        traversal order.
/// @tparam Tuple Type of tuple to rearrange.
/// @tparam Travesal Traversal ordering.
///
/// For example, given the tuple (x, y, z) and the traversal <1, 2, 0>,
/// output the tuple (y, z, x).
//////////////////////////////////////////////////////////////////////
template<typename Tuple, typename Traversal,
         typename Indices = make_index_sequence<tuple_size<Tuple>::value>>
struct reverse_traversal_order;

template<typename Tuple, typename Traversal, std::size_t... Indices>
struct reverse_traversal_order<Tuple, Traversal, index_sequence<Indices...>>
{
  static Tuple apply(Tuple const& src)
  {
    return Tuple(get<tuple_element<Indices, Traversal>::type::value>(src)...);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Computes the modulus of a 1-dimensional number with the
///        linear size of all of the dimensions before the current
///        dimension @p N.
//////////////////////////////////////////////////////////////////////
template<int N>
struct apply_modulo
{
  template<class Tuple>
  static size_t apply(Tuple const& sizes, size_t result)
  {
    return apply_modulo<N-1>::apply(
      tuple_ops::pop_back(sizes),
      result % tuple_ops::fold(sizes, 1, stapl::multiplies<size_t>()));
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case @ref apply_modulo.
//////////////////////////////////////////////////////////////////////
template<>
struct apply_modulo<0>
{
  template<class Tuple>
  static size_t apply(Tuple const&, size_t result)
  { return result; }
};

//////////////////////////////////////////////////////////////////////
/// @brief Divides a given number by all of the elements of a tuple.
///
/// Start with N = N-1 and sizes in traversal order
//////////////////////////////////////////////////////////////////////
template<int N>
struct apply_divide
{
  template<class Tuple>
  static size_t apply(Tuple const& sizes, size_t result)
  {
    return apply_divide<N-1>::apply(sizes, result / get<N>(sizes));
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case @ref apply_divide.
//////////////////////////////////////////////////////////////////////
template<>
struct apply_divide<-1>
{
  template<class Tuple>
  static size_t apply(Tuple const&, size_t result)
  { return result; }
};

//////////////////////////////////////////////////////////////////////
/// @brief Translates a 1-dimensional linearization back to its
///        n-dimensional form.
///
/// This functor assumes that the sizes are in the correct traversal
/// order.
//////////////////////////////////////////////////////////////////////
template<int I, typename GID>
struct reverse_linearize
{
  static GID apply(GID const& m_size, size_t linear, GID gid)
  {
    size_t result =
      apply_modulo<tuple_size<GID>::value-1-I>::apply(
        tuple_ops::pop_back(m_size), linear);

    result = apply_divide<I-1>::apply(m_size, result);

    get<I>(gid) = result;

    return reverse_linearize<I-1, GID>::apply(m_size, linear, gid);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case of recursion for @ref reverse_linearize.
/// @note Intel fails if function operator takes @p gid by reference.
//////////////////////////////////////////////////////////////////////
template<typename GID>
struct reverse_linearize<-1, GID>
{
  static GID apply(GID const&, size_t, GID gid)
  { return gid; }
};

//////////////////////////////////////////////////////////////////////
/// @brief Small function to optimize index linearization by not
/// performing multiplication on the first element.
//////////////////////////////////////////////////////////////////////
template<int Idx>
struct conditional_multiply
{
  template<typename Tuple>
  static size_t apply(Tuple const& plane_sizes, size_t lhs)
  {
    return lhs * get<Idx>(plane_sizes);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for first element of tuple.  Do not perform
/// multiplication and assert the first element in plane_sizes is 1.
//////////////////////////////////////////////////////////////////////
template<>
struct conditional_multiply<0>
{
  template<typename Tuple>
  static size_t apply(Tuple const& plane_sizes, size_t lhs)
  {
    stapl_assert(get<0>(plane_sizes) == 1, "found last idx != 1");
    return lhs;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Reorders input Indices according to provided traversal order,
/// decrements each by corresponding value in @p first and then multiplies
/// by corresponding value in @p plane_sizes, accumulating the results into
/// a linearized index.
//////////////////////////////////////////////////////////////////////
template <int Iterate, int LastIdx, typename Traversal>
struct reorder_localize_linearize
{
  template<typename Tuple, typename Index, typename... Indices>
  static size_t apply(Tuple const& first, Tuple const& plane_sizes,
                    Index&& i, Indices&&... is)

 {
   constexpr size_t idx = std::tuple_element<Iterate, Traversal>::type::value;

   return conditional_multiply<idx>::apply(
     plane_sizes, i - std::get<idx>(first))
     + reorder_localize_linearize<Iterate + 1, LastIdx, Traversal>::apply(
         first, plane_sizes, std::forward<Indices>(is)...);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Base case of the recursion.
/// @note Traversal of the tuple occurs from 0 to tuple_size-1 for performance
/// reasons.
//////////////////////////////////////////////////////////////////////
template<int LastIdx, typename Traversal>
struct reorder_localize_linearize<LastIdx, LastIdx, Traversal>
{
  template<typename Tuple, typename Index>
  static size_t apply(Tuple const& first, Tuple const& plane_sizes, Index&& i)
  {
    constexpr size_t idx = std::tuple_element<LastIdx, Traversal>::type::value;

    return conditional_multiply<idx>::apply(
      plane_sizes, i - std::get<idx>(first));
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Computes the factor by which a component of an n-dimensional GID
/// should be multiplied when linearizing the GID.
//////////////////////////////////////////////////////////////////////
template<int Iterate, int LastIdx>
struct compute_plane_sizes
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Other indices multiplied by size of all inner dimensions.
    std::get<Iterate>(plane_size) =
      std::get<Iterate-1>(size)*std::get<Iterate-1>(plane_size);
    compute_plane_sizes<Iterate+1, LastIdx>::apply(size, plane_size);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for first iteration of @ref compute_plane_sizes.
///
/// The first element represents the innermost dimension of the traversal,
/// and thus isn't multiplied by any of the other dimensions.
/// @note Traversal of the tuple occurs from 0 to tuple_size-1 for performance
/// reasons.
//////////////////////////////////////////////////////////////////////
template<int LastIdx>
struct compute_plane_sizes<0, LastIdx>
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Innermost dimension is multiplied by 1 when linearizing.
    std::get<0>(plane_size) = 1;
    compute_plane_sizes<1, LastIdx>::apply(size, plane_size);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for final iteration of @ref compute_plane_sizes.
///
/// The last element represents the outermost dimension of the traversal,
/// and thus is multiplied by the size of all inner dimensions when linearized.
/// @note Traversal of the tuple occurs from 0 to tuple_size-1 for performance
/// reasons.
//////////////////////////////////////////////////////////////////////
template<int LastIdx>
struct compute_plane_sizes<LastIdx, LastIdx>
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Plane size is size multiplied by size of all inner dimensions.
    std::get<LastIdx>(plane_size) =
      std::get<LastIdx-1>(size)*std::get<LastIdx-1>(plane_size);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Specialization for the 1-D case.
//////////////////////////////////////////////////////////////////////
template<>
struct compute_plane_sizes<0, 0>
{
  template <typename Tuple>
  static void apply(Tuple const& size, Tuple& plane_size)
  {
    // Innermost dimension is multiplied by 1 when linearizing.
    std::get<0>(plane_size) = 1;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Type metafunction reflecting the reduced rank linearizer
/// that a slice operation returns.
//////////////////////////////////////////////////////////////////////
template<std::size_t SliceIndex, typename Traversal, typename GID>
struct slice_result
{
  using slices_tuple_t =
    typename tuple_ops::from_index_sequence<index_sequence<SliceIndex>>::type;

  using new_gid_t =
    typename tuple_ops::result_of::heterogeneous_discard<
      slices_tuple_t, GID>::type;

  using new_traversal_t =
    typename tuple_ops::result_of::heterogeneous_discard<
      slices_tuple_t, Traversal>::type;

  using type = nd_linearize<new_gid_t, new_traversal_t>;
};

template<typename Slices, typename Traversal,
         typename = make_index_sequence<tuple_size<Slices>::value>>
struct indices_of_most_significant;

//////////////////////////////////////////////////////////////////////
/// @brief Metafunction to compute the reordering of a Slices tuple (as a tuple
///        of indices) in descending order of Traversal's most significant
///        (slowest moving) dimensions.
///
///        When given multiple dimensions to slice at a time, we need to slice
///        them in the order of the largest to smallest (where largest and
///        smallest refers to their positions in the real traversal ordering).
///
///        For example, if Slices is <1,2,3> and Traversal is <0,2,4,3,1> then
///        the output would be <2,0,1>.
///
///        This is because we compute:
///          <4-Tr<Sl<0>>, 4-Tr<Sl<1>>, 4-Tr<Sl<2>>> =
///          <4-Tr<1>, 4-Tr<2>, 4-Tr<3>> =
///          <4-2, 4-4, 4-3> =
///          <2,0,1>
///
///        The meaning of <2,0,1> in this case means we need to slice position
///        1 first, then position 2, and then position 0. This boils down to
///        calling slice<2> and then slice<3> and then slice<1>. This is
///        necessary because in actuality, we are slicing the real dimensions of
///        4 then 3 then 2 of the traversal, which are the most significant
///        dimensions descending in order.
///
/// @note  The result of this metafunction only has meaning if the input slices
///        indeed refer to the last |Slices| dimensions of the linearizer and
///        it is only the order that is unknown.
//////////////////////////////////////////////////////////////////////
template<typename Slices, typename Traversal, std::size_t... Indices>
struct indices_of_most_significant<Slices, Traversal,
                                   index_sequence<Indices...>>
{
  static constexpr std::size_t traversal_size = tuple_size<Traversal>::value-1;

  using type = tuple<std::integral_constant<std::size_t,
    traversal_size - tuple_element<
      tuple_element<Indices, Slices>::type::value, Traversal>::type::value
  >...>;
};

//////////////////////////////////////////////////////////////////////
/// @brief Compute what the rest of the slices should be based on the current
///        slice. For example, if the incoming slices for slice() are
///        <0,1,2>, we would need to recursively call slice<0>.slice<0>.slice<0>
///        because we are now referring to the dimensions which are one less.
///        However, if the incoming slices are <2,1,0>, we would instead call
///        slice<2>.slice<1>.slice<0>.
//////////////////////////////////////////////////////////////////////
constexpr std::size_t adjusted_slice(std::size_t FirstSlice,
                                     std::size_t SecondSlice)
{
  return SecondSlice > FirstSlice ? SecondSlice-1 : SecondSlice;
}

} // namespace detail

//////////////////////////////////////////////////////////////////////
/// @brief Translates an n-dimensional @p gid to a 1-dimensional
///        linearization, based on the given @p Traversal order.
//////////////////////////////////////////////////////////////////////
template<typename GID, typename Traversal>
struct nd_linearize
{
public:
  using traversal_type = Traversal;
  using index_type     = GID;
  using gid_type       = size_t;
  using size_type      = index_type;
  using inverse        = nd_reverse_linearize<GID, Traversal>;
  using is_bijective   = std::true_type;

  static constexpr size_t last_idx = tuple_size<size_type>::value - 1;

  size_type         m_size;

  size_type         m_plane_sizes;
  size_type         m_first;

  /// @brief Original unreordered size kept to make slicing logic easier.
  size_type         m_original_size;

  /// @brief Original unreordered first index kept to make slicing logic easier.
  size_type         m_original_first;

public:
  nd_linearize(void) = default;

  explicit
  nd_linearize(size_type size)
    : m_size(detail::traversal_order<GID, Traversal>::apply(size)), m_first(),
      m_original_size(size), m_original_first()
  {
    detail::compute_plane_sizes<0, last_idx>::apply(m_size, m_plane_sizes);
  }

  explicit
  nd_linearize(size_type size, size_type first)
    : m_size(detail::traversal_order<GID, Traversal>::apply(size)),
      m_first(detail::traversal_order<GID, Traversal>::apply(first)),
      m_original_size(size), m_original_first(first)
  {
    detail::compute_plane_sizes<0, last_idx>::apply(m_size, m_plane_sizes);
  }

  explicit
  nd_linearize(nd_reverse_linearize<GID, Traversal> const& other)
    : m_size(other.m_size), m_first()
  {
    detail::compute_plane_sizes<0, last_idx>::apply(m_size, m_plane_sizes);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Slice a single index of the multidimensional space, fixing
  /// its value to that of the passed @p idx parameters. Return an offset
  /// representing the effect of the slicing operation and a reduced rank
  /// linearizer for the remaining indices.
  ///
  /// @todo Consider storing offset internally in @ref nd_linearize to
  /// better abstract behavior.
  //////////////////////////////////////////////////////////////////////
  template<std::size_t SliceIndex>
  std::pair<
    std::size_t, typename detail::slice_result<SliceIndex,Traversal,GID>::type
  >
  slice(size_t const& idx)
  {
    constexpr size_t mapped_index =
      tuple_element<SliceIndex, Traversal>::type::value;

    static_assert(mapped_index == last_idx,
                  "Slicing limited to last in traversal order");

    const size_t multiplier  = get<mapped_index>(m_plane_sizes);
    const std::size_t offset = (idx - get<mapped_index>(m_first)) * multiplier;

    using slice_result_t = detail::slice_result<SliceIndex,Traversal,GID>;
    using SlicesTuple    = typename slice_result_t::slices_tuple_t;
    using NewGID         = typename slice_result_t::new_gid_t;
    using NewTraversal   = typename slice_result_t::new_traversal_t;

    return std::make_pair(
      offset,
      nd_linearize<NewGID, NewTraversal>(
        tuple_ops::heterogeneous_discard<SlicesTuple>(m_original_size),
        tuple_ops::heterogeneous_discard<SlicesTuple>(m_original_first)));
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Slice multiple indices of the multidimensional space.
  /// Recursively called with single @p slice signature above as base case.
  //////////////////////////////////////////////////////////////////////
  template<std::size_t FirstSlice, std::size_t SecondSlice,
           std::size_t... Slices, typename Fixed>
    std::pair<std::size_t,
              decltype(
                std::declval<
                  typename detail::slice_result<
                    FirstSlice, Traversal, GID>::type
                >().template slice<
                    detail::adjusted_slice(FirstSlice, SecondSlice),
                    detail::adjusted_slice(FirstSlice, Slices)...
                  >(
                  tuple_ops::heterogeneous_discard<
                    tuple<std::integral_constant<size_t, 0>>
                  >(std::declval<Fixed>())).second)>
  slice(Fixed const& fixed)
  {
    using discard_tuple_t = tuple<std::integral_constant<size_t, 0>>;

    auto p = slice<FirstSlice>(get<0>(fixed));

    auto q = (p.second).template slice<
      detail::adjusted_slice(FirstSlice, SecondSlice),
      detail::adjusted_slice(FirstSlice, Slices)...>(
        tuple_ops::heterogeneous_discard<discard_tuple_t>(fixed)
      );

    return std::make_pair(p.first + q.first, q.second);
  }

private:
  //////////////////////////////////////////////////////////////////////
  /// @brief Slice multiple indices of the multidimensional space.
  ///    Specialization when Slices is an index_sequence
  //////////////////////////////////////////////////////////////////////
  template<std::size_t... Slices, typename Fixed>
  auto slice_impl(index_sequence<Slices...>, Fixed const& fixed)
    -> decltype(this->template slice<Slices...>(fixed))
  {
    return this->template slice<Slices...>(fixed);
  }

public:

  //////////////////////////////////////////////////////////////////////
  /// @brief Slice multiple indices of the multidimensional space.
  ///        Specialization when Slices is a tuple of integral constants
  ///
  ///        When slicing multiple indices at a time, we first need to rearrange
  ///        the slice dimensions to match the most significant dimensions of
  ///        the traversal.
  ///
  /// @tparam Slices Tuple of integral constants
  /// @param fixed The indices to fix in the slice
  //////////////////////////////////////////////////////////////////////
  template<typename Slices, typename Fixed>
  auto slice(Fixed const& fixed) ->
    decltype(this->slice_impl(typename tuple_ops::to_index_sequence<
      typename tuple_ops::result_of::rearrange<Slices,
        typename detail::indices_of_most_significant<Slices, Traversal>::type
      >::type
    >::type(), fixed))
  {
    // Rearrange the tuple of fixed values
    auto reordered_fixed = detail::traversal_order<Fixed,
      typename detail::indices_of_most_significant<Slices, Traversal>::type
    >::apply(fixed);

    // Rearrange the compile-time tuple of slices
    using reordered_slices = typename tuple_ops::to_index_sequence<
      typename tuple_ops::result_of::rearrange<
        Slices,
        typename detail::indices_of_most_significant<Slices, Traversal>::type
      >::type
    >::type;

    return this->slice_impl(reordered_slices(), reordered_fixed);
  }

public:
  //////////////////////////////////////////////////////////////////////
  /// @brief Localize and then linearize the indices that are the components
  /// of an n-dimensional GID.
  //////////////////////////////////////////////////////////////////////
  template <typename... Indices>
  gid_type operator()(Indices const&... is) const
  {
    return detail::reorder_localize_linearize<0, last_idx, Traversal>::apply(
      m_first, m_plane_sizes, is...);
  }

private:
  template<std::size_t... I>
  gid_type
  linearize_impl(index_type const& gid, index_sequence<I...> const&) const
  {
    return detail::reorder_localize_linearize<0, last_idx, Traversal>::apply(
      m_first, m_plane_sizes, std::get<I>(gid)...);
  }

public:
  //////////////////////////////////////////////////////////////////////
  /// Localize and then linearize the n-dimensional GID provided.
  //////////////////////////////////////////////////////////////////////
  template<typename Indices =
             make_index_sequence<tuple_size<index_type>::value>>
  gid_type operator()(index_type const& gid) const
  {
    return linearize_impl(gid, Indices());
  }

  void define_type(typer& t)
  {
    t.member(m_size);
    t.member(m_plane_sizes);
    t.member(m_first);
    t.member(m_original_size);
    t.member(m_original_first);
  }
}; // struct nd_linearize

//////////////////////////////////////////////////////////////////////
/// @brief Base case for @ref nd_linearize for 1-dimensional GID.
///
/// @todo Possibly use of the f_ident mapping function for this.
//////////////////////////////////////////////////////////////////////
template<typename Traversal>
struct nd_linearize<size_t, Traversal>
{
  using index_type = std::size_t;
  using traversal_type = Traversal;

  size_t m_size;
  size_t m_original_size;
  size_t m_original_first;
  size_t m_first;

  nd_linearize(void)
    : m_first(0)
  { }

  explicit
  nd_linearize(size_t)
    : m_first(0)
  { }

  explicit
  nd_linearize(size_t size, size_t first)
    : m_size(size), m_original_size(size), m_original_first(first),
      m_first(first)
  { }

  size_t operator()(size_t gid) const
  { return gid - m_first; }

  void define_type(typer& t)
  {
    t.member(m_size);
    t.member(m_original_size);
    t.member(m_original_first);
    t.member(m_first);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Translates a 1-dimensional linearized GID to its
///        n-dimensional GID, based on the given @p Traversal order.
//////////////////////////////////////////////////////////////////////
template<typename GID, typename Traversal>
struct nd_reverse_linearize
{
  typedef GID                               gid_type;
  typedef gid_type                          result_type;
  typedef std::size_t                       index_type;
  typedef gid_type                          size_type;
  typedef nd_linearize<GID, Traversal>      inverse;
  typedef std::true_type is_bijective;

  enum { last_idx = tuple_size<size_type>::value - 1 };

  size_type m_size;

  //////////////////////////////////////////////////////////////////////
  /// @todo Is the default constructor needed? if it is size should be
  ///       correctly initialized (default constructed in domain.hpp).
  //////////////////////////////////////////////////////////////////////
  nd_reverse_linearize(void) = default;

  explicit
  nd_reverse_linearize(size_type const& size)
    : m_size(detail::traversal_order<gid_type, Traversal>::apply(size))
  { }

  explicit
  nd_reverse_linearize(nd_linearize<GID, Traversal> const& other)
    : m_size(other.m_size)
  { }

  //////////////////////////////////////////////////////////////////////
  /// @todo merge calls to inverse_linearizer_t::operator() and
  /// inverse_index_order_t::operator() into a single call
  /// to minimize the number of traversals of the tuple.
  //////////////////////////////////////////////////////////////////////
  gid_type operator()(index_type linear) const
  {
    return detail::reverse_traversal_order<gid_type, Traversal>::apply(
      detail::reverse_linearize<last_idx, gid_type>::apply(
        m_size, linear, gid_type())
    );
  }

  void define_type(typer& t)
  {
    t.member(m_size);
  }
}; // struct nd_reverse_linearize

//////////////////////////////////////////////////////////////////////
/// @brief Base case for @ref nd_reverse_linearize for 1-dimensional GID.
///
/// @todo Possible use of the f_ident mapping function for this.
//////////////////////////////////////////////////////////////////////
template<typename Traversal>
struct nd_reverse_linearize<size_t, Traversal>
{
  nd_reverse_linearize(void) = default;

  explicit
  nd_reverse_linearize(size_t)
  { }

  size_t operator()(size_t linear) const
  { return linear; }
};

} // namespace stapl

#endif // STAPL_MAPFUNCS_LINEARIZATION_HPP


#ifndef SKYLARK_GEMM_HPP
#define SKYLARK_GEMM_HPP

#include <boost/mpi.hpp>
#include "exception.hpp"
#include "sparse_matrix.hpp"
#include "computed_matrix.hpp"
#include "../utility/typer.hpp"

#include "Gemm_detail.hpp"

// Defines a generic Gemm function that receives both dense and sparse matrices.

namespace skylark { namespace base {

/**
 * Rename the elemental Gemm function, so that we have unified access.
 */

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& B,
    T beta, elem::Matrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, beta, C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& B,
    elem::Matrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::STAR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    elem::Gemm(oA, oB, alpha, A.LockedMatrix(), B.LockedMatrix(), beta, C.Matrix());
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::STAR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    elem::Gemm(oA, oB, alpha, A.LockedMatrix(), B.LockedMatrix(), C.Matrix());
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& B,
    T beta, elem::DistMatrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, beta, C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& B,
    elem::DistMatrix<T>& C) {
    elem::Gemm(oA, oB, alpha, A, B, C);
}

/**
 * The following combinations is not offered by Elemental, but is useful for us.
 * We implement it partially.
 */

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    // TODO verify sizes etc.

    if ((oA == elem::TRANSPOSE || oA == elem::ADJOINT) && oB == elem::NORMAL) {
        boost::mpi::communicator comm(C.Grid().Comm(), boost::mpi::comm_attach);
        elem::Matrix<T> Clocal(C.Matrix());
        elem::Gemm(oA, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta / T(comm.size()), Clocal);
        boost::mpi::all_reduce(comm,
            Clocal.Buffer(), Clocal.MemorySize(), C.Matrix().Buffer(),
            std::plus<T>());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& B,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::VC, elem::STAR>& C) {
    // TODO verify sizes etc.

    if (oA == elem::NORMAL && oB == elem::NORMAL) {
        elem::Gemm(elem::NORMAL, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta, C.Matrix());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    elem::DistMatrix<T, elem::VC, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {
    // TODO verify sizes etc.

    if ((oA == elem::TRANSPOSE || oA == elem::ADJOINT) && oB == elem::NORMAL) {
        boost::mpi::communicator comm(C.Grid().Comm(), boost::mpi::comm_attach);
        elem::Matrix<T> Clocal(C.Matrix());
        elem::Gemm(oA, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta / T(comm.size()), Clocal);
        boost::mpi::all_reduce(comm,
            Clocal.Buffer(), Clocal.MemorySize(), C.Matrix().Buffer(),
            std::plus<T>());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VR, elem::STAR>& B,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    T beta, elem::DistMatrix<T, elem::VR, elem::STAR>& C) {
    // TODO verify sizes etc.

    if (oA == elem::NORMAL && oB == elem::NORMAL) {
        elem::Gemm(elem::NORMAL, elem::NORMAL,
            alpha, A.LockedMatrix(), B.LockedMatrix(),
            beta, C.Matrix());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::DistMatrix<T, elem::VR, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& B,
    elem::DistMatrix<T, elem::VR, elem::STAR>& C) {

    int C_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

/**
 * Gemm between mixed elemental, sparse input. Output is dense elemental.
 */

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const elem::Matrix<T>& A, const sparse_matrix_t<T>& B,
    T beta, elem::Matrix<T>& C) {
    // TODO verify sizes etc.

    const int* indptr = B.indptr();
    const int* indices = B.indices();
    const T *values = B.locked_values();

    int k = A.Width();
    int n = B.width();
    int m = A.Height();

    if (oA == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oA = elem::TRANSPOSE;

    if (oB == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oB = elem::TRANSPOSE;

    if (oA == elem::ADJOINT || oB == elem::ADJOINT)
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());

    // NN
    if (oA == elem::NORMAL && oB == elem::NORMAL) {

        elem::Scal(beta, C);

        elem::Matrix<T> Ac;
        elem::Matrix<T> Cc;

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for private(Cc, Ac)
#       endif
        for(int col = 0; col < n; col++) {
            elem::View(Cc, C, 0, col, m, 1);
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                int row = indices[j];
                T val = values[j];
                elem::LockedView(Ac, A, 0, row, m, 1);
                elem::Axpy(alpha * val, Ac, Cc);
            }
        }
    }

    // NT
    if (oA == elem::NORMAL && oB == elem::TRANSPOSE) {

        elem::Scal(beta, C);

        elem::Matrix<T> Ac;
        elem::Matrix<T> Cc;

        // Now, we simply think of B has being in CSR mode...
        int row = 0;
        for(int row = 0; row < n; row++) {
            elem::LockedView(Ac, A, 0, row, m, 1);
#           if SKYLARK_HAVE_OPENMP
#           pragma omp parallel for private(Cc)
#           endif
            for (int j = indptr[row]; j < indptr[row + 1]; j++) {
                int col = indices[j];
                T val = values[j];
                elem::View(Cc, C, 0, col, m, 1);
                elem::Axpy(alpha * val, Ac, Cc);
            }
        }
    }

    // TN - TODO: Not tested!
    if (oA == elem::TRANSPOSE && oB == elem::NORMAL) {
        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *a = A.LockedBuffer();
        int lda = A.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for collapse(2)
#       endif
        for (int j = 0; j < n; j++)
            for(int row = 0; row < k; row++) {
                c[j * ldc + row] *= beta;
                 for (int l = indptr[j]; l < indptr[j + 1]; l++) {
                     int rr = indices[l];
                     T val = values[l];
                     c[j * ldc + row] += val * a[j * lda + rr];
                 }
            }
    }

    // TT - TODO: Not tested!
    if (oA == elem::TRANSPOSE && oB == elem::TRANSPOSE) {
        elem::Scal(beta, C);

        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *a = A.LockedBuffer();
        int lda = A.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int row = 0; row < k; row++)
            for(int rb = 0; rb < n; rb++)
                for (int l = indptr[rb]; l < indptr[rb + 1]; l++) {
                    int col = indices[l];
                    c[col * ldc + row] += values[l] * a[row * lda + rb];
                }
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const sparse_matrix_t<T>& A, const elem::Matrix<T>& B,
    T beta, elem::Matrix<T>& C) {
    // TODO verify sizes etc.

    const int* indptr = A.indptr();
    const int* indices = A.indices();
    const double *values = A.locked_values();

    int k = A.width();
    int n = B.Width();
    int m = B.Height();

    if (oA == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oA = elem::TRANSPOSE;

    if (oB == elem::ADJOINT && std::is_same<T, elem::Base<T> >::value)
        oB = elem::TRANSPOSE;

    // NN
    if (oA == elem::NORMAL && oB == elem::NORMAL) {

        elem::Scal(beta, C);

        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *b = B.LockedBuffer();
        int ldb = B.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int i = 0; i < n; i++)
            for(int col = 0; col < k; col++)
                 for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                     int row = indices[j];
                     T val = values[j];
                     c[i * ldc + row] += alpha * val * b[i * ldb + col];
                 }
    }

    // NT
    if (oA == elem::NORMAL && (oB == elem::TRANSPOSE || oB == elem::ADJOINT)) {

        elem::Scal(beta, C);

        elem::Matrix<T> Bc;
        elem::Matrix<T> BTr;
        elem::Matrix<T> Cr;

        for(int col = 0; col < k; col++) {
            elem::LockedView(Bc, B, 0, col, m, 1);
            elem::Transpose(Bc, BTr, oB == elem::ADJOINT);
#           if SKYLARK_HAVE_OPENMP
#           pragma omp parallel for private(Cr)
#           endif
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                int row = indices[j];
                T val = values[j];
                elem::View(Cr, C, row, 0, 1, m);
                elem::Axpy(alpha * val, BTr, Cr);
            }
        }
    }

    // TN - TODO: Not tested!
    if (oA == elem::TRANSPOSE && oB == elem::NORMAL) {
        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *b = B.LockedBuffer();
        int ldb = B.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for collapse(2)
#       endif
        for (int j = 0; j < n; j++)
            for(int row = 0; row < k; row++) {
                c[j * ldc + row] *= beta;
                 for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                     int col = indices[l];
                     T val = values[l];
                     c[j * ldc + row] += val * b[j * ldb + col];
                 }
            }
    }

    // AN - TODO: Not tested!
    if (oA == elem::ADJOINT && oB == elem::NORMAL) {
        double *c = C.Buffer();
        int ldc = C.LDim();

        const double *b = B.LockedBuffer();
        int ldb = B.LDim();

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for collapse(2)
#       endif
        for (int j = 0; j < n; j++)
            for(int row = 0; row < k; row++) {
                c[j * ldc + row] *= beta;
                 for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                     int col = indices[l];
                     T val = elem::Conj(values[l]);
                     c[j * ldc + row] += val * b[j * ldb + col];
                 }
            }
    }

    // TT - TODO: Not tested!
    if (oA == elem::TRANSPOSE && (oB == elem::TRANSPOSE || oB == elem::ADJOINT)) {

        elem::Scal(beta, C);

        elem::Matrix<T> Bc;
        elem::Matrix<T> BTr;
        elem::Matrix<T> Cr;

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for private(Cr, Bc, BTr)
#       endif
        for(int row = 0; row < k; row++) {
            elem::View(Cr, C, row, 0, 1, m);
            for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                int col = indices[l];
                T val = values[l];
                elem::LockedView(Bc, B, 0, col, m, 1);
                elem::Transpose(Bc, BTr, oB == elem::ADJOINT);
                elem::Axpy(alpha * val, BTr, Cr);
            }
        }
    }

    // AT - TODO: Not tested!
    if (oA == elem::ADJOINT && (oB == elem::TRANSPOSE || oB == elem::ADJOINT)) {

        elem::Scal(beta, C);

        elem::Matrix<T> Bc;
        elem::Matrix<T> BTr;
        elem::Matrix<T> Cr;

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for private(Cr, Bc, BTr)
#       endif
        for(int row = 0; row < k; row++) {
            elem::View(Cr, C, row, 0, 1, m);
            for (int l = indptr[row]; l < indptr[row + 1]; l++) {
                int col = indices[l];
                T val = elem::Conj(values[l]);
                elem::LockedView(Bc, B, 0, col, m, 1);
                elem::Transpose(Bc, BTr, oB == elem::ADJOINT);
                elem::Axpy(alpha * val, BTr, Cr);
            }
        }
    }
}

template<typename T>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    T alpha, const sparse_matrix_t<T>& A, const elem::Matrix<T>& B,
    elem::Matrix<T>& C) {
    int C_height = (oA == elem::NORMAL ? A.height() : A.width());
    int C_width = (oB == elem::NORMAL ? B.Width() : B.Height());
    elem::Zeros(C, C_height, C_width);
    base::Gemm(oA, oB, alpha, A, B, T(0), C);
}

#if SKYLARK_HAVE_COMBBLAS
/**
 * Mixed GEMM for Elemental and CombBLAS matrices. For a distributed Elemental
 * input matrix, the output has the same distribution.
 */

/// Gemm for distCombBLAS x distElemental(* / *) -> distElemental (SOMETHING / *)
template<typename index_type, typename value_type, elem::Distribution col_d>
void Gemm(elem::Orientation oA, elem::Orientation oB, double alpha,
          const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
          const elem::DistMatrix<value_type, elem::STAR, elem::STAR> &B,
          double beta,
          elem::DistMatrix<value_type, col_d, elem::STAR> &C) {

    if(oA == elem::NORMAL && oB == elem::NORMAL) {

        if(A.getnol() != B.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(A.getnrow() != C.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(B.Width() != C.Width())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        //XXX: simple heuristic to decide what to communicate (improve!)
        //     or just if A.getncol() < B.Width..
        if(A.getnnz() < B.Height() * B.Width())
            detail::outer_panel_mixed_gemm_impl_nn(alpha, A, B, beta, C);
        else
            detail::inner_panel_mixed_gemm_impl_nn(alpha, A, B, beta, C);
    }
}

/// Gemm for distCombBLAS x distElemental(SOMETHING / *) -> distElemental (* / *)
template<typename index_type, typename value_type, elem::Distribution col_d>
void Gemm(elem::Orientation oA, elem::Orientation oB, double alpha,
          const SpParMat<index_type, value_type, SpDCCols<index_type, value_type> > &A,
          const elem::DistMatrix<value_type, col_d, elem::STAR> &B,
          double beta,
          elem::DistMatrix<value_type, elem::STAR, elem::STAR> &C) {

    if(oA == elem::TRANSPOSE && oB == elem::NORMAL) {

        if(A.getrow() != B.Height())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(A.getncol() != C.Height())
            SKYLARK_THROW_EXCEPTION (
                    base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        if(B.Width() != C.Width())
            SKYLARK_THROW_EXCEPTION (
                base::combblas_exception()
                    << base::error_msg("Gemm: Dimensions do not agree"));

        detail::outer_panel_mixed_gemm_impl_tn(alpha, A, B, beta, C);
    }

}

#endif // SKYLARK_HAVE_COMBBLAS

/* All combinations with computed matrix */

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT>& A,
    const RT& B, typename utility::typer_t<OT>::value_type beta, OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B, beta, C);
}

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT>& A,
    const RT& B, OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B, C);
}

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const RT& A,
    const computed_matrix_t<CT>& B,
    typename utility::typer_t<OT>::value_type beta, OT& C) {
    base::Gemm(oA, oB, alpha, A, B.materialize(), beta, C);
}

template<typename CT, typename RT, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const RT& A,
    const computed_matrix_t<CT>& B, OT& C) {
    base::Gemm(oA, oB, alpha, A, B.materialize(), C);
}

template<typename CT1, typename CT2, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT1>& A,
    const computed_matrix_t<CT2>& B, typename utility::typer_t<OT>::value_type beta,
    OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B.materialize(), beta, C);
}

template<typename CT1, typename CT2, typename OT>
inline void Gemm(elem::Orientation oA, elem::Orientation oB,
    typename utility::typer_t<OT>::value_type alpha, const computed_matrix_t<CT1>& A,
    const computed_matrix_t<CT2>& B, OT& C) {
    base::Gemm(oA, oB, alpha, A.materialize(), B.materialize(), C);
}

} } // namespace skylark::base
#endif // SKYLARK_GEMM_HPP


//==============================================================================
//         Copyright 2015 - J.T. Lapreste
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2015   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_ROUND_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_ROUND_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  round generic tag

      Represents the round function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct round_ : ext::elementwise_<round_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<round_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_round_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::round_, Site> dispatching_round_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::round_, Site>();
    }
    template<class... Args>
    struct impl_round_;
  }
  /*!
    Computes the rounded to nearest integer away from 0

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = round(x);
    @endcode

    Returns the nearest integer to x.

    @par Note:

    aways from 0 means that half integer values are rounded to the nearest
    integer of greatest absolute value

    @param  x value to be rounded

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::round_, round, 1)
  /*!
    round(x,n) rounds aways from 0 to n digits:

    @par semantic:
    For any given value @c x of type @c T and integer n :

    @code
    T r = round(x, n);
    @endcode

    is equivalent to

    @code
    T r = round(x*exp10(n)*exp10(-n));
    @endcode

    @par Note:

    n > 0: round to n digits to the right of the decimal point.

    n = 0: round to the nearest integer.

    n < 0: round to n digits to the left of the decimal point.

    aways from 0 means that half integer values are rounded to the nearest
    integer of greatest absolute value

    @param  x value to be rounded
    @param  n number of digits

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::round_, round, 2)
} }

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 *  @file testDiscreteFactorGraph.cpp
 *  @date Feb 14, 2011
 *  @author Duy-Nguyen Ta
 */

#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteBayesTree.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/map.hpp>
using namespace boost::assign;

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST_UNSAFE( DiscreteFactorGraph, debugScheduler) {
  DiscreteKey PC(0,4), ME(1, 4), AI(2, 4), A(3, 3);

  DiscreteFactorGraph graph;
  graph.add(AI, "1 0 0 1");
  graph.add(AI, "1 1 1 0");
  graph.add(A & AI, "1 1 1 0   1 1 1 1   0 1 1 1");
  graph.add(ME, "0 1 0 0");
  graph.add(ME, "1 1 1 0");
  graph.add(A & ME, "1 1 1 0   1 1 1 1   0 1 1 1");
  graph.add(PC, "1 0 1 0");
  graph.add(PC, "1 1 1 0");
  graph.add(A & PC, "1 1 1 0   1 1 1 1   0 1 1 1");
  graph.add(ME & AI, "0 1 1 1   1 0 1 1   1 1 0 1  1 1 1 0");
  graph.add(PC & ME, "0 1 1 1   1 0 1 1   1 1 0 1  1 1 1 0");
  graph.add(PC & AI, "0 1 1 1   1 0 1 1   1 1 0 1  1 1 1 0");

//  graph.print("Graph: ");
  DecisionTreeFactor product = graph.product();
  DecisionTreeFactor::shared_ptr sum = product.sum(1);
//  sum->print("Debug SUM: ");
  DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum));

//  cond->print("marginal:");

//  pair<DiscreteBayesNet::shared_ptr, DiscreteFactor::shared_ptr> result = EliminateDiscrete(graph, 1);
//  result.first->print("BayesNet: ");
//  result.second->print("New factor: ");
//
  Ordering ordering;
  ordering += Key(0),Key(1),Key(2),Key(3);
  DiscreteEliminationTree eliminationTree(graph, ordering);
//  eliminationTree.print("Elimination tree: ");
  eliminationTree.eliminate(EliminateDiscrete);
//  solver.optimize();
//  DiscreteBayesNet::shared_ptr bayesNet = solver.eliminate();
}

/* ************************************************************************* */
/// Test the () operator of DiscreteFactorGraph
TEST_UNSAFE( DiscreteFactorGraph, DiscreteFactorGraphEvaluationTest) {

  // Three keys P1 and P2
  DiscreteKey P1(0,2), P2(1,2), P3(2,3);

  // Create the DiscreteFactorGraph
  DiscreteFactorGraph graph;
  graph.add(P1, "0.9 0.3");
  graph.add(P2, "0.9 0.6");
  graph.add(P1 & P2, "4 1 10 4");

  // Instantiate Values
  DiscreteFactor::Values values;
  values[0] = 1;
  values[1] = 1;

  // Check if graph evaluation works ( 0.3*0.6*4 )
  EXPECT_DOUBLES_EQUAL( .72, graph(values), 1e-9);

  // Creating a new test with third node and adding unary and ternary factors on it
  graph.add(P3, "0.9 0.2 0.5");
  graph.add(P1 & P2 & P3, "1 2 3 4 5 6 7 8 9 10 11 12");

  // Below values lead to selecting the 8th index in the ternary factor table
  values[0] = 1;
  values[1] = 0;
  values[2] = 1;

  // Check if graph evaluation works (0.3*0.9*1*0.2*8)
  EXPECT_DOUBLES_EQUAL( 4.32, graph(values), 1e-9);

  // Below values lead to selecting the 3rd index in the ternary factor table
  values[0] = 0;
  values[1] = 1;
  values[2] = 0;

  // Check if graph evaluation works (0.9*0.6*1*0.9*4)
  EXPECT_DOUBLES_EQUAL( 1.944, graph(values), 1e-9);

  // Check if graph product works
  DecisionTreeFactor product = graph.product();
  EXPECT_DOUBLES_EQUAL( 1.944, product(values), 1e-9);
}

/* ************************************************************************* */
TEST( DiscreteFactorGraph, test)
{
  // Declare keys and ordering
  DiscreteKey C(0,2), B(1,2), A(2,2);

  // A simple factor graph (A)-fAC-(C)-fBC-(B)
  // with smoothness priors
  DiscreteFactorGraph graph;
  graph.add(A & C, "3 1 1 3");
  graph.add(C & B, "3 1 1 3");

  // Test EliminateDiscrete
  // FIXME: apparently Eliminate returns a conditional rather than a net
  Ordering frontalKeys;
  frontalKeys += Key(0);
  DiscreteConditional::shared_ptr conditional;
  DecisionTreeFactor::shared_ptr newFactor;
  boost::tie(conditional, newFactor) = EliminateDiscrete(graph, frontalKeys);

  // Check Bayes net
  CHECK(conditional);
  DiscreteBayesNet expected;
  Signature signature((C | B, A) = "9/1 1/1 1/1 1/9");
  // cout << signature << endl;
  DiscreteConditional expectedConditional(signature);
  EXPECT(assert_equal(expectedConditional, *conditional));
  expected.add(signature);

  // Check Factor
  CHECK(newFactor);
  DecisionTreeFactor expectedFactor(B & A, "10 6 6 10");
  EXPECT(assert_equal(expectedFactor, *newFactor));

  // add conditionals to complete expected Bayes net
  expected.add(B | A = "5/3 3/5");
  expected.add(A % "1/1");
  //  GTSAM_PRINT(expected);

  // Test elimination tree
  Ordering ordering;
  ordering += Key(0), Key(1), Key(2);
  DiscreteEliminationTree etree(graph, ordering);
  DiscreteBayesNet::shared_ptr actual;
  DiscreteFactorGraph::shared_ptr remainingGraph;
  boost::tie(actual, remainingGraph) = etree.eliminate(&EliminateDiscrete);
  EXPECT(assert_equal(expected, *actual));

//  // Test solver
//  DiscreteBayesNet::shared_ptr actual2 = solver.eliminate();
//  EXPECT(assert_equal(expected, *actual2));

  // Test optimization
  DiscreteFactor::Values expectedValues;
  insert(expectedValues)(0, 0)(1, 0)(2, 0);
  DiscreteFactor::sharedValues actualValues = graph.optimize();
  EXPECT(assert_equal(expectedValues, *actualValues));
}

/* ************************************************************************* */
TEST( DiscreteFactorGraph, testMPE)
{
  // Declare a bunch of keys
  DiscreteKey C(0,2), A(1,2), B(2,2);

  // Create Factor graph
  DiscreteFactorGraph graph;
  graph.add(C & A, "0.2 0.8 0.3 0.7");
  graph.add(C & B, "0.1 0.9 0.4 0.6");
  //  graph.product().print();
  //  DiscreteSequentialSolver(graph).eliminate()->print();

  DiscreteFactor::sharedValues actualMPE = graph.optimize();

  DiscreteFactor::Values expectedMPE;
  insert(expectedMPE)(0, 0)(1, 1)(2, 1);
  EXPECT(assert_equal(expectedMPE, *actualMPE));
}

/* ************************************************************************* */
TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244)
{
  // The factor graph in Darwiche09book, page 244
  DiscreteKey A(4,2), C(3,2), S(2,2), T1(0,2), T2(1,2);

  // Create Factor graph
  DiscreteFactorGraph graph;
  graph.add(S, "0.55 0.45");
  graph.add(S & C, "0.05 0.95 0.01 0.99");
  graph.add(C & T1, "0.80 0.20 0.20 0.80");
  graph.add(S & C & T2, "0.80 0.20 0.20 0.80 0.95 0.05 0.05 0.95");
  graph.add(T1 & T2 & A, "1 0 0 1 0 1 1 0");
  graph.add(A, "1 0");// evidence, A = yes (first choice in Darwiche)
  //graph.product().print("Darwiche-product");
  //  graph.product().potentials().dot("Darwiche-product");
  //  DiscreteSequentialSolver(graph).eliminate()->print();

  DiscreteFactor::Values expectedMPE;
  insert(expectedMPE)(4, 0)(2, 0)(3, 1)(0, 1)(1, 1);

  // Use the solver machinery.
  DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
  DiscreteFactor::sharedValues actualMPE = chordal->optimize();
  EXPECT(assert_equal(expectedMPE, *actualMPE));
//  DiscreteConditional::shared_ptr root = chordal->back();
//  EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9);

  // Let us create the Bayes tree here, just for fun, because we don't use it now
//  typedef JunctionTreeOrdered<DiscreteFactorGraph> JT;
//  GenericMultifrontalSolver<DiscreteFactor, JT> solver(graph);
//  BayesTreeOrdered<DiscreteConditional>::shared_ptr bayesTree = solver.eliminate(&EliminateDiscrete);
////  bayesTree->print("Bayes Tree");
//  EXPECT_LONGS_EQUAL(2,bayesTree->size());

  Ordering ordering;
  ordering += Key(0),Key(1),Key(2),Key(3),Key(4);
  DiscreteBayesTree::shared_ptr bayesTree = graph.eliminateMultifrontal(ordering);
  //  bayesTree->print("Bayes Tree");
  EXPECT_LONGS_EQUAL(2,bayesTree->size());

#ifdef OLD
// Create the elimination tree manually
VariableIndexOrdered structure(graph);
typedef EliminationTreeOrdered<DiscreteFactor> ETree;
ETree::shared_ptr eTree = ETree::Create(graph, structure);
//eTree->print(">>>>>>>>>>> Elimination Tree <<<<<<<<<<<<<<<<<");

// eliminate normally and check solution
DiscreteBayesNet::shared_ptr bayesNet = eTree->eliminate(&EliminateDiscrete);
//  bayesNet->print(">>>>>>>>>>>>>> Bayes Net <<<<<<<<<<<<<<<<<<");
DiscreteFactor::sharedValues actualMPE = optimize(*bayesNet);
EXPECT(assert_equal(expectedMPE, *actualMPE));

// Approximate and check solution
//  DiscreteBayesNet::shared_ptr approximateNet = eTree->approximate();
//  approximateNet->print(">>>>>>>>>>>>>> Approximate Net <<<<<<<<<<<<<<<<<<");
//  EXPECT(assert_equal(expectedMPE, *actualMPE));
#endif
}
#ifdef OLD

/* ************************************************************************* */
/**
 * Key type for discrete conditionals
 * Includes name and cardinality
 */
class Key2 {
private:
std::string wff_;
size_t cardinality_;
public:
/** Constructor, defaults to binary */
Key2(const std::string& name, size_t cardinality = 2) :
wff_(name), cardinality_(cardinality) {
}
const std::string& name() const {
  return wff_;
}

/** provide streaming */
friend std::ostream& operator <<(std::ostream &os, const Key2 &key);
};

struct Factor2 {
std::string wff_;
Factor2() :
wff_("@") {
}
Factor2(const std::string& s) :
wff_(s) {
}
Factor2(const Key2& key) :
wff_(key.name()) {
}

friend std::ostream& operator <<(std::ostream &os, const Factor2 &f);
friend Factor2 operator -(const Key2& key);
};

std::ostream& operator <<(std::ostream &os, const Factor2 &f) {
os << f.wff_;
return os;
}

/** negation */
Factor2 operator -(const Key2& key) {
return Factor2("-" + key.name());
}

/** OR */
Factor2 operator ||(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " || " + factor2.wff_ + ")");
}

/** AND */
Factor2 operator &&(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " && " + factor2.wff_ + ")");
}

/** implies */
Factor2 operator >>(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " >> " + factor2.wff_ + ")");
}

struct Graph2: public std::list<Factor2> {

/** Add a factor graph*/
//  void operator +=(const Graph2& graph) {
//    for(const Factor2& f: graph)
//        push_back(f);
//  }
friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);

};

/** Add a factor */
//Graph2 operator +=(Graph2& graph, const Factor2& factor) {
//  graph.push_back(factor);
//  return graph;
//}
std::ostream& operator <<(std::ostream &os, const Graph2& graph) {
for(const Factor2& f: graph)
os << f << endl;
return os;
}

/* ************************************************************************* */
TEST(DiscreteFactorGraph, Sugar)
{
Key2 M("Mythical"), I("Immortal"), A("Mammal"), H("Horned"), G("Magical");

// Test this desired construction
Graph2 unicorns;
unicorns += M >> -A;
unicorns += (-M) >> (-I && A);
unicorns += (I || A) >> H;
unicorns += H >> G;

// should be done by adapting boost::assign:
// unicorns += (-M) >> (-I && A), (I || A) >> H , H >> G;

cout << unicorns;
}
#endif

/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// Copyright (c) 2011, Thomas Goyne <plorkyeran@aegisub.org>
//
// Permission to use, copy, modify, and distribute this software for any
// purpose with or without fee is hereby granted, provided that the above
// copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
// WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
// ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
// WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
// ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
// OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
//
// Aegisub Project http://www.aegisub.org/

/// @file visual_tool_rotatez.cpp
/// @brief 2D rotation in Z axis visual typesetting tool
/// @ingroup visual_ts

#include "config.h"

#include "visual_tool_rotatez.h"

#include "utils.h"

#include <boost/format.hpp>
#include <cmath>

static const float deg2rad = 3.1415926536f / 180.f;
static const float rad2deg = 180.f / 3.1415926536f;

VisualToolRotateZ::VisualToolRotateZ(VideoDisplay *parent, agi::Context *context)
: VisualTool<VisualDraggableFeature>(parent, context)
, org(new Feature)
{
	features.push_back(*org);
	org->type = DRAG_BIG_TRIANGLE;
}

void VisualToolRotateZ::Draw() {
	if (!active_line) return;

	DrawAllFeatures();

	float radius = (pos - org->pos).Len();
	float oRadius = radius;
	if (radius < 50)
		radius = 50;

	// Set up the projection
	gl.SetOrigin(org->pos);
	gl.SetRotation(rotation_x, rotation_y, 0);
	gl.SetScale(scale);

	// Draw the circle
	gl.SetLineColour(colour[0]);
	gl.SetFillColour(colour[1], 0.3f);
	gl.DrawRing(Vector2D(0, 0), radius + 4, radius - 4);

	// Draw markers around circle
	int markers = 6;
	float markStart = -90.f / markers;
	float markEnd = markStart + (180.f / markers);
	for (int i = 0; i < markers; ++i) {
		float angle = i * (360.f / markers);
		gl.DrawRing(Vector2D(0, 0), radius+30, radius+12, 1.0, angle+markStart, angle+markEnd);
	}

	// Draw the baseline through the origin showing current rotation
	Vector2D angle_vec(Vector2D::FromAngle(angle * deg2rad));
	gl.SetLineColour(colour[3], 1, 2);
	gl.DrawLine(angle_vec * -radius, angle_vec * radius);

	if (org->pos != pos) {
		Vector2D rotated_pos = Vector2D::FromAngle(angle * deg2rad - (pos - org->pos).Angle()) * oRadius;

		// Draw the line from origin to rotated position
		gl.DrawLine(Vector2D(), rotated_pos);

		// Draw the line under the text
		gl.DrawLine(rotated_pos - angle_vec * 20, rotated_pos + angle_vec * 20);
	}

	// Draw the fake features on the ring
	gl.SetLineColour(colour[0], 1.f, 1);
	gl.SetFillColour(colour[1], 0.3f);
	gl.DrawCircle(angle_vec * radius, 4);
	gl.DrawCircle(angle_vec * -radius, 4);

	// Clear the projection
	gl.ResetTransform();

	// Draw line to mouse if it isn't over the origin feature
	if (mouse_pos && (mouse_pos - org->pos).SquareLen() > 100) {
		gl.SetLineColour(colour[0]);
		gl.DrawLine(org->pos, mouse_pos);
	}
}

bool VisualToolRotateZ::InitializeHold() {
	orig_angle = angle + (org->pos - mouse_pos).Angle() * rad2deg;
	return true;
}

void VisualToolRotateZ::UpdateHold() {
	angle = orig_angle - (org->pos - mouse_pos).Angle() * rad2deg;

	if (ctrl_down)
		angle = floorf(angle / 30.f + .5f) * 30.f;

	angle = fmodf(angle + 360.f, 360.f);

	SetSelectedOverride("\\frz", str(boost::format("%.4g") % angle));
}

void VisualToolRotateZ::UpdateDrag(Feature *feature) {
	SetOverride(active_line, "\\org", ToScriptCoords(feature->pos).PStr());
}

void VisualToolRotateZ::DoRefresh() {
	if (!active_line) return;

	pos = FromScriptCoords(GetLinePosition(active_line));
	if (!(org->pos = GetLineOrigin(active_line)))
		org->pos = pos;
	else
		org->pos = FromScriptCoords(org->pos);

	GetLineRotation(active_line, rotation_x, rotation_y, angle);
	GetLineScale(active_line, scale);
}


/* Configuration management */

/* JSON configuration format:
{
  "size": Vector2<int>,             cloth dimensions
  "length": float,                  length of each link
  "friction": float,                friction coefficient
  "gravity": Vector2<float>,        gravity force vector
  "wind": [
    [
        Vector2<float>,             wind region width and height
        Vector2<float>,             wind region position
        Vector2<float>              wind force vector
    ]
  ],
  "structure": {
    "nodes": [],
    "pins": []
  }
}

Vectors can be specified one of two ways: [x, y] and {"x": x, "y": y}

Both wind region size and wind region position allow nulls for x and/or y with
the following default values:
    null region width -> 0.0f
    null region height -> window_height
    null region x position -> 0.0f
    null region y position -> 0.0f

 */

#include <sstream>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <boost/program_options.hpp>
#include <nlohmann/json.hpp>
#include <cerrno>

#include "engine/window_context_handler.hpp"
#include "engine/physics/physics.hpp"
#include "renderer.hpp"
#include "wind.hpp"

namespace po = boost::program_options;
using json = nlohmann::json;

const uint32_t WINDOW_WIDTH_DEFAULT = 1920;
const uint32_t WINDOW_HEIGHT_DEFAULT = 1080;
const uint32_t CLOTH_WIDTH_DEFAULT = 75;
const uint32_t CLOTH_HEIGHT_DEFAULT = 50;
const float LINKS_LENGTH_DEFAULT = 20.0f;
const float ERASE_RADIUS_DEFAULT = 10.0f;
const float MOUSE_RADIUS_DEFAULT = 100.0f;
const float MOUSE_FORCE_DEFAULT = 8000.0f;

/* Struct for maintaining command-line arguments */
struct config {
    enum Status {
        OK = 0,
        EXIT = 1,
        ERROR = 2
    };
    config()
        : debug(false)
        , window_width(WINDOW_WIDTH_DEFAULT)
        , window_height(WINDOW_HEIGHT_DEFAULT)
        , cloth_width(CLOTH_WIDTH_DEFAULT)
        , cloth_height(CLOTH_HEIGHT_DEFAULT)
        , links_length(LINKS_LENGTH_DEFAULT)
        , gravity_x(GRAVITY_X_DEFAULT)
        , gravity_y(GRAVITY_Y_DEFAULT)
        , friction_coef(FRICTION_DEFAULT)
        , disable_default_wind(false)
        , erase_radius(ERASE_RADIUS_DEFAULT)
        , mouse_drag_radius(MOUSE_RADIUS_DEFAULT)
        , mouse_drag_force(MOUSE_FORCE_DEFAULT)
        , initial_zoom(BASE_ZOOM_DEFAULT)
        , cloth_definition_path()
    {}
    /* command-line variables */
    bool debug;
    uint32_t window_width;
    uint32_t window_height;
    uint32_t cloth_width;
    uint32_t cloth_height;
    float links_length;
    float gravity_x;
    float gravity_y;
    float friction_coef;
    bool disable_default_wind;
    float erase_radius;
    float mouse_drag_radius;
    float mouse_drag_force;
    float initial_zoom;
    std::string cloth_definition_path;
    std::vector<Wind> winds;

    /* Parse command-line arguments and return a status; 0 = success */
    Status parseCommandLineArguments(int argc, char* argv[]);

    /* Parse a JSON configuration file and return a status; 0 = success */
    Status parseConfigurationFile(const std::string& fpath);

    /* Build the cloth based on the current configuration */
    void buildCloth(PhysicSolver& solver) const;

    /* Dump the current values to the given ostream */
    void print(std::ostream& os) const;

private:
    /* Extract values from the given json object */
    Status interpretJSON(const json& jobj);

    /* Interpret a value as a V2 */
    template <typename T>
    sf::Vector2<T> interpretVec2JSON(const json& item) const;

    /* Interpret value as a V2 with nulls interpreted as defaults */
    template <typename T>
    sf::Vector2<T> interpretVec2JSON(const json& item, const T& v0def, const T& v1def) const;

};

/* vim: set ts=4 sts=4 sw=4 et: */


#include <boost/simd/memory/output_iterator.hpp>
#include <boost/simd/include/functions/enumerate.hpp>

#include <algorithm>
#include <iterator>
#include <iostream>
#include <vector>

boost::simd::pack<float,4> splatted_iota()
{
  static int seed = 0;
  return seed++;
}

int main()
{
  using boost::simd::pack;
  using boost::simd::output_end;
  using boost::simd::output_begin;
  using boost::simd::output_iterator;

  std::vector<float> data(16);
  typedef std::vector<float>::iterator it_t;

  output_iterator<it_t,4> vb = output_begin<4>(data.begin());
  output_iterator<it_t,4> ve = output_end<4>(data.end());

  std::generate(vb, ve, splatted_iota);

  std::copy(data.begin(), data.end(), std::ostream_iterator<float>(std::cout, " "));
  std::cout << std::endl;
}


#define BOOST_TEST_MODULE Logical
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/logical.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(logical)
{
    const size_t N = 1024;

    vex::vector<int> x(ctx, N);
    x = vex::element_index();

    vex::any_of any_of(ctx);
    vex::all_of all_of(ctx);

    BOOST_CHECK( any_of(x)       );
    BOOST_CHECK(!any_of(0 * x)   );
    BOOST_CHECK( any_of(x > N/2) );
    BOOST_CHECK(!any_of(x < 0)   );

    BOOST_CHECK(!all_of(x)           );
    BOOST_CHECK( all_of((x + 1) > 0) );
    BOOST_CHECK(!all_of(x > N/2)     );
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>
#include <iostream>

#include <armadillo>

#define protected public  // for testing protected member
#include "ssmkit/filter/resampler/systematic.hpp"

using namespace ssmkit;

BOOST_AUTO_TEST_SUITE(filter_resampler_systematic);

BOOST_AUTO_TEST_CASE(ordered_number_generator)
{
  struct AlwaysTrue {
    bool operator()(arma::vec t) { return true; }
  };

  auto resampler = filter::resampler::makeSystematic(AlwaysTrue());
  random::setRandomSeed();
  int N = 500;
  for (int i = 1; i < 100; ++i) {
    auto u = resampler.generateOrderedNumbers(N);
    // first element should be < 1/N
    BOOST_CHECK(u[0] < 1.0 / N);
    // all u elements should be in [0 1)
    BOOST_CHECK(arma::all(u >= 0.0));
    BOOST_CHECK(arma::all(u < 1.0));
    // the difference between elements should be 1/N
    BOOST_CHECK(arma::approx_equal(
        arma::diff(u), arma::ones<arma::vec>(N-1) * 1.0 / N, "absdiff", 0.001));
  }
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @file budgeted_maximum_coverage_long_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-02-17
 */

#include "paal/greedy/set_cover/budgeted_maximum_coverage.hpp"
#include "paal/utils/functors.hpp"
#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/budgeted_maximum_coverage_check.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/algorithm_ext/iota.hpp>

#include <vector>
#include <cstdlib>
#include <algorithm>
#include <iterator>
#include <cmath>

BOOST_AUTO_TEST_CASE(budgeted_maximum_coverage_long) {
    LOGLN("budgeted_maximum_coverage_long");
    const double APPROXIMATION_RATIO = 1 - 1 / std::exp(1.0);
    const int ELEMENTS = 100;
    const int SETSIZE = 5;

    std::vector<int> weight(ELEMENTS);
    std::vector<int> solution(ELEMENTS);
    int optimal = 0;
    srand(42);
    for (int i = 0; i < ELEMENTS; ++i) {
        weight[i] = rand() % 1000;
        optimal += weight[i];
    }
    boost::iota(solution, 0);
    std::random_shuffle(solution.begin(), solution.end());
    std::vector<std::vector<int>> sets_element;
    std::vector<int> cost_of_set;
    for (int i = 0; i < ELEMENTS; i += SETSIZE) {
        for (int j = 0; j < 4; ++j) {
            std::vector<int> set;
            int set_cost = 0;
            for (int k = 0; k < SETSIZE; ++k) {
                if (j == 3) {
                    set.push_back(solution[i + k]);
                    set_cost += weight[solution[i + k]];
                } else {
                    int el;
                    bool copy;
                    do {
                        copy = false;
                        el = rand() % ELEMENTS;
                        for (auto i : set)
                            if (i == el) copy = true;
                    } while (copy);
                    set.push_back(el);
                    set_cost += weight[el];
                }
            }
            sets_element.push_back(set);
            cost_of_set.push_back(set_cost);
        }
    }
    auto sets = boost::irange(0, int(sets_element.size()));
    std::vector<int> result;
    auto element_index = paal::utils::identity_functor{};
    auto set_to_cost= paal::utils::make_array_to_functor(cost_of_set);
    auto set_to_weight = paal::utils::make_array_to_functor(weight);
    auto budget=optimal;
    auto weight_solution = paal::greedy::budgeted_maximum_coverage(
        sets, set_to_cost,
        paal::utils::make_array_to_functor(sets_element),
        std::back_inserter(result), element_index, budget, set_to_weight
        );
    budgeted_maximum_coverage_result_check(
        result, sets_element, set_to_weight, set_to_cost, ELEMENTS, optimal,
        weight_solution, APPROXIMATION_RATIO, budget);
}


#pragma once

#include <bitset>
#include <iostream>
#include <numeric>
#include <stack>
#include <vector>

#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/adaptor/transformed.hpp>

using boost::adaptors::filtered;
using boost::adaptors::transformed;

using vertex_type = int;

class Graph {
    int vertex_count;
    int added_count;
    int removed_count;
    std::stack<std::vector<int>> unified_vertices;

    std::vector<int> vertices;
    std::vector<std::vector<int>> G;
    std::vector<bool> removed;

public:
    Graph(const std::vector<std::vector<int>> &g);
    Graph(const Graph &x) = default;
    Graph(Graph &&x) = default;
    Graph() = delete;

    void add_edge(int x, int y);

    bool is_removed(int x) const;

    bool has_neighbor(int x, int y) const;
    int count_neighbors(int x) const;
    const std::vector<int> &raw_neighbors(int x) const { return G[x]; }
    auto get_neighbors(int x) const {
        return G[x] | filtered([&](const int &edge) { return !(removed[edge]); });
    }
    auto get_vertices() const { return vertices; }
    auto get_valid_vertices() const {
        return vertices | filtered([&](const int &vertex) { return valid_vertex(vertex); });
    }
    void remove_vertex(int x);
    void unremove_vertex(int x);
    bool valid_vertex(int x) const;
    int size() const;
    int num_of_vertices() const;
    bool empty_graph() const;
    void print_graph(std::ostream &out) const;
    void ununify_last();
    int unify_vertices(const int &x, const int &y, const int &v);
    void remove_edge(const int x, const int y);

    std::vector<std::vector<int>> get_g() { return G; }
};

std::ostream &operator<<(std::ostream &out, const Graph &g);


#pragma once

#include <vector>
#include <map>
#include <Eigen/Core>
#include "kd_tree.hpp"

/* Custom exceptions for the roadmap */

struct MissingStateRoadmapException : public std::exception
{
    const char* what() const throw()
    {
        return "Given state is missing from the Roadmap";
    }
};

/* Roadmap Node class */

class Roadmap;

class RoadmapNode {
    Eigen::VectorXd _state;
    Eigen::MatrixXd _neighbors;
    Eigen::VectorXd _costs;

    // Setters are private
    void set_state(const Eigen::VectorXd& value) {_state = value;}
    void set_neighbors(const Eigen::MatrixXd& value) {_neighbors = value;}
    void set_costs(const Eigen::VectorXd& value) {_costs = value;}

    // Private functions that return references
    const Eigen::VectorXd& ref_state() const {return _state;}
    const Eigen::MatrixXd& ref_neighbors() const {return _neighbors;}
    const Eigen::VectorXd& ref_costs() const {return _costs;}

    // Give Roadmap and search algorithms access to the node's private members
    friend class Roadmap;
    friend Eigen::MatrixXd dijkstra(const Roadmap& roadmap, const Eigen::VectorXd& start_state, const Eigen::VectorXd& goal_state);

public:
    RoadmapNode() {};
    RoadmapNode(const Eigen::VectorXd& state, const Eigen::MatrixXd neighbors, const Eigen::VectorXd& costs) : _state(state), _neighbors(neighbors), _costs(costs) {}

    // Getters
    Eigen::VectorXd get_state() const {return _state;}
    Eigen::MatrixXd get_neighbors() const {return _neighbors;}
    Eigen::VectorXd get_costs() const {return _costs;}
};

/* Roadmap class */

template <typename T>
using MapVectorXd = std::map<Eigen::VectorXd, T, std::function<bool(const Eigen::VectorXd&, const Eigen::VectorXd&)>>;

#define MAPVECTORXD(T) MapVectorXd<T> {                                     \
            [](const Eigen::VectorXd& a, const Eigen::VectorXd& b)->bool    \
            {                                                               \
                return std::lexicographical_compare(                        \
                    a.data(), a.data() + a.size(),                          \
                    b.data(), b.data() + b.size());                         \
            }                                                               \
        }                                                                   \

class Roadmap {
    MapVectorXd<RoadmapNode> roadmap;
    kdTree kdtree;

    int _state_size = -1;
    int _n_states = 0;

    // Setters are private
    void set_state_size(int value) {_state_size = value;}
    void increment_n_states() {_n_states++;}

    // Private functions that return references for speed
    const RoadmapNode& ref_node_at(const Eigen::VectorXd& state) const;

    // Give search functions special access to get references for speed
    friend Eigen::MatrixXd dijkstra(const Roadmap& roadmap, const Eigen::VectorXd& start_state, const Eigen::VectorXd& goal_state);

public:
    Roadmap() {
        roadmap = MAPVECTORXD(RoadmapNode);
        set_state_size(-1);
    }

    Roadmap(int state_size) {
        roadmap = MAPVECTORXD(RoadmapNode);
        set_state_size(state_size);
    };

    void add_node(const Eigen::VectorXd& state, const Eigen::MatrixXd& neighborStates, const Eigen::VectorXd& neighborCosts);

    RoadmapNode node_at(const Eigen::VectorXd& state) const;
    RoadmapNode node_nearest(const Eigen::VectorXd& state) const;
    std::vector<RoadmapNode> k_nodes_nearest(const Eigen::VectorXd& state, int k) const;

    Eigen::VectorXd state_nearest(const Eigen::VectorXd& state) const;
    Eigen::MatrixXd k_states_nearest(const Eigen::VectorXd& state, int k) const;

    // Getters
    int get_state_size() const {return _state_size;}
    int get_n_states() const {return _n_states;}
    std::vector<RoadmapNode> get_nodes() const;
    Eigen::MatrixXd get_states() const;
};

Eigen::MatrixXd dijkstra(const Roadmap& roadmap, const Eigen::VectorXd& start_state, const Eigen::VectorXd& goal_state);


#ifndef __IMGPROC_HPP__
#define __IMGPROC_HPP__

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

#include <boost/gil.hpp>
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/image_view_factory.hpp>

#include "imgproc-version.hpp"

namespace imgproc
{

template<class GrayView>
std::vector<double> histogram(const GrayView& view)
{
    const auto type_width = static_cast<size_t>(std::exp2(8 * sizeof(typename GrayView::value_type)));
    std::vector<size_t> cumsum(type_width + 1);
    for (const auto pixel : view)
    {
        if (cumsum.back() == std::numeric_limits<size_t>::max())
            std::for_each(cumsum.begin(), cumsum.end(), [](auto& x) { x /= 2; });

        std::for_each(cumsum.begin() + pixel, cumsum.end(), [](auto& x) { ++x; });
    }

    const double m = cumsum.back();
    std::vector<double> hist(cumsum.size() - 1);
    hist.front() = cumsum.front() / m;
    std::transform(cumsum.begin() + 1, cumsum.end() - 1, cumsum.begin(), hist.begin() + 1, [m](double x, double p) { return (x - p) / m; });

    return hist;
}

template<class Image = boost::gil::gray8_image_t>
Image bar(const std::vector<double>& bars, long width = 512, long height = 512)
{
    Image out{{width, height}};
    auto view = boost::gil::view(out);

    auto step = view.width() / bars.size();

    for (size_t i = 0, s = bars.size(); i < s; ++i)
    {
        const auto h = height * bars[i];
        auto v = boost::gil::subimage_view(view, i * step, height - h, step, h);
        std::fill(v.begin(), v.end(), 255);
    }

    return out;
}

template<class View, class Image = boost::gil::gray8_image_t>
Image transform(const View& view)
{
    namespace bg = boost::gil;

    Image img(view.dimensions());
    bg::matrix3x2<double> mat =
        bg::matrix3x2<double>::get_translate(-bg::point2<double>(200,250)) *
        bg::matrix3x2<double>::get_rotate(-15*3.14/180.0);

    bg::resample_pixels(view, bg::view(img), mat, bg::nearest_neighbor_sampler{});
    return img;
}

} // namespace imgproc

#endif // __IMGPROC_HPP__


#include "bio_test_data.h"

#include <bio/pssm_likelihood.h>
#include <bio/biobase_db.h>
#include <bio/biobase_data_traits.h>
#include <bio/pssm_likelihood_cache.h>
#include <bio/environment.h>
USING_BIO_NS

#include <boost/test/unit_test.hpp>
#include <boost/test/parameterized_test.hpp>
#include <boost/progress.hpp>
using namespace boost;
using namespace boost::unit_test;

#include <iostream>
#include <numeric>
using namespace std;

//#define VERBOSE_CHECKING

void
check_pssm_likelihood(BiobaseParams const & params)
{
	ensure_biobase_params_built();

	cout << "******* check_pssm_likelihood(): Testing with " << params.name.c_str() << " parameters" << endl;

#ifdef VERBOSE_CHECKING
	progress_timer timer;
	const bool verbose = true;
#else
	const bool verbose = false;
#endif

	BiobaseLikelihoods likelihoods(10);

	//build and calculate the likelihoods of the biobase pssm matching matrix
	pssm_likelihood(
		*params.pssm,
		50000,
		likelihoods);

	//build and calculate the likelihoods of the biobase iupac matching matrix
	pssm_likelihood(
		make_pssm_from_iupac(params.iupac->begin(), params.iupac->end()),
		50000,
		likelihoods);

#ifdef VERBOSE_CHECKING
	std::copy(likelihoods.begin(), likelihoods.end(), ostream_iterator<BIO_NS::float_t>(cout, ","));
	cout << endl;
#endif

	//check the likelihoods add to 1
	BOOST_CHECK_EQUAL(1.0, accumulate(likelihoods.begin(), likelihoods.end(), 0.0));

#ifdef VERBOSE_CHECKING
	const BiobaseLikelihoods * l = PssmLikelihoodCache::singleton().get_likelihoods(params.name);
	std::copy(l->begin(), l->end(), ostream_iterator<BIO_NS::float_t>(cout, ","));
	cout << endl;
#endif

}

struct check_likelihoods
{
	void operator()( BIO_NS::float_t likelihood ) const
	{
		BOOST_CHECK( likelihood >= BIO_NS::float_t( 0.0 ) );
		BOOST_CHECK( likelihood <= BIO_NS::float_t( 1.0 ) );

		if ( likelihood > BIO_NS::float_t( 1.0 ) )
		{
			cout << "Problem\n";
		}
	}
};

void check_pssm_likelihood_2()
{
	cout << "******* check_pssm_likelihood_2()" << endl;

	{
		Matrix * pssm = BiobaseDb::singleton().get_entry< MATRIX_DATA >( 328 );
		BiobaseLikelihoods pssm_likelihoods(BioEnvironment::singleton().num_normalisation_quanta);
		pssm_likelihood(
			make_pssm( pssm ),
			BioEnvironment::singleton().max_pssm_likelihood_map_size,
			pssm_likelihoods);

		std::for_each(
			pssm_likelihoods.begin(),
			pssm_likelihoods.end(),
			check_likelihoods() );
	}

	{
		Site * pssm = BiobaseDb::singleton().get_entry< SITE_DATA >( 4538 );
		BiobaseLikelihoods pssm_likelihoods(BioEnvironment::singleton().num_normalisation_quanta);
		pssm_likelihood(
			make_pssm( pssm ),
			BioEnvironment::singleton().max_pssm_likelihood_map_size,
			pssm_likelihoods);

		std::for_each(
			pssm_likelihoods.begin(),
			pssm_likelihoods.end(),
			check_likelihoods() );
	}

}

void register_pssm_likelihoods_tests(test_suite * test)
{
	ensure_biobase_params_built();

	test->add(BOOST_TEST_CASE(&check_pssm_likelihood_2), 0);
	test->add(BOOST_PARAM_TEST_CASE(&check_pssm_likelihood, biobase_params.begin(), biobase_params.end()), 0);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file SubgraphPreconditioner.cpp
 * @date Dec 31, 2009
 * @author Frank Dellaert, Yong-Dian Jian
 */

#include <gtsam/linear/SubgraphPreconditioner.h>

#include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Vector.h>

#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/reversed.hpp>

#include <stdexcept>

using std::cout;
using std::endl;
using std::vector;
using std::ostream;

namespace gtsam {

/* ************************************************************************* */
static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
                           const KeyVector &keys) {
  /* a cache of starting index and dim */
  vector<std::pair<size_t, size_t> > cache;
  cache.reserve(3);

  /* figure out dimension by traversing the keys */
  size_t dim = 0;
  for (const Key &key : keys) {
    const KeyInfoEntry &entry = keyInfo.find(key)->second;
    cache.emplace_back(entry.start, entry.dim);
    dim += entry.dim;
  }

  /* use the cache to fill the result */
  Vector result(dim);
  size_t index = 0;
  for (const auto &p : cache) {
    result.segment(index, p.second) = src.segment(p.first, p.second);
    index += p.second;
  }

  return result;
}

/*****************************************************************************/
static void setSubvector(const Vector &src, const KeyInfo &keyInfo,
                         const KeyVector &keys, Vector &dst) {
  size_t index = 0;
  for (const Key &key : keys) {
    const KeyInfoEntry &entry = keyInfo.find(key)->second;
    const size_t keyDim = entry.dim;
    dst.segment(entry.start, keyDim) = src.segment(index, keyDim);
    index += keyDim;
  }
}

/* ************************************************************************* */
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with
// Cholesky)
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(
    const GaussianFactorGraph &gfg) {
  auto result = boost::make_shared<GaussianFactorGraph>();
  for (const auto &factor : gfg)
    if (factor) {
      auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
      if (!jf) {
        jf = boost::make_shared<JacobianFactor>(*factor);
      }
      result->push_back(jf);
    }
  return result;
}

/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
         parameters_(p) {}

/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
    const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) :
        Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar),
        b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) {
}

/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
  return *xbar_ + Rc1_->backSubstitute(y);
}

/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
  Errors e(y);
  VectorValues x = this->x(y);
  Errors e2 = Ab2()->gaussianErrors(x);
  return 0.5 * (dot(e, e) + dot(e2,e2));
}

/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const {
  VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
  Errors e = (*Ab2() * x - *b2bar());        /* (A2*inv(R1)*y-b2bar) */
  VectorValues v = VectorValues::Zero(x);
  Ab2()->transposeMultiplyAdd(1.0, e, v);    /* A2'*(A2*inv(R1)*y-b2bar) */
  return y + Rc1()->backSubstituteTranspose(v);
}

/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
  Errors e(y);
  VectorValues x = Rc1()->backSubstitute(y);   /* x=inv(R1)*y */
  Errors e2 = *Ab2() * x;                      /* A2*x */
  e.splice(e.end(), e2);
  return e;
}

/* ************************************************************************* */
// In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {

  Errors::iterator ei = e.begin();
  for(const auto& key_value: y) {
    *ei = key_value.second;
    ++ei;
  }

  // Add A2 contribution
  VectorValues x = Rc1()->backSubstitute(y);      // x=inv(R1)*y
  Ab2()->multiplyInPlace(x, ei);                  // use iterator version
}

/* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {

  Errors::const_iterator it = e.begin();
  VectorValues y = zero();
  for(auto& key_value: y) {
    key_value.second = *it;
    ++it;
  }
  transposeMultiplyAdd2(1.0, it, e.end(), y);
  return y;
}

/* ************************************************************************* */
// y += alpha*A'*e
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {

  Errors::const_iterator it = e.begin();
  for(auto& key_value: y) {
    const Vector& ei = *it;
    axpy(alpha, ei, key_value.second);
    ++it;
  }
  transposeMultiplyAdd2(alpha, it, e.end(), y);
}

/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
    Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {

  // create e2 with what's left of e
  // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
  Errors e2;
  while (it != end) e2.push_back(*(it++));

  VectorValues x = VectorValues::Zero(y); // x = 0
  Ab2_->transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
  axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
}

/* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const {
  cout << s << endl;
  Ab2_->print();
}

/*****************************************************************************/
void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
  assert(x.size() == y.size());

  /* back substitute */
  for (const auto &cg : boost::adaptors::reverse(*Rc1_)) {
    /* collect a subvector of x that consists of the parents of cg (S) */
    const KeyVector parentKeys(cg->beginParents(), cg->endParents());
    const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
    const Vector xParent = getSubvector(x, keyInfo_, parentKeys);
    const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys);

    /* compute the solution for the current pivot */
    const Vector solFrontal = cg->R().triangularView<Eigen::Upper>().solve(
        rhsFrontal - cg->S() * xParent);

    /* assign subvector of sol to the frontal variables */
    setSubvector(solFrontal, keyInfo_, frontalKeys, x);
  }
}

/*****************************************************************************/
void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
  /* copy first */
  assert(x.size() == y.size());
  std::copy(y.data(), y.data() + y.rows(), x.data());

  /* in place back substitute */
  for (const auto &cg : *Rc1_) {
    const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
    const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
    const Vector solFrontal =
        cg->R().transpose().triangularView<Eigen::Lower>().solve(
            rhsFrontal);

    // Check for indeterminant solution
    if (solFrontal.hasNaN())
      throw IndeterminantLinearSystemException(cg->keys().front());

    /* assign subvector of sol to the frontal variables */
    setSubvector(solFrontal, keyInfo_, frontalKeys, x);

    /* substract from parent variables */
    for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
      const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
      Eigen::Map<Vector> rhsParent(x.data() + entry.start, entry.dim, 1);
      rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
    }
  }
}

/*****************************************************************************/
void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
  /* identify the subgraph structure */
  const SubgraphBuilder builder(parameters_.builderParams);
  auto subgraph = builder(gfg);

  keyInfo_ = keyInfo;

  /* build factor subgraph */
  GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true);

  /* factorize and cache BayesNet */
  Rc1_ = gfg_subgraph->eliminateSequential();
}

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

} // nsamespace gtsam


#include "mnist.h"

#include <armadillo>
#include <cmath>
#include <cstring>
#include <iostream>
#include <vector>

void read_mnist(std::string filename, arma::Mat<real>& mat) {
  std::ifstream file(filename, std::ios::binary);

  if (file.is_open()) {
    int magic_number = 0;
    int number_of_images = 0;
    int n_rows = 0;
    int n_cols = 0;
    file.read((char*)&magic_number, sizeof(magic_number));
    magic_number = reverse_int(magic_number);
    file.read((char*)&number_of_images, sizeof(number_of_images));
    number_of_images = reverse_int(number_of_images);
    file.read((char*)&n_rows, sizeof(n_rows));
    n_rows = reverse_int(n_rows);
    file.read((char*)&n_cols, sizeof(n_cols));
    n_cols = reverse_int(n_cols);

    for (int i = 0; i < number_of_images; ++i) {
      for (int r = 0; r < n_rows; ++r) {
        for (int c = 0; c < n_cols; ++c) {
          unsigned char temp = 0;
          file.read((char*)&temp, sizeof(temp));
          mat(r * n_cols + c, i) = (real)temp;
        }
      }
    }
  }
}

void read_mnist_label(std::string filename, arma::Row<real>& vec) {
  std::ifstream file(filename, std::ios::binary);

  if (file.is_open()) {
    int magic_number = 0;
    int number_of_images = 0;
    int n_rows = 0;
    int n_cols = 0;
    file.read((char*)&magic_number, sizeof(magic_number));
    magic_number = reverse_int(magic_number);
    file.read((char*)&number_of_images, sizeof(number_of_images));
    number_of_images = reverse_int(number_of_images);

    for (int i = 0; i < number_of_images; ++i) {
      unsigned char temp = 0;
      file.read((char*)&temp, sizeof(temp));
      vec(i) = (real)temp;
    }
  }
}

int reverse_int(int i) {
  unsigned char ch1, ch2, ch3, ch4;
  ch1 = i & 255;
  ch2 = (i >> 8) & 255;
  ch3 = (i >> 16) & 255;
  ch4 = (i >> 24) & 255;
  return ((int)ch1 << 24) + ((int)ch2 << 16) + ((int)ch3 << 8) + ch4;
}