//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_PACK_HPP
#define CRYPTO3_DETAIL_PACK_HPP

#include <nil/crypto3/detail/type_traits.hpp>
#include <nil/crypto3/detail/stream_endian.hpp>
#include <nil/crypto3/detail/exploder.hpp>
#include <nil/crypto3/detail/imploder.hpp>
#include <nil/crypto3/detail/reverser.hpp>
#include <nil/crypto3/detail/predef.hpp>

#include <boost/static_assert.hpp>
#include <boost/predef/other/endian.h>
#include <boost/predef/architecture.h>

#include <algorithm>
#include <climits>
#include <iterator>
#include <type_traits>

namespace nil {
    namespace crypto3 {
        namespace detail {

            /*!
             * @defgroup pack Pack functions
             */

            /*!
             * @brief The group of traits below is used to determine the possibility of fast data copy.
             * By fast data copy we mean that the data is stored contiguously in the memory, so it can be
             * copied faster byte-by-byte. Currently, fast data copy is implemented by memcpy function call.
             */

            /*!
             * @brief host_can_memcpy trait checks whether the data to be copied and the container to be copied to
             * are byte-aligned. Parameter types InT and OutT may refer to pointed data types or to iterator types.
             *
             * @ingroup pack
             *
             * @tparam UnitBits
             * @tparam ValueBits
             * @tparam InT
             * @tparam OutT
             */
            template<int UnitBits, int InputBits, int OutputBits, typename InT, typename OutT>
            struct host_can_memcpy {
                constexpr static const bool value = !(UnitBits % CHAR_BIT) && InputBits >= UnitBits &&
                                                    OutputBits >= UnitBits && sizeof(InT) * CHAR_BIT == InputBits &&
                                                    sizeof(OutT) * CHAR_BIT == OutputBits;
            };

            /*!
             * @brief can_memcpy trait is derived from host_can_memcpy trait and is invoked depending on
             * data endianness. Note that there is a single endianness template parameter since otherwise
             * we have to transform data in accordance with endianness conversion rules.
             *
             * @ingroup pack
             *
             * @tparam Endianness
             * @tparam ValueBits
             * @tparam InT
             * @tparam OutT
             */
            template<typename Endianness, int InputBits, int OutputBits, typename InT, typename OutT>
            struct can_memcpy {
                constexpr static const bool value = InputBits == OutputBits && sizeof(InT) == sizeof(OutT);
            };

            template<int UnitBits, int InputBits, int OutputBits, typename InT, typename OutT>
            struct can_memcpy<stream_endian::host_unit<UnitBits>, InputBits, OutputBits, InT, OutT>
                : host_can_memcpy<UnitBits, InputBits, OutputBits, InT, OutT> { };

#ifdef BOOST_ENDIAN_LITTLE_BYTE_AVAILABLE
            template<int UnitBits, int InputBits, int OutputBits, typename InT, typename OutT>
            struct can_memcpy<stream_endian::little_unit_big_bit<UnitBits>, InputBits, OutputBits, InT, OutT>
                : host_can_memcpy<UnitBits, InputBits, OutputBits, InT, OutT> { };

            template<int UnitBits, int InputBits, int OutputBits, typename InT, typename OutT>
            struct can_memcpy<stream_endian::little_unit_little_bit<UnitBits>, InputBits, OutputBits, InT, OutT>
                : host_can_memcpy<UnitBits, InputBits, OutputBits, InT, OutT> { };

#elif defined(BOOST_ENDIAN_BIG_BYTE_AVAILABLE)
            template<int UnitBits, int InputBits, int OutputBits, typename InT, typename OutT>
            struct can_memcpy<stream_endian::big_unit_big_bit<UnitBits>, ValueBits, InT, OutT>
                : host_can_memcpy<UnitBits, InputBits, OutputBits, InT, OutT> { };
            template<int UnitBits, int InputBits, int OutputBits, typename InT, typename OutT>
            struct can_memcpy<stream_endian::big_unit_little_bit<UnitBits>, ValueBits, InT, OutT>
                : host_can_memcpy<UnitBits, InputBits, OutputBits, , InT, OutT> { };
#endif

            /*!
             * @brief Real_packer is used to transform input data divided into chunks of the bit size InputValueBits
             * represented in input endianness (InputEndianness)
             * into output data (of the same bit length) divided into chunks of the bit size OutputValueBits
             * represented in output endianness (OutputEndianness).
             *
             * The choice of packer depends on the following conditions:
             * 1. input and output chunk size relation (equal, less, or greater);
             * 2. input and output endianness relation (same or different);
             * 3. the possibility of fast data copy using memcpy.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputType
             * @tparam OutputType
             * @tparam SameEndianness
             * @tparam Implode
             * @tparam Explode
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputType, typename OutputType,
                     bool SameEndianness = std::is_same<InputEndianness, OutputEndianness>::value,
                     bool Implode = (InputValueBits < OutputValueBits),
                     bool Explode = (InputValueBits > OutputValueBits)>
            struct real_packer { };

            /*!
             * @brief This real_packer deals with the case of equal sizes (i.e. InputValueBits == OutputValueBits)
             * and same endianness representations (i.e., speaking informally,
             * InputEndianness == OutputEndianness). It packs input elements with ValueBits size represented
             * in Endianness endianness into output elements with the same ValueBits size represented in the
             * same Endianness endianness.
             *
             * @ingroup pack
             *
             * @tparam Endianness
             * @tparam ValueBits
             * @tparam InputType
             * @tparam OutputType
             */
            template<typename Endianness, std::size_t ValueBits, typename InputType, typename OutputType>
            struct real_packer<Endianness, Endianness, ValueBits, ValueBits, InputType, OutputType, true, false,
                               false> {
                /*!
                 * @brief Packs n InputType elements pointed by constant pointer in
                 * (which, hence, cannot be iterated) into OutType elements pointed by out.
                 * This function is invoked only if memcpy call is possible.
                 *
                 * @ingroup pack
                 *
                 * @param in
                 * @param n
                 * @param out
                 *
                 * @return
                 */
                template<std::size_t InputValueBits, std::size_t OutputValueBits>
                inline static typename std::enable_if<
                    can_memcpy<Endianness, InputValueBits, OutputValueBits, InputType, OutputType>::value>::type
                    pack_n(InputType const *in, std::size_t n, OutputType *out) {
                    std::memcpy(out, in, n * sizeof(InputType));
                }

                /*!
                 * @brief Packs n InputType elements pointed by pointer in into OutType elements pointed by out.
                 * This function is invoked only if memcpy call is possible.
                 *
                 * @ingroup pack
                 *
                 * @param in
                 * @param n
                 * @param out
                 *
                 * @return
                 */
                template<std::size_t InputValueBits, std::size_t OutputValueBits>
                inline static typename std::enable_if<
                    can_memcpy<Endianness, InputValueBits, OutputValueBits, InputType, OutputType>::value>::type
                    pack_n(InputType *in, std::size_t n, OutputType *out) {
                    std::memcpy(out, in, n * sizeof(InputType));
                }

                /*!
                 * @brief Packs in_n elements iterated by in into elements iterated by out.
                 *
                 * @ingroup pack
                 *
                 * @tparam InputIterator
                 * @tparam OutputIterator
                 *
                 * @param in
                 * @param in_n
                 * @param out
                 *
                 * @return
                 */
                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t in_n, OutputIterator out) {
                    std::copy(in, in + in_n, out);
                }

                /*!
                 * @brief Packs elements in range [first, last) into elements iterated by out.
                 * This function is invoked only if input and output iterators meet RandomAccessIterator
                 * requirements. However, the restriction can be weakened to ContiguousIterator usage.
                 *
                 * @ingroup pack
                 *
                 * @tparam InputIterator
                 * @tparam OutputIterator
                 *
                 * @param first
                 * @param last
                 * @param random_access_iterator_tag
                 * @param out
                 * @param random_access_iterator_tag
                 *
                 * @return
                 */
                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, std::random_access_iterator_tag,
                                        OutputIterator out, std::random_access_iterator_tag) {
                    pack_n(first, std::distance(first, last), out);
                }

                /*!
                 * @brief Packs elements in range [first, last) into elements iterated by out.
                 * This function is invoked only if input or output iterator doesn't meet RandomAccessIterator
                 * requirements.
                 *
                 * @ingroup pack
                 *
                 * @tparam InputIterator
                 * @tparam InCatT
                 * @tparam OutputIterator
                 * @tparam OutCatT
                 *
                 * @param first
                 * @param last
                 * @param InCatT
                 * @param out
                 * @param OutCatT
                 *
                 * @return
                 */
                template<typename InputIterator, typename InCatT, typename OutputIterator, typename OutCatT>
                inline static void pack(InputIterator first, InputIterator last, InCatT, OutputIterator out, OutCatT) {
                    std::copy(first, last, out);
                }

