/*
[begin_description]
Modification of the implicit Euler method, works with the MTL4 matrix library only.
[end_description]

Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Copyright 2012 Andreas Angelopoulos

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

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED

#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resize.hpp>
#include <boost/numeric/odeint/util/same_size.hpp>

#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template< class Value , class Parameters >
struct is_resizeable< mtl::dense_vector< Value , Parameters > >
{
    typedef boost::true_type type;
    const static bool value = type::value;
};

template< class Value , class Parameters >
struct is_resizeable< mtl::dense2D< Value , Parameters > >
{
    typedef boost::true_type type;
    const static bool value = type::value;
};

template< class Value , class Parameters >
struct is_resizeable< mtl::compressed2D< Value , Parameters > >
{
    typedef boost::true_type type;
    const static bool value = type::value;
};

template< class Value , class Parameters >
struct same_size_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > >
{
    static bool same_size( const mtl::dense_vector< Value , Parameters > &v1 ,
                           const mtl::dense_vector< Value , Parameters > &v2 )
    {
        return mtl::size( v1 ) == mtl::size( v2 );
    }
};

template< class Value , class Parameters >
struct resize_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > >
{
    static void resize( mtl::dense_vector< Value , Parameters > &v1 ,
                        const mtl::dense_vector< Value , Parameters > &v2 )
    {
        v1.change_dim( mtl::size( v2 ) );
    }
};

template< class Value , class MatrixParameters , class VectorParameters >
struct same_size_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
    static bool same_size( const mtl::dense2D< Value , MatrixParameters > &m ,
                           const mtl::dense_vector< Value , VectorParameters > &v )
    {
        return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) );
    }
};

template< class Value , class MatrixParameters , class VectorParameters >
struct resize_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
    static void resize( mtl::dense2D< Value , MatrixParameters > &m ,
                        const mtl::dense_vector< Value , VectorParameters > &v )
    {
        m.change_dim( mtl::size( v ) , mtl::size( v ) , false );
    }
};

template< class Value , class MatrixParameters , class VectorParameters >
struct same_size_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
    static bool same_size( const mtl::compressed2D< Value , MatrixParameters > &m ,
                           const mtl::dense_vector< Value , VectorParameters > &v )
    {
        return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) );
    }
};

template< class Value , class MatrixParameters , class VectorParameters >
struct resize_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
    static void resize( mtl::compressed2D< Value , MatrixParameters > &m ,
                        const mtl::dense_vector< Value , VectorParameters > &v )
    {
        m.change_dim( mtl::size( v ) , mtl::size( v ) );
    }
};

} // namespace odeint
} // namesapce numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED


/*!
@file
Forward declares `boost::hana::Logical`.

@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_FWD_LOGICAL_HPP
#define BOOST_HANA_FWD_LOGICAL_HPP

#include <boost/hana/config.hpp>
#include <boost/hana/detail/std/forward.hpp>
#include <boost/hana/fwd/core/datatype.hpp>
#include <boost/hana/fwd/core/models.hpp>
#include <boost/hana/fwd/core/operators.hpp>

namespace boost { namespace hana {
    //! @ingroup group-concepts
    //! The `Logical` concept represents types with a truth value.
    //!
    //! Intuitively, a `Logical` is just a `bool`, or something that can act
    //! like one. However, in the context of programming with heterogeneous
    //! objects, it becomes extremely important to distinguish between those
    //! objects whose truth value is known at compile-time, and those whose
    //! truth value is only known at runtime. The reason why this is so
    //! important is because it is possible to branch at compile-time on
    //! a condition whose truth value is known at compile-time, and hence
    //! the return type of the enclosing function can depend on that truth
    //! value. However, if the truth value is only known at runtime, then
    //! the compiler has to compile both branches (because any or both of
    //! them may end up being used), which creates the additional requirement
    //! that both branches must evaluate to the same type.
    //!
    //! Specifically, `Logical` (almost) represents a [boolean algebra][1],
    //! which is a mathematical structure encoding the usual properties that
    //! allow us to reason with `bool`. The exact properties that must be
    //! satisfied by any model of `Logical` are rigorously stated in the laws
    //! below.
    //!
    //!
    //! Truth, falsity and logical equivalence
    //! --------------------------------------
    //! A `Logical` `x` is said to be _true-valued_, or sometimes also just
    //! _true_ as an abuse of notation, if
    //! @code
    //!     if_(x, true, false) == true
    //! @endcode
    //!
    //! Similarly, `x` is _false-valued_, or sometimes just _false_, if
    //! @code
    //!     if_(x, true, false) == false
    //! @endcode
    //!
    //! This provides a standard way of converting any `Logical` to a straight
    //! `bool`. The notion of truth value suggests another definition, which
    //! is that of logical equivalence. We will say that two `Logical`s `x`
    //! and `y` are _logically equivalent_ if they have the same truth value.
    //! To denote that some expressions `p` and `q` of a Logical data type are
    //! logically equivalent, we will sometimes also write
    //! @code
    //!     p   if and only if   q
    //! @endcode
    //! which is very common in mathematics. The intuition behind this notation
    //! is that whenever `p` is true-valued, then `q` should be; but when `p`
    //! is false-valued, then `q` should be too. Hence, `p` should be
    //! true-valued when (and only when) `q` is true-valued.
    //!
    //!
    //! Laws
    //! ----
    //! As outlined above, the `Logical` concept almost represents a boolean
    //! algebra. The rationale for this laxity is to allow things like integers
    //! to act like `Logical`s, which is aligned with C++, even though they do
    //! not form a boolean algebra. Even though we depart from the usual
    //! axiomatization of boolean algebras, we have found through experience
    //! that the definition of a Logical given here is largely compatible with
    //! intuition.
    //!
    //! The following laws must be satisfied for any data type `L` modeling
    //! the `Logical` concept. Let `a`, `b` and `c` be objects of a `Logical`
    //! data type, and let `t` and `f` be arbitrary _true-valued_ and
    //! _false-valued_ `Logical`s of that data type, respectively. Then,
    //! @code
    //!     // associativity
    //!     or_(a, or_(b, c))   == or_(or_(a, b), c)
    //!     and_(a, and_(b, c)) == and_(and_(a, b), c)
    //!
    //!     // equivalence through commutativity
    //!     or_(a, b)   if and only if   or_(b, a)
    //!     and_(a, b)  if and only if   and_(b, a)
    //!
    //!     // absorption
    //!     or_(a, and_(a, b)) == a
    //!     and_(a, or_(a, b)) == a
    //!
    //!     // left identity
    //!     or_(a, f)  == a
    //!     and_(a, t) == a
    //!
    //!     // distributivity
    //!     or_(a, and_(b, c)) == and_(or_(a, b), or_(a, c))
    //!     and_(a, or_(b, c)) == or_(and_(a, b), and_(a, c))
    //!
    //!     // complements
    //!     or_(a, not_(a))  is true-valued
    //!     and_(a, not_(a)) is false-valued
    //! @endcode
    //!
    //! > #### Why is the above not a boolean algebra?
    //! > If you look closely, you will find that we depart from the usual
    //! > boolean algebras because:
    //! > 1. we do not require the elements representing truth and falsity to
    //! >    be unique
    //! > 2. we do not enforce commutativity of the `and_` and `or_` operations
    //! > 3. because we do not enforce commutativity, the identity laws become
    //! >    left-identity laws
    //!
    //!
    //! Minimal complete definition
    //! ---------------------------
    //! 1. `eval_if`, `not_` and `while_`\n
    //! @todo
    //!
    //!
    //! Provided models
    //! ---------------
    //! 1. For arithmetic data types\n
    //! A data type `T` is arithmetic if `std::is_arithmetic<T>::value` is
    //! true. For an arithmetic data type `T`, a model of `Logical` is
    //! provided automatically by using the result of the builtin implicit
    //! conversion to `bool` as a truth value. Specifically, the minimal
    //! complete definition for those data types is
    //! @code
    //!     eval_if(cond, then, else_) = cond ? then(id) : else(id)
    //!     not_(cond) = static_cast<T>(cond ? false : true)
    //!     while_(pred, state, f) = equivalent to a normal while loop
    //! @endcode
    //!
    //! > #### Rationale for not providing a model for all contextually convertible to bool data types
    //! > The `not_` method can not be implemented in a meaningful way for all
    //! > of those types. For example, one can not cast a pointer type `T*`
    //! > to bool and then back again to `T*` in a meaningful way. With an
    //! > arithmetic type `T`, however, it is possible to cast from `T` to
    //! > bool and then to `T` again; the result will be `0` or `1` depending
    //! > on the truth value. If you want to use a pointer type or something
    //! > similar in a conditional, it is suggested to explicitly convert it
    //! > to bool by using `to<bool>`.
    //!
    //!
    //! Operators
    //! ---------
    //! For convenience, the following operators are provided as an
    //! equivalent way of calling the corresponding method:
    //! @code
    //!     &&  ->  and_
    //!     ||  ->  or_
    //! @endcode
    //!
    //!
    //! @bug
    //! We can't use perfect forwarding in that MCD because of
    //! [this bug](http://llvm.org/bugs/show_bug.cgi?id=20619).
    //!
    //! @todo
    //! The methods don't short-circuit right now, which is a real bummer.
    //!
    //!
    //! [1]: http://en.wikipedia.org/wiki/Boolean_algebra_(structure)
    struct Logical { };

    //! Conditionally return one of two values based on a condition.
    //! @relates Logical
    //!
    //! Specifically, `then` is returned iff `cond` is true-valued, and
    //! `else_` is returned otherwise. Note that some `Logical` models may
    //! allow `then` and `else_` to have different types, while others may
    //! require both values to have the same type.
    //!
    //!
    //! @param cond
    //! The condition determining which of the two values is returned.
    //!
    //! @param then
    //! The value returned when `cond` is true-valued.
    //!
    //! @param else_
    //! The value returned when `cond` is false-valued.
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/logical.cpp if_
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto if_ = [](auto&& cond, auto&& then, auto&& else_) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename L, typename = void>
    struct if_impl;

    struct _if {
        template <typename Cond, typename Then, typename Else>
        constexpr decltype(auto) operator()(Cond&& cond, Then&& then, Else&& else_) const {
#ifdef BOOST_HANA_CONFIG_CHECK_DATA_TYPES
            static_assert(_models<Logical, typename datatype<Cond>::type>{},
            "hana::if_(cond, then, else) requires cond to be a Logical");
#endif
            return if_impl<typename datatype<Cond>::type>::apply(
                detail::std::forward<Cond>(cond),
                detail::std::forward<Then>(then),
                detail::std::forward<Else>(else_)
            );
        }
    };

    constexpr _if if_{};
#endif

    //! Conditionally execute one of two branches based on a condition.
    //! @relates Logical
    //!
    //! Given a condition and two branches in the form of lambdas, `eval_if`
    //! will evaluate the branch selected by the condition and return the
    //! result. But that's not all; the lambdas must accept a parameter
    //! (usually called `_`), which can be used to defer the compile-time
    //! evaluation of expressions as required. Here's an example:
    //! @code
    //!     template <typename N>
    //!     auto fact(N n) {
    //!         return hana::eval_if(n == hana::int_<0>,
    //!             [](auto _) { return hana::int_<1>; },
    //!             [=](auto _) { return n * fact(_(n) - hana::int_<1>); }
    //!         );
    //!     }
    //! @endcode
    //!
    //! What happens here is that `eval_if` will pass an identity function to
    //! the selected branch. Hence, `_(x)` is always the same as `x`, but the
    //! compiler can't tell until the lambda has been called! Hence, the
    //! compiler has to wait before it instantiates the body of the lambda
    //! and no infinite recursion happens. However, this trick to delay the
    //! instantiation of the lambda's body can only be used when the condition
    //! is known at compile-time, because otherwise both branches have to be
    //! instantiated inside the `eval_if` anyway. Also note that `always` can
    //! be used to make `eval_if` easier to work with:
    //! @code
    //!     template <typename N>
    //!     auto fact(N n) {
    //!         return hana::eval_if(n == hana::int_<0>,
    //!             always(hana::int_<1>),
    //!             [=](auto _) { return n * fact(_(n) - hana::int_<1>); }
    //!         );
    //!     }
    //! @endcode
    //!
    //! There are several caveats to note with our approach to lazy branching.
    //! First, because we're using lambdas, it means that the function's
    //! result can't be used in a constant expression. This is a limitation
    //! of the current version of C++.
    //!
    //! The second caveat is that compilers currently have several bugs
    //! regarding deeply nested lambdas with captures. So you always risk
    //! crashing the compiler, but this is a question of time before it is
    //! not a problem anymore.
    //!
    //! Finally, it means that conditionals can't be written directly inside
    //! unevaluated contexts. The reason is that a lambda can't appear in an
    //! unevaluated context, for example in `decltype`. One way to workaround
    //! this is to completely lift your type computations into variable
    //! templates instead. So instead of writing e.g. (stupid example, just
    //! to show):
    //! @code
    //!     template <typename T>
    //!     struct f : decltype(eval_if(true_,
    //!             [](auto _) { return type<T>; },
    //!             [](auto _) { return type<T>; }
    //!         ))
    //!     { };
    //! @endcode
    //!
    //! you could instead write
    //!
    //! @code
    //!     template <typename T>
    //!     auto f_impl(_type<T> t) {
    //!         return eval_if(true_,
    //!             [](auto) { return type<T>; },
    //!             [](auto) { return type<T>; }
    //!         );
    //!     }
    //!
    //!     template <typename T>
    //!     using f = decltype(f_impl(type<T>));
    //! @endcode
    //!
    //! Now, this hoop-jumping only has to be done in one place, because
    //! you should use normal function notation everywhere else in your
    //! metaprogram to perform type computations. So the syntactic
    //! cost is amortized over the whole program.
    //!
    //!
    //! @param cond
    //! The condition determining which of the two branches is selected.
    //!
    //! @param then
    //! A function called as `then([](auto x) { return x; })` if `cond` is
    //! true-valued.
    //!
    //! @param else_
    //! A function called as `else_([](auto x) { return x; })` if `cond` is
    //! false-valued.
    //!
    //!
    //! Example (purely compile-time condition)
    //! ---------------------------------------
    //! @snippet example/logical.cpp heterogeneous_eval_if
    //!
    //! Example (runtime or `constexpr` condition)
    //! ------------------------------------------
    //! @snippet example/logical.cpp homogeneous_eval_if
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto eval_if = [](auto&& cond, auto&& then, auto&& else_) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename L, typename = void>
    struct eval_if_impl;

    struct _eval_if {
        template <typename Cond, typename Then, typename Else>
        constexpr decltype(auto) operator()(Cond&& cond, Then&& then, Else&& else_) const {
#ifdef BOOST_HANA_CONFIG_CHECK_DATA_TYPES
            static_assert(_models<Logical, typename datatype<Cond>::type>{},
            "hana::eval_if(cond, then, else) requires cond to be a Logical");
#endif
            return eval_if_impl<typename datatype<Cond>::type>::apply(
                detail::std::forward<Cond>(cond),
                detail::std::forward<Then>(then),
                detail::std::forward<Else>(else_)
            );
        }
    };

    constexpr _eval_if eval_if{};
#endif

    //! Apply a function to an initial state while some predicate is satisfied.
    //! @relates Logical
    //!
    //! This method is a natural extension of the `while` language construct
    //! to manipulate a state whose type may change from one iteration to
    //! another. However, note that having a state whose type changes from
    //! one iteration to the other is only possible as long as the predicate
    //! returns a `Logical` whose truth value is known at compile-time.
    //!
    //! Specifically, `while_(pred, state, f)` is equivalent to
    //! @code
    //!     f(...f(f(state)))
    //! @endcode
    //! where `f` is iterated as long as `pred(f(...))` is a true-valued
    //! `Logical`.
    //!
    //!
    //! @param pred
    //! A predicate called on the state or on the result of applying `f` a
    //! certain number of times to the state, and returning whether `f`
    //! should be applied one more time.
    //!
    //! @param state
    //! The initial state on which `f` is applied.
    //!
    //! @param f
    //! A function that is iterated on the initial state. Note that the
    //! return type of `f` may change from one iteration to the other,
    //! but only while `pred` returns a compile-time `Logical`. In other
    //! words, `decltype(f(stateN))` may differ from `decltype(f(stateN+1))`,
    //! but only if `pred(f(stateN))` returns a compile-time `Logical`.
    //!
    //!
    //! Example (purely compile-time condition)
    //! ---------------------------------------
    //! @snippet example/logical.cpp heterogeneous_while
    //!
    //! Example (runtime or `constexpr` condition)
    //! ------------------------------------------
    //! @snippet example/logical.cpp homogeneous_while
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto while_ = [](auto&& pred, auto&& state, auto&& f) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename L, typename = void>
    struct while_impl;

    struct _while {
        template <typename Pred, typename State, typename F>
        constexpr decltype(auto) operator()(Pred&& pred, State&& state, F&& f) const {
            using Cond = decltype(pred(state));

#ifdef BOOST_HANA_CONFIG_CHECK_DATA_TYPES
            static_assert(_models<Logical, typename datatype<Cond>::type>{},
            "hana::while_(pred, state, f) requires pred(state) to be a Logical");
#endif
            return while_impl<typename datatype<Cond>::type>::apply(
                detail::std::forward<Pred>(pred),
                detail::std::forward<State>(state),
                detail::std::forward<F>(f)
            );
        }
    };

    constexpr _while while_{};
#endif

    //! Apply a function to an initial state until some predicate is satisfied.
    //! @relates Logical
    //!
    //! Specifically, `until(pred, state, f)` is equivalent to
    //! @code
    //!     f(...f(f(state)))
    //! @endcode
    //! where `f` is iterated until `pred(f(...))` is a true-valued `Logical`.
    //!
    //!
    //! @param pred
    //! A predicate called on the state or on the result of applying `f` a
    //! certain number of times to the state, and returning whether `f`
    //! should stop being applied.
    //!
    //! @param state
    //! The initial state on which `f` is applied.
    //!
    //! @param f
    //! A function that is iterated on the initial state. Note that the
    //! return type of `f` may change from one iteration to the other,
    //! but only while `pred` returns a compile-time `Logical`. In other
    //! words, `decltype(f(stateN))` may differ from `decltype(f(stateN+1))`,
    //! but only if `pred(f(stateN))` returns a compile-time `Logical`.
    //!
    //!
    //! Example (purely compile-time condition)
    //! ---------------------------------------
    //! @snippet example/logical.cpp heterogeneous_until
    //!
    //! Example (runtime or `constexpr` condition)
    //! ------------------------------------------
    //! @snippet example/logical.cpp homogeneous_until
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto until = [](auto&& pred, auto&& state, auto&& f) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename L, typename = void>
    struct until_impl;

    struct _until {
        template <typename Pred, typename State, typename F>
        constexpr decltype(auto) operator()(Pred&& pred, State&& state, F&& f) const {
            using Cond = decltype(pred(state));

#ifdef BOOST_HANA_CONFIG_CHECK_DATA_TYPES
            static_assert(_models<Logical, typename datatype<Cond>::type>{},
            "hana::until(pred, state, f) requires pred(state) to be a Logical");
#endif
            return until_impl<typename datatype<Cond>::type>::apply(
                detail::std::forward<Pred>(pred),
                detail::std::forward<State>(state),
                detail::std::forward<F>(f)
            );
        }
    };

    constexpr _until until{};
#endif

    //! Negates a `Logical`.
    //! @relates Logical
    //!
    //! This method returns a `Logical` of the same data type, but whose
    //! truth-value is negated. Specifically, `not_(x)` returns a false-valued
    //! `Logical` if `x` is a true-valued `Logical`, and a true-valued one
    //! otherwise.
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/logical.cpp not_
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto not_ = [](auto&& x) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename L, typename = void>
    struct not_impl;

    struct _not {
        template <typename X>
        constexpr decltype(auto) operator()(X&& x) const {
            return not_impl<typename datatype<X>::type>::apply(
                detail::std::forward<X>(x)
            );
        }
    };

    constexpr _not not_{};
#endif

    //! Return whether all the arguments are true-valued.
    //! @relates Logical
    //!
    //! `and_` can be called with one argument or more. When called with
    //! two arguments, `and_` uses tag-dispatching to find the right
    //! implementation. Otherwise,
    //! @code
    //!     and_(x) == x
    //!     and_(x, y, ...z) == and_(and_(x, y), z...)
    //! @endcode
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/logical.cpp and_
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto and_ = [](auto&& x, auto&& ...y) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename L, typename = void>
    struct and_impl;

    struct _and {
        template <typename X, typename Y>
        constexpr decltype(auto) operator()(X&& x, Y&& y) const;

        template <typename X, typename ...Y>
        constexpr decltype(auto) operator()(X&& x, Y&& ...y) const;
    };

    constexpr _and and_{};
#endif

    //! Return whether any of the arguments is true-valued.
    //! @relates Logical
    //!
    //! `or_` can be called with one argument or more. When called with
    //! two arguments, `or_` uses tag-dispatching to find the right
    //! implementation. Otherwise,
    //! @code
    //!     or_(x) == x
    //!     or_(x, y, ...z) == or_(or_(x, y), z...)
    //! @endcode
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/logical.cpp or_
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto or_ = [](auto&& x, auto&& ...y) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename L, typename = void>
    struct or_impl;

    struct _or {
        template <typename X, typename Y>
        constexpr decltype(auto) operator()(X&& x, Y&& y) const;

        template <typename X, typename ...Y>
        constexpr decltype(auto) operator()(X&& x, Y&& ...y) const;
    };

    constexpr _or or_{};
#endif

    template <>
    struct operators::of<Logical>
        : decltype(and_), decltype(or_), decltype(not_)
    { };
}} // end namespace boost::hana

#endif // !BOOST_HANA_FWD_LOGICAL_HPP


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

* references:
*     [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange
*         Format Version 1, February 25, 1998
*     [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere
*         Maps based on GPS Carrier Phase Data Routinely producted by CODE
*         Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996
*-----------------------------------------------------------------------------*/
#include <boost/log/trivial.hpp>

