#ifndef BOOST_QVM_GEN_VEC_OPERATIONS3_HPP_INCLUDED
#define BOOST_QVM_GEN_VEC_OPERATIONS3_HPP_INCLUDED

/// Copyright (c) 2008-2021 Emil Dotchevski and Reverge Studios, Inc.

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

/// This file was generated by a program. Do not edit manually.

#include <boost/qvm/deduce_scalar.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/error.hpp>
#include <boost/qvm/gen/vec_assign3.hpp>
#include <boost/qvm/math.hpp>
#include <boost/qvm/static_assert.hpp>
#include <boost/qvm/throw_exception.hpp>

namespace boost { namespace qvm {

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
    deduce_vec2<A,B,3> >::type
operator+( A const & a, B const & b )
    {
    typedef typename deduce_vec2<A,B,3>::type R;
    BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim==3);
    R r;
    vec_traits<R>::template write_element<0>(r)=vec_traits<A>::template read_element<0>(a)+vec_traits<B>::template read_element<0>(b);
    vec_traits<R>::template write_element<1>(r)=vec_traits<A>::template read_element<1>(a)+vec_traits<B>::template read_element<1>(b);
    vec_traits<R>::template write_element<2>(r)=vec_traits<A>::template read_element<2>(a)+vec_traits<B>::template read_element<2>(b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+;
    }

namespace
qvm_detail
    {
    template <int D>
    struct plus_vv_defined;

    template <>
    struct
    plus_vv_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
    deduce_vec2<A,B,3> >::type
operator-( A const & a, B const & b )
    {
    typedef typename deduce_vec2<A,B,3>::type R;
    BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim==3);
    R r;
    vec_traits<R>::template write_element<0>(r)=vec_traits<A>::template read_element<0>(a)-vec_traits<B>::template read_element<0>(b);
    vec_traits<R>::template write_element<1>(r)=vec_traits<A>::template read_element<1>(a)-vec_traits<B>::template read_element<1>(b);
    vec_traits<R>::template write_element<2>(r)=vec_traits<A>::template read_element<2>(a)-vec_traits<B>::template read_element<2>(b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int D>
    struct minus_vv_defined;

    template <>
    struct
    minus_vv_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
    A &>::type
operator+=( A & a, B const & b )
    {
    vec_traits<A>::template write_element<0>(a)+=vec_traits<B>::template read_element<0>(b);
    vec_traits<A>::template write_element<1>(a)+=vec_traits<B>::template read_element<1>(b);
    vec_traits<A>::template write_element<2>(a)+=vec_traits<B>::template read_element<2>(b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+=;
    }

namespace
qvm_detail
    {
    template <int D>
    struct plus_eq_vv_defined;

    template <>
    struct
    plus_eq_vv_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
    A &>::type
operator-=( A & a, B const & b )
    {
    vec_traits<A>::template write_element<0>(a)-=vec_traits<B>::template read_element<0>(b);
    vec_traits<A>::template write_element<1>(a)-=vec_traits<B>::template read_element<1>(b);
    vec_traits<A>::template write_element<2>(a)-=vec_traits<B>::template read_element<2>(b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-=;
    }

namespace
qvm_detail
    {
    template <int D>
    struct minus_eq_vv_defined;

    template <>
    struct
    minus_eq_vv_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    vec_traits<A>::dim==3 && is_scalar<B>::value,
    deduce_vec2<A,B,vec_traits<A>::dim> >::type
operator*( A const & a, B b )
    {
    typedef typename deduce_vec2<A,B,vec_traits<A>::dim>::type R;
    R r;
    vec_traits<R>::template write_element<0>(r)=vec_traits<A>::template read_element<0>(a)*b;
    vec_traits<R>::template write_element<1>(r)=vec_traits<A>::template read_element<1>(a)*b;
    vec_traits<R>::template write_element<2>(r)=vec_traits<A>::template read_element<2>(a)*b;
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int D>
    struct mul_vs_defined;

    template <>
    struct
    mul_vs_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    is_scalar<A>::value && vec_traits<B>::dim==3,
    deduce_vec2<A,B,vec_traits<B>::dim> >::type
operator*( A a, B const & b )
    {
    typedef typename deduce_vec2<A,B,vec_traits<B>::dim>::type R;
    R r;
    vec_traits<R>::template write_element<0>(r)=a*vec_traits<B>::template read_element<0>(b);
    vec_traits<R>::template write_element<1>(r)=a*vec_traits<B>::template read_element<1>(b);
    vec_traits<R>::template write_element<2>(r)=a*vec_traits<B>::template read_element<2>(b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int D>
    struct mul_sv_defined;

    template <>
    struct
    mul_sv_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class  B>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    vec_traits<A>::dim==3 && is_scalar<B>::value,
    A &>::type
operator*=( A & a, B b )
    {
    vec_traits<A>::template write_element<0>(a)*=b;
    vec_traits<A>::template write_element<1>(a)*=b;
    vec_traits<A>::template write_element<2>(a)*=b;
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*=;
    }

namespace
qvm_detail
    {
    template <int D>
    struct mul_eq_vs_defined;

    template <>
    struct
    mul_eq_vs_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    vec_traits<A>::dim==3 && is_scalar<B>::value,
    deduce_vec2<A,B,vec_traits<A>::dim> >::type
operator/( A const & a, B b )
    {
    typedef typename deduce_vec2<A,B,vec_traits<A>::dim>::type R;
    R r;
    vec_traits<R>::template write_element<0>(r)=vec_traits<A>::template read_element<0>(a)/b;
    vec_traits<R>::template write_element<1>(r)=vec_traits<A>::template read_element<1>(a)/b;
    vec_traits<R>::template write_element<2>(r)=vec_traits<A>::template read_element<2>(a)/b;
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/;
    }

namespace
qvm_detail
    {
    template <int D>
    struct div_vs_defined;

    template <>
    struct
    div_vs_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class  B>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    vec_traits<A>::dim==3 && is_scalar<B>::value,
    A &>::type
operator/=( A & a, B b )
    {
    vec_traits<A>::template write_element<0>(a)/=b;
    vec_traits<A>::template write_element<1>(a)/=b;
    vec_traits<A>::template write_element<2>(a)/=b;
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/=;
    }

namespace
qvm_detail
    {
    template <int D>
    struct div_eq_vs_defined;

    template <>
    struct
    div_eq_vs_defined<3>
        {
        static bool const value=true;
        };
    }

template <class R,class A>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    is_vec<A>::value &&
    vec_traits<R>::dim==3 && vec_traits<A>::dim==3,
    R>::type
convert_to( A const & a )
    {
    R r;
    vec_traits<R>::template write_element<0>(r)=vec_traits<A>::template read_element<0>(a);
    vec_traits<R>::template write_element<1>(r)=vec_traits<A>::template read_element<1>(a);
    vec_traits<R>::template write_element<2>(r)=vec_traits<A>::template read_element<2>(a);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::convert_to;
    }

namespace
qvm_detail
    {
    template <int D>
    struct convert_to_v_defined;

    template <>
    struct
    convert_to_v_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
bool>::type
operator==( A const & a, B const & b )
    {
    return
        vec_traits<A>::template read_element<0>(a)==vec_traits<B>::template read_element<0>(b) &&
        vec_traits<A>::template read_element<1>(a)==vec_traits<B>::template read_element<1>(b) &&
        vec_traits<A>::template read_element<2>(a)==vec_traits<B>::template read_element<2>(b);
    }

namespace
sfinae
    {
    using ::boost::qvm::operator==;
    }

namespace
qvm_detail
    {
    template <int D>
    struct eq_vv_defined;

    template <>
    struct
    eq_vv_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
bool>::type
operator!=( A const & a, B const & b )
    {
    return
        !(vec_traits<A>::template read_element<0>(a)==vec_traits<B>::template read_element<0>(b)) ||
        !(vec_traits<A>::template read_element<1>(a)==vec_traits<B>::template read_element<1>(b)) ||
        !(vec_traits<A>::template read_element<2>(a)==vec_traits<B>::template read_element<2>(b));
    }

namespace
sfinae
    {
    using ::boost::qvm::operator!=;
    }

namespace
qvm_detail
    {
    template <int D>
    struct neq_vv_defined;

    template <>
    struct
    neq_vv_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    vec_traits<A>::dim==3,
    deduce_vec<A> >::type
operator-( A const & a )
    {
    typedef typename deduce_vec<A>::type R;
    R r;
    vec_traits<R>::template write_element<0>(r)=-vec_traits<A>::template read_element<0>(a);
    vec_traits<R>::template write_element<1>(r)=-vec_traits<A>::template read_element<1>(a);
    vec_traits<R>::template write_element<2>(r)=-vec_traits<A>::template read_element<2>(a);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int D>
    struct minus_v_defined;

    template <>
    struct
    minus_v_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    is_vec<A>::value && vec_traits<A>::dim==3,
    typename vec_traits<A>::scalar_type>::type
mag( A const & a )
    {
    typedef typename vec_traits<A>::scalar_type T;
    T const a0=vec_traits<A>::template read_element<0>(a);
    T const a1=vec_traits<A>::template read_element<1>(a);
    T const a2=vec_traits<A>::template read_element<2>(a);
    T const m2=a0*a0+a1*a1+a2*a2;
    T const mag=sqrt(m2);
    return mag;
    }

namespace
sfinae
    {
    using ::boost::qvm::mag;
    }

namespace
qvm_detail
    {
    template <int D>
    struct mag_v_defined;

    template <>
    struct
    mag_v_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    is_vec<A>::value && vec_traits<A>::dim==3,
    typename vec_traits<A>::scalar_type>::type
mag_sqr( A const & a )
    {
    typedef typename vec_traits<A>::scalar_type T;
    T const a0=vec_traits<A>::template read_element<0>(a);
    T const a1=vec_traits<A>::template read_element<1>(a);
    T const a2=vec_traits<A>::template read_element<2>(a);
    T const m2=a0*a0+a1*a1+a2*a2;
    return m2;
    }

namespace
sfinae
    {
    using ::boost::qvm::mag_sqr;
    }

namespace
qvm_detail
    {
    template <int D>
    struct mag_sqr_v_defined;

    template <>
    struct
    mag_sqr_v_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    vec_traits<A>::dim==3,
    deduce_vec<A> >::type
normalized( A const & a )
    {
    typedef typename vec_traits<A>::scalar_type T;
    T const a0=vec_traits<A>::template read_element<0>(a);
    T const a1=vec_traits<A>::template read_element<1>(a);
    T const a2=vec_traits<A>::template read_element<2>(a);
    T const m2=a0*a0+a1*a1+a2*a2;
    if( m2==scalar_traits<typename vec_traits<A>::scalar_type>::value(0) )
        BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
    T const rm=scalar_traits<T>::value(1)/sqrt(m2);
    typedef typename deduce_vec<A>::type R;
    R r;
    vec_traits<R>::template write_element<0>(r)=a0*rm;
    vec_traits<R>::template write_element<1>(r)=a1*rm;
    vec_traits<R>::template write_element<2>(r)=a2*rm;
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::normalized;
    }

template <class A>
BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    vec_traits<A>::dim==3,
    void>::type
normalize( A & a )
    {
    typedef typename vec_traits<A>::scalar_type T;
    T const a0=vec_traits<A>::template read_element<0>(a);
    T const a1=vec_traits<A>::template read_element<1>(a);
    T const a2=vec_traits<A>::template read_element<2>(a);
    T const m2=a0*a0+a1*a1+a2*a2;
    if( m2==scalar_traits<typename vec_traits<A>::scalar_type>::value(0) )
        BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
    T const rm=scalar_traits<T>::value(1)/sqrt(m2);
    vec_traits<A>::template write_element<0>(a)*=rm;
    vec_traits<A>::template write_element<1>(a)*=rm;
    vec_traits<A>::template write_element<2>(a)*=rm;
    }

namespace
sfinae
    {
    using ::boost::qvm::normalize;
    }

namespace
qvm_detail
    {
    template <int D>
    struct normalize_v_defined;

    template <>
    struct
    normalize_v_defined<3>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
    deduce_scalar<typename vec_traits<A>::scalar_type,typename vec_traits<B>::scalar_type> >::type
dot( A const & a, B const & b )
    {
    typedef typename vec_traits<A>::scalar_type Ta;
    typedef typename vec_traits<B>::scalar_type Tb;
    typedef typename deduce_scalar<Ta,Tb>::type Tr;
    Ta const a0=vec_traits<A>::template read_element<0>(a);
    Ta const a1=vec_traits<A>::template read_element<1>(a);
    Ta const a2=vec_traits<A>::template read_element<2>(a);
    Tb const b0=vec_traits<B>::template read_element<0>(b);
    Tb const b1=vec_traits<B>::template read_element<1>(b);
    Tb const b2=vec_traits<B>::template read_element<2>(b);
    Tr const dot=a0*b0+a1*b1+a2*b2;
    return dot;
    }

namespace
sfinae
    {
    using ::boost::qvm::dot;
    }

namespace
qvm_detail
    {
    template <int D>
    struct dot_vv_defined;

    template <>
    struct
    dot_vv_defined<3>
        {
        static bool const value=true;
        };
    }

} }

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_CRTP_BASE_VECTOR_INCLUDE
#define MTL_CRTP_BASE_VECTOR_INCLUDE

#include <boost/mpl/if.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/type_traits/is_base_of.hpp>

#include <boost/utility/enable_if.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/vector/all_vec_expr.hpp>
#include <boost/numeric/mtl/vector/assigner.hpp>
#include <boost/numeric/mtl/vector/decrementer.hpp>
#include <boost/numeric/mtl/vector/incrementer.hpp>
#include <boost/numeric/mtl/operation/mat_cvec_times_expr.hpp>
#include <boost/numeric/mtl/operation/mult.hpp>
#include <boost/numeric/mtl/operation/mat_vec_mult.hpp>
#include <boost/numeric/mtl/operation/mult_assign_mode.hpp>
#include <boost/numeric/mtl/operation/right_scale_inplace.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/flatcat.hpp>
#include <boost/numeric/mtl/utility/is_distributed.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>

#include <boost/numeric/itl/itl_fwd.hpp>

namespace mtl { namespace vector {

namespace detail {

    template <typename Vector, typename Source, typename SCat, typename VCat>
    struct crtp_assign {};

    /// Assign scalar to a vector by setting all values to the scalar
    template <typename Vector, typename Source, typename VCat>
    struct crtp_assign<Vector, Source, VCat, ashape::scal>
    {
	typedef vec_scal_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type(vector, src);
	}
    };

    /// Assign vector to a vector
    template <typename Vector, typename Source, typename Cat>
    struct crtp_assign<Vector, Source, Cat, Cat>
    {
	typedef vec_vec_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type(vector, src);
	}
    };

    template <typename Vector, typename Source>
    struct assign_assigner
    {
	typedef const Vector& type;
	type operator()(Vector& vector, const Source& src)
	{
	    src.assign_to(vector);
	    return vector;
	}
    };

} // namespace detail

template <typename Vector, typename Source>
struct crtp_assign
  : boost::mpl::if_
     <boost::is_base_of<assigner_base, Source>,
      detail::assign_assigner <Vector, Source>,
      detail::crtp_assign<Vector, Source, typename ashape::ashape<Vector>::type, typename ashape::ashape<Source>::type>
     >::type
{};

/// Assign matrix vector product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_assign<Vector, mat_cvec_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const mat_cvec_times_expr<E1, E2>& src)
    {
	vector.checked_change_resource(src);
	set_to_zero(vector);
	mat_cvec_mult(src.first, src.second, vector, assign::assign_sum(), traits::mat_cvec_flatcat<E1>());
	return vector;
    }
};

/// Assign vector matrix product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_assign<Vector, rvec_mat_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const rvec_mat_times_expr<E1, E2>& src)
    {
	vector.checked_change_resource(src);
	rvec_mat_mult(src.first, src.second, vector, assign::assign_sum(), traits::mat_cvec_flatcat<E2>());
	return vector;
    }
};

/// Assign c-style 1D-array, because it's easier to initialize.
template <typename Vector, typename Value, unsigned Rows>
struct crtp_assign<Vector, Value[Rows]>
{
    typedef Vector& type;
    type operator()(Vector& vector, const Value src[Rows])
    {
	typedef typename Collection<Vector>::size_type size_type;

	vector.checked_change_dim(Rows);

	for (size_type r= 0; r < Rows; ++r)
	    vector[r]= src[r];
	return vector;
    }
};

namespace detail {

    template <typename Vector, typename Source, typename SCat, typename VCat>
    struct crtp_plus_assign {};

    /// Assign-add vector to a vector
    template <typename Vector, typename Source, typename Cat>
    struct crtp_plus_assign<Vector, Source, Cat, Cat>
    {
	typedef vec_vec_plus_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type( vector, src );
	}
    };

    /// Increment a vector by a scalar
    template <typename Vector, typename Source, typename VCat>
    struct crtp_plus_assign<Vector, Source, VCat, ashape::scal>
    {
	typedef vec_scal_plus_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type(vector, src);
	}
    };

    template <typename Vector, typename Source>
    struct assign_incrementer
    {
	typedef const Vector& type;
	type operator()(Vector& vector, const Source& src)
	{
	    src.increment_it(vector);
	    return vector;
	}
    };

} // namespace detail

template <typename Vector, typename Source>
struct crtp_plus_assign
  : boost::mpl::if_
     <boost::is_base_of<incrementer_base, Source>,
      detail::assign_incrementer<Vector, Source>,
      detail::crtp_plus_assign<Vector, Source, typename ashape::ashape<Vector>::type,
			       typename ashape::ashape<Source>::type>
     >::type
{};

/// Assign-add matrix vector product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_plus_assign<Vector, mat_cvec_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const mat_cvec_times_expr<E1, E2>& src)
    {
	mat_cvec_mult(src.first, src.second, vector, assign::plus_sum(), traits::mat_cvec_flatcat<E1>());
	return vector;
    }
};

/// Assign-add vector matrix product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_plus_assign<Vector, rvec_mat_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const rvec_mat_times_expr<E1, E2>& src)
    {
	rvec_mat_mult(src.first, src.second, vector, assign::plus_sum(), traits::mat_cvec_flatcat<E2>());
	return vector;
    }
};

namespace detail {

    template <typename Vector, typename Source, typename VCat, typename SCat>
    struct crtp_minus_assign {};

    /// Assign-add vector to a vector
    template <typename Vector, typename Source, typename Cat>
    struct crtp_minus_assign<Vector, Source, Cat, Cat>
    {
	typedef vec_vec_minus_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type(vector, src);
	}
    };

    /// Decrement a vector by a scalar
    template <typename Vector, typename Source, typename VCat>
    struct crtp_minus_assign<Vector, Source, VCat, ashape::scal>
    {
	typedef vec_scal_minus_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type(vector, src);
	}
    };

    template <typename Vector, typename Source>
    struct assign_decrementer
    {
	typedef const Vector& type;
	type operator()(Vector& vector, const Source& src)
	{
	    src.decrement_it(vector);
	    return vector;
	}
    };

} // namespace detail

template <typename Vector, typename Source>
struct crtp_minus_assign
  : boost::mpl::if_
     <boost::is_base_of<decrementer_base, Source>,
      detail::assign_decrementer<Vector, Source>,
      detail::crtp_minus_assign<Vector, Source, typename ashape::ashape<Vector>::type,
			       typename ashape::ashape<Source>::type>
     >::type
{};

/// Assign-subtract matrix vector product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_minus_assign<Vector, mat_cvec_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const mat_cvec_times_expr<E1, E2>& src)
    {
	mat_cvec_mult(src.first, src.second, vector, assign::minus_sum(), traits::mat_cvec_flatcat<E1>());
	return vector;
    }
};

/// Assign-subtract vector matrix product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_minus_assign<Vector, rvec_mat_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const rvec_mat_times_expr<E1, E2>& src)
    {
	rvec_mat_mult(src.first, src.second, vector, assign::minus_sum(), traits::mat_cvec_flatcat<E2>());
	return vector;
    }
};

#if 1
namespace detail {

    template <typename Vector, typename Source, typename SCat, typename VCat>
    struct crtp_times_assign {};

    /// Assign-add vector to a vector
    template <typename Vector, typename Source, typename Cat>
    struct crtp_times_assign<Vector, Source, Cat, Cat>
    {
	typedef vec_vec_times_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type( vector, src );
	}
    };

    /// Increment a vector by a scalar
    template <typename Vector, typename Source, typename VCat>
    struct crtp_times_assign<Vector, Source, VCat, ashape::scal>
    {
	typedef vec_scal_times_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type(vector, src);
	}
    };

    template <typename Vector, typename Source>
    struct assign_multiplyer
    {
	typedef const Vector& type;
	type operator()(Vector& vector, const Source& src)
	{
	    src.multiply_it(vector);
	    return vector;
	}
    };

} // namespace detail

template <typename Vector, typename Source>
struct crtp_times_assign
  : boost::mpl::if_
     <boost::is_base_of<incrementer_base, Source>,
      detail::assign_multiplyer<Vector, Source>,
      detail::crtp_times_assign<Vector, Source, typename ashape::ashape<Vector>::type,
			       typename ashape::ashape<Source>::type>
     >::type
{};

/// Assign-add matrix vector product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_times_assign<Vector, mat_cvec_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const mat_cvec_times_expr<E1, E2>& src)
    {
	mat_cvec_mult(src.first, src.second, vector, assign::times_sum(), traits::mat_cvec_flatcat<E1>());
	return vector;
    }
};

/// Assign-add vector matrix product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_times_assign<Vector, rvec_mat_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const rvec_mat_times_expr<E1, E2>& src)
    {
	rvec_mat_mult(src.first, src.second, vector, assign::times_sum(), traits::mat_cvec_flatcat<E2>());
	return vector;
    }
};

namespace detail {

    template <typename Vector, typename Source, typename SCat, typename VCat>
    struct crtp_div_assign {};

    /// Assign-divide vector to a vector
    template <typename Vector, typename Source, typename Cat>
    struct crtp_div_assign<Vector, Source, Cat, Cat>
    {
	typedef vec_vec_div_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type( vector, src );
	}
    };

    /// Divide a vector by a scalar
    template <typename Vector, typename Source, typename VCat>
    struct crtp_div_assign<Vector, Source, VCat, ashape::scal>
    {
	typedef vec_scal_div_asgn_expr<Vector, Source> type;
	type operator()(Vector& vector, const Source& src)
	{
	    return type(vector, src);
	}
    };

    template <typename Vector, typename Source>
    struct assign_divider
    {
	typedef const Vector& type;
	type operator()(Vector& vector, const Source& src)
	{
	    src.divide_it(vector);
	    return vector;
	}
    };

} // namespace detail

template <typename Vector, typename Source>
struct crtp_div_assign
  : boost::mpl::if_
     <boost::is_base_of<incrementer_base, Source>,
      detail::assign_divider<Vector, Source>,
      detail::crtp_div_assign<Vector, Source, typename ashape::ashape<Vector>::type,
			       typename ashape::ashape<Source>::type>
     >::type
{};

/// Assign-divide matrix vector product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_div_assign<Vector, mat_cvec_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const mat_cvec_times_expr<E1, E2>& src)
    {
	mat_cvec_mult(src.first, src.second, vector, assign::divide_sum(), traits::mat_cvec_flatcat<E1>());
	return vector;
    }
};

/// Assign-divide vector matrix product by calling mult
/** Note that this does not work for arbitrary expressions. **/
template <typename Vector, typename E1, typename E2>
struct crtp_div_assign<Vector, rvec_mat_times_expr<E1, E2> >
{
    typedef Vector& type;
    type operator()(Vector& vector, const rvec_mat_times_expr<E1, E2>& src)
    {
	rvec_mat_mult(src.first, src.second, vector, assign::divide_sum(), traits::mat_cvec_flatcat<E2>());
	return vector;
    }
};

#endif

/// Base class to provide vector assignment operators generically
template <typename Vector, typename ValueType, typename SizeType>
struct crtp_vector_assign
{
    /// Templated assignment implemented by functor to allow for partial specialization
    template <typename E>
    typename boost::disable_if<boost::is_same<Vector, E>,
			       typename crtp_assign<Vector, E>::type>::type
    operator=(const E& e)
    {
	return crtp_assign<Vector, E>()(static_cast<Vector&>(*this), e);
    }