                /*!
                 * @brief Generic function that chooses pack function depending on input and output iterator category.
                 *
                 * @ingroup pack
                 *
                 * @tparam InputIterator
                 * @tparam OutputIterator
                 *
                 * @param first
                 * @param last
                 * @param out
                 *
                 * @return
                 */
                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {

                    typedef typename std::iterator_traits<InputIterator>::iterator_category in_cat;
                    typedef typename std::iterator_traits<OutputIterator>::iterator_category out_cat;

                    pack(first, last, in_cat(), out, out_cat());
                }
            };

            /*!
             * @brief This real_packer deals with the case of equal sizes (i.e. InputValueBits == OutputValueBits)
             * and different endianness representations (or, speaking informally,
             * InputEndianness != OutputEndianness). It invokes functions which pack input elements
             * with ValueBits size represented in InputEndianness endianness into output elements
             * with the same ValueBits size represented in another OutputEndianness endianness.
             *
             * @ingroup pack
             *
             * @tparam UnitBits
             * @tparam InputEndian
             * @tparam OutputEndian
             * @tparam ValueBits
             * @tparam InputType
             * @tparam OutputType
             */
            template<int UnitBits, template<int> class InputEndian, template<int> class OutputEndian,
                     std::size_t ValueBits, typename InputType, typename OutputType>
            struct real_packer<InputEndian<UnitBits>, OutputEndian<UnitBits>, ValueBits, ValueBits, InputType,
                               OutputType, false, false, false> {

                typedef InputEndian<UnitBits> InputEndianness;
                typedef OutputEndian<UnitBits> OutputEndianness;

                typedef unit_reverser<InputEndianness, OutputEndianness, UnitBits> units_reverser;
                typedef bit_reverser<InputEndianness, OutputEndianness, UnitBits> bits_reverser;

                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t in_n, OutputIterator out) {

                    std::transform(in, in + in_n, out, [](InputType const &elem) {
                        return units_reverser::reverse(bits_reverser::reverse(elem));
                    });
                }

                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {

                    std::transform(first, last, out, [](InputType const &elem) {
                        return units_reverser::reverse(bits_reverser::reverse(elem));
                    });
                }
            };

            /*!
             * @brief This real_packer deals with case InputValueBits < OutputValueBits and invokes implode function,
             * which, in its turn, packs input elements with InputValueBits size represented in InputEndianness
             * endianness into output elements with OutputValueBits size represented in OutputEndianness
             * endianness.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputType
             * @tparam OutputType
             * @tparam SameEndianness
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputType, typename OutputType, bool SameEndianness>
            struct real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                               OutputType, SameEndianness, true, false> {

                BOOST_STATIC_ASSERT(!(OutputValueBits % InputValueBits));

                typedef detail::imploder<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits> imploder;

                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t in_n, OutputIterator out) {
                    std::size_t out_n = in_n / (OutputValueBits / InputValueBits);

                    while (out_n--) {
                        OutputType value = OutputType();
                        imploder::implode(in, value);
                        *out++ = value;
                    }
                }

                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {
                    while (first != last) {
                        OutputType value = OutputType();
                        imploder::implode(first, value);
                        *out++ = value;
                    }
                }
            };

            /*!
             * @brief This real_packer deals with case InputValueBits > OutputValueBits and invokes explode function,
             * which, in its turn, packs input elements with InputValueBits size represented in InputEndianness
             * endianness into output elements with OutputValueBits size represented in OutputEndianness
             * endianness.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputType
             * @tparam OutputType
             * @tparam SameEndianness
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputType, typename OutputType, bool SameEndianness>
            struct real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                               OutputType, SameEndianness, false, true> {

                BOOST_STATIC_ASSERT(!(InputValueBits % OutputValueBits));

                typedef detail::exploder<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits> exploder;

                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t in_n, OutputIterator out) {
                    while (in_n--) {
                        InputType const value = *in++;
                        exploder::explode(value, out);
                    }
                }

                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {
                    while (first != last) {
                        InputType const value = *first++;
                        exploder::explode(value, out);
                    }
                }
            };

            /*!
             * @brief This packer deals with arbitrary input and output (but not bool) data elements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputType
             * @tparam OutputType
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputType, typename OutputType>
            struct packer {

                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t n, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                                        OutputType>
                        packer_type;

                    packer_type::pack_n(in, n, out);
                }

                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                                        OutputType>
                        packer_type;

                    packer_type::pack(first, last, out);
                }
            };

            /*!
             * @brief This packer deals with bool input and output data elements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits>
            struct packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, bool, bool> {

                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t n, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, bool, bool>
                        packer_type;

                    packer_type::pack_n(in, n, out);
                }

                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, bool, bool>
                        packer_type;

                    packer_type::pack(first, last, out);
                }
            };

            /*!
             * @brief This packer deals with bool input data and arbitrary (but not bool) output data elements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam OutputType
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename OutputType>
            struct packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, bool, OutputType> {

                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t n, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, bool,
                                        OutputType>
                        packer_type;

                    packer_type::pack_n(in, n, out);
                }

                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, bool,
                                        OutputType>
                        packer_type;

                    packer_type::pack(first, last, out);
                }
            };

            /*!
             * @brief This packer deals with arbitrary (but not bool) input data and bool output data elements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputType
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputType>
            struct packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType, bool> {

                template<typename InputIterator, typename OutputIterator>
                inline static void pack_n(InputIterator in, std::size_t n, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                                        bool>
                        packer_type;

                    packer_type::pack_n(in, n, out);
                }

                template<typename InputIterator, typename OutputIterator>
                inline static void pack(InputIterator first, InputIterator last, OutputIterator out) {
                    typedef real_packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                                        bool>
                        packer_type;

                    packer_type::pack(first, last, out);
                }
            };

            /*!
             * @brief Packs elements from range [first, last) represented in machine-dependent endianness
             * into elements starting from out represented in OutputEndianness endianness.
             *
             * @ingroup pack
             *
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param first
             * @param last
             * @param out
             *
             * @return
             */
            template<typename OutputEndianness, std::size_t InputValueBits, std::size_t OutputValueBits,
                     typename InputIterator, typename OutputIterator>
            inline void pack_to(InputIterator first, InputIterator last, OutputIterator out) {

                typedef typename std::iterator_traits<InputIterator>::value_type InputType;
                typedef typename std::iterator_traits<OutputIterator>::value_type OutputType;

#ifdef BOOST_ENDIAN_BIG_BYTE_AVAILABLE
                typedef packer<stream_endian::big_octet_big_bit, OutputEndianness, InputValueBits, OutputValueBits,
                               InputType, OutputType>
                    packer_type;
#elif defined(BOOST_ENDIAN_LITTLE_BYTE_AVAILABLE)
                typedef packer<stream_endian::little_octet_big_bit, OutputEndianness, InputValueBits, OutputValueBits,
                               InputType, OutputType>
                    packer_type;
#elif defined(BOOST_ENDIAN_BIG_WORD_AVAILABLE)
                typedef packer<stream_endian::big_unit_big_bit<BOOST_ARCH_CURRENT_WORD_BITS>, OutputEndianness, InputValueBits,
                               OutputValueBits, InputType, OutputType>
                    packer_type;
#elif defined(BOOST_ENDIAN_LITTLE_WORD_AVAILABLE)
                typedef packer<stream_endian::little_unit_big_bit<BOOST_ARCH_CURRENT_WORD_BITS>, OutputEndianness,
                               InputValueBits, OutputValueBits, InputType, OutputType>
                    packer_type;
#else
#error "Unknown endianness"
#endif

                packer_type::pack(first, last, out);
            }

            /*!
             * @brief Packs elements from range [first, last) represented in InputEndianness endianness
             * into elements starting from out represented in machine-dependent endianness.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param first
             * @param last
             * @param out
             *
             * @return
             */
            template<typename InputEndianness, std::size_t InputValueBits, std::size_t OutputValueBits,
                     typename InputIterator, typename OutputIterator>
            inline void pack_from(InputIterator first, InputIterator last, OutputIterator out) {

                typedef typename std::iterator_traits<InputIterator>::value_type InputType;
                typedef typename std::iterator_traits<OutputIterator>::value_type OutputType;

#ifdef BOOST_ENDIAN_BIG_BYTE_AVAILABLE
                typedef packer<InputEndianness, stream_endian::big_octet_big_bit, InputValueBits, OutputValueBits,
                               InputType, OutputType>
                    packer_type;
#elif defined(BOOST_ENDIAN_LITTLE_BYTE_AVAILABLE)
                typedef packer<InputEndianness, stream_endian::little_octet_big_bit, InputValueBits, OutputValueBits,
                               InputType, OutputType>
                    packer_type;
#elif defined(BOOST_ENDIAN_BIG_WORD_AVAILABLE)
                typedef packer<InputEndianness, stream_endian::big_unit_big_bit<BOOST_ARCH_CURRENT_WORD_BITS>, InputValueBits,
                               OutputValueBits, InputType, OutputType>
                    packer_type;
#elif defined(BOOST_ENDIAN_LITTLE_WORD_AVAILABLE)
                typedef packer<InputEndianness, stream_endian::little_unit_big_bit<BOOST_ARCH_CURRENT_WORD_BITS>,
                               InputValueBits, OutputValueBits, InputType, OutputType>
                    packer_type;
#else
#error "Unknown endianness"
#endif

                packer_type::pack(first, last, out);
            }

            /*!
             * @brief Packs in_n input elements starting from in into output elements beginning from out.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param in
             * @param in_n
             * @param out
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename OutputIterator>
            inline void pack_n(InputIterator in, std::size_t in_n, OutputIterator out) {
                typedef typename std::iterator_traits<InputIterator>::value_type InputType;
                typedef typename std::iterator_traits<OutputIterator>::value_type OutputType;
                typedef packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                               OutputType>
                    packer_type;

                packer_type::pack_n(in, in_n, out);
            }

            /*!
             * @brief Packs in_n input elements starting from in into in_out elements beginning from out.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param in
             * @param in_n
             * @param out
             * @param out_n
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename OutputIterator>
            inline void pack_n(InputIterator in, std::size_t in_n, OutputIterator out, std::size_t out_n) {
                BOOST_ASSERT(in_n * InputValueBits == out_n * OutputValueBits);

                pack_n<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(in, in_n, out);
            }

            /*!
             * @brief Packs elements from the range [first, last) into elements starting from out.
             * Works for input containers meeting RandomAccessIterator requirements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param first
             * @param last
             * @param random_access_iterator_tag
             * @param out
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename OutputIterator>
            inline void pack(InputIterator first, InputIterator last, std::random_access_iterator_tag,
                             OutputIterator out) {
                pack_n<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(first, last - first, out);
            }

            /*!
             * @brief Packs elements from the range [first, last) into elements starting from out.
             * Works for input containers meeting InCatT category requirements and output containers
             * meeting OutputIterator requirements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam InCatT
             * @tparam OutputIterator
             *
             * @param first
             * @param last
             * @param InCatT
             * @param out
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename InCatT, typename OutputIterator,
                     typename = typename std::enable_if<detail::is_iterator<InputIterator>::value>::type,
                     typename = typename std::enable_if<detail::is_iterator<OutputIterator>::value>::type>
            inline void pack(InputIterator first, InputIterator last, InCatT, OutputIterator out) {
                typedef typename std::iterator_traits<InputIterator>::value_type InputType;
                typedef typename std::iterator_traits<OutputIterator>::value_type OutputType;
                typedef packer<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits, InputType,
                               OutputType>
                    packer_type;

                packer_type::pack(first, last, out);
            }

            /*!
             * @brief Generic function that chooses pack function depending on input iterator category.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param first
             * @param last
             * @param out
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename OutputIterator,
                     typename = typename std::enable_if<detail::is_iterator<OutputIterator>::value>::type>
            inline void pack(InputIterator first, InputIterator last, OutputIterator out) {
                typedef typename std::iterator_traits<InputIterator>::iterator_category in_cat;

                pack<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(first, last, in_cat(), out);
            }

            /*!
             * @brief Packs elements from the range [first, last) into elements starting from out.
             * Works for input and output containers meeting RandomAccessIterator requirements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param in_first
             * @param in_last
             * @param random_access_iterator_tag
             * @param out_first
             * @param out_last
             * @param random_access_iterator_tag
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename OutputIterator>
            inline void pack(InputIterator in_first, InputIterator in_last, std::random_access_iterator_tag,
                             OutputIterator out_first, OutputIterator out_last, std::random_access_iterator_tag) {
                pack_n<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(
                    in_first, in_last - in_first, out_first, out_last - out_first);
            }

            /*!
             * @brief Packs elements from the range [first, last) into elements starting from out.
             * Works for input containers meeting InCatT category requirements and output containers
             * meeting OutCatT category requirements.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam InCatT
             * @tparam OutputIterator
             * @tparam OutCatT
             *
             * @param in_first
             * @param in_last
             * @param InCatT
             * @param out
             * @param OutputIterator
             * @param OutCatT
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename InCatT, typename OutputIterator,
                     typename OutCatT>
            inline void pack(InputIterator in_first, InputIterator in_last, InCatT, OutputIterator out, OutputIterator,
                             OutCatT) {
                pack<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(in_first, in_last, out);
            }

            /*!
             * @brief Generic function that chooses pack function depending on input and output iterator category.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputIterator
             * @tparam OutputIterator
             *
             * @param in_first
             * @param in_last
             * @param out_first
             * @param out_last
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename OutputIterator>
            inline void pack(InputIterator in_first, InputIterator in_last, OutputIterator out_first,
                             OutputIterator out_last) {
                typedef typename std::iterator_traits<InputIterator>::iterator_category in_cat;
                typedef typename std::iterator_traits<OutputIterator>::iterator_category out_cat;

                pack<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(
                    in_first, in_last, in_cat(), out_first, out_last, out_cat());
            }

            /*!
             * @brief Packs immutable data referenced by in into data referenced by out.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputType
             * @tparam OutputType
             *
             * @param in
             * @param out
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputType, typename OutputType>
            inline void pack(const InputType &in, OutputType &out) {
                pack_n<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(in.begin(), in.size(),
                                                                                out.begin(), out.size());
            }

            /*!
             * @brief Packs elements from range [first, last) into data referenced by out with
             * non-arithmetic value type.
             *
             * @ingroup pack
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam InputValueBits
             * @tparam OutputValueBits
             * @tparam InputType
             * @tparam OutputType
             *
             * @param in
             * @param out
             *
             * @return
             */
            template<typename InputEndianness, typename OutputEndianness, std::size_t InputValueBits,
                     std::size_t OutputValueBits, typename InputIterator, typename OutputType,
                     typename = typename std::enable_if<!std::is_arithmetic<OutputType>::value>::type>
            inline void pack(InputIterator first, InputIterator last, OutputType &out) {
                pack_n<InputEndianness, OutputEndianness, InputValueBits, OutputValueBits>(
                    first, std::distance(first, last), out.begin(), out.size());
            }

        }    // namespace detail
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_DETAIL_PACK_HPP