#include "corrections.hpp"
#include "navigation.hpp"
#include "acsConfig.hpp"
#include "constants.hpp"
#include "biasSINEX.hpp"
#include "common.hpp"

#define VAR_NOTEC   SQR(30.0)   /* variance of no tec */
#define MIN_EL      0.0         /* min elevation angle (rad) */
#define MIN_HGT     -1000.0     /* min user height (m) */

/* get index -----------------------------------------------------------------*/
int getindex(
	double value,
	const double* range)
{
	if (range[2] == 0)												return 0;
	if (range[1] > 0 && (value < range[0] || range[1] < value))		return -1;
	if (range[1] < 0 && (value < range[1] || range[0] < value))		return -1;

	return (int) floor((value - range[0]) / range[2] + 0.5);
}

/* get number of items -------------------------------------------------------*/
int nitem(
	const double* range)
{
	return getindex(range[1], range) + 1;
}

/* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/
int dataindex(
	int i,
	int j,
	int k,
	const int* ndata)
{
	if	(  i < 0 || ndata[0] <= i
		|| j < 0 || ndata[1] <= j
		|| k < 0 || ndata[2] <= k)
	{
		return -1;
	}

	return i + ndata[0] * (j + ndata[1] * k);
}

/* read ionex dcb aux data ----------------------------------------------------*/
void readionexdcb(
	std::ifstream& in,
	nav_t* navi)
{
	char buff[1024];
	SinexBias entry;
	bool refObs = false;

	entry.tini.sec	= 2;
	entry.measType	= CODE;
	entry.source	= "ionex";

	BOOST_LOG_TRIVIAL(debug)
	<< "readionexdcb:";

	string line;
	while (std::getline(in, line))
	{
		char* buff = &line[0];

		if (strlen(buff) < 60)
			continue;

		char* label = buff + 60;

		SatSys Sat;

		string id;
		if	( strstr(label,	"COMMENT") == label
			&&strstr(buff,	"Reference observables"))
		{
			char* ptr = strchr(buff, ':');
			if (ptr != NULL)
			{
				string cod1str(ptr + 2, 3);
				string cod2str(ptr + 6, 3);

				E_MeasType dummy1;
				double dummy2 = 0;
				entry.cod1 = str2code(cod1str, dummy1, dummy2);
				entry.cod2 = str2code(cod2str, dummy1, dummy2);

				refObs = true;
			}

			continue;
		}
		else if (strstr(label, "PRN / BIAS / RMS") == label)
		{
			string sat(buff + 3,  3);
			Sat = SatSys(sat.c_str());
			entry.Sat  = Sat;
			entry.name = "";
			id = sat;

			if (!refObs)
			{
				if (Sat.sys == +E_Sys::GPS)
				{
					entry.cod1 = E_ObsCode::L1W;
					entry.cod2 = E_ObsCode::L2W;
				}
				else if (Sat.sys == +E_Sys::GLO)
				{
					entry.cod1 = E_ObsCode::L1P;
					entry.cod2 = E_ObsCode::L2P;
				}
				else
				{
					BOOST_LOG_TRIVIAL(debug)
					<< "ionex invalid satellite: " << id;

					continue;
				}
			}

			if (Sat)
			{
				entry.bias =     str2num(buff,  6, 10) * CLIGHT * 1E-9;
				entry.var  = SQR(str2num(buff, 16, 10) * CLIGHT * 1E-9);

				BOOST_LOG_TRIVIAL(debug)
				<< id << entry.bias;
			}
			else
			{
				BOOST_LOG_TRIVIAL(debug)
				<< "ionex invalid satellite: " << id;

				continue;
			}

			//fallthrough to after the ifs
		}
		else if (strstr(label, "STATION / BIAS / RMS") == label)
		{
			string sys (buff + 3, 1);
			string name(buff + 6, 4);
			Sat = SatSys(sys.c_str());
			entry.Sat  = Sat;
			entry.name = name;
			id = name;

			if (!refObs)
			{
				if (Sat.sys == +E_Sys::GPS)
				{
					entry.cod1 = E_ObsCode::L1W;
					entry.cod2 = E_ObsCode::L2W;
				}
				else if (Sat.sys == +E_Sys::GLO)
				{
					entry.cod1 = E_ObsCode::L1P;
					entry.cod2 = E_ObsCode::L2P;
				}
				else
				{
					BOOST_LOG_TRIVIAL(debug)
					<< "ionex invalid satellite system: " << sys;

					continue;
				}
			}

			if (Sat)
			{
				entry.bias =     str2num(buff, 26, 10) * CLIGHT * 1E-9;
				entry.var  = SQR(str2num(buff, 36, 10) * CLIGHT * 1E-9);

				BOOST_LOG_TRIVIAL(debug)
				<< id << entry.bias;
			}
			else
			{
				BOOST_LOG_TRIVIAL(debug)
				<< "ionex invalid station: " << id;

				continue;
			}

			//fallthrough to after the ifs
		}
		else if (strstr(label, "END OF AUX DATA") == label)
			break;
		else
			continue;

		entry.name = id;

		if	( Sat.sys == +E_Sys::GLO
			&&Sat.prn == 0)
		{
			// this seems to be a receiver
			// for ambiguous GLO receiver bias id (i.e. PRN not specified), duplicate bias entry for each satellite
			for (int prn = MINPRNGLO; prn <= MAXPRNGLO; prn++)
			{
				Sat.prn	= prn;
				id = entry.name + ":" + Sat.id();
				// entry.Sat = Sat;
				pushBiasSinex(id, entry);
			}
		}
		else if	( Sat.sys == +E_Sys::GLO
				&&Sat.prn != 0)
		{
			// this can be a receiver or satellite
			id = id + ":" + Sat.id();
			pushBiasSinex(id, entry);
		}
		else
		{
			// this can be a receiver or satellite
			id = id + ":" + Sat.sysChar();
			pushBiasSinex(id, entry);
		}
	}
}