    /// Assign-add vector expression
    template <class E>
    typename crtp_plus_assign<Vector, E>::type operator+=(const E& e)
    {
	return crtp_plus_assign<Vector, E>()(static_cast<Vector&>(*this), e);
    }

    /// Assign-subtract vector expression
    template <class E>
    typename crtp_minus_assign<Vector, E>::type operator-=(const E& e)
    {
	return crtp_minus_assign<Vector, E>()(static_cast<Vector&>(*this), e);
    }

#if 1
    /// Assign-times vector expression
    template <class E>
    typename crtp_times_assign<Vector, E>::type operator*=(const E& e)
    {
    	return crtp_times_assign<Vector, E>()(static_cast<Vector&>(*this), e);
    }

    /// Assign-div vector expression
    template <class E>
    typename crtp_div_assign<Vector, E>::type operator/=(const E& e)
    {
    	return crtp_div_assign<Vector, E>()(static_cast<Vector&>(*this), e);
    }
#endif

#if 0
    /// Scale vector (in place) with scalar value
    /** In the future, row vectors be possibly scaled by a matrix **/
    template <typename Factor>
    vec_scal_times_asgn_expr<Vector, Factor> operator*=(const Factor& alpha)
    {
	return vec_scal_times_asgn_expr<Vector, Factor>( static_cast<Vector&>(*this), alpha );
    }

