//
//  Copyright (c) 2003
//  Gunter Winkler, Joerg Walter
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_VECTOR_OF_VECTOR_
#define _BOOST_UBLAS_VECTOR_OF_VECTOR_

#include <boost/type_traits.hpp>

#include <boost/numeric/ublas/storage_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>

// Iterators based on ideas of Jeremy Siek

namespace boost { namespace numeric { namespace ublas {

    // uBLAS sparse vector based sparse matrix class
    // FIXME outer vector can be sparse type but it is completely filled
    template<class T, class L, class A>
    class generalized_vector_of_vector:
        public matrix_container<generalized_vector_of_vector<T, L, A> > {

        typedef T &true_reference;
        typedef T *pointer;
        typedef const T *const_pointer;
        typedef L layout_type;
        typedef generalized_vector_of_vector<T, L, A> self_type;
    public:
#ifdef BOOST_UBLAS_ENABLE_PROXY_SHORTCUTS
        using matrix_container<self_type>::operator ();
#endif
        typedef typename A::size_type size_type;
        typedef typename A::difference_type difference_type;
        typedef T value_type;
        typedef const T &const_reference;
#ifndef BOOST_UBLAS_STRICT_VECTOR_SPARSE
        typedef T &reference;
#else
        typedef sparse_matrix_element<self_type> reference;
#endif
        typedef A array_type;
        typedef const matrix_reference<const self_type> const_closure_type;
        typedef matrix_reference<self_type> closure_type;
        typedef typename A::value_type vector_data_value_type;
        typedef vector_data_value_type vector_temporary_type;
        typedef self_type matrix_temporary_type;
        typedef sparse_tag storage_category;
        typedef typename L::orientation_category orientation_category;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector ():
            matrix_container<self_type> (),
            size1_ (0), size2_ (0), data_ (1) {
            const size_type sizeM = layout_type::size_M (size1_, size2_);
             // create size1+1 empty vector elements
            data_.insert_element (sizeM, vector_data_value_type ());
            storage_invariants ();
        }
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector (size_type size1, size_type size2, size_type non_zeros = 0):
            matrix_container<self_type> (),
            size1_ (size1), size2_ (size2), data_ (layout_type::size_M (size1_, size2_) + 1) {
            const size_type sizeM = layout_type::size_M (size1_, size2_);
            const size_type sizem = layout_type::size_m (size1_, size2_);
            for (size_type i = 0; i < sizeM; ++ i) // create size1 vector elements
                data_.insert_element (i, vector_data_value_type ()) .resize (sizem, false);
            data_.insert_element (sizeM, vector_data_value_type ());
            storage_invariants ();
        }
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector (const generalized_vector_of_vector &m):
            matrix_container<self_type> (),
            size1_ (m.size1_), size2_ (m.size2_), data_ (m.data_) {
            storage_invariants ();
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector (const matrix_expression<AE> &ae, size_type non_zeros = 0):
            matrix_container<self_type> (),
            size1_ (ae ().size1 ()), size2_ (ae ().size2 ()), data_ (layout_type::size_M (size1_, size2_) + 1) {
            const size_type sizeM = layout_type::size_M (size1_, size2_);
            const size_type sizem = layout_type::size_m (size1_, size2_);
            for (size_type i = 0; i < sizeM; ++ i) // create size1 vector elements
                data_.insert_element (i, vector_data_value_type ()) .resize (sizem, false);
            data_.insert_element (sizeM, vector_data_value_type ());
            storage_invariants ();
            matrix_assign<scalar_assign> (*this, ae);
        }

        // Accessors
        BOOST_UBLAS_INLINE
        size_type size1 () const {
            return size1_;
        }
        BOOST_UBLAS_INLINE
        size_type size2 () const {
            return size2_;
        }
        BOOST_UBLAS_INLINE
        size_type nnz_capacity () const {
            size_type non_zeros = 0;
            for (const_vectoriterator_type itv = data_.begin (); itv != data_.end (); ++ itv)
                non_zeros += (*itv).nnz_capacity ();
            return non_zeros;
        }
        BOOST_UBLAS_INLINE
        size_type nnz () const {
            size_type non_zeros = 0;
            for (const_vectoriterator_type itv = data_.begin (); itv != data_.end (); ++ itv)
                non_zeros += (*itv).nnz ();
            return non_zeros;
        }

        // Storage accessors
        BOOST_UBLAS_INLINE
        const array_type &data () const {
            return data_;
        }
        BOOST_UBLAS_INLINE
        array_type &data () {
            return data_;
        }

        // Resizing
        BOOST_UBLAS_INLINE
        void resize (size_type size1, size_type size2, bool preserve = true) {
            const size_type oldM = layout_type::size_M (size1_, size2_);
            size1_ = size1;
            size2_ = size2;
            const size_type sizeM = layout_type::size_M (size1_, size2_);
            const size_type sizem = layout_type::size_m (size1_, size2_);
            data ().resize (sizeM + 1, preserve);
            if (preserve) {
                for (size_type i = 0; (i <= oldM) && (i < sizeM); ++ i)
                    ref (data () [i]).resize (sizem, preserve);
                for (size_type i = oldM+1; i < sizeM; ++ i) // create new vector elements
                    data_.insert_element (i, vector_data_value_type ()) .resize (sizem, false);
                if (sizeM > oldM) {
                    data_.insert_element (sizeM, vector_data_value_type ());
                } else {
                    ref (data () [sizeM]).resize (0, false);
                }
            } else {
                for (size_type i = 0; i < sizeM; ++ i)
                    data_.insert_element (i, vector_data_value_type ()) .resize (sizem, false);
                data_.insert_element (sizeM, vector_data_value_type ());
            }
            storage_invariants ();
        }

        // Element support
        BOOST_UBLAS_INLINE
        pointer find_element (size_type i, size_type j) {
            return const_cast<pointer> (const_cast<const self_type&>(*this).find_element (i, j));
        }
        BOOST_UBLAS_INLINE
        const_pointer find_element (size_type i, size_type j) const {
            const size_type elementM = layout_type::index_M (i, j);
            const size_type elementm = layout_type::index_m (i, j);
            // optimise: check the storage_type and index directly if element always exists
            if (boost::is_convertible<typename array_type::storage_category, packed_tag>::value) {
                return & (data () [elementM] [elementm]);
            }
            else {
                const typename array_type::value_type *pv = data ().find_element (elementM);
                if (!pv)
                    return 0;
                return pv->find_element (elementm);
            }
        }

        // Element access
        BOOST_UBLAS_INLINE
        const_reference operator () (size_type i, size_type j) const {
            const_pointer p = find_element (i, j);
            // optimise: check the storage_type and index directly if element always exists
            if (boost::is_convertible<typename array_type::storage_category, packed_tag>::value) {
                BOOST_UBLAS_CHECK (p, internal_logic () );
                return *p;
            }
            else {
                if (p)
                    return *p;
                else
                    return zero_;
            }
        }
        BOOST_UBLAS_INLINE
        reference operator () (size_type i, size_type j) {
#ifndef BOOST_UBLAS_STRICT_MATRIX_SPARSE
            return at_element (i, j);
#else
            return reference (*this, i, j);
#endif
        }

        // Assignment
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector &operator = (const generalized_vector_of_vector &m) {
            if (this != &m) {
                size1_ = m.size1_;
                size2_ = m.size2_;
                data () = m.data ();
            }
            storage_invariants ();
            return *this;
        }
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector &assign_temporary (generalized_vector_of_vector &m) {
            swap (m);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector &operator = (const matrix_expression<AE> &ae) {
            self_type temporary (ae);
            return assign_temporary (temporary);
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector &assign (const matrix_expression<AE> &ae) {
            matrix_assign<scalar_assign> (*this, ae);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector& operator += (const matrix_expression<AE> &ae) {
            self_type temporary (*this + ae);
            return assign_temporary (temporary);
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector &plus_assign (const matrix_expression<AE> &ae) {
            matrix_assign<scalar_plus_assign> (*this, ae);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector& operator -= (const matrix_expression<AE> &ae) {
            self_type temporary (*this - ae);
            return assign_temporary (temporary);
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector &minus_assign (const matrix_expression<AE> &ae) {
            matrix_assign<scalar_minus_assign> (*this, ae);
            return *this;
        }
        template<class AT>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector& operator *= (const AT &at) {
            matrix_assign_scalar<scalar_multiplies_assign> (*this, at);
            return *this;
        }
        template<class AT>
        BOOST_UBLAS_INLINE
        generalized_vector_of_vector& operator /= (const AT &at) {
            matrix_assign_scalar<scalar_divides_assign> (*this, at);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (generalized_vector_of_vector &m) {
            if (this != &m) {
                std::swap (size1_, m.size1_);
                std::swap (size2_, m.size2_);
                data ().swap (m.data ());
            }
            storage_invariants ();
        }
        BOOST_UBLAS_INLINE
        friend void swap (generalized_vector_of_vector &m1, generalized_vector_of_vector &m2) {
            m1.swap (m2);
        }

        // Sorting
        void sort () {
            vectoriterator_type itv (data ().begin ());
            vectoriterator_type itv_end (data ().end ());
            while (itv != itv_end) {
                (*itv).sort ();
                ++ itv;
            }
        }

        // Element insertion and erasure
        BOOST_UBLAS_INLINE
        true_reference insert_element (size_type i, size_type j, const_reference t) {
            const size_type elementM = layout_type::index_M (i, j);
            const size_type elementm = layout_type::index_m (i, j);
            vector_data_value_type& vd (ref (data () [elementM]));
            storage_invariants ();
            return vd.insert_element (elementm, t);
        }
        BOOST_UBLAS_INLINE
        void append_element (size_type i, size_type j, const_reference t) {
            const size_type elementM = layout_type::index_M (i, j);
            const size_type elementm = layout_type::index_m (i, j);
            vector_data_value_type& vd (ref (data () [elementM]));
            storage_invariants ();
            return vd.append_element (elementm, t);
        }
        BOOST_UBLAS_INLINE
        void erase_element (size_type i, size_type j) {
            vectoriterator_type itv (data ().find (layout_type::index_M (i, j)));
            if (itv == data ().end ())
                return;
            (*itv).erase_element (layout_type::index_m (i, j));
            storage_invariants ();
        }
        BOOST_UBLAS_INLINE
        void clear () {
            const size_type sizeM = layout_type::size_M (size1_, size2_);
            // FIXME should clear data () if this is done via value_type/*zero*/() then it is not size preserving
            for (size_type i = 0; i < sizeM; ++ i)
                ref (data () [i]).clear ();
            storage_invariants ();
        }

        // Iterator types
    private:
        // Use vector iterator
        typedef typename A::const_iterator const_vectoriterator_type;
        typedef typename A::iterator vectoriterator_type;
        typedef typename A::value_type::const_iterator const_subiterator_type;
        typedef typename A::value_type::iterator subiterator_type;

        BOOST_UBLAS_INLINE
        true_reference at_element (size_type i, size_type j) {
            return ref (ref (data () [layout_type::index_M (i, j)]) [layout_type::index_m (i, j)]);
        }

    public:
        class const_iterator1;
        class iterator1;
        class const_iterator2;
        class iterator2;
        typedef reverse_iterator_base1<const_iterator1> const_reverse_iterator1;
        typedef reverse_iterator_base1<iterator1> reverse_iterator1;
        typedef reverse_iterator_base2<const_iterator2> const_reverse_iterator2;
        typedef reverse_iterator_base2<iterator2> reverse_iterator2;

        // Element lookup
        // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
        const_iterator1 find1 (int rank, size_type i, size_type j, int direction = 1) const {
            for (;;) {
                const_vectoriterator_type itv (data ().find (layout_type::index_M (i, j)));
                const_vectoriterator_type itv_end (data ().end ());
                if (itv == itv_end)
                    return const_iterator1 (*this, rank, i, j, itv_end, (*(-- itv)).end ());

                const_subiterator_type it ((*itv).find (layout_type::index_m (i, j)));
                const_subiterator_type it_end ((*itv).end ());
                if (rank == 0)
                    return const_iterator1 (*this, rank, i, j, itv, it);
                if (it != it_end && it.index () == layout_type::index_m (i, j))
                    return const_iterator1 (*this, rank, i, j, itv, it);
                if (direction > 0) {
                    if (layout_type::fast_i ()) {
                        if (it == it_end)
                            return const_iterator1 (*this, rank, i, j, itv, it);
                        i = it.index ();
                    } else {
                        if (i >= size1_)
                            return const_iterator1 (*this, rank, i, j, itv, it);
                        ++ i;
                    }
                } else /* if (direction < 0)  */ {
                    if (layout_type::fast_i ()) {
                        if (it == (*itv).begin ())
                            return const_iterator1 (*this, rank, i, j, itv, it);
                        --it;
                        i = it.index ();
                    } else {
                        if (i == 0)
                            return const_iterator1 (*this, rank, i, j, itv, it);
                        -- i;
                    }
                }
            }
        }
        // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
        iterator1 find1 (int rank, size_type i, size_type j, int direction = 1) {
            for (;;) {
                vectoriterator_type itv (data ().find (layout_type::index_M (i, j)));
                vectoriterator_type itv_end (data ().end ());
                if (itv == itv_end)
                    return iterator1 (*this, rank, i, j, itv_end, (*(-- itv)).end ());

                subiterator_type it ((*itv).find (layout_type::index_m (i, j)));
                subiterator_type it_end ((*itv).end ());
                if (rank == 0)
                    return iterator1 (*this, rank, i, j, itv, it);
                if (it != it_end && it.index () == layout_type::index_m (i, j))
                    return iterator1 (*this, rank, i, j, itv, it);
                if (direction > 0) {
                    if (layout_type::fast_i ()) {
                        if (it == it_end)
                            return iterator1 (*this, rank, i, j, itv, it);
                        i = it.index ();
                    } else {
                        if (i >= size1_)
                            return iterator1 (*this, rank, i, j, itv, it);
                        ++ i;
                    }
                } else /* if (direction < 0)  */ {
                    if (layout_type::fast_i ()) {
                        if (it == (*itv).begin ())
                            return iterator1 (*this, rank, i, j, itv, it);
                        --it;
                        i = it.index ();
                    } else {
                        if (i == 0)
                            return iterator1 (*this, rank, i, j, itv, it);
                        -- i;
                    }
                }
            }
        }
        // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
        const_iterator2 find2 (int rank, size_type i, size_type j, int direction = 1) const {
            for (;;) {
                const_vectoriterator_type itv (data ().find (layout_type::index_M (i, j)));
                const_vectoriterator_type itv_end (data ().end ());
                if (itv == itv_end)
                    return const_iterator2 (*this, rank, i, j, itv_end, (*(-- itv)).end ());

                const_subiterator_type it ((*itv).find (layout_type::index_m (i, j)));
                const_subiterator_type it_end ((*itv).end ());
                if (rank == 0)
                    return const_iterator2 (*this, rank, i, j, itv, it);
                if (it != it_end && it.index () == layout_type::index_m (i, j))
                    return const_iterator2 (*this, rank, i, j, itv, it);
                if (direction > 0) {
                    if (layout_type::fast_j ()) {
                        if (it == it_end)
                            return const_iterator2 (*this, rank, i, j, itv, it);
                        j = it.index ();
                    } else {
                        if (j >= size2_)
                            return const_iterator2 (*this, rank, i, j, itv, it);
                        ++ j;
                    }
                } else /* if (direction < 0)  */ {
                    if (layout_type::fast_j ()) {
                        if (it == (*itv).begin ())
                            return const_iterator2 (*this, rank, i, j, itv, it);
                        --it;
                        j = it.index ();
                    } else {
                        if (j == 0)
                            return const_iterator2 (*this, rank, i, j, itv, it);
                        -- j;
                    }
                }
            }
        }
        // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
        iterator2 find2 (int rank, size_type i, size_type j, int direction = 1) {
            for (;;) {
                vectoriterator_type itv (data ().find (layout_type::index_M (i, j)));
                vectoriterator_type itv_end (data ().end ());
                if (itv == itv_end)
                    return iterator2 (*this, rank, i, j, itv_end, (*(-- itv)).end ());

                subiterator_type it ((*itv).find (layout_type::index_m (i, j)));
                subiterator_type it_end ((*itv).end ());
                if (rank == 0)
                    return iterator2 (*this, rank, i, j, itv, it);
                if (it != it_end && it.index () == layout_type::index_m (i, j))
                    return iterator2 (*this, rank, i, j, itv, it);
                if (direction > 0) {
                    if (layout_type::fast_j ()) {
                        if (it == it_end)
                            return iterator2 (*this, rank, i, j, itv, it);
                        j = it.index ();
                    } else {
                        if (j >= size2_)
                            return iterator2 (*this, rank, i, j, itv, it);
                        ++ j;
                    }
                } else /* if (direction < 0)  */ {
                    if (layout_type::fast_j ()) {
                        if (it == (*itv).begin ())
                            return iterator2 (*this, rank, i, j, itv, it);
                        --it;
                        j = it.index ();
                    } else {
                        if (j == 0)
                            return iterator2 (*this, rank, i, j, itv, it);
                        -- j;
                    }
                }
            }
        }

        class const_iterator1:
            public container_const_reference<generalized_vector_of_vector>,
            public bidirectional_iterator_base<sparse_bidirectional_iterator_tag,
                                               const_iterator1, value_type> {
        public:
            typedef typename generalized_vector_of_vector::difference_type difference_type;
            typedef typename generalized_vector_of_vector::value_type value_type;
            typedef typename generalized_vector_of_vector::const_reference reference;
            typedef const typename generalized_vector_of_vector::pointer pointer;

            typedef const_iterator2 dual_iterator_type;
            typedef const_reverse_iterator2 dual_reverse_iterator_type;

            // Construction and destruction
            BOOST_UBLAS_INLINE
            const_iterator1 ():
                container_const_reference<self_type> (), rank_ (), i_ (), j_ (), itv_ (), it_ () {}
            BOOST_UBLAS_INLINE
            const_iterator1 (const self_type &m, int rank, size_type i, size_type j, const const_vectoriterator_type &itv, const const_subiterator_type &it):
                container_const_reference<self_type> (m), rank_ (rank), i_ (i), j_ (j), itv_ (itv), it_ (it) {}
            BOOST_UBLAS_INLINE
            const_iterator1 (const iterator1 &it):
                container_const_reference<self_type> (it ()), rank_ (it.rank_), i_ (it.i_), j_ (it.j_), itv_ (it.itv_), it_ (it.it_) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            const_iterator1 &operator ++ () {
                if (rank_ == 1 && layout_type::fast_i ())
                    ++ it_;
                else {
                    const self_type &m = (*this) ();
                    i_ = index1 () + 1;
                    if (rank_ == 1 && ++ itv_ == m.end1 ().itv_)
                        *this = m.find1 (rank_, i_, j_, 1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index2 () != j_)
                            *this = m.find1 (rank_, i_, j_, 1);
                    }
                }
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator1 &operator -- () {
                if (rank_ == 1 && layout_type::fast_i ())
                    -- it_;
                else {
                    const self_type &m = (*this) ();
                    i_ = index1 () - 1;
                    if (rank_ == 1 && -- itv_ == m.end1 ().itv_)
                        *this = m.find1 (rank_, i_, j_, -1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index2 () != j_)
                            *this = m.find1 (rank_, i_, j_, -1);
                    }
                }
                return *this;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            const_reference operator * () const {
                BOOST_UBLAS_CHECK (index1 () < (*this) ().size1 (), bad_index ());
                BOOST_UBLAS_CHECK (index2 () < (*this) ().size2 (), bad_index ());
                if (rank_ == 1) {
                    return *it_;
                } else {
                    return (*this) () (i_, j_);
                }
            }

#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator2 begin () const {
                const self_type &m = (*this) ();
                return m.find2 (1, index1 (), 0);
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator2 cbegin () const {
                return begin ();
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator2 end () const {
                const self_type &m = (*this) ();
                return m.find2 (1, index1 (), m.size2 ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator2 cend () const {
                return end ();
            }

            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator2 rbegin () const {
                return const_reverse_iterator2 (end ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator2 crbegin () const {
                return rbegin ();
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator2 rend () const {
                return const_reverse_iterator2 (begin ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator2 crend () const {
                return rend ();
            }
#endif

            // Indices
            BOOST_UBLAS_INLINE
            size_type index1 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find1 (0, (*this) ().size1 (), j_), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_M (itv_.index (), it_.index ()) < (*this) ().size1 (), bad_index ());
                    return layout_type::index_M (itv_.index (), it_.index ());
                } else {
                    return i_;
                }
            }
            BOOST_UBLAS_INLINE
            size_type index2 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find1 (0, (*this) ().size1 (), j_), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_m (itv_.index (), it_.index ()) < (*this) ().size2 (), bad_index ());
                    return layout_type::index_m (itv_.index (), it_.index ());
                } else {
                    return j_;
                }
            }

            // Assignment
            BOOST_UBLAS_INLINE
            const_iterator1 &operator = (const const_iterator1 &it) {
                container_const_reference<self_type>::assign (&it ());
                rank_ = it.rank_;
                i_ = it.i_;
                j_ = it.j_;
                itv_ = it.itv_;
                it_ = it.it_;
                return *this;
            }

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const const_iterator1 &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                // BOOST_UBLAS_CHECK (rank_ == it.rank_, internal_logic ());
                if (rank_ == 1 || it.rank_ == 1) {
                    return it_ == it.it_;
                } else {
                    return i_ == it.i_ && j_ == it.j_;
                }
            }

        private:
            int rank_;
            size_type i_;
            size_type j_;
            const_vectoriterator_type itv_;
            const_subiterator_type it_;
        };

        BOOST_UBLAS_INLINE
        const_iterator1 begin1 () const {
            return find1 (0, 0, 0);
        }
        BOOST_UBLAS_INLINE
        const_iterator1 cbegin1 () const {
            return begin1 ();
        }
        BOOST_UBLAS_INLINE
        const_iterator1 end1 () const {
            return find1 (0, size1_, 0);
        }
        BOOST_UBLAS_INLINE
        const_iterator1 cend1 () const {
            return end1 ();
        }

        class iterator1:
            public container_reference<generalized_vector_of_vector>,
            public bidirectional_iterator_base<sparse_bidirectional_iterator_tag,
                                               iterator1, value_type> {
        public:
            typedef typename generalized_vector_of_vector::difference_type difference_type;
            typedef typename generalized_vector_of_vector::value_type value_type;
            typedef typename generalized_vector_of_vector::true_reference reference;
            typedef typename generalized_vector_of_vector::pointer pointer;

            typedef iterator2 dual_iterator_type;
            typedef reverse_iterator2 dual_reverse_iterator_type;

            // Construction and destruction
            BOOST_UBLAS_INLINE
            iterator1 ():
                container_reference<self_type> (), rank_ (), i_ (), j_ (), itv_ (), it_ () {}
            BOOST_UBLAS_INLINE
            iterator1 (self_type &m, int rank, size_type i, size_type j, const vectoriterator_type &itv, const subiterator_type &it):
                container_reference<self_type> (m), rank_ (rank), i_ (i), j_ (j), itv_ (itv), it_ (it) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            iterator1 &operator ++ () {
                if (rank_ == 1 && layout_type::fast_i ())
                    ++ it_;
                else {
                    self_type &m = (*this) ();
                    i_ = index1 () + 1;
                    if (rank_ == 1 && ++ itv_ == m.end1 ().itv_)
                        *this = m.find1 (rank_, i_, j_, 1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index2 () != j_)
                            *this = m.find1 (rank_, i_, j_, 1);
                    }
                }
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator1 &operator -- () {
                if (rank_ == 1 && layout_type::fast_i ())
                    -- it_;
                else {
                    self_type &m = (*this) ();
                    i_ = index1 () - 1;
                    if (rank_ == 1 && -- itv_ == m.end1 ().itv_)
                        *this = m.find1 (rank_, i_, j_, -1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index2 () != j_)
                            *this = m.find1 (rank_, i_, j_, -1);
                    }
                }
                return *this;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            true_reference operator * () const {
                BOOST_UBLAS_CHECK (index1 () < (*this) ().size1 (), bad_index ());
                BOOST_UBLAS_CHECK (index2 () < (*this) ().size2 (), bad_index ());
                if (rank_ == 1) {
                    return *it_;
                } else {
                    return (*this) ().at_element (i_, j_);
                }
            }

#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            iterator2 begin () const {
                self_type &m = (*this) ();
                return m.find2 (1, index1 (), 0);
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            iterator2 end () const {
                self_type &m = (*this) ();
                return m.find2 (1, index1 (), m.size2 ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            reverse_iterator2 rbegin () const {
                return reverse_iterator2 (end ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            reverse_iterator2 rend () const {
                return reverse_iterator2 (begin ());
            }
#endif

            // Indices
            BOOST_UBLAS_INLINE
            size_type index1 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find1 (0, (*this) ().size1 (), j_), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_M (itv_.index (), it_.index ()) < (*this) ().size1 (), bad_index ());
                    return layout_type::index_M (itv_.index (), it_.index ());
                } else {
                    return i_;
                }
            }
            BOOST_UBLAS_INLINE
            size_type index2 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find1 (0, (*this) ().size1 (), j_), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_m (itv_.index (), it_.index ()) < (*this) ().size2 (), bad_index ());
                    return layout_type::index_m (itv_.index (), it_.index ());
                } else {
                    return j_;
                }
            }

            // Assignment
            BOOST_UBLAS_INLINE
            iterator1 &operator = (const iterator1 &it) {
                container_reference<self_type>::assign (&it ());
                rank_ = it.rank_;
                i_ = it.i_;
                j_ = it.j_;
                itv_ = it.itv_;
                it_ = it.it_;
                return *this;
            }

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const iterator1 &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                // BOOST_UBLAS_CHECK (rank_ == it.rank_, internal_logic ());
                if (rank_ == 1 || it.rank_ == 1) {
                    return it_ == it.it_;
                } else {
                    return i_ == it.i_ && j_ == it.j_;
                }
            }

        private:
            int rank_;
            size_type i_;
            size_type j_;
            vectoriterator_type itv_;
            subiterator_type it_;

            friend class const_iterator1;
        };

        BOOST_UBLAS_INLINE
        iterator1 begin1 () {
            return find1 (0, 0, 0);
        }
        BOOST_UBLAS_INLINE
        iterator1 end1 () {
            return find1 (0, size1_, 0);
        }

        class const_iterator2:
            public container_const_reference<generalized_vector_of_vector>,
            public bidirectional_iterator_base<sparse_bidirectional_iterator_tag,
                                               const_iterator2, value_type> {
        public:
            typedef typename generalized_vector_of_vector::difference_type difference_type;
            typedef typename generalized_vector_of_vector::value_type value_type;
            typedef typename generalized_vector_of_vector::const_reference reference;
            typedef const typename generalized_vector_of_vector::pointer pointer;

            typedef const_iterator1 dual_iterator_type;
            typedef const_reverse_iterator1 dual_reverse_iterator_type;

            // Construction and destruction
            BOOST_UBLAS_INLINE
            const_iterator2 ():
                container_const_reference<self_type> (), rank_ (), i_ (), j_ (), itv_ (), it_ () {}
            BOOST_UBLAS_INLINE
            const_iterator2 (const self_type &m, int rank, size_type i, size_type j, const const_vectoriterator_type &itv, const const_subiterator_type &it):
                container_const_reference<self_type> (m), rank_ (rank), i_ (i), j_ (j), itv_ (itv), it_ (it) {}
            BOOST_UBLAS_INLINE
            const_iterator2 (const iterator2 &it):
                container_const_reference<self_type> (it ()), rank_ (it.rank_), i_ (it.i_), j_ (it.j_), itv_ (it.itv_), it_ (it.it_) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            const_iterator2 &operator ++ () {
                if (rank_ == 1 && layout_type::fast_j ())
                    ++ it_;
                else {
                    const self_type &m = (*this) ();
                    j_ = index2 () + 1;
                    if (rank_ == 1 && ++ itv_ == m.end2 ().itv_)
                        *this = m.find2 (rank_, i_, j_, 1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index1 () != i_)
                            *this = m.find2 (rank_, i_, j_, 1);
                    }
                }
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator2 &operator -- () {
                if (rank_ == 1 && layout_type::fast_j ())
                    -- it_;
                else {
                    const self_type &m = (*this) ();
                    j_ = index2 () - 1;
                    if (rank_ == 1 && -- itv_ == m.end2 ().itv_)
                        *this = m.find2 (rank_, i_, j_, -1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index1 () != i_)
                            *this = m.find2 (rank_, i_, j_, -1);
                    }
                }
                return *this;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            const_reference operator * () const {
                BOOST_UBLAS_CHECK (index1 () < (*this) ().size1 (), bad_index ());
                BOOST_UBLAS_CHECK (index2 () < (*this) ().size2 (), bad_index ());
                if (rank_ == 1) {
                    return *it_;
                } else {
                    return (*this) () (i_, j_);
                }
            }

#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator1 begin () const {
                const self_type &m = (*this) ();
                return m.find1 (1, 0, index2 ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator1 cbegin () const {
                return begin ();
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator1 end () const {
                const self_type &m = (*this) ();
                return m.find1 (1, m.size1 (), index2 ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_iterator1 cend () const {
                return end ();
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator1 rbegin () const {
                return const_reverse_iterator1 (end ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator1 crbegin () const {
                return rbegin ();
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator1 rend () const {
                return const_reverse_iterator1 (begin ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            const_reverse_iterator1 crend () const {
                return rend ();
            }
#endif

            // Indices
            BOOST_UBLAS_INLINE
            size_type index1 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find2 (0, i_, (*this) ().size2 ()), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_M (itv_.index (), it_.index ()) < (*this) ().size1 (), bad_index ());
                    return layout_type::index_M (itv_.index (), it_.index ());
                } else {
                    return i_;
                }
            }
            BOOST_UBLAS_INLINE
            size_type index2 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find2 (0, i_, (*this) ().size2 ()), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_m (itv_.index (), it_.index ()) < (*this) ().size2 (), bad_index ());
                    return layout_type::index_m (itv_.index (), it_.index ());
                } else {
                    return j_;
                }
            }

            // Assignment
            BOOST_UBLAS_INLINE
            const_iterator2 &operator = (const const_iterator2 &it) {
                container_const_reference<self_type>::assign (&it ());
                rank_ = it.rank_;
                i_ = it.i_;
                j_ = it.j_;
                itv_ = it.itv_;
                it_ = it.it_;
                return *this;
            }

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const const_iterator2 &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                // BOOST_UBLAS_CHECK (rank_ == it.rank_, internal_logic ());
                if (rank_ == 1 || it.rank_ == 1) {
                    return it_ == it.it_;
                } else {
                    return i_ == it.i_ && j_ == it.j_;
                }
            }

        private:
            int rank_;
            size_type i_;
            size_type j_;
            const_vectoriterator_type itv_;
            const_subiterator_type it_;
        };

        BOOST_UBLAS_INLINE
        const_iterator2 begin2 () const {
            return find2 (0, 0, 0);
        }
        BOOST_UBLAS_INLINE
        const_iterator2 cbegin2 () const {
            return begin2 ();
        }
        BOOST_UBLAS_INLINE
        const_iterator2 end2 () const {
            return find2 (0, 0, size2_);
        }
        BOOST_UBLAS_INLINE
        const_iterator2 cend2 () const {
            return end2 ();
        }

        class iterator2:
            public container_reference<generalized_vector_of_vector>,
            public bidirectional_iterator_base<sparse_bidirectional_iterator_tag,
                                               iterator2, value_type> {
        public:
            typedef typename generalized_vector_of_vector::difference_type difference_type;
            typedef typename generalized_vector_of_vector::value_type value_type;
            typedef typename generalized_vector_of_vector::true_reference reference;
            typedef typename generalized_vector_of_vector::pointer pointer;

            typedef iterator1 dual_iterator_type;
            typedef reverse_iterator1 dual_reverse_iterator_type;

            // Construction and destruction
            BOOST_UBLAS_INLINE
            iterator2 ():
                container_reference<self_type> (), rank_ (), i_ (), j_ (), itv_ (), it_ () {}
            BOOST_UBLAS_INLINE
            iterator2 (self_type &m, int rank, size_type i, size_type j, const vectoriterator_type &itv, const subiterator_type &it):
                container_reference<self_type> (m), rank_ (rank), i_ (i), j_ (j), itv_ (itv), it_ (it) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            iterator2 &operator ++ () {
                if (rank_ == 1 && layout_type::fast_j ())
                    ++ it_;
                else {
                    self_type &m = (*this) ();
                    j_ = index2 () + 1;
                    if (rank_ == 1 && ++ itv_ == m.end2 ().itv_)
                        *this = m.find2 (rank_, i_, j_, 1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index1 () != i_)
                            *this = m.find2 (rank_, i_, j_, 1);
                    }
                }
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator2 &operator -- () {
                if (rank_ == 1 && layout_type::fast_j ())
                    -- it_;
                else {
                    self_type &m = (*this) ();
                    j_ = index2 () - 1;
                    if (rank_ == 1 && -- itv_ == m.end2 ().itv_)
                        *this = m.find2 (rank_, i_, j_, -1);
                    else if (rank_ == 1) {
                        it_ = (*itv_).begin ();
                        if (it_ == (*itv_).end () || index1 () != i_)
                            *this = m.find2 (rank_, i_, j_, -1);
                    }
                }
                return *this;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            true_reference operator * () const {
                BOOST_UBLAS_CHECK (index1 () < (*this) ().size1 (), bad_index ());
                BOOST_UBLAS_CHECK (index2 () < (*this) ().size2 (), bad_index ());
                if (rank_ == 1) {
                    return *it_;
                } else {
                    return (*this) ().at_element (i_, j_);
                }
            }

#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            iterator1 begin () const {
                self_type &m = (*this) ();
                return m.find1 (1, 0, index2 ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            iterator1 end () const {
                self_type &m = (*this) ();
                return m.find1 (1, m.size1 (), index2 ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            reverse_iterator1 rbegin () const {
                return reverse_iterator1 (end ());
            }
            BOOST_UBLAS_INLINE
#ifdef BOOST_UBLAS_MSVC_NESTED_CLASS_RELATION
            typename self_type::
#endif
            reverse_iterator1 rend () const {
                return reverse_iterator1 (begin ());
            }
#endif

            // Indices
            BOOST_UBLAS_INLINE
            size_type index1 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find2 (0, i_, (*this) ().size2 ()), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_M (itv_.index (), it_.index ()) < (*this) ().size1 (), bad_index ());
                    return layout_type::index_M (itv_.index (), it_.index ());
                } else {
                    return i_;
                }
            }
            BOOST_UBLAS_INLINE
            size_type index2 () const {
                BOOST_UBLAS_CHECK (*this != (*this) ().find2 (0, i_, (*this) ().size2 ()), bad_index ());
                if (rank_ == 1) {
                    BOOST_UBLAS_CHECK (layout_type::index_m (itv_.index (), it_.index ()) < (*this) ().size2 (), bad_index ());
                    return layout_type::index_m (itv_.index (), it_.index ());
                } else {
                    return j_;
                }
            }

            // Assignment
            BOOST_UBLAS_INLINE
            iterator2 &operator = (const iterator2 &it) {
                container_reference<self_type>::assign (&it ());
                rank_ = it.rank_;
                i_ = it.i_;
                j_ = it.j_;
                itv_ = it.itv_;
                it_ = it.it_;
                return *this;
            }

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const iterator2 &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                // BOOST_UBLAS_CHECK (rank_ == it.rank_, internal_logic ());
                if (rank_ == 1 || it.rank_ == 1) {
                    return it_ == it.it_;
                } else {
                    return i_ == it.i_ && j_ == it.j_;
                }
            }

        private:
            int rank_;
            size_type i_;
            size_type j_;
            vectoriterator_type itv_;
            subiterator_type it_;

            friend class const_iterator2;
        };

        BOOST_UBLAS_INLINE
        iterator2 begin2 () {
            return find2 (0, 0, 0);
        }
        BOOST_UBLAS_INLINE
        iterator2 end2 () {
            return find2 (0, 0, size2_);
        }

        // Reverse iterators

        BOOST_UBLAS_INLINE
        const_reverse_iterator1 rbegin1 () const {
            return const_reverse_iterator1 (end1 ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator1 crbegin1 () const {
            return rbegin1 ();
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator1 rend1 () const {
            return const_reverse_iterator1 (begin1 ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator1 crend1 () const {
            return rend1 ();
        }

        BOOST_UBLAS_INLINE
        reverse_iterator1 rbegin1 () {
            return reverse_iterator1 (end1 ());
        }
        BOOST_UBLAS_INLINE
        reverse_iterator1 rend1 () {
            return reverse_iterator1 (begin1 ());
        }

        BOOST_UBLAS_INLINE
        const_reverse_iterator2 rbegin2 () const {
            return const_reverse_iterator2 (end2 ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator2 crbegin2 () const {
            return rbegin2 ();
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator2 rend2 () const {
            return const_reverse_iterator2 (begin2 ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator2 crend2 () const {
            return rend2 ();
        }

        BOOST_UBLAS_INLINE
        reverse_iterator2 rbegin2 () {
            return reverse_iterator2 (end2 ());
        }
        BOOST_UBLAS_INLINE
        reverse_iterator2 rend2 () {
            return reverse_iterator2 (begin2 ());
        }

         // Serialization
        template<class Archive>
        void serialize(Archive & ar, const unsigned int /* file_version */){

            // we need to copy to a collection_size_type to get a portable
            // and efficient serialization
            serialization::collection_size_type s1 (size1_);
            serialization::collection_size_type s2 (size2_);

            // serialize the sizes
            ar & serialization::make_nvp("size1",s1)
               & serialization::make_nvp("size2",s2);

            // copy the values back if loading
            if (Archive::is_loading::value) {
                size1_ = s1;
                size2_ = s2;
            }

            ar & serialization::make_nvp("data", data_);

            storage_invariants();
        }

    private:
        void storage_invariants () const
        {
            BOOST_UBLAS_CHECK (layout_type::size_M (size1_, size2_) + 1 == data_.size (), internal_logic ());
            BOOST_UBLAS_CHECK (data ().begin () != data ().end (), internal_logic ());

        }
        size_type size1_;
        size_type size2_;
        array_type data_;
        static const value_type zero_;
    };

    template<class T, class L, class A>
    const typename generalized_vector_of_vector<T, L, A>::value_type generalized_vector_of_vector<T, L, A>::zero_ = value_type/*zero*/();

}}}

#endif


//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

#ifndef BOOST_UBLAS_TENSOR_EXPRESSIONS_HPP
#define BOOST_UBLAS_TENSOR_EXPRESSIONS_HPP

#include <cstddef>
#include <boost/numeric/ublas/expression_types.hpp>

namespace boost   {
namespace numeric {
namespace ublas   {

template<class element_type, class storage_format, class storage_type>
class tensor;

template<class size_type>
class basic_extents;

//TODO: put in fwd.hpp
struct tensor_tag {};

}
}
}

namespace boost   {
namespace numeric {
namespace ublas   {
namespace detail  {

/** @\brief base class for tensor expressions
 *
 * \note implements crtp - no use of virtual function calls
 *
 * \tparam T type of the tensor
 * \tparam E type of the derived expression (crtp)
 *
 **/
template<class T, class E>
struct tensor_expression
    : public ublas_expression<E>
{
	//	static const unsigned complexity = 0;
	using expression_type = E;
	using type_category = tensor_tag;
	using tensor_type = T;

	BOOST_UBLAS_INLINE
	auto const& operator()() const { return *static_cast<const expression_type*> (this); }

protected :
	explicit tensor_expression() = default;
	tensor_expression(const tensor_expression&) = delete;
	tensor_expression& operator=(const tensor_expression&) = delete;
};

template<class T, class EL, class ER, class OP>
struct binary_tensor_expression
    : public tensor_expression <T, binary_tensor_expression<T,EL,ER,OP>>
{
	using self_type = binary_tensor_expression<T,EL,ER,OP>;
	using tensor_type  = T;
	using binary_operation = OP;
	using expression_type_left  = EL;
	using expression_type_right = ER;
	using derived_type =  tensor_expression <tensor_type,self_type>;

	using size_type = typename tensor_type::size_type;

	explicit binary_tensor_expression(expression_type_left  const& l, expression_type_right const& r, binary_operation o)
	  : el(l) , er(r) , op(o) {}
	binary_tensor_expression() = delete;
	binary_tensor_expression(const binary_tensor_expression& l) = delete;
	binary_tensor_expression(binary_tensor_expression&& l)
	  : el(l.el), er(l.er), op(l.op) {}

	BOOST_UBLAS_INLINE
	decltype(auto)  operator()(size_type i) const { return op(el(i), er(i)); }

	expression_type_left const& el;
	expression_type_right const& er;
	binary_operation op;
};

/// @brief helper function to simply instantiation of lambda proxy class
template<class T, class EL, class ER, class OP>
auto make_binary_tensor_expression( tensor_expression<T,EL> const& el, tensor_expression<T,ER> const& er, OP op)
{
	return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
auto make_binary_tensor_expression( matrix_expression<EL> const& el, tensor_expression<T,ER> const& er, OP op)
{
	return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
auto make_binary_tensor_expression( tensor_expression<T,EL> const& el, matrix_expression<ER> const& er, OP op)
{
	return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
auto make_binary_tensor_expression( vector_expression<EL> const& el, tensor_expression<T,ER> const& er, OP op)
{
	return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
auto make_binary_tensor_expression( tensor_expression<T,EL> const& el, vector_expression<ER> const& er, OP op)
{
	return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class E, class OP>
struct unary_tensor_expression
    : public tensor_expression <T, unary_tensor_expression<T,E,OP>>
{

	using self_type = unary_tensor_expression<T,E,OP>;
	using tensor_type  = T;
	using expression_type = E;

	using derived_type = tensor_expression <T, unary_tensor_expression<T,E,OP>>;

	using size_type = typename tensor_type::size_type;

	explicit unary_tensor_expression(E const& ee, OP o) : e(ee) , op(o) {}
	unary_tensor_expression() = delete;
	unary_tensor_expression(const unary_tensor_expression& l) = delete;
	unary_tensor_expression(unary_tensor_expression&& l)
	  : e(l.e), op(op.l) {}

	BOOST_UBLAS_INLINE
	decltype(auto) operator()(size_type i) const { return op(e(i)); }

	E const& e;
	OP op;
};

// \brief helper function to simply instantiation of lambda proxy class
template<class T, class E, class OP>
auto make_unary_tensor_expression( tensor_expression<T,E> const& e, OP op)
{
	return unary_tensor_expression<T,E,OP>( e() , op);
}

template<class T, class E, class OP>
auto make_unary_tensor_expression( matrix_expression<E> const& e, OP op)
{
	return unary_tensor_expression<T,E,OP>( e() , op);
}

template<class T, class E, class OP>
auto make_unary_tensor_expression( vector_expression<E> const& e, OP op)
{
	return unary_tensor_expression<T,E,OP>( e() , op);
}

}
}
}
}
#endif


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#if !BOOST_PP_IS_ITERATING

#ifndef CPPQEDCORE_QUANTUMOPERATOR_TRIDIAGONAL_TCC_INCLUDED
#define CPPQEDCORE_QUANTUMOPERATOR_TRIDIAGONAL_TCC_INCLUDED

#include "Tridiagonal.h"

#include "ComplexArrayExtensions.h"

#include <boost/lambda/lambda.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/range/algorithm_ext/for_each.hpp>

#include <boost/preprocessor/iteration/iterate.hpp>
#include <boost/preprocessor/repetition/enum.hpp>

#include <algorithm>

namespace quantumoperator {

namespace details {

template<bool IS_MULTIPLICATION, int RANK1, int RANK2> // if not multiply then sum
const typename Tridiagonal<RANK1+RANK2>::Diagonals
directDiagonals(const typename Tridiagonal<RANK1>::Diagonals& ds1,
                const typename Tridiagonal<RANK2>::Diagonals& ds2)
{
  using namespace blitzplusplus; // implies blitz
  using namespace linalg       ;

  typedef Tridiagonal<RANK1+RANK2> TridiagonalRes;
  typedef Tridiagonal<RANK1> Tridiagonal1;
  typedef Tridiagonal<RANK2> Tridiagonal2;

  typename TridiagonalRes::Diagonals res;
  size_t length=ds2.numElements();

  for (size_t i=0; i<ds1.numElements(); ++i) for (size_t j=0; j<length; ++j) {
      const typename Tridiagonal1::Diagonal& d1=ds1(i);
      const typename Tridiagonal2::Diagonal& d2=ds2(j);
      typename TridiagonalRes::Diagonal& d=res(i*length+j);

      d.reference(doDirect<IS_MULTIPLICATION,RANK1,RANK2>(d1,d2));

    }

  return res;

} // NEEDS_WORK this is now runtime, but (a smaller) part of it could be done compile-time. Ugh. Ouch.

} // details

template<int RANK> template<int RANK2>
Tridiagonal<RANK>::Tridiagonal(const Tridiagonal<RANK2>& t1, const Tridiagonal<RANK-RANK2>& t2)
  : Base(blitzplusplus::concatenateTinies(t1.getDimensions(),t2.getDimensions())),
    diagonals_(blitzplusplus::ShallowCopy(),details::directDiagonals<true,RANK2,RANK-RANK2>(t1.get(),t2.get())),
    differences_(blitzplusplus::concatenateTinies(t1.getDifferences(),t2.getDifferences())),
    tCurrent_(t1.getTime()),
    freqs_(blitzplusplus::ShallowCopy(),details::directDiagonals<false,RANK2,RANK-RANK2>(t1.getFreqs(),t2.getFreqs()))
{
  if (t1.getTime()!=t2.getTime()) throw std::runtime_error("Tridiagonal time mismatch");
}

template<int RANK>
Tridiagonal<RANK>&
Tridiagonal<RANK>::operator*=(dcomp dc)
{
  boost::for_each(diagonals_,boost::lambda::_1*=dc);
  return *this;
}

template<int RANK>
const Tridiagonal<RANK> Tridiagonal<RANK>::hermitianConjugate() const
{
  struct helper {

    static int transposeIndex(int ind)
    {
      int res=0;
      // std::cout<<ind<<' ';
      for (int k=RANK-1; k>=0; k--) {
        int threeToK=int(floor(pow(3.,k)));
        int ik=int(floor(ind/double(threeToK)));
        res+=((3-ik)%3)*threeToK;
        ind-=ik*threeToK;
      }
      // std::cout<<res<<std::endl;
      return res;
    }

  };

  Tridiagonal res(*this,Diagonals(),differences_,tCurrent_,freqs_);

  for (int ind=0; ind<LENGTH; ind++)
    res.diagonals_(helper::transposeIndex(ind)).reference(Diagonal(conj(diagonals_(ind))));
  // Here a new copy of the conjugated diagonal gets created & referenced.

  // freqs_ at any moment stores the correct transposed values as well

  return res;

}

template<int RANK>
Tridiagonal<RANK>&
Tridiagonal<RANK>::operator+=(const Tridiagonal& tridiag)
{
  boost::for_each(tridiag.differences_,differences_,[](size_t otherDifference, size_t& difference) {
    if (!difference) difference=otherDifference;
    else if (otherDifference && difference!=otherDifference) throw std::invalid_argument("Tridiagonal structure mismatch is binOp1");
  });

  boost::for_each(tridiag.diagonals_,diagonals_,[](const typename Tridiagonal<RANK>::Diagonal& from, typename Tridiagonal<RANK>::Diagonal& to) {
    if (from.size()) {
      if (!to.size()) {
        to.resize(from.shape());
        to=from;
      }
      else to+=from; // This will check for the compatibility of shapes
    }
  });

  boost::for_each(tridiag.freqs_,freqs_,[](const typename Tridiagonal<RANK>::Diagonal& from, typename Tridiagonal<RANK>::Diagonal& to) {
    if (from.size()) {
      if (to.size() && all(to!=from)) throw std::runtime_error("Tridiagonal structure mismatch is operator+=");
      else {
        to.resize(from.shape());
        to=from;
      }
    }
  });

  return *this;

}

template<int RANK>
Tridiagonal<RANK>& Tridiagonal<RANK>::propagate(double t)
{
  // Diagonal (i=0) left out
  if (double dt=t-tCurrent_)
    for (int i=1; i<LENGTH; i++)
      if (diagonals_(i).size() && freqs_(i).size())
        diagonals_(i)*=exp(dt*freqs_(i));
  tCurrent_=t;
  return *this;
}

template<int RANK>
Tridiagonal<RANK>
furnishWithFreqs(const Tridiagonal<RANK>& tridiag, const typename Tridiagonal<RANK>::Diagonal& mainDiagonal)
{
  Tridiagonal<RANK> res(tridiag);
  return res.furnishWithFreqs(mainDiagonal);
}

template<int RANK>
std::ostream& operator<<(std::ostream& os, const Tridiagonal<RANK>& tridiag)
{
  using namespace std;
  os<<tridiag.get()<<endl<<tridiag.getFreqs()<<endl<<tridiag.getDifferences()<<endl<<tridiag.getDimensions()<<endl;
  return os;
}

} // quantumoperator

#ifndef DO_CONSIDER_EXPLICITLY_SPECIALIZED_TRIDIAGONAL_APPLIES

namespace quantumoperator {

template<int RANK> template<typename ICW>
void Tridiagonal<RANK>::FillRangesHelper::operator()(ICW)
{
  using blitz::Range;

  static const int i=ICW::value;

  ranges_(i)(0)=Range(0,ubound_(i));
  ranges_(i)(2)=((
                  ranges_(i)(1)=Range(0,ubound_(i)-int(k_(i)))
                  )+int(k_(i)));

}

template<int RANK>
const typename Tridiagonal<RANK>::Ranges Tridiagonal<RANK>::fillRanges(const typename StateVectorLow::T_index& ubound) const
{
  Ranges res;

  mpl::for_each<tmptools::Ordinals<RANK> >(FillRangesHelper(res,ubound,differences_));
  return res;
}

template<int RANK>
void Tridiagonal<RANK>::apply(const StateVectorLow& psi, StateVectorLow& dpsidt) const
{
  static const int step=tmptools::Power_v<3,RANK-1>;

  using tmptools::Vector, tmptools::integral_c;

  const Ranges ranges(fillRanges(psi.ubound()));

  doApply<0*step,Vector<0>,Vector<0>,Vector<0> >(integral_c<RANK-1>(),ranges,psi,dpsidt);
  doApply<1*step,Vector<2>,Vector<1>,Vector<1> >(integral_c<RANK-1>(),ranges,psi,dpsidt);
  doApply<2*step,Vector<1>,Vector<1>,Vector<2> >(integral_c<RANK-1>(),ranges,psi,dpsidt);

}

template<int RANK> template<int START, typename V_DPSIDT, typename V_A, typename V_PSI, int REMAINING>
void Tridiagonal<RANK>::doApply(tmptools::integral_c<REMAINING>,
                                const Ranges& ranges, const StateVectorLow& psi, StateVectorLow& dpsidt) const
{
  static const int step=tmptools::Power_v<3,REMAINING-1>;

  using mpl::push_back, tmptools::integral_c;

  doApply<START+0*step,
          typename push_back<V_DPSIDT,integral_c<0> >::type,
          typename push_back<V_A     ,integral_c<0> >::type,
          typename push_back<V_PSI   ,integral_c<0> >::type>
    (integral_c<REMAINING-1>(),ranges,psi,dpsidt);

  doApply<START+1*step,
          typename push_back<V_DPSIDT,integral_c<2> >::type,
          typename push_back<V_A     ,integral_c<1> >::type,
          typename push_back<V_PSI   ,integral_c<1> >::type>
    (integral_c<REMAINING-1>(),ranges,psi,dpsidt);

  doApply<START+2*step,
          typename push_back<V_DPSIDT,integral_c<1> >::type,
          typename push_back<V_A     ,integral_c<1> >::type,
          typename push_back<V_PSI   ,integral_c<2> >::type>
    (integral_c<REMAINING-1>(),ranges,psi,dpsidt);

}

} // quantumoperator

#define BOOST_PP_ITERATION_LIMITS (1,QUANTUMOPERATOR_TRIDIAGONAL_MAX_RANK)
#define BOOST_PP_FILENAME_1 "Tridiagonal.tcc"

#include BOOST_PP_ITERATE()

#undef BOOST_PP_FILENAME_1
#undef BOOST_PP_ITERATION_LIMITS

#else // DO_CONSIDER_EXPLICITLY_SPECIALIZED_TRIDIAGONAL_APPLIES

namespace quantumoperator {

template<>
void Tridiagonal<1>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<2>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<3>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<4>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<5>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<6>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<7>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<8>::apply(const StateVectorLow&, StateVectorLow&) const;

template<>
void Tridiagonal<9>::apply(const StateVectorLow&, StateVectorLow&) const;

} // quantumoperator

#endif // DO_CONSIDER_EXPLICITLY_SPECIALIZED_TRIDIAGONAL_APPLIES

#endif // CPPQEDCORE_QUANTUMOPERATOR_TRIDIAGONAL_TCC_INCLUDED

#else  // BOOST_PP_IS_ITERATING

#define rank BOOST_PP_ITERATION()

#define AT_helper(V,i) ranges(i)(at_c<V,i>::type::value)

#define DPSIDT_print(z,m,data) AT_helper(V_DPSIDT,m)
#define      A_print(z,m,data) AT_helper(V_A     ,m)
#define    PSI_print(z,m,data) AT_helper(V_PSI   ,m)

template<> template<int START, typename V_DPSIDT, typename V_A, typename V_PSI>
void quantumoperator::Tridiagonal<rank>::doApply(tmptools::integral_c<0>,
                                                 const Ranges& ranges, const StateVectorLow& psi, StateVectorLow& dpsidt) const
{
  using mpl::at_c;

  if (diagonals_(START).size())
    dpsidt             (BOOST_PP_ENUM(rank,DPSIDT_print,~))
      +=
      diagonals_(START)(BOOST_PP_ENUM(rank,     A_print,~))
      *
      psi              (BOOST_PP_ENUM(rank,   PSI_print,~))
      ;

}

#undef    PSI_print
#undef      A_print
#undef DPSIDT_print

#undef AT_helper

#undef rank

#endif // BOOST_PP_IS_ITERATING


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RWLIBS_PATHPLANNERS_PRMPLANNER_HPP
#define RWLIBS_PATHPLANNERS_PRMPLANNER_HPP

#include "PartialIndexTable.hpp"

#include <rw/common/Timer.hpp>
#include <rw/core/Ptr.hpp>
#include <rw/math/Metric.hpp>
#include <rw/pathplanning/QToQPlanner.hpp>
#include <rwlibs/algorithms/kdtree/KDTreeQ.hpp>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <memory>

namespace rw { namespace kinematics {
    class State;
}}    // namespace rw::kinematics
namespace rw { namespace pathplanning {
    class QConstraint;
}}    // namespace rw::pathplanning
namespace rw { namespace pathplanning {
    class QEdgeConstraint;
}}    // namespace rw::pathplanning
namespace rw { namespace pathplanning {
    class QSampler;
}}    // namespace rw::pathplanning
namespace rw { namespace proximity {
    class CollisionDetector;
}}    // namespace rw::proximity

namespace rwlibs { namespace pathplanners {
    /** @addtogroup pathplanners */
    /*@{*/

    /**
     * @brief Implements a probabilistic roadmap (PRM) planner.
     *
     * The PRMPlanner is implemented freely after [1], and has a number of options:
     *
     * - Lazy Collision Checking: Using lazy collision checking as in [2], the
     * planner can be used for single as well as multiple queries.
     *
     * - Nearest Neighbor Search: The algorithm can either use a partial index
     * table [3] or a simple brute force method to do the nearest neighbor
     * search.
     *
     * - Shortest Path Algorithm: Using the Boost Graph Library, both A* and
     * Dijkstra's Algorithm may be used for finding the shortest path.
     *
     * As default the algorithm runs with lazy collision checking, brute force
     * neighbor search and with A* for shortest path search.
     *
     * As metric the PRMPlanner uses a WeightedEuclideanMetric for which it
     * estimates the weights such that it provides a worst-case estimate of the
     * Cartesian motion of the robots given a change in the configuration.
     *
     * Example of use
     * \code
     *      PRMPlanner* prm = new PRMPlanner(device, workcell, state, collisionDetector,
     * resolution); prm->setCollisionCheckingStrategy(PRMPlanner::LAZY);
     *      prm->setNeighSearchStrategy(PRMPlanner::BRUTE_FORCE);
     *      prm->setShortestPathSearchStrategy(PRMPlanner::A_STAR);
     *      prm->buildRoadmap(1000);
     *      Path path;
     *      bool pathFound = prm->query(qstart, qgoal, path, maxtime);
     * \endcode
     *
     * [1]: Probabilistic Roadmaps for Path Planning in High-Dimensional
     *      Configuration Spaces, L.E. Kavraki, P. Svestka, J-C. Latombe, M.H.
     *      Overmars. IEEE Transactions on Robotics and Automation, Vol. 12, pages
     *      566-580, 1996
     *
     * [2]: Path Planning using Lazy PRM, R. Bohlin, L.E. Kavraki. Proceedings
     *      of the IEEE International Conference on Robotics and Automation, Vol. 1,
     *      pages 521-528, 2000
     *
     * [3]: On Delaying Collision Checking in PRM Planning - Application to
     *      Multi-Robot Coordination, G. Sanchez, J.C. Latombe. The International
     *      Journal of Robotics Research, Vol. 21, No. 1, pages 5-26, 2002
     */
    class PRMPlanner : public rw::pathplanning::QToQPlanner
    {
      public:
        //! @brief smart pointer type to this class
        typedef rw::core::Ptr< PRMPlanner > Ptr;

        /**
         * @brief Constructs PRMPlanner
         *
         * Constructs a PRMPlanner with a given setup. This method does NOT build the roadmap.
         * The method estimates the movements of the robot to construct a weighted metric as
         * explained in the general introduction.
         *
         * @param device [in] Device to plan for
         * @param state [in] State giving the setup of the workcell
         * @param collisionDetector [in] CollisionDetector to use
         *
         * @param resolution [in] Cartesian distance the robot is allowed to move
         * between collision checks.
         */
        PRMPlanner (rw::models::Device* device, const rw::kinematics::State& state,
                    rw::proximity::CollisionDetector* collisionDetector, double resolution);

        /**
           @brief Constructs PRMPlanner

           @param constraint [in] Collision constraint
           @param sampler [in] Configuration space sampler
           @param resolution [in] Collision checking resolution
           @param device [in] Device characteristics
           @param state [in] State of rest of the workcell
        */
        PRMPlanner (rw::core::Ptr< rw::pathplanning::QConstraint > constraint,
                    rw::core::Ptr< rw::pathplanning::QSampler > sampler, double resolution,
                    const rw::models::Device& device, const rw::kinematics::State& state);

        /**
         * @brief Destructor
         */
        virtual ~PRMPlanner ();

        /**
         * @brief Build the roadmap with the setup specified
         * @param nodecount [in] Number of nodes to insert
         */
        void buildRoadmap (size_t nodecount);

        /**
         * @brief Sets the desired average number of neighbors. Default value is 20.
         * @param n [in] Desired average number of neighbors
         */
        void setNeighborCount (size_t n);

        /**
         * @brief Enumeration for selecting the node neighbor search strategy
         */
        enum NeighborSearchStrategy {
            BRUTE_FORCE = 0,     /*!< Run through all node and look a which a sufficient close. */
            PARTIAL_INDEX_TABLE, /*!< Use a partial index table to make an more efficient lookup. */
            KDTREE               /*!< Use KD tree for neighbor search */
        };

        /**
         * @brief Sets up the nearest neighbor search strategy
         *
         * @param neighborSearchStrategy [in] The nearest neighbor search strategy
         */
        void setNeighSearchStrategy (NeighborSearchStrategy neighborSearchStrategy);

        /**
         * @brief Sets up the number of dimensions for the partial index table
         *
         * This setting only applies when using the PARTIAL_INDEX_TABLE strategy for nearest
         * neighbor search.
         *
         * \b dimensions should be within \f$[1; _device->getDOF()]\f$. The optimal
         * value of \b dimensions is a tradeoff between memory usage and time.
         * Selecting a value too high compared to the number of nodes in the roadmap
         * may introduce an increase in time due to additional bookkeeping.
         *
         * The default value is set to 4, which is found suitable for most devices
         * with 6 or 7 degrees of freedom.
         *
         * @param dimensions [in] Number of dimensions, which should be
         */
        void setPartialIndexTableDimensions (size_t dimensions);

        /**
         * @brief Enumeration for selecting the collision checking strategy
         */
        enum CollisionCheckingStrategy {
            LAZY = 0,  /*!< Perform a Lazy collision checking (no checking on construction).*/
            NODECHECK, /*!< Only check node on construction, leave edges unchecked. */
            FULL       /*!<Perform a full check of both nodes and edges. */
        };

        /**
         * @brief Sets up the collision checking strategy
         *
         * Note: Do not call this after the buildRoadmap as it may result in paths with collisions
         * @param collisionCheckingStrategy [in] The collision checking strategy
         */
        void setCollisionCheckingStrategy (CollisionCheckingStrategy collisionCheckingStrategy);

        /**
         * @brief Enumeration for selecing the shortest path search strategy
         */
        enum ShortestPathSearchStrategy {
            A_STAR = 0, /*!< Use A* to search for shortest path. */
            DIJKSTRA    /*!< Use Dijkstra to search for shortest path. */
        };

        /**
         * @brief Sets up the shortest path search strategy
         *
         * Generally A* is the fastest algorithm, but given a small roadmap Dijkstra may
         * perform better.
         *
         * @param shortestPathSearchStrategy [in] shortestPathSearchStrategy
         */
        void setShortestPathSearchStrategy (ShortestPathSearchStrategy shortestPathSearchStrategy);

        /**
         * @brief Sets the max time of A* before terminating and calling dijkstra
         *
         * The A* implementation in the boost graph library has a reported bug, which on
         * some platforms in rare occasions may cause it to loop infinitely. If A* uses
         * more than this specified time it will break off and call dijkstra instead.
         *
         * Default value for this timeout is 1second.
         *
         * @param timeout [in] Timeout time.
         */
        void setAStarTimeOutTime (double timeout);

        /**
         * @brief print timing stats from previous run.
         */
        void printTimeStats ();

      private:
        bool doQuery (const rw::math::Q& qInit, const rw::math::Q& qGoal,
                      rw::trajectory::QPath& path, const rw::pathplanning::StopCriteria& stop);

      private:
        bool _roadmap_initialized;
        rw::core::Ptr< rw::pathplanning::QConstraint > _constraint;
        rw::core::Ptr< rw::pathplanning::QSampler > _sampler;
        double _resolution;

        rw::core::Ptr< rw::pathplanning::QEdgeConstraint > _edge;

        std::pair< rw::math::Q, rw::math::Q > _bounds;

        rw::math::Q _metricWeights;
        rw::math::QMetric::Ptr _metric;

        double _Rneighbor;
        size_t _Nneighbor;
        NeighborSearchStrategy _neighborSearchStrategy;
        size_t _partialIndexTableDimensions;
        CollisionCheckingStrategy _collisionCheckingStrategy;

        ShortestPathSearchStrategy _shortestPathSearchStrategy;
        double _astarTimeOutTime;

        // for stats testing
        rw::common::Timer collisionTimer;
        rw::common::Timer roadmapBuildTimer;
        rw::common::Timer queryTimer;
        rw::common::Timer shortestPathTimer;
        rw::common::Timer neighborTimer;
        rw::common::Timer enhanceTimer;

        /**
         * @brief The data contained in the PRM graph node
         */
        class NodeData
        {
          public:
            //! Joint configuration
            rw::math::Q q;

            //! Has the node been checked for collision
            bool checked;

          private:
            friend class boost::serialization::access;
            // When the class Archive corresponds to an output archive, the
            // & operator is defined similar to <<.  Likewise, when the class Archive
            // is a type of input archive the & operator is defined similar to >>.
            template< class Archive > void serialize (Archive& ar, const unsigned int version)
            {
                ar& q;
                ar& checked;
            }
        };

        /**
         * @brief The data contained in the PRM graph edge
         */
        class EdgeData
        {
          public:
            //! The edge-weight (defined as pPath(left, right))
            double weight;

            /**
             * @brief The resolution to which this node has been checked
             * (if resolution < _resolution then the edge is considered checked)
             */
            double resolution;

            rw::math::Q q1;
            rw::math::Q q2;

          private:
            friend class boost::serialization::access;
            // When the class Archive corresponds to an output archive, the
            // & operator is defined similar to <<.  Likewise, when the class Archive
            // is a type of input archive the & operator is defined similar to >>.
            template< class Archive > void serialize (Archive& ar, const unsigned int version)
            {
                ar& weight;
                ar& resolution;
                ar& q1;
                ar& q2;
            }
        };

        /**
         * @brief The PRM (Probabilistic RoadMap)
         */
        typedef boost::adjacency_list< boost::listS, boost::listS, boost::undirectedS, NodeData,
                                       EdgeData >
            PRM;

        //! The roadmap
        PRM _graph;

        //! List of seeds for the enhancement step
        std::vector< rw::math::Q > _seeds;

        //! A PRM node
        typedef PRM::vertex_descriptor Node;

        //! A PRM edge
        typedef PRM::edge_descriptor Edge;

        std::shared_ptr< prm::PartialIndexTable< Node > > _partialIndexTable;
        rwlibs::algorithms::KDTreeQ< Node >::Ptr _kdtree;
        std::list< const rwlibs::algorithms::KDTreeQ< Node >::KDNode* > _kdnodesSearchResult;

        void initialize (const rw::models::Device& device, const rw::kinematics::State& state);

        bool inCollision (const rw::math::Q& a, const rw::math::Q& b) const;
        bool addEdge (Node n1, Node n2, double dist);

        void addEdges (Node node);

        Node addNode (const rw::math::Q& q, bool checked);

        double estimateRneighbor (size_t roadmapsize);

        bool searchForShortestPathDijkstra (const Node& nInit, const Node& nGoal,
                                            std::list< Node >& result);
        bool searchForShortestPathAstar (const Node& nInit, const Node& nGoal,
                                         std::list< Node >& result);

        bool inCollision (std::list< Node >& path);

        bool enhanceEdgeCheck (Edge& e);
        void removeCollidingNode (Node node);
        void removeCollidingEdge (Edge edge);

        void enhanceAround (const rw::math::Q& q);
        void enhanceRoadmap ();

        size_t _enhanceAroundSeedCount;
        size_t _enhanceRandomFromSeedsCnt;
        size_t _enhanceRandomCnt;

        class EdgeCompare
        {
          private:
            PRM* _prm;

          public:
            EdgeCompare (PRM* prm) : _prm (prm) {}

            bool operator() (const Edge& e1, const Edge& e2) const
            {
                // return 1;
                return (*_prm)[e1].resolution > (*_prm)[e2].resolution;
            }
        };

        /**
         * @brief The heuristic distance meassure used by the A* shortest path search
         */
        class PathHeuristic : public boost::astar_heuristic< PRM, double >
        {
          public:
            /**
             * @brief Creates object
             * @param lazy [in] the lazy PRM path planner
             * @param nGoal [in] the goal node
             */
            PathHeuristic (PRM& prm, const rw::math::QMetric* metric, const Node& nGoal) :
                _prm (prm), _metric (metric), _qGoal (prm[nGoal].q)
            {}

            /**
             * @brief Calculates distance from n to goal
             * @param n [in] the node n
             * @return the distance
             */
            double operator() (const Node& n) { return _metric->distance (_qGoal, _prm[n].q); }

          private:
            PRM& _prm;
            //! Controlling pathplanner
            const rw::math::QMetric* _metric;
            //! Goal configuration
            rw::math::Q _qGoal;
        };
    };

    /* @} */
}}    // namespace rwlibs::pathplanners

#endif    // end include guard


// Copyright 2018 Ulf Adams
//
// The contents of this file may be used under the terms of the Apache License,
// Version 2.0.
//
//    (See accompanying file LICENSE-Apache or copy at
//     http://www.apache.org/licenses/LICENSE-2.0)
//
// Alternatively, the contents of this file may be used under the terms of
// the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE-Boost or copy at
//     https://www.boost.org/LICENSE_1_0.txt)
//
// Unless required by applicable law or agreed to in writing, this software
// is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.

/*
    This is a derivative work
*/

#ifndef BOOST_JSON_DETAIL_RYU_DETAIL_D2S_INTRINSICS_HPP
#define BOOST_JSON_DETAIL_RYU_DETAIL_D2S_INTRINSICS_HPP

#include <boost/json/config.hpp>

// This sets BOOST_JSON_RYU_32_BIT_PLATFORM as a side effect if applicable.
#include <boost/json/detail/ryu/detail/common.hpp>

#if defined(BOOST_JSON_RYU_HAS_64_BIT_INTRINSICS)
#include <intrin.h>
#endif

namespace boost {
namespace json {
namespace detail {

namespace ryu {
namespace detail {

#if defined(BOOST_JSON_RYU_HAS_64_BIT_INTRINSICS)

inline uint64_t umul128(const uint64_t a, const uint64_t b, uint64_t* const productHi) {
  return _umul128(a, b, productHi);
}

inline uint64_t shiftright128(const uint64_t lo, const uint64_t hi, const uint32_t dist) {
  // For the __shiftright128 intrinsic, the shift value is always
  // modulo 64.
  // In the current implementation of the double-precision version
  // of Ryu, the shift value is always < 64. (In the case
  // RYU_OPTIMIZE_SIZE == 0, the shift value is in the range [49, 58].
  // Otherwise in the range [2, 59].)
  // Check this here in case a future change requires larger shift
  // values. In this case this function needs to be adjusted.
  BOOST_JSON_ASSERT(dist < 64);
  return __shiftright128(lo, hi, (unsigned char) dist);
}

#else // defined(HAS_64_BIT_INTRINSICS)

inline uint64_t umul128(const uint64_t a, const uint64_t b, uint64_t* const productHi) {
  // The casts here help MSVC to avoid calls to the __allmul library function.
  const uint32_t aLo = (uint32_t)a;
  const uint32_t aHi = (uint32_t)(a >> 32);
  const uint32_t bLo = (uint32_t)b;
  const uint32_t bHi = (uint32_t)(b >> 32);

  const uint64_t b00 = (uint64_t)aLo * bLo;
  const uint64_t b01 = (uint64_t)aLo * bHi;
  const uint64_t b10 = (uint64_t)aHi * bLo;
  const uint64_t b11 = (uint64_t)aHi * bHi;

  const uint32_t b00Lo = (uint32_t)b00;
  const uint32_t b00Hi = (uint32_t)(b00 >> 32);

  const uint64_t mid1 = b10 + b00Hi;
  const uint32_t mid1Lo = (uint32_t)(mid1);
  const uint32_t mid1Hi = (uint32_t)(mid1 >> 32);

  const uint64_t mid2 = b01 + mid1Lo;
  const uint32_t mid2Lo = (uint32_t)(mid2);
  const uint32_t mid2Hi = (uint32_t)(mid2 >> 32);

  const uint64_t pHi = b11 + mid1Hi + mid2Hi;
  const uint64_t pLo = ((uint64_t)mid2Lo << 32) | b00Lo;

  *productHi = pHi;
  return pLo;
}

inline uint64_t shiftright128(const uint64_t lo, const uint64_t hi, const uint32_t dist) {
  // We don't need to handle the case dist >= 64 here (see above).
  BOOST_JSON_ASSERT(dist < 64);
#if defined(RYU_OPTIMIZE_SIZE) || !defined(RYU_32_BIT_PLATFORM)
  BOOST_JSON_ASSERT(dist > 0);
  return (hi << (64 - dist)) | (lo >> dist);
#else
  // Avoid a 64-bit shift by taking advantage of the range of shift values.
  BOOST_JSON_ASSERT(dist >= 32);
  return (hi << (64 - dist)) | ((uint32_t)(lo >> 32) >> (dist - 32));
#endif
}

#endif // defined(HAS_64_BIT_INTRINSICS)

#ifdef RYU_32_BIT_PLATFORM

// Returns the high 64 bits of the 128-bit product of a and b.
inline uint64_t umulh(const uint64_t a, const uint64_t b) {
  // Reuse the umul128 implementation.
  // Optimizers will likely eliminate the instructions used to compute the
  // low part of the product.
  uint64_t hi;
  umul128(a, b, &hi);
  return hi;
}

// On 32-bit platforms, compilers typically generate calls to library
// functions for 64-bit divisions, even if the divisor is a constant.
//
// E.g.:
// https://bugs.llvm.org/show_bug.cgi?id=37932
// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=17958
// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=37443
//
// The functions here perform division-by-constant using multiplications
// in the same way as 64-bit compilers would do.
//
// NB:
// The multipliers and shift values are the ones generated by clang x64
// for expressions like x/5, x/10, etc.

inline uint64_t div5(const uint64_t x) {
  return umulh(x, 0xCCCCCCCCCCCCCCCDu) >> 2;
}

inline uint64_t div10(const uint64_t x) {
  return umulh(x, 0xCCCCCCCCCCCCCCCDu) >> 3;
}

inline uint64_t div100(const uint64_t x) {
  return umulh(x >> 2, 0x28F5C28F5C28F5C3u) >> 2;
}

inline uint64_t div1e8(const uint64_t x) {
  return umulh(x, 0xABCC77118461CEFDu) >> 26;
}

inline uint64_t div1e9(const uint64_t x) {
  return umulh(x >> 9, 0x44B82FA09B5A53u) >> 11;
}

inline uint32_t mod1e9(const uint64_t x) {
  // Avoid 64-bit math as much as possible.
  // Returning (uint32_t) (x - 1000000000 * div1e9(x)) would
  // perform 32x64-bit multiplication and 64-bit subtraction.
  // x and 1000000000 * div1e9(x) are guaranteed to differ by
  // less than 10^9, so their highest 32 bits must be identical,
  // so we can truncate both sides to uint32_t before subtracting.
  // We can also simplify (uint32_t) (1000000000 * div1e9(x)).
  // We can truncate before multiplying instead of after, as multiplying
  // the highest 32 bits of div1e9(x) can't affect the lowest 32 bits.
  return ((uint32_t) x) - 1000000000 * ((uint32_t) div1e9(x));
}

#else // RYU_32_BIT_PLATFORM

inline uint64_t div5(const uint64_t x) {
  return x / 5;
}

inline uint64_t div10(const uint64_t x) {
  return x / 10;
}

inline uint64_t div100(const uint64_t x) {
  return x / 100;
}

inline uint64_t div1e8(const uint64_t x) {
  return x / 100000000;
}

inline uint64_t div1e9(const uint64_t x) {
  return x / 1000000000;
}

inline uint32_t mod1e9(const uint64_t x) {
  return (uint32_t) (x - 1000000000 * div1e9(x));
}

#endif // RYU_32_BIT_PLATFORM

inline uint32_t pow5Factor(uint64_t value) {
  uint32_t count = 0;
  for (;;) {
    BOOST_JSON_ASSERT(value != 0);
    const uint64_t q = div5(value);
    const uint32_t r = ((uint32_t) value) - 5 * ((uint32_t) q);
    if (r != 0) {
      break;
    }
    value = q;
    ++count;
  }
  return count;
}

// Returns true if value is divisible by 5^p.
inline bool multipleOfPowerOf5(const uint64_t value, const uint32_t p) {
  // I tried a case distinction on p, but there was no performance difference.
  return pow5Factor(value) >= p;
}

// Returns true if value is divisible by 2^p.
inline bool multipleOfPowerOf2(const uint64_t value, const uint32_t p) {
  BOOST_JSON_ASSERT(value != 0);
  // return __builtin_ctzll(value) >= p;
  return (value & ((1ull << p) - 1)) == 0;
}

} // detail
} // ryu

} // detail
} // json
} // boost

#endif