/* read ionex header ---------------------------------------------------------*/
double readionexh(
	std::ifstream& in,
	double* lats,
	double* lons,
	double* hgts,
	double& rb,
	double& nexp,
	nav_t* navi)
{
	double ver = 0;

	BOOST_LOG_TRIVIAL(debug)
	<< "readionexh:";

	string line;
	while (std::getline(in, line))
	{
		char* buff = &line[0];

		if (strlen(buff) < 60)
			continue;

		char* label = buff + 60;

		if (strstr(label, "IONEX VERSION / TYPE") == label)
		{
			if (buff[20] == 'I')
				ver = str2num(buff, 0, 8);

			BOOST_LOG_TRIVIAL(debug)
			<< " ver= " << ver;
		}
		else if (strstr(label, "BASE RADIUS") == label)
		{
			rb = str2num(buff, 0, 8);

			BOOST_LOG_TRIVIAL(debug)
			<< " rad= " << rb;
		}
		else if (strstr(label, "HGT1 / HGT2 / DHGT") == label)
		{
			hgts[0] = str2num(buff, 2, 6);
			hgts[1] = str2num(buff, 8, 6);
			hgts[2] = str2num(buff, 14, 6);

			BOOST_LOG_TRIVIAL(debug)
			<< " heights= " << hgts[0] << " " << hgts[1] << " " << hgts[2];
		}
		else if (strstr(label, "LAT1 / LAT2 / DLAT") == label)
		{
			lats[0] = str2num(buff, 2, 6);
			lats[1] = str2num(buff, 8, 6);
			lats[2] = str2num(buff, 14, 6);

			BOOST_LOG_TRIVIAL(debug)
			<< " lats= " << lats[0] << " " << lats[1] << " " << lats[2];
		}
		else if (strstr(label, "LON1 / LON2 / DLON") == label)
		{
			lons[0] = str2num(buff, 2,  6);
			lons[1] = str2num(buff, 8,  6);
			lons[2] = str2num(buff, 14, 6);

			BOOST_LOG_TRIVIAL(debug)
			<< " lons= " << lons[0] << " " << lons[1] << " " << lons[2];
		}
		else if ( strstr(label, "EXPONENT") == label)
		{
			nexp = str2num(buff, 0, 6);
		}
		else if	( strstr(label,	"START OF AUX DATA") == label
				&&strstr(buff,	"DIFFERENTIAL CODE BIASES"))
		{
			readionexdcb(in, navi);
		}
		else if (strstr(label, "END OF HEADER") == label)
		{
			return ver;
		}
	}

	return 0;
}