    /// Devide vector (in place) by a scalar value
	// added by Hui Li 12/11/2007
    template <typename Factor>
    vec_scal_div_asgn_expr<Vector, Factor> operator/=(const Factor& alpha)
    {
	return vec_scal_div_asgn_expr<Vector, Factor>( static_cast<Vector&>(*this), alpha );
    }
#endif
    /// Check whether source and target have compatible resources and adapt empty target
    /** For expressions like u= v + w, u can be set to the size of v and w if still is 0. **/
    template <typename Src>
    void checked_change_resource(const Src& src)
    {	checked_change_resource_aux(src, typename mtl::traits::is_distributed<Vector>::type()); }

    template <typename Src>
    void checked_change_resource_aux(const Src& src, boost::mpl::false_)
    {   checked_change_dim(mtl::vector::size(src));  }

    /// Check whether vector size is compatible or if vector is 0 change it s.
    void checked_change_dim(SizeType s)
    {
	Vector& vector= static_cast<Vector&>(*this);
	vector.check_dim(s);
	vector.change_dim(s);
    }
};

template <typename Vector, typename ValueType, typename SizeType>
struct const_crtp_base_vector
{};

template <typename Vector, typename ValueType, typename SizeType>
struct mutable_crtp_base_vector
  : public crtp_vector_assign<Vector, ValueType, SizeType>
{};

template <typename Vector, typename ValueType, typename SizeType>
struct crtp_base_vector
  : boost::mpl::if_<boost::is_const<Vector>,
		    const_crtp_base_vector<Vector, ValueType, SizeType>,
		    mutable_crtp_base_vector<Vector, ValueType, SizeType>
                   >::type
{};

}} // namespace mtl::vector

#endif // MTL_CRTP_BASE_VECTOR_INCLUDE


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file transform.cpp

#include <chrono>
#include <iostream>
#include <fstream>
#include <memory>
#include <string>

#include <boost/lexical_cast.hpp>

#include "mcrl2/core/detail/print_utility.h"
#include "mcrl2/data/rewriter.h"
#include "mcrl2/pbes/io.h"
#include "mcrl2/pbes/remove_equations.h"
#include "mcrl2/pbes/rewrite.h"
#include "mcrl2/pbes/rewriters/data_rewriter.h"
#include "mcrl2/pbes/rewriters/enumerate_quantifiers_rewriter.h"
#include "mcrl2/pbes/rewriters/one_point_rule_rewriter.h"
#include "mcrl2/pbes/rewriters/quantifiers_inside_rewriter.h"
#include "mcrl2/pbes/rewriters/simplify_quantifiers_rewriter.h"
#include "mcrl2/pbes/rewriters/simplify_rewriter.h"