/**
* Copyright 2018 Woods Hole Oceanographic Institution
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
*    this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
*    this list of conditions and the following disclaimer in the documentation
*    and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
*    may be used to endorse or promote products derived from this software
*    without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

// lots of this implementation blatently stolen from:
// https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp
#include "dsros_dvl_plugin.hh"

#include <Eigen/Core>
#include <Eigen/SVD>

using namespace gazebo;

GZ_REGISTER_SENSOR_PLUGIN(dsrosRosDvlSensor);

dsrosRosDvlSensor::dsrosRosDvlSensor() : SensorPlugin() {

    sensor = NULL;
    seed = 0;
};

dsrosRosDvlSensor::~dsrosRosDvlSensor() {
    if (connection.get()) {
        connection.reset();
    }
    node->shutdown();
}

void dsrosRosDvlSensor::Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) {
    sdf = sdf_;
    sensor = std::dynamic_pointer_cast<gazebo::sensors::DsrosDvlSensor>(sensor_);
    if (sensor == NULL) {
        ROS_FATAL("Error! Unable to convert sensor pointer!");
        return;
    }
    sensor->SetActive(true);

    if (!LoadParameters()) {
        ROS_FATAL("Error loading parameters for sensor plugin!");
        return;
    }

    if (!ros::isInitialized()) {
        ROS_FATAL("ROS has not been initialized properly...");
        return;
    }

    node = new ros::NodeHandle(this->robot_namespace);

    dvl_data_publisher = node->advertise<ds_sensor_msgs::Dvl>(topic_name, 1);
    rng_publisher = node->advertise<ds_sensor_msgs::Ranges3D>(ranges_topic_name, 1);
    pt_data_publisher  = node->advertise<sensor_msgs::PointCloud>(topic_name + "_cloud", 1);
    current_profile_publisher = node->advertise<ds_sensor_msgs::Adcp>(topic_name + "_current", 1);
    pt_data_publisher  = node->advertise<sensor_msgs::PointCloud>(topic_name + "_cloud", 1);
    connection = gazebo::event::Events::ConnectWorldUpdateBegin(
                boost::bind(&dsrosRosDvlSensor::UpdateChild, this, _1));
    last_time = sensor->LastUpdateTime();

    // Initialize data members and the Adcp message for current profiling
    current_profile_cell_depth = (sensor->RangeMax() - sensor->RangeMin()) / water_track_bins;
    current_profile_bin0_distance = sensor->RangeMin() + current_profile_cell_depth / 2.0;
    for (size_t beam = 0; beam < 4; beam++)
    {
        ignition::math::Vector3d unit_vec = sensor->GetBeamUnitVec(beam);
        adcp.beam_unit_vec[beam].x = unit_vec.X();
        adcp.beam_unit_vec[beam].y = unit_vec.Y();
        adcp.beam_unit_vec[beam].z = unit_vec.Z();
    } // for (size_t beam = 0;...
    adcp.vel_bin_beams.resize(water_track_bins);
    for (size_t bin = 0; bin < water_track_bins; bin++)
    {
        if (current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_BEAM)
        {
            adcp.vel_bin_beams[bin].velocity_bin_beam.resize(4);
//            adcp.vel_bin_beams[bin].bin_intensity.resize(4);     // Commented out in msg def for now
//            adcp.vel_bin_beams[bin].bin_correlation.resize(4);   // Commented out in msg def for now
        }
        else
        {
            adcp.vel_bin_beams[bin].velocity_bin_beam.resize(1);
//            adcp.vel_bin_beams[bin].bin_intensity.resize(1);     // Commented out in msg def for now
//            adcp.vel_bin_beams[bin].bin_correlation.resize(1);   // Commented out in msg def for now
        }
    } // for (size_t bin = 0;...
}

void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {

    common::Time current_time = sensor->LastUpdateTime();

    if(update_rate>0 && (current_time-last_time).Double() < 1.0/update_rate) {
        return;
    }

    // add noise and recompute the velocity
    std::vector<double> ranges(4);
    Eigen::VectorXd raw_beam_vel(4);
    Eigen::VectorXd beam_vel(4);
    Eigen::VectorXd beam_wtr_vel(4);
    Eigen::MatrixXd beam_unit(4,3);
    Eigen::MatrixXd beam_wtr_unit(4,3);
    Eigen::Vector3d velocity;
    Eigen::Vector3d wtr_velocity;
    double speed = 0;
    double course = 0;
    double altitude = 0;
    double wtr_speed = 0;
    double wtr_course = 0;
    int fillIn = 0;
    for (size_t i=0; i<sensor->NumBeams(); i++) {

        ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(i);
        if (sensor->BeamValid(i)) {
            ranges[i] = sensor->GetBeamRange(i) + GaussianKernel(0, gaussian_noise_range);
            raw_beam_vel(i) = sensor->GetBeamVelocity(i) + GaussianKernel(0, gaussian_noise_vel);
            beam_vel(fillIn) = raw_beam_vel(i);
            beam_unit(fillIn, 0) = beamUnit.X();
            beam_unit(fillIn, 1) = beamUnit.Y();
            beam_unit(fillIn, 2) = beamUnit.Z();
            altitude += ranges[i];
            fillIn++;
        } else {
            ranges[i] = std::numeric_limits<double>::quiet_NaN();
        }
        beam_wtr_vel(i) = sensor->GetBeamWaterVelocity(i) + GaussianKernel(0, gaussian_noise_wtr_vel);
        beam_wtr_unit(i, 0) = beamUnit.X();
        beam_wtr_unit(i, 1) = beamUnit.Y();
        beam_wtr_unit(i, 2) = beamUnit.Z();
    }

    if (fillIn >= 3) {
        // trim the arrays
        beam_unit = beam_unit.topRows(fillIn);
        beam_vel = beam_vel.head(fillIn);

        // solve a least-squares problem to get velocity based on the noisy ranges
        velocity = beam_unit.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(beam_vel);

        altitude /= static_cast<float>(fillIn);
        altitude *= cos(M_PI/6); // convert range to altitude

        speed = sqrt(velocity(0)*velocity(0) + velocity(1)*velocity(1));
        course = atan2(velocity(0), velocity(1)) * 180.0/M_PI;
        if (course < 0) {
            course += 360.0;
        }
    }
    // same method to compute a water track velocity based on the noisy beam water velocities
    wtr_velocity = beam_wtr_unit.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(beam_wtr_vel);
    wtr_speed = sqrt(wtr_velocity(0)*wtr_velocity(0) + wtr_velocity(1)*wtr_velocity(1));
    wtr_course = atan2(wtr_velocity(0), wtr_velocity(1)) * 180.0/M_PI;
    if (wtr_course < 0) {
        wtr_course += 360.0;
    }

    // publish DVL data message
    if(dvl_data_publisher.getNumSubscribers() > 0) {

        // prepare message header
        msg.header.frame_id = frame_name;
        msg.header.stamp.sec = current_time.sec;
        msg.header.stamp.nsec = current_time.nsec;
        msg.header.seq++;

        msg.ds_header.io_time.sec = current_time.sec;
        msg.ds_header.io_time.nsec = current_time.nsec;

        if ((fillIn >= 3) || (!water_track_enabled)) {
            msg.velocity.x = velocity(0);
            msg.velocity.y = velocity(1);
            msg.velocity.z = velocity(2);
            msg.course_gnd = course;
            msg.speed_gnd = speed;
            msg.velocity_mode = ds_sensor_msgs::Dvl::DVL_MODE_BOTTOM;
            msg.velocity_covar[0] = gaussian_noise_vel*gaussian_noise_vel;
            msg.velocity_covar[1] = 0;
            msg.velocity_covar[2] = 0;

            msg.velocity_covar[3] = 0;
            msg.velocity_covar[4] = gaussian_noise_vel*gaussian_noise_vel;
            msg.velocity_covar[5] = 0;

            msg.velocity_covar[6] = 0;
            msg.velocity_covar[7] = 0;
            msg.velocity_covar[8] = gaussian_noise_vel*gaussian_noise_vel;
        } else {
            msg.velocity.x = wtr_velocity(0);
            msg.velocity.y = wtr_velocity(1);
            msg.velocity.z = wtr_velocity(2);
            msg.course_gnd = wtr_course;
            msg.speed_gnd = wtr_speed;
            msg.velocity_mode = ds_sensor_msgs::Dvl::DVL_MODE_WATER;
            msg.velocity_covar[0] = gaussian_noise_wtr_vel*gaussian_noise_wtr_vel;
            msg.velocity_covar[1] = 0;
            msg.velocity_covar[2] = 0;

            msg.velocity_covar[3] = 0;
            msg.velocity_covar[4] = gaussian_noise_wtr_vel*gaussian_noise_wtr_vel;
            msg.velocity_covar[5] = 0;

            msg.velocity_covar[6] = 0;
            msg.velocity_covar[7] = 0;
            msg.velocity_covar[8] = gaussian_noise_wtr_vel*gaussian_noise_wtr_vel;
        }
        msg.num_good_beams = sensor->ValidBeams();
        msg.speed_sound = 1500.0;
        msg.altitude = altitude;

        for (size_t i=0; i<sensor->NumBeams(); i++) {
            msg.range[i] = ranges[i];
            msg.range_covar[i] = gaussian_noise_range*gaussian_noise_range;
            ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(i);
            msg.beam_unit_vec[i].x = beamUnit.X();
            msg.beam_unit_vec[i].y = beamUnit.Y();
            msg.beam_unit_vec[i].z = beamUnit.Z();
            if (fillIn >= 3) {
                msg.raw_velocity[i] = raw_beam_vel(i);
                msg.raw_velocity_covar[i] = gaussian_noise_vel*gaussian_noise_vel;
            } else {
                msg.raw_velocity[i] = beam_wtr_vel(i);
                msg.raw_velocity_covar[i] = gaussian_noise_wtr_vel*gaussian_noise_wtr_vel;
            }
        }

        //ROS_INFO_STREAM("DVL_SENDING_INST: " <<velocity(0) <<" " <<velocity(1) <<" " <<velocity(2));

        msg.coordinate_mode = ds_sensor_msgs::Dvl::DVL_COORD_INSTRUMENT;
        msg.dvl_time = static_cast<double>(current_time.sec) + static_cast<double>(current_time.nsec)/1.0e9;

        // publish data
        dvl_data_publisher.publish(msg);
        ros::spinOnce();
    }

    // publish current profile (ADCP) message
    if ((current_profile_publisher.getNumSubscribers() > 0) &&
        ((current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_BEAM) ||
         (current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_INSTRUMENT)) &&
        (water_track_enabled)) {  // No current profile if water track unavailable or unsupported mode
        // prepare message header
        adcp.header.frame_id = frame_name;
        adcp.header.stamp.sec = current_time.sec;
        adcp.header.stamp.nsec = current_time.nsec;
        adcp.header.seq++;

        msg.ds_header.io_time.sec = current_time.sec;
        msg.ds_header.io_time.nsec = current_time.nsec;

        // Fill message-level fields
        adcp.coordinate_mode = current_profile_coord_mode;
        adcp.adcp_type = ds_sensor_msgs::Adcp::ADCP_TYPE_PISTON;
        adcp.cells = water_track_bins;
        adcp.cell_depth = current_profile_cell_depth;
        adcp.bin0_distance = current_profile_bin0_distance;

        // Fill in beam-specific bin velocities as water velocity plus noise
        // out to the beam's current range.  No solution (0.0) beyond that
        for (size_t bin = 0; bin < water_track_bins; bin ++)
        {
            double bin_range = current_profile_bin0_distance +
                               (current_profile_cell_depth * bin);

            if (current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_BEAM)
            {   // Calculate a velocity in beam coordinates for each beam for every cell
                for (size_t beam=0; beam < sensor->NumBeams(); beam++)
                {
                    // add noise to the beam's bin-specific velocity
                    double bin_velocity = sensor->GetBeamWaterVelocityBin(beam, bin) +
                                          GaussianKernel(0, gaussian_noise_wtr_vel);
                    if (bin_velocity != gazebo::sensors::DsrosDvlBeam::NO_VELOCITY)
                    {
                        ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(beam);
                        adcp.vel_bin_beams[bin].velocity_bin_beam[beam].x = -bin_velocity * beamUnit.X();
                        adcp.vel_bin_beams[bin].velocity_bin_beam[beam].y = -bin_velocity * beamUnit.Y();
                        adcp.vel_bin_beams[bin].velocity_bin_beam[beam].z = -bin_velocity * beamUnit.Z();
                    } // if (beam_range >=...
                    else
                    {
                        adcp.vel_bin_beams[bin].velocity_bin_beam[beam].x = 0.0;
                        adcp.vel_bin_beams[bin].velocity_bin_beam[beam].y = 0.0;
                        adcp.vel_bin_beams[bin].velocity_bin_beam[beam].z = 0.0;
                    } // else
                } // for (size_t beam = 0;...
            } // if (current_profile_coord_mode ==...
            else
            {   // Calculate a single velocity in instrument coordinates for every cell
                bool range_solution = true;
                for (size_t beam=0; beam < sensor->NumBeams(); beam++)
                {
                    double bin_velocity = sensor->GetBeamWaterVelocityBin(beam, bin);
                    if (range_solution &&
                        (bin_velocity != gazebo::sensors::DsrosDvlBeam::NO_VELOCITY))
                    {
                        ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(beam);
                        beam_wtr_vel(beam) = sensor->GetBeamWaterVelocityBin(beam, bin) +
                                             GaussianKernel(0, gaussian_noise_wtr_vel);
                        beam_wtr_unit(beam, 0) = beamUnit.X();
                        beam_wtr_unit(beam, 1) = beamUnit.Y();
                        beam_wtr_unit(beam, 2) = beamUnit.Z();
                    } // if (!no_solution...
                    else
                    {
                        range_solution = false;
                    } // else
                } // for (size_t beam=0;...
                if (range_solution)  // if bin for any beam is beyond bottom, solution will be 0
                {
                    Eigen::Vector3d bin_velocity =
                        beam_wtr_unit.jacobiSvd(Eigen::ComputeThinU |
                                                Eigen::ComputeThinV).
                                      solve(beam_wtr_vel);
                    adcp.vel_bin_beams[bin].velocity_bin_beam[0].x = -bin_velocity[0];
                    adcp.vel_bin_beams[bin].velocity_bin_beam[0].y = -bin_velocity[1];
                    adcp.vel_bin_beams[bin].velocity_bin_beam[0].z = -bin_velocity[2];
                } // if (range_solution)
                else
                {
                    adcp.vel_bin_beams[bin].velocity_bin_beam[0].x = 0.0;
                    adcp.vel_bin_beams[bin].velocity_bin_beam[0].y = 0.0;
                    adcp.vel_bin_beams[bin].velocity_bin_beam[0].z = 0.0;
                } // else
            } // else
        } // for (size_t bin = 0;...

        current_profile_publisher.publish(adcp);
        ros::spinOnce();
    }

    // publish point cloud message
    if (pt_data_publisher.getNumSubscribers() > 0) {
        // prepare message header
        pt_msg.header.frame_id = pointcloud_frame;
        pt_msg.header.stamp.sec = current_time.sec;
        pt_msg.header.stamp.nsec = current_time.nsec;
        pt_msg.header.seq++;

        // fill in some points
        size_t NUM_PTS_PER_BEAM = 1000;
        double range = sensor->RangeMax() - sensor->RangeMin();
        pt_msg.points.resize(NUM_PTS_PER_BEAM*sensor->NumBeams()); // use 100 pts
        size_t fillIn = 0;
        for (size_t j=0; j<sensor->NumBeams(); j++) {

            ignition::math::Pose3d beamPose = sensor->GetBeamPose(j);
            for (size_t i=0; i<NUM_PTS_PER_BEAM; i++) {
                ignition::math::Vector3d vec;
                vec.X() = 0;
                vec.Y() = 0;
                vec.Z() = sensor->RangeMin() + static_cast<double>(i)/static_cast<double>(
                                NUM_PTS_PER_BEAM-1)*(range);

                ignition::math::Vector3d beam = beamPose.Rot().RotateVector(vec) + beamPose.Pos();
                pt_msg.points[fillIn].x = beam.X();
                pt_msg.points[fillIn].y = beam.Y();
                pt_msg.points[fillIn].z = beam.Z();
                fillIn++;
            }
        }
        // publish data
        pt_data_publisher.publish(pt_msg);
        ros::spinOnce();
    }

    // publish DVL contact range message
    if (rng_publisher.getNumSubscribers() > 0) {
        // prepare message header
        rng.header.frame_id = frame_name;
        rng.header.stamp.sec = current_time.sec;
        rng.header.stamp.nsec = current_time.nsec;
        rng.header.seq++;

        // fill in some points
        size_t NUM_PTS_PER_BEAM = 1000;
        rng.ranges.resize(sensor->NumBeams());
        for (size_t j=0; j<sensor->NumBeams(); j++) {
            // correctly report ranges in the instrument frame
            ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(j);
      	    rng.ranges[j].range.point.x = ranges[j]*beamUnit.X();
      	    rng.ranges[j].range.point.y = ranges[j]*beamUnit.Y();
      	    rng.ranges[j].range.point.z = ranges[j]*beamUnit.Z();
      	    if (sensor->BeamValid(j)) {
                rng.ranges[j].range_validity = ds_sensor_msgs::Range3D::RANGE_VALID;
                rng.ranges[j].range_quality = 250;
            } else {
                rng.ranges[j].range_validity = ds_sensor_msgs::Range3D::RANGE_INDETERMINANT;
                rng.ranges[j].range_quality = 10;
            }
    	      rng.ranges[j].range.header.frame_id = frame_name;
	          rng.ranges[j].range.header.stamp.sec = current_time.sec;
      	    rng.ranges[j].range.header.stamp.nsec = current_time.nsec;
        }
        // publish data
        rng_publisher.publish(rng);
        ros::spinOnce();
    }

    last_time = current_time;
}

double dsrosRosDvlSensor::GaussianKernel(double mu, double sigma) {
  // generation of two normalized uniform random variables
  double U1 = static_cast<double>(rand_r(&seed)) / static_cast<double>(RAND_MAX);
  double U2 = static_cast<double>(rand_r(&seed)) / static_cast<double>(RAND_MAX);

  // using Box-Muller transform to obtain a varaible with a standard normal distribution
  double Z0 = sqrt(-2.0 * ::log(U1)) * cos(2.0*M_PI * U2);

  // scaling
  Z0 = sigma * Z0 + mu;
  return Z0;
}

bool dsrosRosDvlSensor::LoadParameters() {

//loading parameters from the sdf file

  //NAMESPACE
  if (sdf->HasElement("robotNamespace"))
  {
    robot_namespace =  sdf->Get<std::string>("robotNamespace") +"/";
    ROS_INFO_STREAM("<robotNamespace> set to: "<<robot_namespace);
  }
  else
  {
    std::string scoped_name = sensor->ParentName();
    std::size_t it = scoped_name.find("::");

    robot_namespace = "/" +scoped_name.substr(0,it)+"/";
    ROS_WARN_STREAM("missing <robotNamespace>, set to default: " << robot_namespace);
  }

  //TOPIC
  if (sdf->HasElement("topicName"))
  {
    topic_name =  sdf->Get<std::string>("topicName");
    ROS_INFO_STREAM("<topicName> set to: "<<topic_name);
  }
  else
  {
    topic_name = "/dvl";
    ROS_WARN_STREAM("missing <topicName>, set to /namespace/default: " << topic_name);
  }

  //RANGES TOPIC
  if (sdf->HasElement("rangesTopicName"))
  {
    ranges_topic_name =  sdf->Get<std::string>("rangesTopicName");
    ROS_INFO_STREAM("<rangesTopicName> set to: "<<ranges_topic_name);
  }
  else
  {
    ranges_topic_name = "/dvl_ranges";
    ROS_WARN_STREAM("missing <rangesTopicName>, set to /namespace/default: " << ranges_topic_name);
  }

  //BODY NAME
  if (sdf->HasElement("frameName"))
  {
    frame_name =  sdf->Get<std::string>("frameName");
    ROS_INFO_STREAM("<frameName> set to: "<<frame_name);
  }
  else
  {
    ROS_FATAL("missing <frameName>, cannot proceed");
    return false;
  }

  if (sdf->HasElement("pointcloudFrame")) {
    pointcloud_frame =  sdf->Get<std::string>("pointcloudFrame");
    ROS_INFO_STREAM("<pointcloudFrame> set to: "<<pointcloud_frame);
  }
  else
  {
    ROS_FATAL("missing <pointcloudFrame>, cannot proceed");
    return false;

  }

  //UPDATE RATE
  if (sdf->HasElement("updateRateHZ"))
  {
    update_rate =  sdf->Get<double>("updateRateHZ");
    ROS_INFO_STREAM("<updateRateHZ> set to: " << update_rate);
  }
  else
  {
    update_rate = 1.0;
    ROS_WARN_STREAM("missing <updateRateHZ>, set to default: " << update_rate);
  }

  //NOISE
  if (sdf->HasElement("gaussianNoiseBeamVel"))
  {
    gaussian_noise_vel =  sdf->Get<double>("gaussianNoiseBeamVel");
    ROS_INFO_STREAM("<gaussianNoiseBeamVel> set to: " << gaussian_noise_vel);
  }
  else
  {
    gaussian_noise_vel = 0.0;
    ROS_WARN_STREAM("missing <gaussianNoiseBeamVel>, set to default: " << gaussian_noise_vel);
  }

  if (sdf->HasElement("gaussianNoiseBeamWtrVel"))
  {
    gaussian_noise_wtr_vel =  sdf->Get<double>("gaussianNoiseBeamWtrVel");
    ROS_INFO_STREAM("<gaussianNoiseBeamWtrVel> set to: " << gaussian_noise_wtr_vel);
  }
  else
  {
    gaussian_noise_wtr_vel = 2.0 * gaussian_noise_vel;
    ROS_WARN_STREAM("missing <gaussianNoiseBeamWtrVel>, set to default: " << gaussian_noise_wtr_vel);
  }

  if (sdf->HasElement("gaussianNoiseBeamRange"))
  {
    gaussian_noise_range =  sdf->Get<double>("gaussianNoiseBeamRange");
    ROS_INFO_STREAM("<gaussianNoiseBeamRange> set to: " << gaussian_noise_range);
  }
  else
  {
    gaussian_noise_range = 0.0;
    ROS_WARN_STREAM("missing <gaussianNoiseBeamRange>, set to default: " << gaussian_noise_range);
  }

  //WATER TRACKING
  if (sdf->HasElement("enableWaterTrack"))
  {
    water_track_enabled = sdf->Get<bool>("enableWaterTrack");
    ROS_INFO_STREAM("<enableWaterTrack> set to: " << water_track_enabled);
  }
  else
  {
    water_track_enabled = false;
    ROS_WARN_STREAM("missing <enableWaterTrack>, set to default: " << water_track_enabled);
  }

  if (sdf->HasElement("currentProfileCoordMode") && water_track_enabled)
  {
    current_profile_coord_mode = sdf->Get<int>("currentProfileCoordMode");
    ROS_INFO_STREAM("<currentProfileCoordMode> set to: " << current_profile_coord_mode);
  }
  else if (water_track_enabled)
  {
    current_profile_coord_mode = ds_sensor_msgs::Adcp::ADCP_COORD_BEAM;
    ROS_WARN_STREAM("missing <currentProfileCoordMode>, set to default: " << current_profile_coord_mode);
  }

  if (sdf->HasElement("waterTrackBins") && water_track_enabled)
  {
    water_track_bins = sdf->Get<int>("waterTrackBins");
    ROS_INFO_STREAM("<waterTrackBins> set to: " << water_track_bins);
  }
  else if (water_track_enabled)
  {
    water_track_bins = 1;
    ROS_WARN_STREAM("missing <waterTrackBins>, set to default: " << water_track_bins);
  }

  return true;
}


//-*****************************************************************************
//
// Copyright (c) 2009-2011,
//  Sony Pictures Imageworks, Inc. and
//  Industrial Light & Magic, a division of Lucasfilm Entertainment Company Ltd.
//
// All rights reserved.
//
// 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 Sony Pictures Imageworks, nor
// Industrial Light & Magic nor the names of their 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
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//-*****************************************************************************

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

#include <iostream>
#include <string>

namespace client
{
    namespace qi = boost::spirit::qi;
    namespace ascii = boost::spirit::ascii;

    ///////////////////////////////////////////////////////////////////////////////
    //  Parse roman hundreds (100..900) numerals using the symbol table.
    //  Notice that the data associated with each slot is the parser's attribute
    //  (which is passed to attached semantic actions).
    ///////////////////////////////////////////////////////////////////////////////
    //[tutorial_roman_hundreds
    struct hundreds_ : qi::symbols<char, unsigned>
    {
        hundreds_()
        {
            add
                ("C"    , 100)
                ("CC"   , 200)
                ("CCC"  , 300)
                ("CD"   , 400)
                ("D"    , 500)
                ("DC"   , 600)
                ("DCC"  , 700)
                ("DCCC" , 800)
                ("CM"   , 900)
            ;
        }

    } hundreds;
    //]

    ///////////////////////////////////////////////////////////////////////////////
    //  Parse roman tens (10..90) numerals using the symbol table.
    ///////////////////////////////////////////////////////////////////////////////
    //[tutorial_roman_tens
    struct tens_ : qi::symbols<char, unsigned>
    {
        tens_()
        {
            add
                ("X"    , 10)
                ("XX"   , 20)
                ("XXX"  , 30)
                ("XL"   , 40)
                ("L"    , 50)
                ("LX"   , 60)
                ("LXX"  , 70)
                ("LXXX" , 80)
                ("XC"   , 90)
            ;
        }

    } tens;
    //]

    ///////////////////////////////////////////////////////////////////////////////
    //  Parse roman ones (1..9) numerals using the symbol table.
    ///////////////////////////////////////////////////////////////////////////////
    //[tutorial_roman_ones
    struct ones_ : qi::symbols<char, unsigned>
    {
        ones_()
        {
            add
                ("I"    , 1)
                ("II"   , 2)
                ("III"  , 3)
                ("IV"   , 4)
                ("V"    , 5)
                ("VI"   , 6)
                ("VII"  , 7)
                ("VIII" , 8)
                ("IX"   , 9)
            ;
        }

    } ones;
    //]

    ///////////////////////////////////////////////////////////////////////////////
    //  roman (numerals) grammar
    //
    //      Note the use of the || operator. The expression
    //      a || b reads match a or b and in sequence. Try
    //      defining the roman numerals grammar in YACC or
    //      PCCTS. Spirit rules! :-)
    ///////////////////////////////////////////////////////////////////////////////
    //[tutorial_roman_grammar
    template <typename Iterator>
    struct roman : qi::grammar<Iterator, unsigned()>
    {
        roman() : roman::base_type(start)
        {
            using qi::eps;
            using qi::lit;
            using qi::_val;
            using qi::_1;
            using ascii::char_;

            start = eps             [_val = 0] >>
                (
                    +lit('M')       [_val += 1000]
                    ||  hundreds    [_val += _1]
                    ||  tens        [_val += _1]
                    ||  ones        [_val += _1]
                )
            ;
        }

        qi::rule<Iterator, unsigned()> start;
    };
    //]
}

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "\t\tRoman Numerals Parser\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Type a Roman Numeral ...or [q or Q] to quit\n\n";

    typedef std::string::const_iterator iterator_type;
    typedef client::roman<iterator_type> roman;

    roman roman_parser; // Our grammar

    std::string str;
    unsigned result;
    while ( std::getline( std::cin, str ) )
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        std::string::const_iterator iter = str.begin();
        std::string::const_iterator end = str.end();
        //[tutorial_roman_grammar_parse
        bool r = parse(iter, end, roman_parser, result);

        if (r && iter == end)
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing succeeded\n";
            std::cout << "result = " << result << std::endl;
            std::cout << "-------------------------\n";
        }
        else
        {
            std::string rest(iter, end);
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "stopped at: \": " << rest << "\"\n";
            std::cout << "-------------------------\n";
        }
        //]
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


#ifndef STAN_LANG_GRAMMARS_TERM_GRAMMAR_DEF_HPP
#define STAN_LANG_GRAMMARS_TERM_GRAMMAR_DEF_HPP

#include <stan/lang/ast.hpp>
#include <stan/lang/grammars/expression_grammar.hpp>
#include <stan/lang/grammars/indexes_grammar.hpp>
#include <stan/lang/grammars/semantic_actions.hpp>
#include <stan/lang/grammars/term_grammar.hpp>
#include <stan/lang/grammars/whitespace_grammar.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <string>
#include <sstream>
#include <vector>

BOOST_FUSION_ADAPT_STRUCT(stan::lang::index_op,
                          (stan::lang::expression, expr_)
                          (std::vector<std::vector<stan::lang::expression> >,
                           dimss_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::index_op_sliced,
                          (stan::lang::expression, expr_)
                          (std::vector<stan::lang::idx>, idxs_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::integrate_ode,
                          (std::string, integration_function_name_)
                          (std::string, system_function_name_)
                          (stan::lang::expression, y0_)
                          (stan::lang::expression, t0_)
                          (stan::lang::expression, ts_)
                          (stan::lang::expression, theta_)
                          (stan::lang::expression, x_)
                          (stan::lang::expression, x_int_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::integrate_ode_control,
                          (std::string, integration_function_name_)
                          (std::string, system_function_name_)
                          (stan::lang::expression, y0_)
                          (stan::lang::expression, t0_)
                          (stan::lang::expression, ts_)
                          (stan::lang::expression, theta_)
                          (stan::lang::expression, x_)
                          (stan::lang::expression, x_int_)
                          (stan::lang::expression, rel_tol_)
                          (stan::lang::expression, abs_tol_)
                          (stan::lang::expression, max_num_steps_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::algebra_solver,
                           (std::string, system_function_name_)
                           (stan::lang::expression, y_)
                           (stan::lang::expression, theta_)
                           (stan::lang::expression, x_r_)
                           (stan::lang::expression, x_i_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::algebra_solver_control,
                           (std::string, system_function_name_)
                           (stan::lang::expression, y_)
                           (stan::lang::expression, theta_)
                           (stan::lang::expression, x_r_)
                           (stan::lang::expression, x_i_)
                           (stan::lang::expression, rel_tol_)
                           (stan::lang::expression, fun_tol_)
                           (stan::lang::expression, max_num_steps_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::univariate_integral_control,
                          (std::string, integration_function_name_)
                          (std::string, system_function_name_)
                          (stan::lang::expression, t0_)
                          (stan::lang::expression, t1_)
                          (stan::lang::expression, theta_)
                          (stan::lang::expression, x_r_)
                          (stan::lang::expression, x_i_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::generalOdeModel_control,
                          (std::string, integration_function_name_)
                          (std::string, system_function_name_)
                          (stan::lang::expression, nCmt_)
                          (stan::lang::expression, time_)
                          (stan::lang::expression, amt_)
                          (stan::lang::expression, rate_)
                          (stan::lang::expression, ii_)
                          (stan::lang::expression, evid_)
                          (stan::lang::expression, cmt_)
                          (stan::lang::expression, addl_)
                          (stan::lang::expression, ss_)
                          (stan::lang::expression, pMatrix_)
                          (stan::lang::expression, biovar_)
                          (stan::lang::expression, tlag_)
                          (stan::lang::expression, rel_tol_)
                          (stan::lang::expression, abs_tol_)
                          (stan::lang::expression, max_num_steps_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::fun,
                          (std::string, name_)
                          (std::vector<stan::lang::expression>, args_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::array_expr,
                          (std::vector<stan::lang::expression>, args_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::row_vector_expr,
                          (std::vector<stan::lang::expression>, args_) )

BOOST_FUSION_ADAPT_STRUCT(stan::lang::int_literal,
                          (int, val_)
                          (stan::lang::expr_type, type_))

BOOST_FUSION_ADAPT_STRUCT(stan::lang::double_literal,
                          (double, val_)
                          (stan::lang::expr_type, type_) )

namespace stan {

  namespace lang {

    template <typename Iterator>
    term_grammar<Iterator>::term_grammar(variable_map& var_map,
                                         std::stringstream& error_msgs,
                                         expression_grammar<Iterator>& eg)
      : term_grammar::base_type(term_r),
        var_map_(var_map),
        error_msgs_(error_msgs),
        expression_g(eg),
        indexes_g(var_map, error_msgs, eg) {
      using boost::spirit::qi::_1;
      using boost::spirit::qi::_a;
      using boost::spirit::qi::_b;
      using boost::spirit::qi::_c;
      using boost::spirit::qi::_d;
      using boost::spirit::qi::char_;
      using boost::spirit::qi::double_;
      using boost::spirit::qi::eps;
      using boost::spirit::qi::int_;
      using boost::spirit::qi::hold;
      using boost::spirit::qi::lexeme;
      using boost::spirit::qi::lit;
      using boost::spirit::qi::no_skip;
      using boost::spirit::qi::string;
      using boost::spirit::qi::_pass;
      using boost::spirit::qi::_val;
      using boost::spirit::qi::labels::_r1;

      term_r.name("expression");
      term_r
        = (negated_factor_r(_r1)[assign_lhs_f(_val, _1)]
            >> *((lit('*') > negated_factor_r(_r1)
                             [multiplication_f(_val, _1,
                                           boost::phoenix::ref(error_msgs_))])
                 | (lit('/') > negated_factor_r(_r1)
                               [division_f(_val, _1,
                                           boost::phoenix::ref(error_msgs_))])
                 | (lit('%') > negated_factor_r(_r1)
                               [modulus_f(_val, _1, _pass,
                                          boost::phoenix::ref(error_msgs_))])
                 | (lit('\\') > negated_factor_r(_r1)
                                [left_division_f(_val, _pass, _1,
                                         boost::phoenix::ref(error_msgs_))])
                 | (lit(".*") > negated_factor_r(_r1)
                                [elt_multiplication_f(_val, _1,
                                          boost::phoenix::ref(error_msgs_))])
                 | (lit("./") > negated_factor_r(_r1)
                                [elt_division_f(_val, _1,
                                        boost::phoenix::ref(error_msgs_))])));

      negated_factor_r.name("expression");
      negated_factor_r
        = lit('-') >> negated_factor_r(_r1)
                      [negate_expr_f(_val, _1, _pass,
                                     boost::phoenix::ref(error_msgs_))]
        | lit('!') >> negated_factor_r(_r1)
                      [logical_negate_expr_f(_val, _1,
                                             boost::phoenix::ref(error_msgs_))]
        | lit('+') >> negated_factor_r(_r1)[assign_lhs_f(_val, _1)]
        | exponentiated_factor_r(_r1)[assign_lhs_f(_val, _1)];

      exponentiated_factor_r.name("expression");
      exponentiated_factor_r
        = idx_factor_r(_r1)[assign_lhs_f(_val, _1)]
        >> -(lit('^')
             > negated_factor_r(_r1)
               [exponentiation_f(_val, _1, _r1, _pass,
                                 boost::phoenix::ref(error_msgs_))]);

      idx_factor_r.name("expression");
      idx_factor_r
        =  factor_r(_r1)[assign_lhs_f(_val, _1)]
        > *( ( (+dims_r(_r1))[assign_lhs_f(_a, _1)]
               > eps
               [add_expression_dimss_f(_val, _a, _pass,
                                       boost::phoenix::ref(error_msgs_) )] )
            | (indexes_g(_r1)[assign_lhs_f(_b, _1)]
               > eps[add_idxs_f(_val, _b, _pass,
                              boost::phoenix::ref(error_msgs_))])
            | (lit("'")
               > eps[transpose_f(_val, _pass,
                                 boost::phoenix::ref(error_msgs_))]) );

      integrate_ode_control_r.name("expression");
      integrate_ode_control_r
        %= ( (string("integrate_ode_rk45") >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("integrate_ode_bdf") >> no_skip[!char_("a-zA-Z0-9_")]) )
        >> lit('(')              // >> allows backtracking to non-control
        >> identifier_r          // 1) system function name (function only)
        >> lit(',')
        >> expression_g(_r1)     // 2) y0
        >> lit(',')
        >> expression_g(_r1)     // 3) t0 (data only)
        >> lit(',')
        >> expression_g(_r1)     // 4) ts (data only)
        >> lit(',')
        >> expression_g(_r1)     // 5) theta
        >> lit(',')
        >> expression_g(_r1)     // 6) x (data only)
        >> lit(',')
        >> expression_g(_r1)     // 7) x_int (data only)
        >> lit(',')
        >> expression_g(_r1)     // 8) relative tolerance (data only)
        >> lit(',')
        >> expression_g(_r1)     // 9) absolute tolerance (data only)
        >> lit(',')
        >> expression_g(_r1)     // 10) maximum number of steps (data only)
        > lit(')')
          [validate_integrate_ode_control_f(_val, boost::phoenix::ref(var_map_),
                                            _pass,
                                            boost::phoenix::ref(error_msgs_))];

      integrate_ode_r.name("expression");
      integrate_ode_r
        %= ( (string("integrate_ode_rk45") >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("integrate_ode_bdf") >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("integrate_ode") >> no_skip[!char_("a-zA-Z0-9_")])
               [deprecated_integrate_ode_f(boost::phoenix::ref(error_msgs_))] )
        > lit('(')
        > identifier_r          // 1) system function name (function only)
        > lit(',')
        > expression_g(_r1)     // 2) y0
        > lit(',')
        > expression_g(_r1)     // 3) t0 (data only)
        > lit(',')
        > expression_g(_r1)     // 4) ts (data only)
        > lit(',')
        > expression_g(_r1)     // 5) theta
        > lit(',')
        > expression_g(_r1)     // 6) x (data only)
        > lit(',')
        > expression_g(_r1)     // 7) x_int (data only)
        > lit(')')
          [validate_integrate_ode_f(_val, boost::phoenix::ref(var_map_),
                                    _pass, boost::phoenix::ref(error_msgs_))];

      algebra_solver_control_r.name("expression");
      algebra_solver_control_r
        %= lit("algebra_solver")
        >> lit('(')
        >> identifier_r          // 1) system function name (function only)
        >> lit(',')
        >> expression_g(_r1)     // 2) y (data only)
        >> lit(',')
        >> expression_g(_r1)     // 3) theta
        >> lit(',')
        >> expression_g(_r1)     // 4) x_r (data only)
        >> lit(',')
        >> expression_g(_r1)     // 5) x_i (data only)
        >> lit(',')
        >> expression_g(_r1)     // 6) relative tolerance (data only)
        >> lit(',')
        >> expression_g(_r1)     // 7) function tolerance (data only)
        >> lit(',')
        >> expression_g(_r1)     // 8) maximum number of steps (data only)
        > lit(')')
          [validate_algebra_solver_control_f(_val,
                                             boost::phoenix::ref(var_map_),
                                             _pass,
                                             boost::phoenix::ref(error_msgs_))];

      algebra_solver_r.name("expression");
      algebra_solver_r
        %= lit("algebra_solver")
        > lit('(')
        > identifier_r          // 1) system function name (function only)
        > lit(',')
        > expression_g(_r1)     // 2) y (data only)
        > lit(',')
        > expression_g(_r1)     // 3) theta
        > lit(',')
        > expression_g(_r1)     // 4) x_r (data only)
        > lit(',')
        > expression_g(_r1)     // 5) x_i (data only)
        > lit(')')
        [validate_algebra_solver_f(_val, boost::phoenix::ref(var_map_),
                                   _pass, boost::phoenix::ref(error_msgs_))];

      univariate_integral_control_r.name("expression");
      univariate_integral_control_r
        %= ( (string("univariate_integral_rk45") >>
              no_skip[!char_("a-zA-Z0-9_")])
             | (string("univariate_integral_bdf") >>
                no_skip[!char_("a-zA-Z0-9_")]) )
        > lit('(')
        > identifier_r          // 1) system function name (function only)
        > lit(',')
        > expression_g(_r1)     // 2) t0 (data only)
        > lit(',')
        > expression_g(_r1)     // 2) t1 (data only)
        > lit(',')
        > expression_g(_r1)     // 3) theta
        > lit(',')
        > expression_g(_r1)     // 4) x_r (data only)
        > lit(',')
        > expression_g(_r1)     // 5) x_i (data only)
        > lit(')')
        [validate_univariate_integral_control_f(_val,
                                       boost::phoenix::ref(var_map_),
                                       _pass,
                                       boost::phoenix::ref(error_msgs_))];

      generalOdeModel_control_r.name("expression");
      generalOdeModel_control_r
        %= ( (string("generalOdeModel_bdf") >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("generalOdeModel_rk45")
                >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("mixOde1CptModel_rk45")
                >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("mixOde1CptModel_bdf")
                >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("mixOde2CptModel_rk45")
                >> no_skip[!char_("a-zA-Z0-9_")])
             | (string("mixOde2CptModel_bdf")
                >> no_skip[!char_("a-zA-Z0-9_")]))
        > lit('(')            // >> allows backtracking to non-control
        > identifier_r        // 1) system function name (function only)
        > lit(',')
        > expression_g(_r1)   // 2) nCmt
        > lit(',')
        > expression_g(_r1)   // 3) time
        > lit(',')
        > expression_g(_r1)   // 4) amt
        > lit(',')
        > expression_g(_r1)   // 5) rate
        > lit(',')
        > expression_g(_r1)   // 6) ii
        > lit(',')
        > expression_g(_r1)   // 7) evid (data only)
        > lit(',')
        > expression_g(_r1)   // 8) cmt (data only)
        > lit(',')
        > expression_g(_r1)   // 9) addl (data only)
        > lit(',')
        > expression_g(_r1)   // 10) ss (data only)
        > lit(',')
        > expression_g(_r1)   // 11) pMatrix
        > lit(',')
        > expression_g(_r1)   // 12) biovar
        > lit(',')
        > expression_g(_r1)   // 13) tlag
        > lit(',')
        > expression_g(_r1)   // 14) relative tolerance (data only)
        > lit(',')
        > expression_g(_r1)   // 15) absolute tolerance (data only)
        > lit(',')
        > expression_g(_r1)   // 16) maximum number of steps
        > lit(')')
        [validate_generalOdeModel_control_f(_val,
                               boost::phoenix::ref(var_map_), _pass,
                               boost::phoenix::ref(error_msgs_))];

      factor_r.name("expression");
      factor_r =
        integrate_ode_control_r(_r1)[assign_lhs_f(_val, _1)]
        | integrate_ode_r(_r1)[assign_lhs_f(_val, _1)]
        | algebra_solver_control_r(_r1)[assign_lhs_f(_val, _1)]
        | algebra_solver_r(_r1)[assign_lhs_f(_val, _1)]
        | univariate_integral_control_r(_r1)[assign_lhs_f(_val, _1)]
        | generalOdeModel_control_r(_r1)[assign_lhs_f(_val, _1)]
        | (fun_r(_r1)[assign_lhs_f(_b, _1)]
           > eps[set_fun_type_named_f(_val, _b, _r1, _pass,
                                      boost::phoenix::ref(var_map_),
                                      boost::phoenix::ref(error_msgs_))])
        | (variable_r[assign_lhs_f(_a, _1)]
           > eps[set_var_type_f(_a, _val, boost::phoenix::ref(var_map_),
                                boost::phoenix::ref(error_msgs_),
                                _pass)])
        | int_literal_r[assign_lhs_f(_val, _1)]
        | double_literal_r[assign_lhs_f(_val, _1)]
        | (array_expr_r(_r1)[assign_lhs_f(_c, _1)]
           > eps[infer_array_expr_type_f(_val, _c, _r1, _pass,
                                       boost::phoenix::ref(var_map_),
                                       boost::phoenix::ref(error_msgs_))])
        | (vec_expr_r(_r1)[assign_lhs_f(_d, _1)]
           > eps[infer_vec_or_matrix_expr_type_f(_val, _d, _r1, _pass,
                                     boost::phoenix::ref(var_map_),
                                     boost::phoenix::ref(error_msgs_))])
        | (lit('(')
           > expression_g(_r1)[assign_lhs_f(_val, _1)]
           > lit(')'));

      int_literal_r.name("integer literal");
      int_literal_r
        %= int_
        >> !(lit('.') | lit('e') | lit('E'));

      double_literal_r.name("real literal");
      double_literal_r
        %= double_;

      fun_r.name("function and argument expressions");
      fun_r
        %= (hold[identifier_r[is_prob_fun_f(_1, _pass)]]
            >> &lit('(')
            > prob_args_r(_r1))
        | (identifier_r >> args_r(_r1));

      identifier_r.name("identifier");
      identifier_r
        %= lexeme[char_("a-zA-Z")
                  >> *char_("a-zA-Z0-9_.")];

      prob_args_r.name("probability function argument");
      prob_args_r
        %= (lit('(') >> lit(')'))
        | hold[lit('(')
               >> expression_g(_r1)
               >> lit(')')]
        | (lit('(')
           >> expression_g(_r1)
           >> (lit(',')
               [require_vbar_f(_pass, boost::phoenix::ref(error_msgs_))]
               | (eps > lit('|')))
           >> (expression_g(_r1) % ',')
           >> lit(')'));

      args_r.name("function arguments");
      args_r
        %= (lit('(') >> lit(')'))
        | (lit('(') >> (expression_g(_r1) % ',') >> lit(')'));

      dim_r.name("array dimension (integer expression)");
      dim_r
        %= expression_g(_r1)
        >> eps[validate_int_expr_silent_f(_val, _pass)];

      dims_r.name("array dimensions");
      dims_r
        %= lit('[')
        >> (dim_r(_r1)
           % ',' )
        >> lit(']');

      variable_r.name("variable name");
      variable_r
        %= identifier_r
        > !lit('(');    // negative lookahead to prevent failure in
                        // fun to try to evaluate as variable [cleaner
                        // error msgs]

      array_expr_r.name("array expression");
      array_expr_r
        %=  lit('{')
        >> expression_g(_r1) % ','
        >> lit('}');

      vec_expr_r.name("row vector or matrix expression");
      vec_expr_r
        %=  lit('[')
        >> expression_g(_r1) % ','
        >> lit(']');
    }
  }
}
#endif