/* read ionex body -----------------------------------------------------------*/
int readionexb(
	std::ifstream& in,
	const double* lats,
	const double* lons,
	const double* hgts,
	double rb,
	double nexp,
	nav_t* navi)
{
	GTime time = {};
	int type = 0;

	// if (fdebug)
	// 	fprintf(fdebug, "readionexb:\n");

	string line;
	while (std::getline(in, line))
	{
		char* buff = &line[0];
		char* label = buff + 60;

		if (strlen(buff) < 60)
			continue;

		if 		(strstr(label, "START OF TEC MAP")		== label)
		{
			type = 1;
			time.time = 0;

		}
		else if (strstr(label, "END OF TEC MAP")		== label)
		{
			// if (fdebug)
			// 	fprintf(fdebug, "%5ld data and %5ld rms entries for %s\n", navi->tecList[time.time].data.size(), navi->tecList[time.time].rms.size(), time.to_string(0).c_str());

			type = 0;
		}
		else if (strstr(label, "START OF RMS MAP")		== label)
		{
			type = 2;
			time.time = 0;
		}
		else if (strstr(label, "END OF RMS MAP")		== label)
		{
			// if (fdebug)
			// 	fprintf(fdebug, "%5ld data and %5ld rms entries for %s\n", navi->tecList[time.time].data.size(), navi->tecList[time.time].rms.size(), time.to_string(0).c_str());

			type = 0;
		}
		else if (strstr(label, "EPOCH OF CURRENT MAP")	== label)
		{
			if (str2time(buff, 0, 36, time))
			{
				// fprintf(fdebug, "ionex epoch invalid: %-36.36s\n", buff);
				continue;
			}

			auto& epochTec = navi->tecMap[time];

			if (type == 1)
			{
				epochTec.time		= time;
				epochTec.ndata[0]	= nitem(lats);
				epochTec.ndata[1]	= nitem(lons);
				epochTec.ndata[2]	= nitem(hgts);
				epochTec.rb			= rb;

				for (int i = 0; i < 3; i++)
				{
					epochTec.lats[i] = lats[i];
					epochTec.lons[i] = lons[i];
					epochTec.hgts[i] = hgts[i];
				}

				epochTec.tecPointVector.resize(epochTec.ndata[0] * epochTec.ndata[1] * epochTec.ndata[2]);

				std::fill(epochTec.tecPointVector.begin(), epochTec.tecPointVector.end(), TECPoint{});
			}
		}
		else if	( strstr(label, "LAT/LON1/LON2/DLON/H")	== label
				&& time.time
				&& type)
		{
			double lon[3];
			double	lat		= str2num(buff, 2,  6);
					lon[0]	= str2num(buff, 8,  6);
					lon[1]	= str2num(buff, 14, 6);
					lon[2]	= str2num(buff, 20, 6);
			double	hgt		= str2num(buff, 26, 6);

			int i = getindex(lat, lats);
			int k = getindex(hgt, hgts);
			int n = nitem(lon);

			auto& epochTec = navi->tecMap[time];

			for (int m = 0; m < n; m++)
			{
				if	(  m % 16 == 0
					&& !std::getline(in, line))
					break;

				buff = &line[0];

				int j = getindex(lon[0] + lon[2] * m, lons);

				int index = dataindex(i, j, k, epochTec.ndata);
				if (index  < 0)
					continue;

				double x = str2num(buff, m % 16 * 5, 5);
				if (x == 9999)
					continue;

				if (type == 1)		epochTec.tecPointVector[index].data	= x * pow(10, nexp);
				if (type == 2)		epochTec.tecPointVector[index].rms	= x * pow(10, nexp);
			}
		}
	}

	return 1;
}

/* read ionex tec grid file ----------------------------------------------------
* read ionex ionospheric tec grid file
* args   : char   *file       I   ionex tec grid file
*                                 (wind-card * is expanded)
*          nav_t  *nav        IO  navigation data
*                                 nav->nt, nav->ntmax and nav->tec are modified
* notes  : see ref [1]
*-----------------------------------------------------------------------------*/
void readtec(
	string file,
	nav_t* navi)
{
	BOOST_LOG_TRIVIAL(debug)
	<< "readtec : file=" << file;

	std::ifstream inputStream(file);
	if (!inputStream)
	{
		BOOST_LOG_TRIVIAL(error)
		<< "ionex file open error " << file;

		return;
	}

	/* read ionex header */
	double nexp = -1;
	double rb	= 0;
	double lats[3] = {};
	double lons[3] = {};
	double hgts[3] = {};
	double version = readionexh(inputStream, lats, lons, hgts, rb, nexp, navi);
	if (version <= 0)
	{
		BOOST_LOG_TRIVIAL(error)
		<< "ionex file format error " << file;

		return;
	}

	/* read ionex body */
	readionexb(inputStream, lats, lons, hgts, rb, nexp, navi);
}

/* interpolate tec grid data -------------------------------------------------*/
int interptec(
	const tec_t& tec,
	int k,
	const double* posp,
	double& value,
	double& rms)
{
	// if (fdebug)
	// 	fprintf(fdebug, "%s: k=%d posp=%.2f %.2f\n",__FUNCTION__, k, posp[0]*R2D, posp[1]*R2D);

	value	= 0;
	rms		= 0;

	if	( tec.lats[2] == 0
		||tec.lons[2] == 0)
	{
		return 0;
	}

	double dlat = posp[0] * R2D - tec.lats[0];
	double dlon = posp[1] * R2D - tec.lons[0];

	if (tec.lons[2] > 0)	dlon -= floor( dlon / 360) * 360; /*  0<=dlon<360 */
	else					dlon += floor(-dlon / 360) * 360; /* -360<dlon<=0 */

	double a = dlat / tec.lats[2];
	double b = dlon / tec.lons[2];
	int i = (int) floor(a); 			a -= i;
	int j = (int) floor(b); 			b -= j;

	/* get gridded tec data */
	double d[4] = {};
	double r[4] = {};
	for (int n = 0; n < 4; n++)
	{
		int index = dataindex(i + (n % 2), j + (n < 2 ? 0 : 1), k, tec.ndata);
		if (index < 0)
			continue;

		auto& tecPoint = tec.tecPointVector[index];

		d[n] = tecPoint.data;
		r[n] = tecPoint.rms;
	}

	if	(  d[0] > 0
		&& d[1] > 0
		&& d[2] > 0
		&& d[3] > 0)
	{
		/* bilinear interpolation (inside of grid) */
		value	= (1 - a) * (1 - b) * d[0] 		+ a * (1 - b) * d[1] 		+ (1 - a) * b * d[2] 		+ a * b * d[3];
		rms		= (1 - a) * (1 - b) * r[0] 		+ a * (1 - b) * r[1] 		+ (1 - a) * b * r[2] 		+ a * b * r[3];

		// if (fdebug)
		// 	fprintf(fdebug, "  gridpoints: %8.2f %8.2f %8.2f %8.2f -> %9.3f\n", d[0], d[1], d[2], d[3], value);
	}
	/* nearest-neighbour extrapolation (outside of grid) */
	else if (a <=	0.5 && b <= 0.5 && d[0] > 0) 	{	value = d[0];		rms = r[0];		}
	else if (a >	0.5 && b <= 0.5 && d[1] > 0) 	{	value = d[1];		rms = r[1];		}
	else if (a <=	0.5 && b >	0.5 && d[2] > 0) 	{	value = d[2];		rms = r[2];		}
	else if (a >	0.5 && b >	0.5 && d[3] > 0) 	{	value = d[3];		rms = r[3];		}
	else
	{
		i = 0;

		for (int n = 0; n < 4; n++)
		if (d[n] > 0)
		{
			i++;
			value	+= d[n];
			rms		+= r[n];
		}

		if (i == 0)
			return 0;

		value	/= i;
		rms		/= i;
	}

	return 1;
}