#include "mcrl2/utilities/input_output_tool.h"

using namespace mcrl2;

/// \brief Saves text to the file filename, or to stdout if filename equals "-".
inline
void write_text(const std::string& filename, const std::string& text)
{
  if (filename.empty())
  {
    std::cout << text;
  }
  else
  {
    std::ofstream out(filename);
    out << text;
  }
}

/// \brief
struct command
{
  std::string name;
  const std::string& input_filename;
  const std::string& output_filename;
  const std::vector<std::string>& options;

  command(const std::string& name_,
          const std::string& input_filename_,
          const std::string& output_filename_,
          const std::vector<std::string>& options_
         )
    : name(name_),
      input_filename(input_filename_),
      output_filename(output_filename_),
      options(options_)
  {}

  virtual void execute() = 0;
};

struct pbescommand: public command
{
  pbes_system::pbes pbesspec;

  pbescommand(const std::string& name,
              const std::string& input_filename,
              const std::string& output_filename,
              const std::vector<std::string>& options
             )
    : command(name, input_filename, output_filename, options)
  {}

  void execute()
  {
    pbes_system::load_pbes(pbesspec, input_filename);
  }
};

// PBES rewriters
struct rewrite_pbes_data_rewriter_command: public pbescommand
{
  rewrite_pbes_data_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-data-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    data::rewriter r(pbesspec.data());
    pbes_system::data_rewriter<data::rewriter> R(r);
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct rewrite_pbes_enumerate_quantifiers_rewriter_command: public pbescommand
{
  rewrite_pbes_enumerate_quantifiers_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-enumerate-quantifiers-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    data::rewriter r(pbesspec.data());
    pbes_system::enumerate_quantifiers_rewriter R(r, pbesspec.data());
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct rewrite_pbes_simplify_rewriter_command: public pbescommand
{
  rewrite_pbes_simplify_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-simplify-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    pbes_system::simplify_rewriter R;
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct rewrite_pbes_simplify_data_rewriter_command: public pbescommand
{
  rewrite_pbes_simplify_data_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-simplify-data-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    data::rewriter r(pbesspec.data());
    pbes_system::simplify_data_rewriter<data::rewriter> R(r);
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct rewrite_pbes_simplify_quantifiers_rewriter_command: public pbescommand
{
  rewrite_pbes_simplify_quantifiers_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-simplify-quantifiers-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    pbes_system::simplify_quantifiers_rewriter R;
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct rewrite_pbes_simplify_quantifiers_data_rewriter_command: public pbescommand
{
  rewrite_pbes_simplify_quantifiers_data_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-simplify-quantifiers-data-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    data::rewriter r(pbesspec.data());
    pbes_system::simplify_data_rewriter<data::rewriter> R(r);
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct rewrite_pbes_one_point_rule_rewriter_command: public pbescommand
{
  rewrite_pbes_one_point_rule_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-one-point-rule-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    pbes_system::one_point_rule_rewriter R;
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct rewrite_pbes_quantifiers_inside_rewriter_command: public pbescommand
{
  rewrite_pbes_quantifiers_inside_rewriter_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("pbes-quantifiers-inside-rewriter", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    pbes_system::quantifiers_inside_rewriter R;
    pbes_rewrite(pbesspec, R);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

struct remove_unused_pbes_equations_command: public pbescommand
{
  remove_unused_pbes_equations_command(const std::string& input_filename, const std::string& output_filename, const std::vector<std::string>& options)
    : pbescommand("remove-unused-pbes-equations", input_filename, output_filename, options)
  {}

  void execute()
  {
    pbescommand::execute();
    pbes_system::remove_unreachable_variables(pbesspec);
    pbes_system::save_pbes(pbesspec, output_filename);
  }
};

class transform_tool: public utilities::tools::input_output_tool
{
  protected:
    typedef utilities::tools::input_output_tool super;

    std::string algorithm_and_options;
    int algorithm_number = -1;
    bool print_algorithms = false;

    void parse_options(const utilities::command_line_parser& parser)
    {
      super::parse_options(parser);
      algorithm_and_options = parser.option_argument("algorithm");
      algorithm_number = parser.option_argument_as<int>("number");
      print_algorithms = parser.options.count("print-algorithms") > 0;
    }

    void add_options(utilities::interface_description& desc)
    {
      super::add_options(desc);
      desc.add_option("algorithm", utilities::make_optional_argument<std::string>("NAME", ""), "the algorithm that is to be applied", 'a');
      desc.add_option("number", utilities::make_optional_argument<int>("NAME", "-1"), "the number of the algorithm that is to be applied", 'n');
      desc.add_option("print-algorithms", "print the available algorithms", 'p');
    }

    inline
    void add_command(std::map<std::string, std::shared_ptr<command>>& commands, const std::shared_ptr<command>& command) const
    {
      commands[command->name] = command;
    }

  public:
    transform_tool()
      : super("mcrl2transform",
              "Wieger Wesselink",
              "applies a transformation to a PBES",
              "Transform the PBES in INFILE and write the result to OUTFILE. If OUTFILE "
              "is not present, stdout is used. If INFILE is not present, stdin is used."
             )
    {}

    bool run()
    {
      std::vector<std::string> options;
      std::set<std::string> algorithms;
      std::string algorithm;
      std::map<std::string, std::shared_ptr<command>> commands;

      // PBES algorithms
      add_command(commands, std::make_shared<rewrite_pbes_data_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<rewrite_pbes_enumerate_quantifiers_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<rewrite_pbes_simplify_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<rewrite_pbes_simplify_data_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<rewrite_pbes_simplify_quantifiers_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<rewrite_pbes_simplify_quantifiers_data_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<rewrite_pbes_one_point_rule_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<rewrite_pbes_quantifiers_inside_rewriter_command>(input_filename(), output_filename(), options));
      add_command(commands, std::make_shared<remove_unused_pbes_equations_command>(input_filename(), output_filename(), options));

      for (auto i = commands.begin(); i != commands.end(); ++i)
      {
        algorithms.insert(i->first);
      }

      if (algorithm_number >= 0 && !algorithm_and_options.empty())
      {
        throw mcrl2::runtime_error("It is not allowed to set both number and algorithm!");
      }

      // print the algorithms
      if (print_algorithms || (algorithm_number < 0 && algorithm_and_options.empty()))
      {
        int index = 1;
        std::cout << "The following algorithms are available:" << std::endl;
        for (auto const& algorithm: algorithms)
        {
          std::cout << index++ << ") " << algorithm << std::endl;
        }
        return true;
      }

      // if a number was specified, lookup the corresponding algorithm
      if (algorithm_number >= 0)
      {
        int index = 1;
        for (auto const& algo: algorithms)
        {
          if (index++ == algorithm_number)
          {
            algorithm = algo;
          }
        }
      }
      else
      {
        options = utilities::regex_split(algorithm_and_options, "\\s+");
        algorithm = options[0];
        options.erase(options.begin());
      }

      // run the algorithm
      auto i = commands.find(algorithm);
      if (i == commands.end())
      {
        throw std::runtime_error("Unknown algorithm " + algorithm);
      }
      i->second->execute();

      return true;
    }
};

int main(int argc, char* argv[])
{
  return transform_tool().execute(argc, argv);
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_MULTIBODY_FRAMES_HPP_
#define CROCODDYL_MULTIBODY_FRAMES_HPP_

#include <Eigen/Dense>
#include <pinocchio/spatial/se3.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/spatial/force.hpp>

namespace crocoddyl {

typedef std::size_t FrameIndex;

struct FrameTranslation {
  FrameTranslation(const FrameIndex& frame, const Eigen::Vector3d& oxf) : frame(frame), oxf(oxf) {}
  friend std::ostream& operator<<(std::ostream& os, const FrameTranslation& X) {
    os << "      frame: " << X.frame << std::endl << "translation: " << std::endl << X.oxf.transpose() << std::endl;
    return os;
  }

  FrameIndex frame;
  Eigen::Vector3d oxf;
};

struct FrameRotation {
  FrameRotation(const FrameIndex& frame, const Eigen::Matrix3d& oRf) : frame(frame), oRf(oRf) {}
  friend std::ostream& operator<<(std::ostream& os, const FrameRotation& X) {
    os << "   frame: " << X.frame << std::endl << "rotation: " << std::endl << X.oRf << std::endl;
    return os;
  }

  FrameIndex frame;
  Eigen::Matrix3d oRf;
};

struct FramePlacement {
  FramePlacement(const FrameIndex& frame, const pinocchio::SE3& oMf) : frame(frame), oMf(oMf) {}
  friend std::ostream& operator<<(std::ostream& os, const FramePlacement& X) {
    os << "    frame: " << X.frame << std::endl << "placement: " << std::endl << X.oMf << std::endl;
    return os;
  }

  FrameIndex frame;
  pinocchio::SE3 oMf;
};

struct FrameMotion {
  FrameMotion(const FrameIndex& frame, const pinocchio::Motion& oMf) : frame(frame), oMf(oMf) {}
  friend std::ostream& operator<<(std::ostream& os, const FrameMotion& X) {
    os << " frame: " << X.frame << std::endl << "motion: " << std::endl << X.oMf << std::endl;
    return os;
  }

  FrameIndex frame;
  pinocchio::Motion oMf;
};

struct FrameForce {
  FrameForce(const FrameIndex& frame, const pinocchio::Force& oFf) : frame(frame), oFf(oFf) {}
  friend std::ostream& operator<<(std::ostream& os, const FrameForce& X) {
    os << "frame: " << X.frame << std::endl << "force: " << std::endl << X.oFf << std::endl;
    return os;
  }

  FrameIndex frame;
  pinocchio::Force oFf;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_MULTIBODY_FRAMES_HPP_


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#include <algorithm>
#include <vector>

#include <popart/error.hpp>
#include <popart/logging.hpp>
#include <popart/maxclique.hpp>

#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/undirected_graph.hpp>

namespace popart {
namespace graphclique {

class AGraphImpl : public boost::adjacency_matrix<boost::undirectedS> {
public:
  AGraphImpl(int size_) : boost::adjacency_matrix<boost::undirectedS>(size_) {}
  AGraphImpl(AGraphImpl const &other)
      : boost::adjacency_matrix<boost::undirectedS>(other) {}
};

AGraph::AGraph(int size_) : pimpl(new AGraphImpl(size_)) {}

AGraph::AGraph(AGraph const &other)
    : pimpl(std::make_unique<AGraphImpl>(*other.pimpl)) {}

AGraph::~AGraph() {}

void AGraph::addEdge(int i, int j) { boost::add_edge(i, j, *pimpl); }

int AGraph::numVertices() const {
  return static_cast<int>(boost::num_vertices(*pimpl));
}

bool AGraph::getEdge(int i, int j) const { return pimpl->get_edge(i, j); }

MaxClique::MaxClique(AGraph agraph) : agraph_(agraph) {}

bool MaxClique::getEdge(int i, int j) const {
  if (i < 0 || i >= agraph_.numVertices()) {
    throw error("Vertex index i={} out of bounds.", i);
  }
  if (j < 0 || j >= agraph_.numVertices()) {
    throw error("Vertex index j={} out of bounds.", j);
  }
  return agraph_.getEdge(i, j);
}

std::vector<std::vector<int>>
MaxClique::getMaximumCliques(int minSize, int maxCount, const float stepLimit) {
  stepLimit_  = stepLimit;
  numVerices_ = agraph_.numVertices();

  std::vector<bool> used(numVerices_, false);
  std::vector<std::vector<int>> cliques;

  do {
    // Prepare with remaining vertices
    vertices_.clear();
    colors_.clear();
    groupColor_.clear();
    maxGroupColor_.clear();
    steps_ = 0;

    vertices_.reserve(numVerices_);
    for (int i = 0; i < agraph_.numVertices(); ++i) {
      if (!used[i]) {
        vertices_.emplace_back(i, 0);
      }
    }

    if (vertices_.size() != numVerices_) {
      throw error("Inconsistent vertices size {} vs. numVertices {}",
                  vertices_.size(),
                  numVerices_);
    }

    colors_.resize(numVerices_ + 1);
    for (auto &color : colors_) {
      color.reserve(numVerices_ + 1);
    }
    stepCount_.resize(numVerices_ + 1);

    // Main algorithm
    updateDegree(vertices_);
    sortByDegree(vertices_);
    setColorsToDegrees(vertices_);
    for (int i = 0; i < numVerices_ + 1; ++i) {
      stepCount_[i].first  = 0;
      stepCount_[i].second = 0;
    }

    // Branch-and-bound
    logging::trace("[MaxClique] branch-and-bound for {} vertices",
                   vertices_.size());
    branch(vertices_, 1);

    // Check if maximum clique in this iteration fulfills criteria
    if (maxGroupColor_.size() >= minSize) {
      cliques.push_back(maxGroupColor_);
      for (int i : maxGroupColor_) {
        used[i] = true;
        // Remove vertices already in a group
        numVerices_--;
      }
    }
  } while (maxGroupColor_.size() >= minSize && cliques.size() < maxCount &&
           numVerices_ > 0);

  return cliques;
}

void MaxClique::updateDegree(Vertices &vs) {
  for (int i = 0; i < vs.size(); i++) {
    int degree = 0;
    for (int j = 0; j < vs.size(); j++) {
      if (getEdge(vs[i].first, vs[j].first)) {
        degree++;
      }
    }
    vs[i].second = degree;
  }
}

void MaxClique::sortByDegree(Vertices &vs) {
  std::sort(vs.begin(), vs.end(), [](const Vertex &v0, const Vertex &v1) {
    return v0.second > v1.second;
  });
}

void MaxClique::setColorsToDegrees(Vertices &vs) {
  const int max_degree = vs.front().second;
  for (int i = 0; i < max_degree; i++) {
    vs[i].second = i + 1;
  }
  for (int i = max_degree; i < vs.size(); i++) {
    vs[i].second = max_degree + 1;
  }
}

void MaxClique::sortByColor(Vertices &vs) {
  int j       = 0;
  int upper_k = 1;
  int lower_k =
      static_cast<int>(maxGroupColor_.size() - groupColor_.size()) + 1;
  colors_[1].clear();
  colors_[2].clear();
  int k = 1;
  for (int i = 0; i < vs.size(); ++i) {
    int vindex = vs.at(i).first;
    k          = 1;
    while (colorCut(vindex, colors_[k])) {
      k++;
    }
    if (k > upper_k) {
      upper_k = k;
      colors_[upper_k + 1].clear();
    }
    colors_[k].push_back(vindex);
    if (k < lower_k) {
      vs.at(j).first = vindex;
      ++j;
    }
  }
  if (j > 0) {
    vs.at(j - 1).second = 0;
  }
  if (lower_k <= 0) {
    lower_k = 1;
  }
  for (k = lower_k; k <= upper_k; ++k) {
    for (int i = 0; i < colors_[k].size(); ++i) {
      vs.at(j).first  = colors_[k].at(i);
      vs.at(j).second = k;
      ++j;
    }
  }
}

bool MaxClique::colorCut(const int vindex, const ColorGroup &cg) {
  for (int i = 0; i < cg.size(); ++i)
    if (getEdge(vindex, cg.at(i)))
      return true;
  return false;
}

void MaxClique::verticesCut(const Vertices &vs0, Vertices &vs1) {
  for (int i = 0; i < vs0.size() - 1; ++i) {
    if (getEdge(vs0.back().first, vs0.at(i).first))
      vs1.emplace_back(vs0.at(i).first, 0);
  }
}

void MaxClique::branch(Vertices vs, int depth) {
  stepCount_.at(depth).first =
      (stepCount_.at(depth).first + stepCount_.at(depth - 1).first -
       stepCount_.at(depth).second);

  stepCount_.at(depth).second = (stepCount_.at(depth - 1).first);

  while (!vs.empty()) {
    if (groupColor_.size() + vs.back().second > maxGroupColor_.size()) {
      groupColor_.push_back(vs.back().first);
      Vertices vsx;
      vsx.reserve(vs.size());
      verticesCut(vs, vsx);
      if (vsx.size()) {
        ++steps_;
        if (static_cast<float>(stepCount_.at(depth).first) / steps_ <
            stepLimit_) {
          updateDegree(vsx);
          sortByDegree(vsx);
        }
        sortByColor(vsx);
        stepCount_.at(depth).first++;
        branch(vsx, depth + 1);
      } else if (groupColor_.size() > maxGroupColor_.size()) {
        maxGroupColor_ = groupColor_;
      }
      groupColor_.pop_back();
    } else {
      return;
    }
    vs.pop_back();
  }
}

} // namespace graphclique
} // namespace popart


/*
The Original EM/MPM algorithm was developed by Mary L. Comer and is distributed
under the BSD License.
Copyright (c) <2010>, <Mary L. Comer>
All rights reserved.

[1] Comer, Mary L., and Delp, Edward J.,  ÒThe EM/MPM Algorithm for Segmentation
of Textured Images: Analysis and Further Experimental Results,Ó IEEE Transactions
on Image Processing, Vol. 9, No. 10, October 2000, pp. 1731-1744.

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

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

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

Neither the name of <Mary L. Comer> nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

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

/* acvmpm.c */

/* Modified by Joel Dumke on 1/30/09 */

/* Heavily modified from the original by Michael A. Jackson for BlueQuartz Software
 * and funded by the Air Force Research Laboratory, Wright-Patterson AFB.
 */
#include "MPMCalculation.h"

#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include "EMMPMLib/Core/EMMPM.h"
#include "EMMPMLib/Common/MSVCDefines.h"
#include "EMMPMLib/Common/EMMPM_Math.h"
#include "EMMPMLib/Common/EMTime.h"
#include "EMMPMLib/Core/EMMPMUtilities.h"

#define USE_TBB_TASK_GROUP 0
#if defined (EMMPMLib_USE_PARALLEL_ALGORITHMS)
#include <tbb/task_scheduler_init.h>
#include <tbb/parallel_for.h>
#include <tbb/blocked_range2d.h>
#include <tbb/partitioner.h>
#include <tbb/task_group.h>

#endif

#define COMPUTE_C_CLIQUE( C, x, y, ci, cj)\
if((x) < 0 || (x) >= colEnd || (y) < 0 || (y) >= rowEnd) {\
  C[ci][cj] = classes;\
}\
else\
{\
  ij = (cols * (y)) + (x);\
  C[ci][cj] = xt[ij];\
}

/**
 * @class ParallelCalcLoop ParallelCalcLoop.h EMMPM/Curvature/ParallelCalcLoop.h
 * @brief This class can calculate the parts of the MPM loop in parallel
 * @author Michael A. Jackson for BlueQuartz Software
 * @date March 11, 2012
 * @version 1.0
 */
class ParallelMPMLoop
{
  public:
#if USE_TBB_TASK_GROUP
    ParallelCalcLoop(EMMPM_Data* dPtr, real_t* ykPtr, real_t* rnd, int rowStart, int rowEnd,
                     int colStart, int colEnd) :
    m_RowStart(rowStart),
    m_RowEnd(rowEnd),
    m_ColStart(colStart),
    m_ColEnd(colEnd),
#else
    ParallelMPMLoop(EMMPM_Data* dPtr, real_t* ykPtr, real_t* rnd) :
#endif

    data(dPtr),
    yk(ykPtr),
    rnd(rnd)
    {}
    virtual ~ParallelMPMLoop(){}

    void calc(int rowStart, int rowEnd,
                  int colStart, int colEnd) const
    {
      //uint64_t millis = EMMPM_getMilliSeconds();
    //  int l;
      real_t prior;
      int32_t ij, lij;
      int rows = data->rows;
      int cols = data->columns;
      int classes = data->classes;

      real_t xrnd, current;
      real_t post[EMMPM_MAX_CLASSES], sum, edge;

      size_t nsCols = data->columns - 1;
      size_t ewCols = data->columns;
      size_t swCols = data->columns - 1;
      size_t nwCols = data->columns - 1;

      unsigned char* xt = data->xt;
      real_t* probs = data->probs;
      real_t* ccost = data->ccost;
      real_t* ns = data->ns;
      real_t* ew = data->ew;
      real_t* sw = data->sw;
      real_t* nw = data->nw;
      real_t curvature_value = (real_t)0.0;

      int C[3][3]; // This is the Clique for the current Pixel
//      --------- X -----
//      |   | 0 | 1 | 2 |
//      -----------------
//   Y  | 0 |   |   |   |
//      -----------------
//      | 1 |   | P |   |
//      -----------------
//      | 2 |   |   |   |
//
// When we calculate the "C" matrix if the pixel value for the specific index of
// the clique would be off the image then a value = number of classes is
// used for the C[i][j]. That way we can figure out if we are off the image

      std::stringstream ss;
      unsigned int cSize = classes + 1;
      real_t* coupling = data->couplingBeta;

      for (int32_t y = rowStart; y < rowEnd; y++)
       {
         for (int32_t x = colStart; x < colEnd; x++)
         {

           /* -------------  */
          COMPUTE_C_CLIQUE(C, x-1, y-1, 0, 0);
          COMPUTE_C_CLIQUE(C,   x, y-1, 1, 0);
          COMPUTE_C_CLIQUE(C, x+1, y-1, 2, 0);
          COMPUTE_C_CLIQUE(C, x-1,   y, 0, 1);
          COMPUTE_C_CLIQUE(C, x+1,   y, 2, 1);
          COMPUTE_C_CLIQUE(C, x-1, y+1, 0, 2);
          COMPUTE_C_CLIQUE(C,   x, y+1, 1, 2);
          COMPUTE_C_CLIQUE(C, x+1, y+1, 2, 2);

#if 0
          if (y == rowStart + 1 && x == colStart + 1)
          {
              ss << "------------------------------" << std::endl;
              ss << "|" << C[0][0] << "\t" << C[1][0] << "\t" << C[2][0] << std::endl;
              ss << "|" << C[0][1] << "\t" << C[1][1] << "\t" << C[2][1] << std::endl;
              ss << "|" << C[0][2] << "\t" << C[1][2] << "\t" << C[2][2] << std::endl;
              ss << "------------------------------" << std::endl;
              std::cout << ss.str() << std::endl;
          }
#endif

          ij = (cols*y)+x;
          sum = 0;
          for (int l = 0; l < classes; ++l)
          {
            prior = 0;
            edge = 0;

            prior += coupling[(cSize*l)+ C[0][0]];
            prior += coupling[(cSize*l)+ C[1][0]];
            prior += coupling[(cSize*l)+ C[2][0]];
            prior += coupling[(cSize*l)+ C[0][1]];
            prior += coupling[(cSize*l)+ C[2][1]];
            prior += coupling[(cSize*l)+ C[0][2]];
            prior += coupling[(cSize*l)+ C[1][2]];
            prior += coupling[(cSize*l)+ C[2][2]];

#if 0
            if (y == rowStart + 1 && x == colStart + 1)
            {
                std::cout << "Class: " << l << "\t prior: " << prior << std::endl;
            }
#endif

            // now check for the gradient penalty. If our current class is NOT equal
            // to the class at index[i][j] AND the value of C[i][j] does NOT equal
            // to the Number of Classes then add in the gradient penalty.
            if (data->useGradientPenalty)
            {
              if (C[0][0] != l && C[0][0] != classes) edge += sw[(swCols*(y-1))+x-1];
              if (C[1][0] != l && C[1][0] != classes) edge += ew[(ewCols*(y-1))+x];
              if (C[2][0] != l && C[2][0] != classes) edge += nw[(nwCols*(y-1))+x];
              if (C[0][1] != l && C[0][1] != classes) edge += ns[(nsCols*y)+x-1];
              if (C[2][1] != l && C[2][1] != classes) edge += ns[(nsCols*y)+x];
              if (C[0][2] != l && C[0][2] != classes) edge += nw[(nwCols*y)+x-1];
              if (C[1][2] != l && C[1][2] != classes) edge += ew[(ewCols*y)+x];
              if (C[2][2] != l && C[2][2] != classes) edge += sw[(swCols*y)+x];
            }

            lij = (cols * rows * l) + (cols * y) + x;
            curvature_value = 0.0;
            if (data->useCurvaturePenalty)
            {
              curvature_value = data->beta_c * ccost[lij];
            }
            real_t arg = data->workingKappa * (yk[lij] - (prior) - (edge) - (curvature_value) - data->w_gamma[l]);
            post[l] = expf(arg);
            sum += post[l];
          }

           xrnd = rnd[ij];
           current = 0.0;

           for (int l = 0; l < classes; l++)
           {
             lij = (cols * rows * l) + ij;
             real_t arg = post[l] / sum;
             if ((xrnd >= current) && (xrnd <= (current + arg)))
             {
               xt[ij] = l;
               probs[lij] += 1.0;
             }
             current += arg;
           }
#if 0
           Dont even THINK about using this code...
                   This classifys the pixel based on the largest
                   in magnitude  probability
           real_t max = 0.0;
           int maxClass = 0;
           for (int l = 0; l < classes; l++)
           {
             lij = (cols * rows * l) + ij;
             //real_t arg = post[l] / sum;
             if (probs[lij] > max)
             {
                 max = probs[lij];
                 maxClass = l;
             }
           }
           //Assign class based on Maximum probability
           xt[ij] = maxClass;
#endif
         }
       }
    //  std::cout << "     --" << EMMPM_getMilliSeconds() - millis << "--" << std::endl;
     }

#if defined (EMMPMLib_USE_PARALLEL_ALGORITHMS)
#if USE_TBB_TASK_GROUP
    void operator()() const
    {
      calc(m_RowStart, m_RowEnd, m_ColStart, m_ColEnd);
    }
#else
    void operator()(const tbb::blocked_range2d<int> &r) const
    {
      calc(r.rows().begin(), r.rows().end(), r.cols().begin(), r.cols().end());
    }
#endif
#endif

private:
#if USE_TBB_TASK_GROUP
    int m_RowStart;
    int m_RowEnd;
    int m_ColStart;
    int m_ColEnd;
#endif
    const EMMPM_Data* data;
    const real_t* yk;
    const real_t* rnd;

};

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
MPMCalculation::MPMCalculation() :
m_StatsDelegate(NULL)
{
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
MPMCalculation::~MPMCalculation()
{
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void MPMCalculation::execute()
{
  EMMPM_Data* data = m_Data.get();

  real_t* yk;
  real_t sqrt2pi, con[EMMPM_MAX_CLASSES];
  real_t post[EMMPM_MAX_CLASSES];

  // int k, l;
 // unsigned int i, j, d;
  size_t ld, ijd, lij;
  unsigned int dims = data->dims;
  unsigned int rows = data->rows;
  unsigned int cols = data->columns;
  unsigned int classes = data->classes;

//  int rowEnd = rows/2;
  unsigned char* y = data->y;
  real_t* probs = data->probs;
  real_t* m = data->mean;
  real_t* v = data->variance;

  char msgbuff[256];
  float totalLoops;

  int currentLoopCount;

//  real_t curvature_value = (real_t)0.0;

  memset(post, 0, EMMPM_MAX_CLASSES * sizeof(real_t));
  memset(con, 0,  EMMPM_MAX_CLASSES * sizeof(real_t));

  totalLoops = (float)(data->emIterations * data->mpmIterations + data->mpmIterations);
  memset(msgbuff, 0, 256);
  data->progress++;

  yk = (real_t*)malloc(cols * rows * classes * sizeof(real_t));

  sqrt2pi = sqrt(2.0 * M_PI);

  for (uint32_t l = 0; l < classes; l++)
  {
    con[l] = 0;
    for (uint32_t d = 0; d < dims; d++)
    {
      ld = dims * l + d;
      con[l] += -log(sqrt2pi * sqrt(v[ld]));
    }
  }

  for (uint32_t i = 0; i < rows; i++)
  {
    for (uint32_t j = 0; j < cols; j++)
    {
      for (uint32_t l = 0; l < classes; l++)
      {
        lij = (cols * rows * l) + (cols * i) + j;
        probs[lij] = 0;
        yk[lij] = con[l];
        for (uint32_t d = 0; d < dims; d++)
        {
          ld = dims * l + d;
          ijd = (dims * cols * i) + (dims * j) + d;
          yk[lij] += ((y[ijd] - m[ld]) * (y[ijd] - m[ld]) / (-2.0 * v[ld]));
        }
      }
    }
  }

  const float rangeMin = 0;
  const float rangeMax = 1.0f;
  typedef boost::uniform_real<real_t> NumberDistribution;
  typedef boost::mt19937 RandomNumberGenerator;
  typedef boost::variate_generator<RandomNumberGenerator&,
                                   NumberDistribution> Generator;

  NumberDistribution distribution(rangeMin, rangeMax);
  RandomNumberGenerator generator;
  Generator numberGenerator(generator, distribution);
  generator.seed(EMMPM_getMilliSeconds()); // seed with the current time

  // Generate all the numbers up front
  size_t total = rows * cols;
  std::vector<real_t> rndNumbers(total);
  real_t* rndNumbersPtr = &(rndNumbers.front());
  for(size_t i = 0; i < total; ++i)
  {
    rndNumbersPtr[i] = numberGenerator(); // Work directly with the pointer for speed.
  }

  //unsigned long long int millis = EMMPM_getMilliSeconds();
  //std::cout << "------------------------------------------------" << std::endl;
  /* Perform the MPM loops */
  for (int32_t k = 0; k < data->mpmIterations; k++)
  {

    data->currentMPMLoop = k;
    if (data->cancel) { data->progress = 100.0; break; }
    data->inside_mpm_loop = 1;

#if defined (EMMPMLib_USE_PARALLEL_ALGORITHMS)
    tbb::task_scheduler_init init;
    int threads = init.default_num_threads();
#if USE_TBB_TASK_GROUP
    tbb::task_group* g = new tbb::task_group;
    unsigned int rowIncrement = rows/threads;
    unsigned int rowStop = 0 + rowIncrement;
    unsigned int rowStart = 0;
    for (int t = 0; t < threads; ++t)
    {
      g->run(ParallelCalcLoop(data, yk, &(rndNumbers.front()), rowStart, rowStop, 0, cols) );
      rowStart = rowStop;
      rowStop = rowStop + rowIncrement;
      if (rowStop >= rows)
      {
        rowStop = rows;
      }
    }
    g->wait();
    delete g;
#else
    tbb::parallel_for(tbb::blocked_range2d<int>(0, rows, rows/threads, 0, cols, cols),
                      ParallelMPMLoop(data, yk, &(rndNumbers.front())),
                      tbb::simple_partitioner());
#endif

#else
    ParallelMPMLoop pcl(data, yk, &(rndNumbers.front()));
    pcl.calc(0, rows, 0, cols);
#endif

    //std::cout << "Counter: " << counter << std::endl;
    EMMPMUtilities::ConvertXtToOutputImage(getData());

    data->currentMPMLoop = k;
   // snprintf(msgbuff, 256, "MPM Loop %d", data->currentMPMLoop);
   // notify(msgbuff, 0, UpdateProgressMessage);

    currentLoopCount = data->mpmIterations * data->currentEMLoop + data->currentMPMLoop;
    data->progress = currentLoopCount / totalLoops * 100.0;

   // notify("", data->progress, UpdateProgressValue);
    if (m_StatsDelegate != NULL)
    {
      m_StatsDelegate->reportProgress(m_Data);
    }
  }
#if 0
  #if defined (EMMPMLib_USE_PARALLEL_ALGORITHMS)
    std::cout << "Parrallel MPM Loop Time to Complete:";
#else
    std::cout << "Serial MPM Loop Time To Complete: ";
#endif
    std::cout << (EMMPM_getMilliSeconds() - millis) << std::endl;
#endif

  data->inside_mpm_loop = 0;

  if (!data->cancel)
  {
  /* Normalize probabilities */
    for (uint32_t i = 0; i < data->rows; i++)
    {
      for (uint32_t j = 0; j < data->columns; j++)
      {
        for (uint32_t l = 0; l < classes; l++)
        {
          lij = (cols * rows * l) + (cols * i) + j;
          data->probs[lij] = data->probs[lij] / (real_t)data->mpmIterations;
        }
      }
    }
  }

  /* Clean Up Memory */
  free(yk);

}


#include <iostream>
#include <typeinfo>

#include <map>

using std::cout;
using std::endl;

#include "viennagrid/forwards.hpp"

#include "viennagrid/storage/view.hpp"
#include "viennagrid/storage/container_collection.hpp"
#include "viennagrid/storage/inserter.hpp"
#include "viennagrid/storage/id_generator.hpp"
#include "viennagrid/storage/hidden_key_map.hpp"
#include "viennagrid/storage/range.hpp"

#include "viennagrid/topology/vertex.hpp"
#include "viennagrid/topology/line.hpp"
#include "viennagrid/topology/simplex.hpp"

#include "viennagrid/element/element_key.hpp"
#include "viennagrid/element/element_orientation.hpp"

#include "viennagrid/config/element_config.hpp"
#include "viennagrid/config/mesh_config.hpp"

#include "viennagrid/point.hpp"

#include "viennagrid/mesh/mesh.hpp"
#include "viennagrid/mesh/element_creation.hpp"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ublas = boost::numeric::ublas;

typedef ublas::vector<double> vector_type;

class my_mesh_config
{
  private:
    //typedef viennagrid::storage::pointer_handle_tag handle_tag;
    //typedef viennagrid::storage::iterator_handle_tag handle_tag;
    typedef viennagrid::storage::id_handle_tag          handle_tag;

  public:

    typedef viennagrid::config::result_of::full_mesh_config< viennagrid::tetrahedron_tag, vector_type, handle_tag >::type     type;
};

int main()
{

    //
    // typedefing and setting up the geometric mesh
    //

    typedef viennagrid::mesh<my_mesh_config> mesh_type;
    mesh_type mesh;

    //
    // typedefs for the element types
    //

    typedef viennagrid::result_of::element<mesh_type, viennagrid::vertex_tag>::type vertex_type;
    typedef viennagrid::result_of::handle<mesh_type, viennagrid::vertex_tag>::type vertex_handle_type;

    typedef viennagrid::result_of::element<mesh_type, viennagrid::line_tag>::type line_type;
    typedef viennagrid::result_of::element<mesh_type, viennagrid::triangle_tag>::type triangle_type;

    typedef viennagrid::result_of::element<mesh_type, viennagrid::tetrahedron_tag>::type tetrahedron_type;
    typedef viennagrid::result_of::handle<mesh_type, viennagrid::tetrahedron_tag>::type tetrahedron_handle_type;

//     typedef viennagrid::result_of::handle<mesh_type, viennagrid::polygon_tag>::type tetrahedron_handle_type;

    //
    // Adding a tetrahedron
    //

    // creates four vertices within the mesh, vh is short vor vertex handle
    // make_element is responsible for resizing all meta-info container which are associated with vertex_type
    vertex_handle_type vh0 = viennagrid::make_vertex( mesh );
    vertex_handle_type vh1 = viennagrid::make_vertex( mesh );
    vertex_handle_type vh2 = viennagrid::make_vertex( mesh );
    vertex_handle_type vh3 = viennagrid::make_vertex( mesh );

    // create geometric information for the vertices
    vector_type p0(3);
    p0[0] = 0.0; p0[1] = 0.0; p0[2] = 0.0;
    vector_type p1(3);
    p1[0] = 1.0; p1[1] = 0.0; p1[2] = 0.0;
    vector_type p2(3);
    p2[0] = 0.0; p2[1] = 1.0; p2[2] = 0.0;
    vector_type p3(3);
    p3[0] = 0.0; p3[1] = 0.0; p3[2] = 1.0;

    // set the geometric information for the vertices
    // is equivalent to viennagrid::look_up<vector_type>(mesh, vhX)
    viennagrid::point(mesh, vh0) = p0;
    viennagrid::point(mesh, vh1) = p1;
    viennagrid::point(mesh, vh2) = p2;
    viennagrid::point(mesh, vh3) = p3;

    // creates a handle buffer for the vertex handles of the tetdrahedron
    std::vector<vertex_handle_type> handles(4);
    handles[0] = vh0; handles[1] = vh1; handles[2] = vh2; handles[3] = vh3;

    // creates the tetrahedron within the mesh, all boundary cell generation is done here implicit
    tetrahedron_handle_type tet = viennagrid::make_element<tetrahedron_type>( mesh, handles.begin(), handles.end() );
    std::cout << tet << std::endl;

    // set a double value to a tetdrahedron
//     viennagrid::look_up<double>(mesh, tet) = 1.0;

    //
    // display the mesh content
    //

    cout << "All vertices of the mesh" << endl;
    std::copy( viennagrid::elements<viennagrid::vertex_tag>(mesh).begin(), viennagrid::elements<viennagrid::vertex_tag>(mesh).end(), std::ostream_iterator<vertex_type>(cout, "\n") );
    cout << endl;

    cout << "All lines of the mesh" << endl;
    std::copy( viennagrid::elements<viennagrid::line_tag>(mesh).begin(), viennagrid::elements<viennagrid::line_tag>(mesh).end(), std::ostream_iterator<line_type>(cout, "\n") );
    cout << endl;

    cout << "All triangles of the mesh" << endl;
    std::copy( viennagrid::elements<viennagrid::triangle_tag>(mesh).begin(), viennagrid::elements<viennagrid::triangle_tag>(mesh).end(), std::ostream_iterator<triangle_type>(cout, "\n") );
    cout << endl;

    cout << "All tetraherons of the mesh" << endl;
    std::copy( viennagrid::elements<viennagrid::tetrahedron_tag>(mesh).begin(), viennagrid::elements<viennagrid::tetrahedron_tag>(mesh).end(), std::ostream_iterator<tetrahedron_type>(cout, "\n") );
    cout << endl;

    //
    // doing some boundary cell iteration
    //

    const tetrahedron_type & test_tet = *viennagrid::elements<viennagrid::tetrahedron_tag>(mesh).begin();
    const triangle_type & test_tri = *viennagrid::elements<viennagrid::triangle_tag>(mesh).begin();

    typedef viennagrid::result_of::const_element_range<tetrahedron_type, viennagrid::triangle_tag>::type tetrahedron_triangle_range;
    typedef viennagrid::result_of::const_iterator<tetrahedron_triangle_range>::type tetrahedron_triangle_iterator;

    cout << "All triangles of the first tetdrahedron in the mesh" << endl;
    tetrahedron_triangle_range tri_range = viennagrid::elements<viennagrid::triangle_tag>(test_tet);
    for (tetrahedron_triangle_iterator it = tri_range.begin(); it != tri_range.end(); ++it)
        cout << *it << endl;
    cout << endl;

    cout << "Once more with std::copy" << endl;
    std::copy( tri_range.begin(), tri_range.end(), std::ostream_iterator<triangle_type>(cout, "\n") );
    cout << endl;

    typedef viennagrid::result_of::const_element_range<triangle_type, viennagrid::line_tag>::type triangle_line_range;
    typedef viennagrid::result_of::const_iterator<triangle_line_range>::type triangle_line_iterator;

    cout << "All lines of the first triangle in the mesh" << endl;
    triangle_line_range lin_range = viennagrid::elements<viennagrid::line_tag>(test_tri);
    for (triangle_line_iterator it = lin_range.begin(); it != lin_range.end(); ++it)
        cout << *it << endl;
    cout << endl;

    cout << "Once more with std::copy" << endl;
    std::copy( lin_range.begin(), lin_range.end(), std::ostream_iterator<line_type>(cout, "\n") );
    cout << endl;

    //
    // geometric iteration
    //

    // iterating over all vertices and piping out the point information
//     typedef viennagrid::result_of::const_element_range<tetrahedron_type, viennagrid::vertex_tag>::type tetrahedron_vertex_range;
//     typedef viennagrid::result_of::const_iterator<tetrahedron_vertex_range>::type tetrahedron_vertex_iterator;
//
//     cout << "All vertices of the first tetdrahedron in the mesh USING ncells<dim>()" << endl;
//     tetrahedron_vertex_range vtx_range = viennagrid::elements<viennagrid::vertex_tag>(test_tet);
//     for (tetrahedron_vertex_iterator it = vtx_range.begin(); it != vtx_range.end(); ++it)
//         cout << *it << " geometric information: " << viennagrid::look_up<vector_type>( mesh, *it ) << endl;
//     cout << endl;
//
//
//     typedef viennagrid::result_of::const_element_range<mesh_type, viennagrid::vertex_tag>::type mesh_vertex_range_2;
//     typedef viennagrid::result_of::const_iterator<mesh_vertex_range_2>::type mesh_vertex_iterator_2;
//
//     cout << "All vertices of the first tetdrahedron in the mesh USING elements<tag>()" << endl;
//     mesh_vertex_range_2 mesh_vtx_range_2 = viennagrid::elements<viennagrid::vertex_tag>(mesh);
//     for (mesh_vertex_iterator_2 it = mesh_vtx_range_2.begin(); it != mesh_vtx_range_2.end(); ++it)
//         cout << *it << " geometric information: " << viennagrid::look_up<vector_type>( mesh, *it ) << endl;
//     cout << endl;
//
//
//     typedef viennagrid::result_of::const_element_range<mesh_type, vertex_type>::type mesh_vertex_range_3;
//     typedef viennagrid::result_of::const_iterator<mesh_vertex_range_3>::type mesh_vertex_iterator_3;
//
//     cout << "All vertices of the first tetdrahedron in the mesh USING elements<type>()" << endl;
//     mesh_vertex_range_3 mesh_vtx_range_3 = viennagrid::elements<vertex_type>(mesh);
//     for (mesh_vertex_iterator_3 it = mesh_vtx_range_3.begin(); it != mesh_vtx_range_3.end(); ++it)
//         cout << *it << " geometric information: " << viennagrid::look_up<vector_type>( mesh, *it ) << endl;
//     cout << endl;
//
//
//     // iterating over all tetrahedrons and piping out the double meta-information
//     typedef viennagrid::result_of::const_element_range<mesh_type, viennagrid::tetrahedron_tag>::type tetrahedron_range;
//     typedef viennagrid::result_of::const_iterator<tetrahedron_range>::type tetrahedron_iterator;
//
//     cout << "All tetdrahedrons in the mesh" << endl;
//     tetrahedron_range tet_range = viennagrid::elements<viennagrid::tetrahedron_tag>(mesh);
//     for (tetrahedron_iterator it = tet_range.begin(); it != tet_range.end(); ++it)
//     {
//         cout << *it << endl;
//         cout << "   geometric information: " << viennagrid::look_up<double>( mesh, *it ) << endl;
//     }
//     cout << endl;

    const mesh_type & test = mesh;

    typedef viennagrid::result_of::const_element_range<mesh_type, viennagrid::vertex_tag>::type const_vertex_range;
    typedef viennagrid::result_of::iterator<const_vertex_range>::type const_vertex_iterator;

    const_vertex_range r = viennagrid::elements<viennagrid::vertex_tag>(test);
    for (const_vertex_iterator i = r.begin(); i != r.end(); ++i)
    {
        cout << *i << endl;
        cout << viennagrid::point(test, *i) << endl;
    }

    return 0;
}


// Copyright 2010 The Trustees of Indiana University.

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

//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_GRAPH_LOOP_ERASED_RANDOM_WALK_HPP
#define BOOST_GRAPH_LOOP_ERASED_RANDOM_WALK_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/random.hpp>
#include <boost/next_prior.hpp>
#include <vector>
#include <boost/assert.hpp>

namespace boost
{

struct BOOST_SYMBOL_VISIBLE loop_erased_random_walk_stuck
: public std::exception
{
    virtual ~loop_erased_random_walk_stuck() throw() {}
    inline virtual const char* what() const throw()
    {
        return "Loop-erased random walk found a vertex with no out-edges";
    }
};

// Do a loop-erased random walk from vertex s to any vertex colored black (or
// actually any color other than white or gray) in the color map.  The color
// white is for vertices that are not part of the path, while gray is for
// those that are on the path (for cycle detection).  The vector path is used
// for temporary storage and as the result of the algorithm; while all
// elements of the path except the last have their colors set to gray upon
// return.  Vertex s must start off colored white.
//
// Useful references:
// http://everything2.com/title/loop-erased+random+walk
// Wikipedia page on "Loop-Erased Random Walk"

template < typename Graph, typename ColorMap, typename NextEdge >
void loop_erased_random_walk(const Graph& g,
    typename boost::graph_traits< Graph >::vertex_descriptor s,
    NextEdge next_edge, ColorMap color,
    std::vector< typename boost::graph_traits< Graph >::vertex_descriptor >&
        path)
{
    typedef typename boost::graph_traits< Graph >::vertex_descriptor
        vertex_descriptor;
    typedef
        typename boost::graph_traits< Graph >::edge_descriptor edge_descriptor;
    typedef typename boost::property_traits< ColorMap >::value_type color_t;
    typedef boost::color_traits< color_t > color_gen;

    BOOST_ASSERT(get(color, s) == color_gen::white());
    path.clear();
    path.push_back(s);
    put(color, s, color_gen::gray());
    while (true)
    {
        edge_descriptor e = next_edge(s, g);
        vertex_descriptor t = target(e, g);
        color_t t_color = get(color, t);
        if (t_color == color_gen::white())
        {
            path.push_back(t);
            put(color, t, color_gen::gray());
            s = t;
        }
        else if (t_color == color_gen::gray())
        {
            // Found a loop; delete from path from the first occurrence of t to
            // the end, coloring vertices white.
            typename std::vector< vertex_descriptor >::iterator it
                = std::find(path.begin(), path.end(), t);
            BOOST_ASSERT(it != path.end());
            ++it;
            for (typename std::vector< vertex_descriptor >::iterator j = it;
                 j != path.end(); ++j)
            {
                put(color, *j, color_gen::white());
            }
            path.erase(it, path.end());
            s = t;
        }
        else
        {
            // Done
            path.push_back(t);
            break;
        }
    }
}

template < typename Graph, typename Gen > class unweighted_random_out_edge_gen
{
    Gen& gen;

    typedef boost::graph_traits< Graph > gt;

public:
    unweighted_random_out_edge_gen(Gen& gen) : gen(gen) {}

    typename gt::edge_descriptor operator()(
        typename gt::vertex_descriptor src, const Graph& g) const
    {
        if (out_degree(src, g) == 0)
            throw loop_erased_random_walk_stuck();
        return boost::random_out_edge(g, src, gen);
    }
};

template < typename Graph, typename WeightMap, typename Gen >
class weighted_random_out_edge_gen
{
    WeightMap weight;
    Gen& gen;

    typedef boost::graph_traits< Graph > gt;

public:
    weighted_random_out_edge_gen(const WeightMap& weight, Gen& gen)
    : weight(weight), gen(gen)
    {
    }

    typename gt::edge_descriptor operator()(
        typename gt::vertex_descriptor src, const Graph& g) const
    {
        if (out_degree(src, g) == 0)
            throw loop_erased_random_walk_stuck();
        return boost::weighted_random_out_edge(g, src, weight, gen);
    }
};
}

#endif // BOOST_GRAPH_LOOP_ERASED_RANDOM_WALK_HPP