/* ionosphere delay by tec grid data -----------------------------------------*/
int iondelay(
	GTime time,
	const tec_t& tec,
	const double* pos,
	const double* azel,
	int opt,
	double& delay,
	double& var)
{
	// if (fdebug)
	// 	fprintf(fdebug, "%s: time=%s pos=%.1f %.1f azel=%.1f %.1f\n", __FUNCTION__, time.to_string(0).c_str(), pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D);

	delay	= 0;
	var		= 0;

	for (int i = 0; i < tec.ndata[2]; i++)
	{
		double hion = tec.hgts[0] + tec.hgts[2] * i;

		/* ionospheric pierce point position */
		double posp[3] = {};
		double fs = ionppp(pos, azel, tec.rb, hion, posp);

		if (opt & 2)
		{
			/* modified single layer mapping function (M-SLM) ref [2] */
			double rp = tec.rb / (tec.rb + hion) * sin(0.9782 * (PI / 2 - azel[1]));
			fs = 1 / sqrt(1 - rp * rp);
		}

		if (opt & 1)
		{
			/* earth rotation correction (sun-fixed coordinate) */
			posp[1] += 2 * PI * (time - tec.time) / 86400;
		}

		/* interpolate tec grid data */
		double rms;
		double vtec;
		if (!interptec(tec, i, posp, vtec, rms))
			return 0;

		const double fact = 40.30E16 / FREQ1 / FREQ1; /* tecu->L1 iono (m) */
		delay	+= fact * fs * vtec;
		var		+= SQR(fact * fs * rms);
	}

	// if (fdebug)
	// 	fprintf(fdebug, "%s: delay=%7.2f std=%6.2f\n",__FUNCTION__, delay, sqrt(var));

	return 1;
}

/* ionosphere model by tec grid data -------------------------------------------
* compute ionospheric delay by tec grid data
* args   : gtime_t time     I   time (gpst)
*          nav_t  *nav      I   navigation data
*          double *pos      I   receiver position {lat,lon,h} (rad,m)
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
*          int    opt       I   model option
*                                bit0: 0:earth-fixed,1:sun-fixed
*                                bit1: 0:single-layer,1:modified single-layer
*          double *delay    O   ionospheric delay (L1) (m)
*          double *var      O   ionospheric dealy (L1) variance (m^2)
* return : status (1:ok,0:error)
* notes  : before calling the function, read tec grid data by calling readtec()
*          return ok with delay=0 and var=VAR_NOTEC if el<MIN_EL or h<MIN_HGT
*-----------------------------------------------------------------------------*/
int iontec(
	GTime time,
	const nav_t*	nav,
	const double*	pos,
	const double*	azel,
	int				opt,
	double&			delay,
	double&			var)
{
	// if (fdebug)
	// 	fprintf(fdebug, "iontec  : time=%s pos=%.1f %.1f azel=%.1f %.1f nt=%ld\n", time.to_string(0).c_str(), pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D, nav->tecList.size());

	delay	= 0;
	var		= VAR_NOTEC;

	if	(  azel[1]	< MIN_EL
		|| pos[2]	< MIN_HGT)
	{
		return 1;
	}

	auto it = nav->tecMap.lower_bound(time);
	if (it == nav->tecMap.end())
	{
		// if (fdebug)
		// 	fprintf(fdebug, "%s: tec grid out of period\n", time.to_string(0).c_str());

		return 1;
	}

	int stat[2] = {};
	double dels[2];
	double vars[2];

	auto& [t0, tec0] = *it;
	stat[0] = iondelay(time, tec0, pos, azel, opt, dels[0], vars[0]);

	if (it == nav->tecMap.begin())
	{
		delay	= dels[0];
		var		= vars[0];
		return stat[0];
	}

	//go forward and get the next timestep if available
	it--;

	auto& [t1, tec1] = *it;
	stat[1] = iondelay(time, tec1, pos, azel, opt, dels[1], vars[1]);

	if	(  stat[0]
		&& stat[1])
	{
		/* linear interpolation by time */
		double tt	= (tec1.time	- tec0.time);
		double a	= (time			- tec0.time) / tt;

		delay	= dels[0] * (1 - a) + dels[1] * a;
		var		= vars[0] * (1 - a) + vars[1] * a;
	}
	else if (stat[0])   /* nearest-neighbour extrapolation by time */
	{
		delay	= dels[0];
		var		= vars[0];
	}
	else if (stat[1])
	{
		delay	= dels[1];
		var		= vars[1];
	}
	else
	{
		// if (fdebug)
		// 	fprintf(fdebug, "%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n",  time.to_string(0).c_str(), pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D);

		return 0;
	}

	// if (fdebug)
	// 	fprintf(fdebug, "iontec  : delay=%5.2f std=%5.2f\n", delay, sqrt(var));

	return 1;
}


/** \file utils.hpp
 * collection of general utilities
 */

#ifndef UTILS_HPP
#define UTILS_HPP

#include <iostream>
#include <fstream>
#include <string>
#include <cassert>
#include <bitset>
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

#define default_buckets boost::unordered::detail::default_bucket_count

#ifndef M_PI
#define M_PI 3.14159265359f
#endif
#define byte unsigned char
#define SIZE_T_BITS (unsigned)(8*sizeof(size_t))
// avoid the clutter for map-emplaces where both constructors take just one argument
#define DEEP_EMPLACE(x,y) emplace(std::piecewise_construct, std::make_tuple(x), std::make_tuple(y))
#define DEEP_EMPLACE_TWO(x,y,z) emplace(std::piecewise_construct, std::make_tuple(x), std::make_tuple(y, z))
// avoid the clutter for map-emplaces where the target is default constructed
#define DEFAULT_EMPLACE(x) emplace(std::piecewise_construct, std::make_tuple(x), std::make_tuple())
// merge two lists
#define SPLICE_LISTS(x,y) x.splice(x.end(), y)

#ifdef LONG_WIDTH
  #define MAX_TW (byte)LONG_WIDTH
#else
  #define MAX_TW (byte)64
#endif

// on debuglevel 3 all DEBUG1, DEBUG2, and DEBUG3 statements are evaluated
#ifndef NDEBUG
  #ifndef debuglevel
    #define debuglevel 5
  #endif
#else
  #define debuglevel 0
#endif

#if debuglevel > 0
#define DEBUG1(x) x
#else
#define DEBUG1(x)
#endif

#if debuglevel > 1
#define DEBUG2(x) x
#else
#define DEBUG2(x)
#endif

#if debuglevel > 2
#define DEBUG3(x) x
#else
#define DEBUG3(x)
#endif

#if debuglevel > 3
#define DEBUG4(x) x
#else
#define DEBUG4(x)
#endif

#if debuglevel > 4
#define DEBUG5(x) x
#else
#define DEBUG5(x)
#endif

#if debuglevel > 5
#define DEBUG6(x) x
#else
#define DEBUG6(x)
#endif

#ifdef STATISTICS
#define STAT(x) x
#else
#define STAT(x)
#endif

// inverse error function (approximative)
// thx to
// https://stackoverflow.com/questions/27229371/inverse-error-function-in-c
// and
// A handy approximation for the error function and its inverse" by Sergei Winitzki.
float erf_inv_apx(const float& p)
{
  const float sgn = (p < 0) ? -1.0f : 1.0f;

  // lnx = log(1 - x*x)
  const float lnx = logf((1 - p)*(1 + p));

  const float tt1 = 2/(M_PI*0.147) + 0.5f * lnx;
  const float tt2 = 1/(0.147) * lnx;

  return sgn*sqrtf(-tt1 + sqrtf(tt1*tt1 - tt2));
}

//! return whether a pair is pareto-smaller than another pair
template <typename ElementA, typename ElementB>
bool pareto_le(const std::pair<ElementA,ElementB>& p1, const std::pair<ElementA,ElementB>& p2)
{
  return (p1.first <= p2.first) && (p1.second <= p2.second);
}

//! output list of things
template<typename Element>
std::ostream& operator<<(std::ostream& os, const std::list<Element>& lst)
{
  for(auto i : lst) os << i << "  ";
  return os;
}
//! output map of things
template<typename Element1, typename Element2>
std::ostream& operator<<(std::ostream& os, const boost::unordered_map<Element1, Element2>& map)
{
  for(auto i : map) os << i << "  ";
  return os;
}

//! a more readable containment check
/** \param s any container object that implements find() and cend()
 * \param el some element to check containment of in s
 */
template <class Set, typename Element>
inline bool contains(const Set& s, const Element& el)
{
  return s.find(el) != s.cend();
}

//! a hash computation for an unordered set, XORing its members
template<typename T>
size_t hash_value(const boost::unordered_set<T>& S)
{
  size_t result = 0;
  for(const auto& i : S)
    result = (result << 1) ^ hash_value(i);
  return result;
}

//! set intersection for unordered sets
/** removes all elements from S1 that are not in S2 */
template<typename T>
void operator&=(boost::unordered_set<T>& S1, const boost::unordered_set<T>& S2)
{
  for(typename boost::unordered_set<T>::iterator i = S1.begin(); i != S1.end();)
    if(!contains(S2, *i)) i = S1.erase(i); else ++i;
}

//! setminus for unordered sets
/** removes all elements from S1 that are in S2 */
template<typename T>
void operator-=(boost::unordered_set<T>& S1, const boost::unordered_set<T>& S2)
{
  for(typename boost::unordered_set<T>::iterator i = S1.begin(); i != S1.end();)
    if(contains(S2, *i)) i = S1.erase(i); else ++i;
}

//! symmetric set difference for unordered sets
/** removes all elements of S1 that are not in S2 and emplaces copies of all elements of S2 that are not in S1 */
template<typename T>
void operator^=(boost::unordered_set<T>& S1, const boost::unordered_set<T>& S2)
{
  for(typename boost::unordered_set<T>::iterator i = S1.begin(); i != S1.end();)
    if(contains(S2, *i)) i = S1.erase(i); else ++i;
  for(typename boost::unordered_set<T>::iterator i = S2.begin(); i != S2.end(); ++i) S1.emplace(*i);
}

//! testing whether a file exists by trying to open it
inline bool file_exists(const std::string& filename)
{
  return std::ifstream(filename.c_str()).good();
}

//! reverse a pair of things, that is, turn (x,y) into (y,x)
template<typename A, typename B>
inline std::pair<B,A> reverse(const std::pair<A,B>& p)
{
  return {p.second, p.first};
}

//! add two pairs of things
template <typename A, typename B>
std::pair<A,B> operator+(const std::pair<A,B>& l, const std::pair<A,B>& r)
{
	return {l.first + r.first, l.second + r.second};
}

template <typename A, typename B>
std::ostream& operator<<(std::ostream& os, const std::pair<A,B>& p)
{
	return os << "("<<p.first<<", "<<p.second<<")";
}

#endif


#ifndef STAN_MODEL_INDEXING_RVALUE_HPP
#define STAN_MODEL_INDEXING_RVALUE_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <stan/math/prim/mat.hpp>
#include <stan/model/indexing/index.hpp>
#include <stan/model/indexing/index_list.hpp>
#include <stan/model/indexing/rvalue_at.hpp>
#include <stan/model/indexing/rvalue_index_size.hpp>
#include <stan/model/indexing/rvalue_return.hpp>
#include <vector>

namespace stan {

namespace model {

// all indexing from 1

/**
 * Return the result of indexing a specified value with
 * a nil index list, which just returns the value.
 *
 * Types:  T[] : T
 *
 * @tparam T Scalar type.
 * @param[in] c Value to index.
 * @return Input value.
 */
template <typename T>
inline T rvalue(const T& c, const nil_index_list& /*idx*/,
                const char* /*name*/ = "", int /*depth*/ = 0) {
  return c;
}

/**
 * Return the result of indexing the specified Eigen vector with a
 * sequence containing one single index, returning a scalar.
 *
 * Types:  vec[single] : scal
 *
 * @tparam T Scalar type.
 * @param[in] v Vector being indexed.
 * @param[in] idx One single index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing vector.
 */
template <typename T>
inline T rvalue(const Eigen::Matrix<T, Eigen::Dynamic, 1>& v,
                const cons_index_list<index_uni, nil_index_list>& idx,
                const char* name = "ANON", int depth = 0) {
  int ones_idx = idx.head_.n_;
  math::check_range("vector[single] indexing", name, v.size(), ones_idx);
  return v.coeff(ones_idx - 1);
}

/**
 * Return the result of indexing the specified Eigen row vector
 * with a sequence containing one single index, returning a
 * scalar.
 *
 * Types:  rowvec[single] : scal
 *
 * @tparam T Scalar type.
 * @param[in] rv Row vector being indexed.
 * @param[in] idx One single index in list.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing row vector.
 */
template <typename T>
inline T rvalue(const Eigen::Matrix<T, 1, Eigen::Dynamic>& rv,
                const cons_index_list<index_uni, nil_index_list>& idx,
                const char* name = "ANON", int depth = 0) {
  int n = idx.head_.n_;
  math::check_range("row_vector[single] indexing", name, rv.size(), n);
  return rv.coeff(n - 1);
}

/**
 * Return the result of indexing the specified Eigen vector with a
 * sequence containing one multiple index, returning a vector.
 *
 * Types: vec[multiple] : vec
 *
 * @tparam T Scalar type.
 * @tparam I Multi-index type.
 * @param[in] v Eigen vector.
 * @param[in] idx Index consisting of one multi-index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing vector.
 */
template <typename T, typename I>
inline typename boost::disable_if<boost::is_same<I, index_uni>,
                                  Eigen::Matrix<T, Eigen::Dynamic, 1> >::type
rvalue(const Eigen::Matrix<T, Eigen::Dynamic, 1>& v,
       const cons_index_list<I, nil_index_list>& idx, const char* name = "ANON",
       int depth = 0) {
  int size = rvalue_index_size(idx.head_, v.size());
  Eigen::Matrix<T, Eigen::Dynamic, 1> a(size);
  for (int i = 0; i < size; ++i) {
    int n = rvalue_at(i, idx.head_);
    math::check_range("vector[multi] indexing", name, v.size(), n);
    a(i) = v.coeff(n - 1);
  }
  return a;
}

/**
 * Return the result of indexing the specified Eigen row vector
 * with a sequence containing one multiple index, returning a row
 * vector.
 *
 * Types:  row_vec[multiple] : rowvec
 *
 * @tparam T Scalar type.
 * @tparam I Multi-index type.
 * @param[in] rv Eigen row vector.
 * @param[in] idx Index consisting of one multi-index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing vector.
 */
template <typename T, typename I>
inline typename boost::disable_if<boost::is_same<I, index_uni>,
                                  Eigen::Matrix<T, 1, Eigen::Dynamic> >::type
rvalue(const Eigen::Matrix<T, 1, Eigen::Dynamic>& rv,
       const cons_index_list<I, nil_index_list>& idx, const char* name = "ANON",
       int depth = 0) {
  int size = rvalue_index_size(idx.head_, rv.size());
  Eigen::Matrix<T, 1, Eigen::Dynamic> a(size);
  for (int i = 0; i < size; ++i) {
    int n = rvalue_at(i, idx.head_);
    math::check_range("row_vector[multi] indexing", name, rv.size(), n);
    a(i) = rv.coeff(n - 1);
  }
  return a;
}

/**
 * Return the result of indexing the specified Eigen matrix with a
 * sequence consisting of one single index, returning a row vector.
 *
 * Types:  mat[single] : rowvec
 *
 * @tparam T Scalar type.
 * @param[in] a Eigen matrix.
 * @param[in] idx Index consisting of one uni-index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing matrix.
 */
template <typename T>
inline Eigen::Matrix<T, 1, Eigen::Dynamic> rvalue(
    const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& a,
    const cons_index_list<index_uni, nil_index_list>& idx,
    const char* name = "ANON", int depth = 0) {
  int n = idx.head_.n_;
  math::check_range("matrix[uni] indexing", name, a.rows(), n);
  return a.row(n - 1);
}

/**
 * Return the result of indexing the specified Eigen matrix with a
 * sequence consisting of a one multiple index, returning a matrix.
 *
 * Types:  mat[multiple] : mat
 *
 * @tparam T Scalar type.
 * @tparam I Type of multiple index.
 * @param[in] a Matrix to index.
 * @param[in] idx Index consisting of single multiple index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing matrix.
 */
template <typename T, typename I>
inline typename boost::disable_if<
    boost::is_same<I, index_uni>,
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >::type
rvalue(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& a,
       const cons_index_list<I, nil_index_list>& idx, const char* name = "ANON",
       int depth = 0) {
  int n_rows = rvalue_index_size(idx.head_, a.rows());
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b(n_rows, a.cols());
  for (int i = 0; i < n_rows; ++i) {
    int n = rvalue_at(i, idx.head_);
    math::check_range("matrix[multi] indexing", name, a.rows(), n);
    b.row(i) = a.row(n - 1);
  }
  return b;
}

/**
 * Return the result of indexing the specified Eigen matrix with a
 * sequence consisting of two single indexes, returning a scalar.
 *
 * Types:  mat[single,single] : scalar
 *
 * @tparam T Scalar type.
 * @param[in] a Matrix to index.
 * @param[in] idx Pair of single indexes.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing matrix.
 */
template <typename T>
inline T rvalue(
    const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& a,
    const cons_index_list<index_uni,
                          cons_index_list<index_uni, nil_index_list> >& idx,
    const char* name = "ANON", int depth = 0) {
  int m = idx.head_.n_;
  int n = idx.tail_.head_.n_;
  math::check_range("matrix[uni,uni] indexing, row", name, a.rows(), m);
  math::check_range("matrix[uni,uni] indexing, col", name, a.cols(), n);
  return a.coeff(m - 1, n - 1);
}

/**
 * Return the result of indexing the specified Eigen matrix with a
 * sequence consisting of a single index and multiple index,
 * returning a row vector.
 *
 * Types:  mat[single,multiple] : row vector
 *
 * @tparam T Scalar type.
 * @tparam I Type of multiple index.
 * @param[in] a Matrix to index.
 * @param[in] idx Pair of single index and multiple index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing matrix.
 */
template <typename T, typename I>
inline typename boost::disable_if<boost::is_same<I, index_uni>,
                                  Eigen::Matrix<T, 1, Eigen::Dynamic> >::type
rvalue(
    const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& a,
    const cons_index_list<index_uni, cons_index_list<I, nil_index_list> >& idx,
    const char* name = "ANON", int depth = 0) {
  int m = idx.head_.n_;
  math::check_range("matrix[uni,multi] indexing, row", name, a.rows(), m);
  Eigen::Matrix<T, 1, Eigen::Dynamic> r = a.row(m - 1);
  return rvalue(r, idx.tail_);
}

/**
 * Return the result of indexing the specified Eigen matrix with a
 * sequence consisting of a multiple index and a single index,
 * returning a vector.
 *
 * Types:  mat[multiple,single] : vector
 *
 * @tparam T Scalar type.
 * @tparam I Type of multiple index.
 * @param[in] a Matrix to index.
 * @param[in] idx Pair multiple index and single index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing matrix.
 */
template <typename T, typename I>
inline typename boost::disable_if<boost::is_same<I, index_uni>,
                                  Eigen::Matrix<T, Eigen::Dynamic, 1> >::type
rvalue(
    const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& a,
    const cons_index_list<I, cons_index_list<index_uni, nil_index_list> >& idx,
    const char* name = "ANON", int depth = 0) {
  int rows = rvalue_index_size(idx.head_, a.rows());
  Eigen::Matrix<T, Eigen::Dynamic, 1> c(rows);
  for (int i = 0; i < rows; ++i) {
    int m = rvalue_at(i, idx.head_);
    int n = idx.tail_.head_.n_;
    math::check_range("matrix[multi,uni] index row", name, a.rows(), m);
    math::check_range("matrix[multi,uni] index col", name, a.cols(), n);
    c(i) = a.coeff(m - 1, n - 1);
  }
  return c;
}

/**
 * Return the result of indexing the specified Eigen matrix with a
 * sequence consisting of a pair o multiple indexes, returning a
 * a matrix.
 *
 * Types:  mat[multiple,multiple] : mat
 *
 * @tparam T Scalar type.
 * @tparam I Type of multiple index.
 * @param[in] a Matrix to index.
 * @param[in] idx Pair of multiple indexes.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing matrix.
 */
template <typename T, typename I1, typename I2>
inline typename boost::disable_if_c<
    boost::is_same<I1, index_uni>::value
        || boost::is_same<I2, index_uni>::value,
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >::type
rvalue(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& a,
       const cons_index_list<I1, cons_index_list<I2, nil_index_list> >& idx,
       const char* name = "ANON", int depth = 0) {
  int rows = rvalue_index_size(idx.head_, a.rows());
  int cols = rvalue_index_size(idx.tail_.head_, a.cols());
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> c(rows, cols);
  for (int j = 0; j < cols; ++j) {
    for (int i = 0; i < rows; ++i) {
      int m = rvalue_at(i, idx.head_);
      int n = rvalue_at(j, idx.tail_.head_);
      math::check_range("matrix[multi,multi] row index", name, a.rows(), m);
      math::check_range("matrix[multi,multi] col index", name, a.cols(), n);
      c(i, j) = a.coeff(m - 1, n - 1);
    }
  }
  return c;
}

/**
 * Return the result of indexing the specified array with
 * a list of indexes beginning with a single index;  the result is
 * determined recursively.  Note that arrays are represented as
 * standard library vectors.
 *
 * Types:  std::vector<T>[single | L] : T[L]
 *
 * @tparam T Type of list elements.
 * @tparam L Index list type for indexes after first index.
 * @param[in] c Container of list elements.
 * @param[in] idx Index list beginning with single index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing array.
 */
template <typename T, typename L>
inline
    typename rvalue_return<std::vector<T>, cons_index_list<index_uni, L> >::type
    rvalue(const std::vector<T>& c, const cons_index_list<index_uni, L>& idx,
           const char* name = "ANON", int depth = 0) {
  int n = idx.head_.n_;
  math::check_range("array[uni,...] index", name, c.size(), n);
  return rvalue(c[n - 1], idx.tail_, name, depth + 1);
}

/**
 * Return the result of indexing the specified array with
 * a list of indexes beginning with a multiple index;  the result is
 * determined recursively.  Note that arrays are represented as
 * standard library vectors.
 *
 * Types:  std::vector<T>[multiple | L] : std::vector<T[L]>
 *
 * @tparam T Type of list elements.
 * @tparam L Index list type for indexes after first index.
 * @param[in] c Container of list elements.
 * @param[in] idx Index list beginning with multiple index.
 * @param[in] name String form of expression being evaluated.
 * @param[in] depth Depth of indexing dimension.
 * @return Result of indexing array.
 */
template <typename T, typename I, typename L>
inline typename rvalue_return<std::vector<T>, cons_index_list<I, L> >::type
rvalue(const std::vector<T>& c, const cons_index_list<I, L>& idx,
       const char* name = "ANON", int depth = 0) {
  typename rvalue_return<std::vector<T>, cons_index_list<I, L> >::type result;
  for (int i = 0; i < rvalue_index_size(idx.head_, c.size()); ++i) {
    int n = rvalue_at(i, idx.head_);
    math::check_range("array[multi,...] index", name, c.size(), n);
    result.push_back(rvalue(c[n - 1], idx.tail_, name, depth + 1));
  }
  return result;
}

}  // namespace model
}  // namespace stan
#endif


#include <rbdl/rbdl.h>

#include "urdfreader.h"

#include <assert.h>
#include <iostream>
#include <fstream>
#include <map>
#include <stack>

#ifdef RBDL_USE_ROS_URDF_LIBRARY
  #include <urdf_model/model.h>
  #include <urdf_parser/urdf_parser.h>
  #include <boost/shared_ptr.hpp>

  typedef urdf::LinkSharedPtr LinkPtr;
  typedef const urdf::LinkConstSharedPtr ConstLinkPtr;
  typedef urdf::JointSharedPtr JointPtr;
  typedef urdf::ModelInterfaceSharedPtr ModelPtr;

#else
  #include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
  #include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>

  typedef my_shared_ptr<urdf::Link> LinkPtr;
  typedef const my_shared_ptr<const urdf::Link> ConstLinkPtr;
  typedef my_shared_ptr<urdf::Joint> JointPtr;
  typedef my_shared_ptr<urdf::ModelInterface> ModelPtr;

#endif

using namespace std;

namespace RigidBodyDynamics
{

namespace Addons
{

using namespace Math;

typedef vector<LinkPtr> URDFLinkVector;
typedef vector<JointPtr> URDFJointVector;
typedef map<string, LinkPtr> URDFLinkMap;
typedef map<string, JointPtr> URDFJointMap;

bool construct_model(Model *rbdl_model, ModelPtr urdf_model, bool floating_base,
                     bool verbose)
{
  LinkPtr urdf_root_link;

  URDFLinkMap link_map;
  link_map = urdf_model->links_;

  URDFJointMap joint_map;
  joint_map = urdf_model->joints_;

  vector<string> joint_names;

  // Holds the links that we are processing in our depth first traversal
  // with the top element being the current link.
  stack<LinkPtr> link_stack;
  // Holds the child joint index of the current link
  stack<int> joint_index_stack;

  // add the bodies in a depth-first order of the model tree
  link_stack.push(link_map[(urdf_model->getRoot()->name)]);

  // add the root body
  ConstLinkPtr &root = urdf_model->getRoot();
  Vector3d root_inertial_rpy;
  Vector3d root_inertial_position;
  Matrix3d root_inertial_inertia;
  double root_inertial_mass;

  if (root->inertial) {
    root_inertial_mass = root->inertial->mass;

    root_inertial_position.set(
      root->inertial->origin.position.x,
      root->inertial->origin.position.y,
      root->inertial->origin.position.z);

    root_inertial_inertia(0, 0) = root->inertial->ixx;
    root_inertial_inertia(0, 1) = root->inertial->ixy;
    root_inertial_inertia(0, 2) = root->inertial->ixz;

    root_inertial_inertia(1, 0) = root->inertial->ixy;
    root_inertial_inertia(1, 1) = root->inertial->iyy;
    root_inertial_inertia(1, 2) = root->inertial->iyz;

    root_inertial_inertia(2, 0) = root->inertial->ixz;
    root_inertial_inertia(2, 1) = root->inertial->iyz;
    root_inertial_inertia(2, 2) = root->inertial->izz;

    root->inertial->origin.rotation.getRPY(root_inertial_rpy[0],
                                           root_inertial_rpy[1],
                                           root_inertial_rpy[2]);

    Body root_link = Body(root_inertial_mass,
                          root_inertial_position,
                          root_inertial_inertia);

    Joint root_joint(JointTypeFixed);
    if (floating_base) {
      root_joint = JointTypeFloatingBase;
    }

    SpatialTransform root_joint_frame = SpatialTransform();

    if (verbose) {
      cout << "+ Adding Root Body " << endl;
      cout << "  joint frame: " << root_joint_frame << endl;
      if (floating_base) {
        cout << "  joint type : floating" << endl;
      } else {
        cout << "  joint type : fixed" << endl;
      }
      cout << "  body inertia: " << endl
           << root_link.mInertia << endl;
      cout << "  body mass   : " << root_link.mMass << endl;
      cout << "  body name   : " << root->name << endl;
    }

    rbdl_model->AppendBody(root_joint_frame,
                           root_joint,
                           root_link,
                           root->name);
  }

  // depth first traversal: push the first child onto our joint_index_stack
  joint_index_stack.push(0);

  while (link_stack.size() > 0) {
    LinkPtr cur_link = link_stack.top();

    unsigned int joint_idx = joint_index_stack.top();

    // Add any child bodies and increment current joint index if we still
    // have child joints to process.
    if (joint_idx < cur_link->child_joints.size()) {
      JointPtr cur_joint = cur_link->child_joints[joint_idx];

      // increment joint index
      joint_index_stack.pop();
      joint_index_stack.push(joint_idx + 1);

      link_stack.push(link_map[cur_joint->child_link_name]);
      joint_index_stack.push(0);

      if (verbose) {
        for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++) {
          cout << "  ";
        }
        cout << "joint '" << cur_joint->name << "' child link '" <<
             link_stack.top()->name << "' type = " << cur_joint->type << endl;
      }

      joint_names.push_back(cur_joint->name);
    } else {
      link_stack.pop();
      joint_index_stack.pop();
    }
  }

  unsigned int j;
  for (j = 0; j < joint_names.size(); j++) {
    JointPtr urdf_joint = joint_map[joint_names[j]];
    LinkPtr urdf_parent = link_map[urdf_joint->parent_link_name];
    LinkPtr urdf_child = link_map[urdf_joint->child_link_name];

    // determine where to add the current joint and child body
    unsigned int rbdl_parent_id = 0;

    if (urdf_parent->name != "base_link") {
      rbdl_parent_id = rbdl_model->GetBodyId(urdf_parent->name.c_str());
    }

    if (rbdl_parent_id == std::numeric_limits<unsigned int>::max())
      cerr << "Error while processing joint '" << urdf_joint->name
           << "': parent link '" << urdf_parent->name
           << "' could not be found." << endl;

    // create the joint
    Joint rbdl_joint;
    if (urdf_joint->type == urdf::Joint::REVOLUTE ||
        urdf_joint->type == urdf::Joint::CONTINUOUS) {
      rbdl_joint = Joint(SpatialVector(urdf_joint->axis.x, urdf_joint->axis.y,
                                       urdf_joint->axis.z, 0., 0., 0.));
    } else if (urdf_joint->type == urdf::Joint::PRISMATIC) {
      rbdl_joint = Joint(SpatialVector(0., 0., 0., urdf_joint->axis.x,
                                       urdf_joint->axis.y, urdf_joint->axis.z));
    } else if (urdf_joint->type == urdf::Joint::FIXED) {
      rbdl_joint = Joint(JointTypeFixed);
    } else if (urdf_joint->type == urdf::Joint::FLOATING) {
      // todo: what order of DoF should be used?
      rbdl_joint = Joint(
                     SpatialVector(0., 0., 0., 1., 0., 0.),
                     SpatialVector(0., 0., 0., 0., 1., 0.),
                     SpatialVector(0., 0., 0., 0., 0., 1.),
                     SpatialVector(1., 0., 0., 0., 0., 0.),
                     SpatialVector(0., 1., 0., 0., 0., 0.),
                     SpatialVector(0., 0., 1., 0., 0., 0.));
    } else if (urdf_joint->type == urdf::Joint::PLANAR) {
      // todo: which two directions should be used that are perpendicular
      // to the specified axis?
      cerr << "Error while processing joint '" << urdf_joint->name <<
           "': planar joints not yet supported!" << endl;
      return false;
    }

    // compute the joint transformation
    Vector3d joint_rpy;
    Vector3d joint_translation;
    urdf_joint->parent_to_joint_origin_transform.rotation.getRPY(joint_rpy[0],
        joint_rpy[1], joint_rpy[2]);
    joint_translation.set(
      urdf_joint->parent_to_joint_origin_transform.position.x,
      urdf_joint->parent_to_joint_origin_transform.position.y,
      urdf_joint->parent_to_joint_origin_transform.position.z);
    SpatialTransform rbdl_joint_frame =
      Xrot(joint_rpy[0], Vector3d(1., 0., 0.)) * Xrot(joint_rpy[1], Vector3d(0., 1.,
          0.)) * Xrot(joint_rpy[2], Vector3d(0., 0.,
                      1.)) * Xtrans(Vector3d(joint_translation));

    // assemble the body
    Vector3d link_inertial_position;
    Vector3d link_inertial_rpy;
    Matrix3d link_inertial_inertia = Matrix3d::Zero();
    double link_inertial_mass = 0.;

    // but only if we actually have inertial data
    if (urdf_child->inertial) {
      link_inertial_mass = urdf_child->inertial->mass;

      link_inertial_position.set(
        urdf_child->inertial->origin.position.x,
        urdf_child->inertial->origin.position.y,
        urdf_child->inertial->origin.position.z);
      urdf_child->inertial->origin.rotation.getRPY(link_inertial_rpy[0],
          link_inertial_rpy[1], link_inertial_rpy[2]);

      link_inertial_inertia(0, 0) = urdf_child->inertial->ixx;
      link_inertial_inertia(0, 1) = urdf_child->inertial->ixy;
      link_inertial_inertia(0, 2) = urdf_child->inertial->ixz;

      link_inertial_inertia(1, 0) = urdf_child->inertial->ixy;
      link_inertial_inertia(1, 1) = urdf_child->inertial->iyy;
      link_inertial_inertia(1, 2) = urdf_child->inertial->iyz;

      link_inertial_inertia(2, 0) = urdf_child->inertial->ixz;
      link_inertial_inertia(2, 1) = urdf_child->inertial->iyz;
      link_inertial_inertia(2, 2) = urdf_child->inertial->izz;

      if (link_inertial_rpy != Vector3d(0., 0., 0.)) {
        cerr << "Error while processing body '" << urdf_child->name <<
             "': rotation of body frames not yet supported. Please rotate" <<
             "the joint frame instead."
             << endl;
        return false;
      }
    }

    Body rbdl_body = Body(link_inertial_mass, link_inertial_position,
                          link_inertial_inertia);

    if (verbose) {
      cout << "+ Adding Body: " << urdf_child->name << endl;
      cout << "  parent_id  : " << rbdl_parent_id << endl;
      cout << "  joint frame: " << rbdl_joint_frame << endl;
      cout << "  joint dofs : " << rbdl_joint.mDoFCount << endl;
      for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) {
        cout << "    " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl;
      }
      cout << "  body inertia: " << endl
           << rbdl_body.mInertia << endl;
      cout << "  body mass   : " << rbdl_body.mMass << endl;
      cout << "  body name   : " << urdf_child->name << endl;
    }

    if (urdf_joint->type == urdf::Joint::FLOATING) {
      Matrix3d zero_matrix = Matrix3d::Zero();
      Body null_body(0., Vector3d::Zero(3), zero_matrix);
      Joint joint_txtytz(JointTypeTranslationXYZ);
      string trans_body_name = urdf_child->name + "_Translate";
      rbdl_model->AddBody(rbdl_parent_id, rbdl_joint_frame, joint_txtytz, null_body,
                          trans_body_name);

      Joint joint_euler_zyx(JointTypeEulerXYZ);
      rbdl_model->AppendBody(SpatialTransform(), joint_euler_zyx, rbdl_body,
                             urdf_child->name);
    } else {
      rbdl_model->AddBody(rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body,
                          urdf_child->name);
    }
  }

  return true;
}

RBDL_DLLAPI bool URDFReadFromFile(const char *filename, Model *model,
                                  bool floating_base, bool verbose)
{
  ifstream model_file(filename);
  if (!model_file) {
    cerr << "Error opening file '" << filename << "'." << endl;
    abort();
  }

  // reserve memory for the contents of the file
  string model_xml_string;
  model_file.seekg(0, std::ios::end);
  model_xml_string.reserve(model_file.tellg());
  model_file.seekg(0, std::ios::beg);
  model_xml_string.assign((std::istreambuf_iterator<char>(model_file)),
                          std::istreambuf_iterator<char>());

  model_file.close();

  return URDFReadFromString(model_xml_string.c_str(), model, floating_base,
                            verbose);
}

RBDL_DLLAPI bool URDFReadFromString(const char *model_xml_string, Model *model,
                                    bool floating_base, bool verbose)
{
  assert(model);

  ModelPtr urdf_model = urdf::parseURDF(model_xml_string);

  if (!construct_model(model, urdf_model, floating_base, verbose)) {
    cerr << "Error constructing model from urdf file." << endl;
    return false;
  }

  model->gravity.set(0., 0., -9.81);

  return true;
}

} // namespace Addons

} // namespace RigidBodyDynamics