//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef BOOST_UBLAS_VECTOR_H
#define BOOST_UBLAS_VECTOR_H

#include <boost/numeric/ublas/config.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector_assign.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

// Iterators based on ideas of Jeremy Siek

namespace boost { namespace numeric { namespace ublas {

    // Array based vector class
    template<class T, class A>
    class vector:
        public vector_expression<vector<T, A> > {
    public:
#ifndef BOOST_UBLAS_NO_PROXY_SHORTCUTS
        BOOST_UBLAS_USING vector_expression<vector<T, A> >::operator ();
#endif
        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;
        typedef T value_type;
        // typedef const T &const_reference;
        typedef typename type_traits<T>::const_reference const_reference;
        typedef T &reference;
        typedef const T *const_pointer;
        typedef T *pointer;
        typedef A array_type;
        typedef const A const_array_type;
        typedef const vector<T, A> const_self_type;
        typedef vector<T, A> self_type;
#ifndef BOOST_UBLAS_CT_REFERENCE_BASE_TYPEDEFS
        typedef const vector_const_reference<const_self_type> const_closure_type;
#else
        typedef const vector_reference<const_self_type> const_closure_type;
#endif
        typedef vector_reference<self_type> closure_type;
        typedef typename A::const_iterator const_iterator_type;
        typedef typename A::iterator iterator_type;
        typedef dense_tag storage_category;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        vector ():
            size_ (0), data_ (0) {}
        BOOST_UBLAS_EXPLICIT BOOST_UBLAS_INLINE
        vector (size_type size):
            size_ (size), data_ (size) {}
        BOOST_UBLAS_INLINE
        vector (size_type size, const array_type &data):
            size_ (size), data_ (data) {}
        BOOST_UBLAS_INLINE
        vector (const vector &v):
            size_ (v.size_), data_ (v.data_) {}
        template<class AE>
        BOOST_UBLAS_INLINE
        vector (const vector_expression<AE> &ae):
            size_ (ae ().size ()), data_ (ae ().size ()) {
            vector_assign (scalar_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
        }

        // Accessors
        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }
        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 size) {
            size_ = size;
            data ().resize (size);
        }

        // Element access
        BOOST_UBLAS_INLINE
        const_reference operator () (size_type i) const {
            return data () [i];
        }
        BOOST_UBLAS_INLINE
        reference operator () (size_type i) {
            return data () [i];
        }

        BOOST_UBLAS_INLINE
        const_reference operator [] (size_type i) const {
            return (*this) (i);
        }
        BOOST_UBLAS_INLINE
        reference operator [] (size_type i) {
            return (*this) (i);
        }

        // Assignment
        BOOST_UBLAS_INLINE
        vector &operator = (const vector &v) {
            BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
            size_ = v.size_;
            data () = v.data ();
            return *this;
        }
        BOOST_UBLAS_INLINE
        vector &assign_temporary (vector &v) {
            swap (v);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        vector &operator = (const vector_expression<AE> &ae) {
#ifdef BOOST_UBLAS_MUTABLE_TEMPORARY
            return assign_temporary (self_type (ae));
#else
            // return assign (self_type (ae));
            self_type temporary (ae);
            return assign_temporary (temporary);
#endif
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        vector &reset (const vector_expression<AE> &ae) {
            self_type temporary (ae);
            resize (temporary.size ());
            return assign_temporary (temporary);
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        vector &assign (const vector_expression<AE> &ae) {
            vector_assign (scalar_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        vector &operator += (const vector_expression<AE> &ae) {
#ifdef BOOST_UBLAS_MUTABLE_TEMPORARY
            return assign_temporary (self_type (*this + ae));
#else
            // return assign (self_type (*this + ae));
            self_type temporary (*this + ae);
            return assign_temporary (temporary);
#endif
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        vector &plus_assign (const vector_expression<AE> &ae) {
            vector_assign (scalar_plus_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        vector &operator -= (const vector_expression<AE> &ae) {
#ifdef BOOST_UBLAS_MUTABLE_TEMPORARY
            return assign_temporary (self_type (*this - ae));
#else
            // return assign (self_type (*this - ae));
            self_type temporary (*this - ae);
            return assign_temporary (temporary);
#endif
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        vector &minus_assign (const vector_expression<AE> &ae) {
            vector_assign (scalar_minus_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
            return *this;
        }
        template<class AT>
        BOOST_UBLAS_INLINE
        vector &operator *= (const AT &at) {
            vector_assign_scalar (scalar_multiplies_assign<value_type, AT> (), *this, at);
            return *this;
        }
        template<class AT>
        BOOST_UBLAS_INLINE
        vector &operator /= (const AT &at) {
            vector_assign_scalar (scalar_divides_assign<value_type, AT> (), *this, at);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (vector &v) {
            // Too unusual semantic.
            // BOOST_UBLAS_CHECK (this != &v, external_logic ());
            if (this != &v) {
                // Precondition for container relaxed as requested during review.
                // BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
                std::swap (size_, v.size_);
                data ().swap (v.data ());
            }
        }
#ifndef BOOST_UBLAS_NO_MEMBER_FRIENDS
        BOOST_UBLAS_INLINE
        friend void swap (vector &v1, vector &v2) {
            v1.swap (v2);
        }
#endif

        // Element insertion and erasure
        // These functions should work with std::vector.
        // Thanks to Kresimir Fresl for spotting this.
        BOOST_UBLAS_INLINE
        void insert (size_type i, const_reference t) {
            BOOST_UBLAS_CHECK (data () [i] == value_type (), bad_index ());
            // data ().insert (data ().begin () + i, t);
            data () [i] = t;
        }
        BOOST_UBLAS_INLINE
        void erase (size_type i) {
            // data ().erase (data ().begin () + i);
            data () [i] = value_type ();
        }
        BOOST_UBLAS_INLINE
        void clear () {
            // data ().clear ();
            std::fill (data ().begin (), data ().end (), value_type ());
        }

#ifdef BOOST_UBLAS_USE_INDEXED_ITERATOR
        typedef indexed_iterator<self_type, dense_random_access_iterator_tag> iterator;
        typedef indexed_const_iterator<self_type, dense_random_access_iterator_tag> const_iterator;
#else
        class const_iterator;
        class iterator;
#endif

        // Element lookup
        BOOST_UBLAS_INLINE
        const_iterator find (size_type i) const {
#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
            return const_iterator (*this, data ().begin () + i);
#else
            return const_iterator (*this, i);
#endif
        }
        BOOST_UBLAS_INLINE
        iterator find (size_type i) {
#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
            return iterator (*this, data ().begin () + i);
#else
            return iterator (*this, i);
#endif
        }
        BOOST_UBLAS_INLINE
        const_iterator find_first (size_type i) const {
            return find (i);
        }
        BOOST_UBLAS_INLINE
        iterator find_first (size_type i) {
            return find (i);
        }
        BOOST_UBLAS_INLINE
        const_iterator find_last (size_type i) const {
            return find (i);
        }
        BOOST_UBLAS_INLINE
        iterator find_last (size_type i) {
            return find (i);
        }

        // Iterators simply are pointers.

#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
        class const_iterator:
            public container_const_reference<vector>,
            public random_access_iterator_base<dense_random_access_iterator_tag,
                                               const_iterator, value_type> {
        public:
            typedef dense_random_access_iterator_tag iterator_category;
#ifdef BOOST_MSVC_STD_ITERATOR
            typedef const_reference reference;
#else
            typedef typename vector::difference_type difference_type;
            typedef typename vector::value_type value_type;
            typedef typename vector::const_reference reference;
            typedef typename vector::const_pointer pointer;
#endif

            // Construction and destruction
            BOOST_UBLAS_INLINE
            const_iterator ():
                container_const_reference<self_type> (), it_ () {}
            BOOST_UBLAS_INLINE
            const_iterator (const self_type &v, const const_iterator_type &it):
                container_const_reference<self_type> (v), it_ (it) {}
            BOOST_UBLAS_INLINE
#ifndef BOOST_UBLAS_QUALIFIED_TYPENAME
            const_iterator (const iterator &it):
#else
            const_iterator (const typename self_type::iterator &it):
#endif
                container_const_reference<self_type> (it ()), it_ (it.it_) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            const_iterator &operator ++ () {
                ++ it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -- () {
                -- it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator += (difference_type n) {
                it_ += n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -= (difference_type n) {
                it_ -= n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            difference_type operator - (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ - it.it_;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            reference operator * () const {
                BOOST_UBLAS_CHECK (index () < (*this) ().size (), bad_index ());
                return *it_;
            }

            // Index
            BOOST_UBLAS_INLINE
            size_type index () const {
                return it_ - (*this) ().begin ().it_;
            }

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

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ == it.it_;
            }
            BOOST_UBLAS_INLINE
            bool operator < (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ < it.it_;
            }

        private:
            const_iterator_type it_;

            friend class iterator;
        };
#endif

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return find_first (0);
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return find_first (size_);
        }

#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
        class iterator:
            public container_reference<vector>,
            public random_access_iterator_base<dense_random_access_iterator_tag,
                                               iterator, value_type> {
        public:
            typedef dense_random_access_iterator_tag iterator_category;
#ifndef BOOST_MSVC_STD_ITERATOR
            typedef typename vector::difference_type difference_type;
            typedef typename vector::value_type value_type;
            typedef typename vector::reference reference;
            typedef typename vector::pointer pointer;
#endif

            // Construction and destruction
            BOOST_UBLAS_INLINE
            iterator ():
                container_reference<self_type> (), it_ () {}
            BOOST_UBLAS_INLINE
            iterator (self_type &v, const iterator_type &it):
                container_reference<self_type> (v), it_ (it) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            iterator &operator ++ () {
                ++ it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator &operator -- () {
                -- it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator &operator += (difference_type n) {
                it_ += n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator &operator -= (difference_type n) {
                it_ -= n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            difference_type operator - (const iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ - it.it_;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            reference operator * () const {
                BOOST_UBLAS_CHECK (index () < (*this) ().size (), bad_index ());
                return *it_;
            }

            // Index
            BOOST_UBLAS_INLINE
            size_type index () const {
                return it_ - (*this) ().begin ().it_;
            }

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

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ == it.it_;
            }
            BOOST_UBLAS_INLINE
            bool operator < (const iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ < it.it_;
            }

        private:
            iterator_type it_;

            friend class const_iterator;
        };
#endif

        BOOST_UBLAS_INLINE
        iterator begin () {
            return find_first (0);
        }
        BOOST_UBLAS_INLINE
        iterator end () {
            return find_first (size_);
        }

        // Reverse iterator

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef reverse_iterator_base<const_iterator, value_type, const_reference> const_reverse_iterator;
#else
        typedef reverse_iterator_base<const_iterator> const_reverse_iterator;
#endif

        BOOST_UBLAS_INLINE
        const_reverse_iterator rbegin () const {
            return const_reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator rend () const {
            return const_reverse_iterator (begin ());
        }

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef reverse_iterator_base<iterator, value_type, reference> reverse_iterator;
#else
        typedef reverse_iterator_base<iterator> reverse_iterator;
#endif

        BOOST_UBLAS_INLINE
        reverse_iterator rbegin () {
            return reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        reverse_iterator rend () {
            return reverse_iterator (begin ());
        }

    private:
        size_type size_;
        array_type data_;
    };

    // Unit vector class
    template<class T>
    class unit_vector:
        public vector_expression<unit_vector<T> > {
    public:
#ifndef BOOST_UBLAS_NO_PROXY_SHORTCUTS
        BOOST_UBLAS_USING vector_expression<unit_vector<T> >::operator ();
#endif
        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;
        typedef T value_type;
        // typedef const T &const_reference;
        typedef typename type_traits<T>::const_reference const_reference;
        typedef T &reference;
        typedef const T *const_pointer;
        typedef T *pointer;
        typedef const unit_vector<T> const_self_type;
        typedef unit_vector<T> self_type;
#ifndef BOOST_UBLAS_CT_REFERENCE_BASE_TYPEDEFS
        typedef const vector_const_reference<const_self_type> const_closure_type;
#else
        typedef const vector_reference<const_self_type> const_closure_type;
#endif
        typedef size_type const_iterator_type;
        typedef packed_tag storage_category;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        unit_vector ():
            size_ (0), index_ (0) {}
        BOOST_UBLAS_INLINE
        unit_vector (size_type size, size_type index):
            size_ (size), index_ (index) {}
        BOOST_UBLAS_INLINE
        unit_vector (const unit_vector &v):
            size_ (v.size_), index_ (v.index_) {}

        // Accessors
        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }
        BOOST_UBLAS_INLINE
        size_type index () const {
            return index_;
        }

        // Resizing
        BOOST_UBLAS_INLINE
        void resize (size_type size) {
            size_ = size;
        }

        // Element access
        BOOST_UBLAS_INLINE
        const_reference operator () (size_type i) const {
            return i == index_ ? one_ : zero_;
        }

        BOOST_UBLAS_INLINE
        const_reference operator [] (size_type i) const {
            return (*this) (i);
        }

        // Assignment
        BOOST_UBLAS_INLINE
        unit_vector &operator = (const unit_vector &v) {
            BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
            size_ = v.size_;
            index_ = v.index_;
            return *this;
        }
        BOOST_UBLAS_INLINE
        unit_vector &assign_temporary (unit_vector &v) {
            swap (v);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (unit_vector &v) {
            // Too unusual semantic.
            // BOOST_UBLAS_CHECK (this != &v, external_logic ());
            if (this != &v) {
                // Precondition for container relaxed as requested during review.
                // BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
                std::swap (size_, v.size_);
                std::swap (index_, v.index_);
            }
        }
#ifndef BOOST_UBLAS_NO_MEMBER_FRIENDS
        BOOST_UBLAS_INLINE
        friend void swap (unit_vector &v1, unit_vector &v2) {
            v1.swap (v2);
        }
#endif

#ifdef BOOST_UBLAS_USE_INDEXED_ITERATOR
        typedef indexed_const_iterator<self_type, packed_random_access_iterator_tag> iterator;
        typedef indexed_const_iterator<self_type, packed_random_access_iterator_tag> const_iterator;
#else
        class const_iterator;
#endif

        // Element lookup
        BOOST_UBLAS_INLINE
        const_iterator find_first (size_type i) const {
            return const_iterator (*this, std::max (i, index_));
        }
        BOOST_UBLAS_INLINE
        const_iterator find_last (size_type i) const {
            return const_iterator (*this, std::min (i, index_ + 1));
        }

        // Iterators simply are pointers.

#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
        class const_iterator:
            public container_const_reference<unit_vector>,
            public random_access_iterator_base<packed_random_access_iterator_tag,
                                               const_iterator, value_type> {
        public:
            typedef packed_random_access_iterator_tag iterator_category;
#ifdef BOOST_MSVC_STD_ITERATOR
            typedef const_reference reference;
#else
            typedef typename unit_vector::difference_type difference_type;
            typedef typename unit_vector::value_type value_type;
            typedef typename unit_vector::const_reference reference;
            typedef typename unit_vector::const_pointer pointer;
#endif

            // Construction and destruction
            BOOST_UBLAS_INLINE
            const_iterator ():
                container_const_reference<unit_vector> (), it_ () {}
            BOOST_UBLAS_INLINE
            const_iterator (const unit_vector &v, const const_iterator_type &it):
                container_const_reference<unit_vector> (v), it_ (it) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            const_iterator &operator ++ () {
                ++ it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -- () {
                -- it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator += (difference_type n) {
                it_ += n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -= (difference_type n) {
                it_ -= n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            difference_type operator - (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ - it.it_;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            reference operator * () const {
                BOOST_UBLAS_CHECK (index () < (*this) ().size (), bad_index ());
                return (*this) () (index ());
            }

            // Index
            BOOST_UBLAS_INLINE
            size_type index () const {
                return it_;
            }

            // Assignment
            BOOST_UBLAS_INLINE
            const_iterator &operator = (const const_iterator &it) {
                container_const_reference<unit_vector>::assign (&it ());
                it_ = it.it_;
                return *this;
            }

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ == it.it_;
            }
            BOOST_UBLAS_INLINE
            bool operator < (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ < it.it_;
            }

        private:
            const_iterator_type it_;
        };

        typedef const_iterator iterator;
#endif

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return find_first (0);
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return find_first (size_);
        }

        // Reverse iterator

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef reverse_iterator_base<const_iterator, value_type, const_reference> const_reverse_iterator;
#else
        typedef reverse_iterator_base<const_iterator> const_reverse_iterator;
#endif

        BOOST_UBLAS_INLINE
        const_reverse_iterator rbegin () const {
            return const_reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator rend () const {
            return const_reverse_iterator (begin ());
        }

    private:
        size_type size_;
        size_type index_;
        static value_type zero_;
        static value_type one_;
    };

    template<class T>
    typename unit_vector<T>::value_type unit_vector<T>::zero_ = 0;
    template<class T>
    typename unit_vector<T>::value_type unit_vector<T>::one_ = 1;

    // Zero vector class
    template<class T>
    class zero_vector:
        public vector_expression<zero_vector<T> > {
    public:
#ifndef BOOST_UBLAS_NO_PROXY_SHORTCUTS
        BOOST_UBLAS_USING vector_expression<zero_vector<T> >::operator ();
#endif
        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;
        typedef T value_type;
        // typedef const T &const_reference;
        typedef typename type_traits<T>::const_reference const_reference;
        typedef T &reference;
        typedef const T *const_pointer;
        typedef T *pointer;
        typedef const zero_vector<T> const_self_type;
        typedef zero_vector<T> self_type;
#ifndef BOOST_UBLAS_CT_REFERENCE_BASE_TYPEDEFS
        typedef const vector_const_reference<const_self_type> const_closure_type;
#else
        typedef const vector_reference<const_self_type> const_closure_type;
#endif
        typedef size_type const_iterator_type;
        typedef sparse_tag storage_category;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        zero_vector ():
            size_ (0) {}
        BOOST_UBLAS_EXPLICIT BOOST_UBLAS_INLINE
        zero_vector (size_type size):
            size_ (size) {}
        BOOST_UBLAS_INLINE
        zero_vector (const zero_vector &v):
            size_ (v.size_) {}

        // Accessors
        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }

        // Resizing
        BOOST_UBLAS_INLINE
        void resize (size_type size) {
            size_ = size;
        }

        // Element access
        BOOST_UBLAS_INLINE
        const_reference operator () (size_type i) const {
            return zero_;
        }

        BOOST_UBLAS_INLINE
        const_reference operator [] (size_type i) const {
            return (*this) (i);
        }

        // Assignment
        BOOST_UBLAS_INLINE
        zero_vector &operator = (const zero_vector &v) {
            BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
            size_ = v.size_;
            return *this;
        }
        BOOST_UBLAS_INLINE
        zero_vector &assign_temporary (zero_vector &v) {
            swap (v);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (zero_vector &v) {
            // Too unusual semantic.
            // BOOST_UBLAS_CHECK (this != &v, external_logic ());
            if (this != &v) {
                // Precondition for container relaxed as requested during review.
                // BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
                std::swap (size_, v.size_);
            }
        }
#ifndef BOOST_UBLAS_NO_MEMBER_FRIENDS
        BOOST_UBLAS_INLINE
        friend void swap (zero_vector &v1, zero_vector &v2) {
            v1.swap (v2);
        }
#endif

        class const_iterator;

        // Element lookup
        BOOST_UBLAS_INLINE
        const_iterator find_first (size_type i) const {
            return const_iterator (*this, 0);
        }
        BOOST_UBLAS_INLINE
        const_iterator find_last (size_type i) const {
            return const_iterator (*this, 0);
        }

        // Iterators simply are pointers.

        class const_iterator:
            public container_const_reference<zero_vector>,
            public bidirectional_iterator_base<sparse_bidirectional_iterator_tag,
                                               const_iterator, value_type> {
        public:
            typedef sparse_bidirectional_iterator_tag iterator_category;
#ifdef BOOST_MSVC_STD_ITERATOR
            typedef const_reference reference;
#else
            typedef typename zero_vector::difference_type difference_type;
            typedef typename zero_vector::value_type value_type;
            typedef typename zero_vector::const_reference reference;
            typedef typename zero_vector::const_pointer pointer;
#endif

            // Construction and destruction
            BOOST_UBLAS_INLINE
            const_iterator ():
                container_const_reference<self_type> (), it_ () {}
            BOOST_UBLAS_INLINE
            const_iterator (const self_type &v, const const_iterator_type &it):
                container_const_reference<self_type> (v), it_ (it) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            const_iterator &operator ++ () {
                ++ it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -- () {
                -- it_;
                return *this;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            reference operator * () const {
                BOOST_UBLAS_CHECK (index () < (*this) ().size (), bad_index ());
                return (*this) () (index ());
            }

            // Index
            BOOST_UBLAS_INLINE
            size_type index () const {
                return it_;
            }

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

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ == it.it_;
            }

        private:
            const_iterator_type it_;
        };

        typedef const_iterator iterator;

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return find_first (0);
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return find_first (size_);
        }

        // Reverse iterator

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef reverse_iterator_base<const_iterator, value_type, const_reference> const_reverse_iterator;
#else
        typedef reverse_iterator_base<const_iterator> const_reverse_iterator;
#endif

        BOOST_UBLAS_INLINE
        const_reverse_iterator rbegin () const {
            return const_reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator rend () const {
            return const_reverse_iterator (begin ());
        }

    private:
        size_type size_;
        static value_type zero_;
    };

    template<class T>
    typename zero_vector<T>::value_type zero_vector<T>::zero_ = 0;

    // Scalar vector class
    template<class T>
    class scalar_vector:
        public vector_expression<scalar_vector<T> > {
    public:
#ifndef BOOST_UBLAS_NO_PROXY_SHORTCUTS
        BOOST_UBLAS_USING vector_expression<scalar_vector<T> >::operator ();
#endif
        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;
        typedef T value_type;
        // typedef const T &const_reference;
        typedef typename type_traits<T>::const_reference const_reference;
        typedef T &reference;
        typedef const T *const_pointer;
        typedef T *pointer;
        typedef const scalar_vector<T> const_self_type;
        typedef scalar_vector<T> self_type;
#ifndef BOOST_UBLAS_CT_REFERENCE_BASE_TYPEDEFS
        typedef const vector_const_reference<const_self_type> const_closure_type;
#else
        typedef const vector_reference<const_self_type> const_closure_type;
#endif
        typedef size_type const_iterator_type;
        typedef dense_tag storage_category;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        scalar_vector ():
            size_ (0), value_ () {}
        BOOST_UBLAS_INLINE
        scalar_vector (size_type size, const value_type &value):
            size_ (size), value_ (value) {}
        BOOST_UBLAS_INLINE
        scalar_vector (const scalar_vector &v):
            size_ (v.size_), value_ (v.value_) {}

        // Accessors
        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }

        // Resizing
        BOOST_UBLAS_INLINE
        void resize (size_type size) {
            size_ = size;
        }

        // Element access
        BOOST_UBLAS_INLINE
        const_reference operator () (size_type i) const {
            return value_;
        }

        BOOST_UBLAS_INLINE
        const_reference operator [] (size_type i) const {
            return value_;
        }

        // Assignment
        BOOST_UBLAS_INLINE
        scalar_vector &operator = (const scalar_vector &v) {
            BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
            size_ = v.size_;
            value_ = v.value_;
            return *this;
        }
        BOOST_UBLAS_INLINE
        scalar_vector &assign_temporary (scalar_vector &v) {
            swap (v);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (scalar_vector &v) {
            // Too unusual semantic.
            // BOOST_UBLAS_CHECK (this != &v, external_logic ());
            if (this != &v) {
                // Precondition for container relaxed as requested during review.
                // BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
                std::swap (size_, v.size_);
                std::swap (value_, v.value_);
            }
        }
#ifndef BOOST_UBLAS_NO_MEMBER_FRIENDS
        BOOST_UBLAS_INLINE
        friend void swap (scalar_vector &v1, scalar_vector &v2) {
            v1.swap (v2);
        }
#endif

#ifdef BOOST_UBLAS_USE_INDEXED_ITERATOR
        typedef indexed_const_iterator<self_type, dense_random_access_iterator_tag> iterator;
        typedef indexed_const_iterator<self_type, dense_random_access_iterator_tag> const_iterator;
#else
        class const_iterator;
#endif

        // Element lookup
        BOOST_UBLAS_INLINE
        const_iterator find (size_type i) const {
            return const_iterator (*this, i);
        }
        BOOST_UBLAS_INLINE
        const_iterator find_first (size_type i) const {
            return find (i);
        }
        BOOST_UBLAS_INLINE
        const_iterator find_last (size_type i) const {
            return find (i);
        }

        // Iterators simply are pointers.

#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
        class const_iterator:
            public container_const_reference<scalar_vector>,
            public random_access_iterator_base<dense_random_access_iterator_tag,
                                               const_iterator, value_type> {
        public:
            typedef dense_random_access_iterator_tag iterator_category;
#ifdef BOOST_MSVC_STD_ITERATOR
            typedef const_reference reference;
#else
            typedef typename scalar_vector::difference_type difference_type;
            typedef typename scalar_vector::value_type value_type;
            typedef typename scalar_vector::const_reference reference;
            typedef typename scalar_vector::const_pointer pointer;
#endif

            // Construction and destruction
            BOOST_UBLAS_INLINE
            const_iterator ():
                container_const_reference<scalar_vector> (), it_ () {}
            BOOST_UBLAS_INLINE
            const_iterator (const scalar_vector &v, const const_iterator_type &it):
                container_const_reference<scalar_vector> (v), it_ (it) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            const_iterator &operator ++ () {
                ++ it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -- () {
                -- it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator += (difference_type n) {
                it_ += n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -= (difference_type n) {
                it_ -= n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            difference_type operator - (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ - it.it_;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            reference operator * () const {
                BOOST_UBLAS_CHECK (index () < (*this) ().size (), bad_index ());
                return (*this) () (index ());
            }

            // Index
            BOOST_UBLAS_INLINE
            size_type index () const {
                return it_;
            }

            // Assignment
            BOOST_UBLAS_INLINE
            const_iterator &operator = (const const_iterator &it) {
                container_const_reference<scalar_vector>::assign (&it ());
                it_ = it.it_;
                return *this;
            }

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ == it.it_;
            }
            BOOST_UBLAS_INLINE
            bool operator < (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ < it.it_;
            }

        private:
            const_iterator_type it_;
        };

        typedef const_iterator iterator;
#endif

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return find_first (0);
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return find_first (size_);
        }

        // Reverse iterator

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef reverse_iterator_base<const_iterator, value_type, const_reference> const_reverse_iterator;
#else
        typedef reverse_iterator_base<const_iterator> const_reverse_iterator;
#endif

        BOOST_UBLAS_INLINE
        const_reverse_iterator rbegin () const {
            return const_reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator rend () const {
            return const_reverse_iterator (begin ());
        }

    private:
        size_type size_;
        value_type value_;
    };

    // Array based vector class
    template<class T, std::size_t N>
    class c_vector:
        public vector_expression<c_vector<T, N> > {
    public:
#ifndef BOOST_UBLAS_NO_PROXY_SHORTCUTS
        BOOST_UBLAS_USING vector_expression<c_vector<T, N> >::operator ();
#endif
        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;
        typedef T value_type;
        // typedef const T &const_reference;
        typedef typename type_traits<T>::const_reference const_reference;
        typedef T &reference;
        typedef const T *const_pointer;
        typedef T *pointer;
        typedef const c_vector<T, N> const_self_type;
        typedef c_vector<T, N> self_type;
#ifndef BOOST_UBLAS_CT_REFERENCE_BASE_TYPEDEFS
        typedef const vector_const_reference<const_self_type> const_closure_type;
#else
        typedef const vector_reference<const_self_type> const_closure_type;
#endif
        typedef vector_reference<self_type> closure_type;
        typedef const T *const_iterator_type;
        typedef T *iterator_type;
        typedef dense_tag storage_category;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        c_vector ():
            size_ (N) /* , data_ () */ {}
        BOOST_UBLAS_EXPLICIT BOOST_UBLAS_INLINE
        c_vector (size_type size):
            size_ (size) /* , data_ () */ {
            if (size_ > N)
                // Raising exceptions abstracted as requested during review.
                // throw std::bad_alloc ();
                bad_size ().raise ();
        }
        BOOST_UBLAS_INLINE
        c_vector (const c_vector &v):
            size_ (v.size_) /* , data_ () */ {
            if (size_ > N)
                // Raising exceptions abstracted as requested during review.
                // throw std::bad_alloc ();
                bad_size ().raise ();
            *this = v;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector (const vector_expression<AE> &ae):
            size_ (ae ().size ()) /* , data_ () */ {
            if (size_ > N)
                // Raising exceptions abstracted as requested during review.
                // throw std::bad_alloc ();
                bad_size ().raise ();
            vector_assign (scalar_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
        }

        // Accessors
        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }

        // Resizing
        BOOST_UBLAS_INLINE
        void resize (size_type size) {
            if (size > N)
                // Raising exceptions abstracted as requested during review.
                // throw std::bad_alloc ();
                bad_size ().raise ();
            // The content of the array is intentionally not copied.
            size_ = size;
        }

        // Element access
        BOOST_UBLAS_INLINE
        const_reference operator () (size_type i) const {
            BOOST_UBLAS_CHECK (i < size_,  bad_index ());
            return data_ [i];
        }
        BOOST_UBLAS_INLINE
        reference operator () (size_type i) {
            BOOST_UBLAS_CHECK (i < size_, bad_index ());
            return data_ [i];
        }

        BOOST_UBLAS_INLINE
        const_reference operator [] (size_type i) const {
            return (*this) (i);
        }
        BOOST_UBLAS_INLINE
        reference operator [] (size_type i) {
            return (*this) (i);
        }

        // Assignment
        BOOST_UBLAS_INLINE
        c_vector &operator = (const c_vector &v) {
            BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
            size_ = v.size_;
            std::copy (v.data_, v.data_ + v.size_, data_);
            return *this;
        }
        BOOST_UBLAS_INLINE
        c_vector &assign_temporary (c_vector &v) {
            swap (v);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector &operator = (const vector_expression<AE> &ae) {
#ifdef BOOST_UBLAS_MUTABLE_TEMPORARY
            return assign_temporary (self_type (ae));
#else
            // return assign (self_type (ae));
            self_type temporary (ae);
            return assign_temporary (temporary);
#endif
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector &reset (const vector_expression<AE> &ae) {
            self_type temporary (ae);
            resize (temporary.size ());
            return assign_temporary (temporary);
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector &assign (const vector_expression<AE> &ae) {
            vector_assign (scalar_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector &operator += (const vector_expression<AE> &ae) {
#ifdef BOOST_UBLAS_MUTABLE_TEMPORARY
            return assign_temporary (self_type (*this + ae));
#else
            // return assign (self_type (*this + ae));
            self_type temporary (*this + ae);
            return assign_temporary (temporary);
#endif
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector &plus_assign (const vector_expression<AE> &ae) {
            vector_assign (scalar_plus_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
            return *this;
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector &operator -= (const vector_expression<AE> &ae) {
#ifdef BOOST_UBLAS_MUTABLE_TEMPORARY
            return assign_temporary (self_type (*this - ae));
#else
            // return assign (self_type (*this - ae));
            self_type temporary (*this - ae);
            return assign_temporary (temporary);
#endif
        }
        template<class AE>
        BOOST_UBLAS_INLINE
        c_vector &minus_assign (const vector_expression<AE> &ae) {
            vector_assign (scalar_minus_assign<value_type, BOOST_UBLAS_TYPENAME AE::value_type> (), *this, ae);
            return *this;
        }
        template<class AT>
        BOOST_UBLAS_INLINE
        c_vector &operator *= (const AT &at) {
            vector_assign_scalar (scalar_multiplies_assign<value_type, AT> (), *this, at);
            return *this;
        }
        template<class AT>
        BOOST_UBLAS_INLINE
        c_vector &operator /= (const AT &at) {
            vector_assign_scalar (scalar_divides_assign<value_type, AT> (), *this, at);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (c_vector &v) {
            // Too unusual semantic.
            // BOOST_UBLAS_CHECK (this != &v, external_logic ());
            if (this != &v) {
                BOOST_UBLAS_CHECK (size_ == v.size_, bad_size ());
                std::swap (size_, v.size_);
                std::swap_ranges (data_, data_ + size_, v.data_);
            }
        }
#ifndef BOOST_UBLAS_NO_MEMBER_FRIENDS
        BOOST_UBLAS_INLINE
        friend void swap (c_vector &v1, c_vector &v2) {
            v1.swap (v2);
        }
#endif

        // Element insertion and erasure
        BOOST_UBLAS_INLINE
        void insert (size_type i, const_reference t) {
            BOOST_UBLAS_CHECK (i < size_, bad_index ());
            BOOST_UBLAS_CHECK (data_ [i] == value_type (), bad_index ());
            data_ [i] = t;
        }
        BOOST_UBLAS_INLINE
        void erase (size_type i) {
            BOOST_UBLAS_CHECK (i < size_, bad_index ());
            data_ [i] = value_type ();
        }
        BOOST_UBLAS_INLINE
        void clear () {
            std::fill (data_, data_ + size_, value_type ());
        }

#ifdef BOOST_UBLAS_USE_INDEXED_ITERATOR
        typedef indexed_iterator<self_type, dense_random_access_iterator_tag> iterator;
        typedef indexed_const_iterator<self_type, dense_random_access_iterator_tag> const_iterator;
#else
        class const_iterator;
        class iterator;
#endif

        // Element lookup
        BOOST_UBLAS_INLINE
        const_iterator find (size_type i) const {
#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
            return const_iterator (*this, &data_ [i]);
#else
            return const_iterator (*this, i);
#endif
        }
        BOOST_UBLAS_INLINE
        iterator find (size_type i) {
#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
            return iterator (*this, &data_ [i]);
#else
            return iterator (*this, i);
#endif
        }
        BOOST_UBLAS_INLINE
        const_iterator find_first (size_type i) const {
            return find (i);
        }
        BOOST_UBLAS_INLINE
        iterator find_first (size_type i) {
            return find (i);
        }
        BOOST_UBLAS_INLINE
        const_iterator find_last (size_type i) const {
            return find (i);
        }
        BOOST_UBLAS_INLINE
        iterator find_last (size_type i) {
            return find (i);
        }

        // Iterators simply are pointers.

#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
        class const_iterator:
            public container_const_reference<c_vector>,
            public random_access_iterator_base<dense_random_access_iterator_tag,
                                               const_iterator, value_type> {
        public:
            typedef dense_random_access_iterator_tag iterator_category;
#ifdef BOOST_MSVC_STD_ITERATOR
            typedef const_reference reference;
#else
            typedef typename c_vector::difference_type difference_type;
            typedef typename c_vector::value_type value_type;
            typedef typename c_vector::const_reference reference;
            typedef typename c_vector::const_pointer pointer;
#endif

            // Construction and destruction
            BOOST_UBLAS_INLINE
            const_iterator ():
                container_const_reference<self_type> (), it_ () {}
            BOOST_UBLAS_INLINE
            const_iterator (const self_type &v, const const_iterator_type &it):
                container_const_reference<self_type> (v), it_ (it) {}
            BOOST_UBLAS_INLINE
#ifndef BOOST_UBLAS_QUALIFIED_TYPENAME
            const_iterator (const iterator &it):
#else
            const_iterator (const typename self_type::iterator &it):
#endif
                container_const_reference<self_type> (it ()), it_ (it.it_) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            const_iterator &operator ++ () {
                ++ it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -- () {
                -- it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator += (difference_type n) {
                it_ += n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            const_iterator &operator -= (difference_type n) {
                it_ -= n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            difference_type operator - (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ - it.it_;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            reference operator * () const {
                BOOST_UBLAS_CHECK (index () < (*this) ().size (), bad_index ());
                return *it_;
            }

            // Index
            BOOST_UBLAS_INLINE
            size_type index () const {
                const self_type &v = (*this) ();
                return it_ - v.begin ().it_;
            }

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

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ == it.it_;
            }
            BOOST_UBLAS_INLINE
            bool operator < (const const_iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ < it.it_;
            }

        private:
            const_iterator_type it_;

            friend class iterator;
        };
#endif

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return find_first (0);
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return find_first (size_);
        }

#ifndef BOOST_UBLAS_USE_INDEXED_ITERATOR
        class iterator:
            public container_reference<c_vector>,
            public random_access_iterator_base<dense_random_access_iterator_tag,
                                               iterator, value_type> {
        public:
            typedef dense_random_access_iterator_tag iterator_category;
#ifndef BOOST_MSVC_STD_ITERATOR
            typedef typename c_vector::difference_type difference_type;
            typedef typename c_vector::value_type value_type;
            typedef typename c_vector::reference reference;
            typedef typename c_vector::pointer pointer;
#endif

            // Construction and destruction
            BOOST_UBLAS_INLINE
            iterator ():
                container_reference<self_type> (), it_ () {}
            BOOST_UBLAS_INLINE
            iterator (self_type &v, const iterator_type &it):
                container_reference<self_type> (v), it_ (it) {}

            // Arithmetic
            BOOST_UBLAS_INLINE
            iterator &operator ++ () {
                ++ it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator &operator -- () {
                -- it_;
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator &operator += (difference_type n) {
                it_ += n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            iterator &operator -= (difference_type n) {
                it_ -= n;
                return *this;
            }
            BOOST_UBLAS_INLINE
            difference_type operator - (const iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ - it.it_;
            }

            // Dereference
            BOOST_UBLAS_INLINE
            reference operator * () const {
                BOOST_UBLAS_CHECK (index () < (*this) ().size (), bad_index ());
                return *it_;
            }

            // Index
            BOOST_UBLAS_INLINE
            size_type index () const {
                self_type &v = (*this) ();
                return it_ - v.begin ().it_;
            }

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

            // Comparison
            BOOST_UBLAS_INLINE
            bool operator == (const iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ == it.it_;
            }
            BOOST_UBLAS_INLINE
            bool operator < (const iterator &it) const {
                BOOST_UBLAS_CHECK (&(*this) () == &it (), external_logic ());
                return it_ < it.it_;
            }

        private:
            iterator_type it_;

            friend class const_iterator;
        };
#endif

        BOOST_UBLAS_INLINE
        iterator begin () {
            return find_first (0);
        }
        BOOST_UBLAS_INLINE
        iterator end () {
            return find_first (size_);
        }

        // Reverse iterator

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef reverse_iterator_base<const_iterator, value_type, const_reference> const_reverse_iterator;
#else
        typedef reverse_iterator_base<const_iterator> const_reverse_iterator;
#endif

        BOOST_UBLAS_INLINE
        const_reverse_iterator rbegin () const {
            return const_reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator rend () const {
            return const_reverse_iterator (begin ());
        }

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef reverse_iterator_base<iterator, value_type, reference> reverse_iterator;
#else
        typedef reverse_iterator_base<iterator> reverse_iterator;
#endif

        BOOST_UBLAS_INLINE
        reverse_iterator rbegin () {
            return reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        reverse_iterator rend () {
            return reverse_iterator (begin ());
        }

    private:
        size_type size_;
        value_type data_ [N];
    };

}}}

#endif


// exact C++ implementation of lowe's sift program
// Copyright (C) zerofrog(@gmail.com), 2008-2009
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
//This program is distributed in the hope that it will be useful,
//but WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//Lesser GNU General Public License for more details.
//
//You should have received a copy of the GNU Lesser General Public License
//along with this program.  If not, see <http://www.gnu.org/licenses/>.
#define BOOST_ENABLE_ASSERT_HANDLER

#include <vector>

#define PY_ARRAY_UNIQUE_SYMBOL PyArrayHandle
#include <boost/python.hpp>
#include <boost/python/exception_translator.hpp>
#include <pyconfig.h>
#include <numpy/arrayobject.h>

#include <boost/format.hpp>

#include "siftfast.h"

#define CHECK_POINTER(p) { \
    if( (p) == NULL ) throw siftfast_exception("invalid pointer"); \
}

using namespace boost::python;
using namespace std;

extern int DoubleImSize;
extern int Scales;
extern float InitSigma;
extern float PeakThresh;

struct siftfast_exception : std::exception
{
    siftfast_exception() : std::exception(), _s("unknown exception") {}
    siftfast_exception(const string& s) : std::exception() { _s = "siftfast: " + s; }
    virtual ~siftfast_exception() noexcept {}
    char const* what() noexcept const { return _s.c_str(); }
    string _s;
};

#if defined(BOOST_ENABLE_ASSERT_HANDLER)
namespace boost
{
inline void assertion_failed(char const * expr, char const * function, char const * file, long line)
{
    throw siftfast_exception(str(boost::format("[%s:%d] -> %s, expr: %s")%file%line%function%expr));
}
}
#endif

void translate_siftfast_exception(siftfast_exception const& e)
{
    // Use the Python 'C' API to set up an exception object
    PyErr_SetString(PyExc_RuntimeError, e.what());
}

inline object toPyArrayN(const float* pvalues, int N)
{
    npy_intp dims[] = {N};
    PyObject *pyvalues = PyArray_SimpleNew(1,dims, PyArray_FLOAT);
    if( pvalues != NULL )
        memcpy(PyArray_DATA(pyvalues),pvalues,N*sizeof(float));
    return static_cast<numeric::array>(handle<>(pyvalues));
}

template <typename T>
inline vector<T> ExtractArray(object o)
{
    vector<T> v(len(o));
    for(size_t i = 0; i < v.size(); ++i)
        v[i] = extract<T>(o[i]);
    return v;
}

inline vector<float> ExtractFloatArray(object oraw)
{
    object o = oraw.attr("flat");

    // check the types of o
    extract<float> xr(o[0]);
    if( xr.check() )
        return ExtractArray<float>(o);

    vector<float> v(len(o));
    object onew = ((numeric::array)oraw).astype("f8").attr("flat");
    for(size_t i = 0; i < v.size(); ++i)
        v[i] = (float)(extract<double>(onew[i]));
    return v;
}

class PyImage
{
public:
    PyImage(int width, int height) : width(width), height(height)
    {
        BOOST_ASSERT(width>0&&height>0);
        stride = (width+3)&~3;
        vimage.resize(height*stride);
    }
    PyImage(object oimage)
    {
        object shape = oimage.attr("shape");
        BOOST_ASSERT(len(shape)==2);
        width = extract<int>(shape[1]);
        height = extract<int>(shape[0]);
        stride = (width+3)&~3;
        vimage.resize(height*stride);
        SetData(oimage);
    }

    void SetData(object arr)
    {
        object shape = arr.attr("shape");
        if( len(shape) != 2 )
            throw siftfast_exception("array needs 2 dimensions");
        if( height != extract<int>(shape[0]) )
            throw siftfast_exception("array rows do not match height");
        if( width != extract<int>(shape[1]) )
            throw siftfast_exception("array columns do not match width");

        string dtype = extract<string>(arr.attr("dtype").attr("name"));
        if( (dtype.size() >= 3 && dtype[0] == 'i' && dtype[1] == 'n' && dtype[2] == 't') ||
            (dtype.size() >= 4 && dtype[1] == 'i' && dtype[2] == 'n' && dtype[3] == 't') ) {
            extract<int> xi(arr[0][0]);
            if( xi.check() ) {
                for(int i = 0; i < height; ++i)
                    for(int j = 0; j < width; ++j)
                        vimage[i*stride+j] = extract<int>(arr[i][j])*(1.0f/255.0f);
                return;
            }
        }

        extract<float> xr(arr[0][0]);
        if( xr.check() ) {
            for(int i = 0; i < height; ++i)
                for(int j = 0; j < width; ++j)
                    vimage[i*stride+j] = extract<float>(arr[i][j]);
            return;
        }
        extract<double> xd(arr[0][0]);
        if( xd.check() ) {
            for(int i = 0; i < height; ++i)
                for(int j = 0; j < width; ++j)
                    vimage[i*stride+j] = extract<double>(arr[i][j]);
            return;
        }

        throw siftfast_exception("array not in correct format");
    }

    int width,height,stride;
    vector<float> vimage;
};

class Image_pickle_suite : public pickle_suite
{
public:
    static tuple getinitargs(const PyImage& im)
    {
        return make_tuple(im.width,im.height,im.stride,toPyArrayN(&im.vimage[0],im.vimage.size()));
    }
};

object PyGetKeypoints(PyImage& im)
{
    struct ImageSt siftimage;
    siftimage.rows = im.height;
    siftimage.cols = im.width;
    siftimage.pixels = &im.vimage[0];
    siftimage.stride = im.stride;

    Keypoint keypts = GetKeypoints(&siftimage);

    int numkeys = 0;
    Keypoint key = keypts;
    while(key) {
        numkeys++;
        key = key->next;
    }

    npy_intp dims[2] = {numkeys,4};
    PyObject *pyframes = PyArray_SimpleNew(2,dims, PyArray_FLOAT);
    float* pframes = (float*)PyArray_DATA(pyframes);

    dims[1] = 128;
    PyObject *pydesc = PyArray_SimpleNew(2,dims, PyArray_FLOAT);
    float* pdesc = (float*)PyArray_DATA(pydesc);

    int index = 0;
    key = keypts;
    while(key) {

        for(int j = 0; j < 128; ++j)
            pdesc[128*index+j] = key->descrip[j];

        pframes[4*index+0] = key->col;
        pframes[4*index+1] = key->row;
        pframes[4*index+2] = key->ori;
        pframes[4*index+3] = key->scale;

        key = key->next;
        ++index;
    }

    FreeKeypoints(keypts);
    DestroyAllImages();

    return make_tuple(static_cast<numeric::array>(handle<>(pyframes)),static_cast<numeric::array>(handle<>(pydesc)));
}

object PyGetKeypoints(numeric::array oarray)
{
    PyImage pimage(oarray);
    return PyGetKeypoints(pimage);
}

struct DummyStruct {};

struct int_from_int
{
    int_from_int()
    {
        converter::registry::push_back(&convertible, &construct, type_id<int>());
    }

    static void* convertible( PyObject* obj)
    {
        PyObject* newobj = PyNumber_Int(obj);
        if (!PyString_Check(obj) && newobj) {
            Py_DECREF(newobj);
            return obj;
        }
        else {
            if (newobj) {
                Py_DECREF(newobj);
            }
            PyErr_Clear();
            return 0;
        }
    }

    static void construct(PyObject* _obj, converter::rvalue_from_python_stage1_data* data)
    {
        PyObject* newobj = PyNumber_Int(_obj);
        int* storage = (int*)((converter::rvalue_from_python_storage<int>*)data)->storage.bytes;
        *storage = extract<int>(newobj);
        Py_DECREF(newobj);
        data->convertible = storage;
    }
};

template<typename T>
struct T_from_number
{
    T_from_number()
    {
        converter::registry::push_back(&convertible, &construct, type_id<T>());
    }

    static void* convertible( PyObject* obj)
    {
        PyObject* newobj = PyNumber_Float(obj);
        if (!PyString_Check(obj) && newobj) {
            Py_DECREF(newobj);
            return obj;
        }
        else {
            if (newobj) {
                Py_DECREF(newobj);
            }
            PyErr_Clear();
            return 0;
        }
    }

    static void construct(PyObject* _obj, converter::rvalue_from_python_stage1_data* data)
    {
        PyObject* newobj = PyNumber_Float(_obj);
        T* storage = (T*)((converter::rvalue_from_python_storage<T>*)data)->storage.bytes;
        *storage = extract<T>(newobj);
        Py_DECREF(newobj);
        data->convertible = storage;
    }
};

int GetDoubleImSize() { return DoubleImSize; }
void SetDoubleImSize(int i) { DoubleImSize = i; }
int GetScales() { return Scales; }
void SetScales(int i) { Scales = i; }
float GetInitSigma() { return InitSigma; }
void SetInitSigma(float f) { InitSigma = f; }
float GetPeakThresh() { return PeakThresh; }
void SetPeakThresh(float f) { PeakThresh = f; }

BOOST_PYTHON_MODULE(siftfastpy)
{
    import_array();
    numeric::array::set_module_and_type("numpy", "ndarray");
    register_exception_translator<siftfast_exception>(&translate_siftfast_exception);
    int_from_int();
    T_from_number<float>();
    T_from_number<double>();

    def("DestroyAllResources",DestroyAllResources);
    object (*pkeypoints1)(PyImage&) = PyGetKeypoints;
    object (*pkeypoints2)(numeric::array) = PyGetKeypoints;
    def("GetKeypoints",pkeypoints1,args("image"));
    def("GetKeypoints",pkeypoints2,args("array"));

    class_<PyImage>("Image", no_init)
        .def(init<int,int>())
        .def(init<object>())
        .def_readonly("width",&PyImage::width)
        .def_readonly("height",&PyImage::height)
        .def("SetData",&PyImage::SetData,args("array"))
        .def_pickle(Image_pickle_suite())
        ;

    {
        scope options = class_<DummyStruct>("options")
            .add_property("DoubleImSize",GetDoubleImSize,SetDoubleImSize)
            .add_property("Scales",GetScales,SetScales)
            .add_property("InitSigma",GetInitSigma,SetInitSigma)
            .add_property("PeakThresh",GetPeakThresh,SetPeakThresh)
            ;
    }
}


//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <fstream>
#include <map>
#include <list>

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/thread.hpp>

#include <parmcb/parmcb.hpp>
#include <parmcb/util.hpp>

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

#define USAGE "Computes the minimum cycle basis of a weighted undirected graph given in DIMACS format."

int main(int argc, char *argv[]) {

    po::variables_map vm;
    try {
        po::options_description desc(USAGE);
        // @formatter:off
        desc.add_options()
                ("help,h", "Help")
                ("verbose,v", po::value<bool>()->default_value(false)->implicit_value(true), "Verbose")
                ("signed", po::value<bool>()->default_value(true), "Use the signed graph algorithm")
                ("fvstrees", po::value<bool>()->default_value(false), "Use cycles collection from feedback vertex set trees")
                ("isotrees", po::value<bool>()->default_value(false), "Use isometric cycles collection")
                ("parallel,p", po::value<bool>()->default_value(true), "Use parallelization")
                ("printcycles", po::value<bool>()->default_value(false)->implicit_value(true), "Print cycles")
                ("cores", po::value<int>()->default_value(0), "Number of cores")
                ("input-file,I",po::value<std::string>(), "Input filename");
        // @formatter:on
        po::positional_options_description pos_desc;
        pos_desc.add("input-file", 1);
        po::command_line_parser parser { argc, argv };
        parser.options(desc).positional(pos_desc).allow_unregistered();
        po::parsed_options parsed_options = parser.run();
        po::store(parsed_options, vm);

        if (vm.count("help")) {
            std::cout << desc << std::endl;
            return EXIT_SUCCESS;
        }
        po::notify(vm);

        if (!vm.count("input-file")) {
            std::cerr << "Input file missing. See usage by calling with -h ." << std::endl;
            exit(EXIT_FAILURE);
        }

    } catch (const po::error &ex) {
        std::cerr << "Invalid arguments:" << ex.what() << std::endl;
        return EXIT_FAILURE;
    }

    typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_weight_t, double> > graph_t;
    typedef graph_traits<graph_t>::edge_descriptor edge_descriptor;

    // create graph
    graph_t graph;
    FILE *fp = fopen(vm["input-file"].as<std::string>().c_str(), "r");
    if (fp == NULL) {
        std::cerr << "Failed to open input file." << std::endl;
        exit(EXIT_FAILURE);
    }
    parmcb::read_dimacs_from_file(fp, graph);
    fclose(fp);

    if (parmcb::has_loops(graph)) {
        std::cerr << "Graph has loops, aborting.." << std::endl;
        return EXIT_FAILURE;
    }
    if (parmcb::has_multiple_edges(graph)) {
        std::cerr << "Graph has multiple edges, aborting.." << std::endl;
        return EXIT_FAILURE;
    }
    if (parmcb::has_non_positive_weights(graph, get(boost::edge_weight, graph))) {
        std::cerr << "Graph has negative or zero weight edges, aborting.." << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "Graph has " << num_vertices(graph) << " vertices" << std::endl;
    std::cout << "Graph has " << num_edges(graph) << " edges" << std::endl;
    std::cout << std::flush;

    std::size_t cores = 0;
    if (vm.count("cores")) {
        cores = vm["cores"].as<int>();
    }
    if (cores == 0) {
        cores = boost::thread::hardware_concurrency();
    }
    if (vm["verbose"].as<bool>() && vm["parallel"].as<bool>()) {
        std::cout << "Using cores: " << cores << std::endl;
    }

    boost::timer::cpu_timer timer;

    std::list<std::list<edge_descriptor>> cycles;
    double mcb_weight;
    if (vm["signed"].as<bool>()) {
        if (vm["parallel"].as<bool>()) {
            std::cout << "Using PAR_MCB_SVA_SIGNED" << std::endl;
            mcb_weight = parmcb::mcb_sva_signed_tbb(graph, get(boost::edge_weight, graph), std::back_inserter(cycles),
                    cores);
        } else {
            std::cout << "Using MCB_SVA_SIGNED" << std::endl;
            mcb_weight = parmcb::mcb_sva_signed(graph, get(boost::edge_weight, graph), std::back_inserter(cycles));
        }
    } else if (vm["fvstrees"].as<bool>()) {
        if (vm["parallel"].as<bool>()) {
            std::cout << "Using PAR_MCB_SVA_FVS_TREES" << std::endl;
            mcb_weight = parmcb::mcb_sva_fvs_trees_tbb(graph, get(boost::edge_weight, graph), std::back_inserter(cycles));
        } else {
            std::cout << "Using MCB_SVA_FVS_TREES" << std::endl;
            mcb_weight = parmcb::mcb_sva_fvs_trees(graph, get(boost::edge_weight, graph), std::back_inserter(cycles));
        }
    } else {
        if (vm["parallel"].as<bool>()) {
            std::cout << "Using PAR_MCB_SVA_ISO_TREES" << std::endl;
            mcb_weight = parmcb::mcb_sva_iso_trees_tbb(graph, get(boost::edge_weight, graph), std::back_inserter(cycles));
        } else {
            std::cout << "Using MCB_SVA_ISO_TREES" << std::endl;
            mcb_weight = parmcb::mcb_sva_iso_trees(graph, get(boost::edge_weight, graph), std::back_inserter(cycles));
        }
    }

    timer.stop();

    std::cout << "MCB weight = " << mcb_weight << std::endl;

    if (vm["printcycles"].as<bool>()) {
        std::cout << "MCB cycles" << std::endl;
        for (auto it = cycles.begin(); it != cycles.end(); it++) {
            auto cycle = *it;

            for (auto eit = cycle.begin(); eit != cycle.end(); eit++) {
                std::cout << *eit << " ";
            }
            std::cout << std::endl;

            assert(parmcb::is_cycle(graph, cycle));
        }
    }

    if (vm["verbose"].as<bool>()) {
        std::cout << "time:" << timer.format();
    }

    return EXIT_SUCCESS;
}


#define BIORBD_API_EXPORTS
#include "Utils/Equation.h"

#include <boost/lexical_cast.hpp>
#include "Utils/Error.h"

std::vector<biorbd::utils::Equation> biorbd::utils::Equation::prepareMathSymbols(){
    // Classés en ordre de priorité des opérations
    std::vector<biorbd::utils::Equation> symbols;
    symbols.push_back("(");
    symbols.push_back(")");
    symbols.push_back("e");
    symbols.push_back("/");
    symbols.push_back("*");
    symbols.push_back("+");
    symbols.push_back("-");

    return symbols;
}

biorbd::utils::Equation::Equation() :
    biorbd::utils::String("")
{

}

biorbd::utils::Equation::Equation(const char *c) :
    biorbd::utils::String(c)
{

}

biorbd::utils::Equation::Equation(const biorbd::utils::String &s) :
    biorbd::utils::String(s)
{

}

biorbd::utils::Equation::Equation(const std::basic_string<char> &c) :
    biorbd::utils::String(c)
{

}

std::vector<biorbd::utils::Equation> biorbd::utils::Equation::splitIntoEquation(
        biorbd::utils::Equation wholeEq,
        const std::map<biorbd::utils::Equation, double>& variables)
{
    // variable de sortie
    std::vector<biorbd::utils::Equation> eq;

    // Remplacer les variables par un nombre
    replaceVar(wholeEq, variables);

    // Déclaration des marqueurs arithmétiques
    const std::vector<biorbd::utils::Equation>& symbols(prepareMathSymbols());

    // Sur tout le long du string
    while (1){
        int firstIdx(static_cast<int>(wholeEq.size())+1);// Mettre un index trop grand
        unsigned int toStop(0); // Mettre stop a 0
        // Balayer chaque symbols pour voir s'ils se trouvent dans l'equation
        for (unsigned int i=0; i<symbols.size(); ++i){
            int idx = static_cast<int>(wholeEq.find(symbols[i]));
            if (idx < 0){ // Si non, le noter
                ++toStop;
                continue;
            }
            if (idx < firstIdx && idx != -1) // si oui, regarder s'il est avant un autre symbol
                firstIdx = idx;
        }
        if (toStop == symbols.size()){ // S'il ne reste aucun symbole, sortir du while
            eq.push_back(wholeEq);
            break;
        }
        if (wholeEq.size() == 1){
            eq.push_back(wholeEq(0));
            break;
        }
        if (firstIdx+1 == static_cast<int>(wholeEq.size())){
            eq.push_back(wholeEq.substr(0,static_cast<unsigned int>(firstIdx)));
            eq.push_back(wholeEq(static_cast<unsigned int>(firstIdx)));
            break;
        }
        else if (firstIdx == 0){
            if (!wholeEq(0).compare("-")){
                // Si l'équation commencer par un "-"
                std::vector<biorbd::utils::Equation> tp(splitIntoEquation(wholeEq.substr(1), variables));
                if (eq.size() != 0 && !eq[eq.size()-1].compare("e") ){
                    // special case of scientific notation (1e-x)
                    eq.push_back(wholeEq(0));
                    wholeEq = wholeEq.substr(1);
                } else if (!tp[0].compare("(")) {
                    // Resolve now the parenthese to apply the minus after
                    size_t idx = wholeEq.find_first_of(")");
                    biorbd::utils::Equation newWholeEq(wholeEq.substr(2, idx-2));
                    double res(-1*resolveEquation(splitIntoEquation(newWholeEq, variables)));
                    wholeEq = std::to_string(res) + wholeEq.substr(idx+1);
                } else {
                    tp[0] = "-" + tp[0];
                    eq.insert(eq.end(), tp.begin(), tp.end());
                    return eq;
                }
            }
            else if (!wholeEq(0).compare("+")){
                // Si l'équation commencer par un +
                std::vector<biorbd::utils::Equation> tp(splitIntoEquation(wholeEq.substr(1), variables));
                if (eq.size() != 0) // S'il n'y a rien avant, est faux dans les cas 1ex
                    eq.push_back("+");
                eq.insert(eq.end(), tp.begin(), tp.end());
                return eq;
            }
            else{
                eq.push_back(wholeEq(0));
                wholeEq = wholeEq.substr(1);
            }
        }
        else{
            eq.push_back(wholeEq.substr(0,static_cast<unsigned int>(firstIdx))); // Prendre tout ce qui vient avant le symbole
            eq.push_back(wholeEq(static_cast<unsigned int>(firstIdx))); // Récupérer le symbole
            wholeEq = wholeEq.substr(static_cast<unsigned int>(firstIdx)+1); // Garder tout ce qu'il y a apres le symbole et recommencer
        }

    }

    // Remplacer les constantes par un nombre
    replaceCste(eq);

    // Retourner l'équation
    return eq;
}

void biorbd::utils::Equation::replaceCste(
        std::vector<biorbd::utils::Equation> &eq)
{
    for (unsigned int i=0; i<eq.size(); ++i)
        if (!eq[i].tolower().compare("pi"))
            eq[i] = boost::lexical_cast< std::string>(PI);
}

void biorbd::utils::Equation::replaceVar(
        biorbd::utils::Equation &eq,
        const std::map<biorbd::utils::Equation, double>& variables)
{
    for (auto var : variables)
        while (eq.find(var.first) != std::string::npos){
            size_t pos(eq.find(var.first));
            size_t length(var.first.length());
            eq = eq.substr(0, pos) + "(" +
                    std::to_string(var.second) + ")" +
                    eq.substr(pos + length);
        }
}

double biorbd::utils::Equation::resolveEquation(
        std::vector<biorbd::utils::Equation> wholeEq)
{
    return resolveEquation(wholeEq,0);
}

double biorbd::utils::Equation::resolveEquation(
        std::vector<biorbd::utils::Equation> eq,
        unsigned int math)
{
    // Si on a traité tout
    if (eq.size() == 1)
        return boost::lexical_cast<double>(eq[0]);

    // Déclaration des marqueurs arithmétiques
    const std::vector<biorbd::utils::Equation>& symbols(prepareMathSymbols());
    std::vector<biorbd::utils::Equation> eq2;
    bool continuer(true);

    for (unsigned int j=0; j<eq.size(); ++j){
        if (!eq[j].compare(symbols[math]) && continuer){
            if (j==0 && (!symbols[math].compare("+") || !symbols[math].compare("-"))){
                // Écraser la valeur précédente
                if (!symbols[math].compare("+"))
                    eq2[j-1] = boost::lexical_cast<std::string>(0.0 + boost::lexical_cast<double>(eq[j+1]));
                else if (!symbols[math].compare("-"))
                    eq2[j-1] = boost::lexical_cast<std::string>(0.0 - boost::lexical_cast<double>(eq[j+1]));
            }
            else{
                // Écraser la valeur précédente
                if (!symbols[math].compare("(")){
                    std::vector<biorbd::utils::Equation> eq_tp;
                    bool foundIdx(false);
                    int cmpValues(0);
                    unsigned int cmpOpen(0);
                    for (unsigned int k=j+1; k<eq.size();++k){
                        if (!eq[k].compare("("))
                            cmpOpen++;
                        else if (!eq[k].compare(")")){
                            if (cmpOpen == 0){
                                foundIdx = true;
                                break;
                            }
                            else
                                cmpOpen--;
                        }

                        eq_tp.push_back(eq[k]);
                        ++cmpValues;
                    }
                    biorbd::utils::Error::check(foundIdx, "You must close brackets!");

                    eq2.push_back(boost::lexical_cast<std::string>(boost::lexical_cast<std::string>(resolveEquation(eq_tp))));
                    j+=static_cast<unsigned int>(cmpValues);
                }
                else if (!symbols[math].compare("/"))
                    eq2[j-1] = boost::lexical_cast<std::string>(boost::lexical_cast<double>(eq[j-1]) / boost::lexical_cast<double>(eq[j+1]));
                else if (!symbols[math].compare("*"))
                    eq2[j-1] = boost::lexical_cast<std::string>(boost::lexical_cast<double>(eq[j-1]) * boost::lexical_cast<double>(eq[j+1]));
                else if (!symbols[math].compare("+"))
                    eq2[j-1] = boost::lexical_cast<std::string>(boost::lexical_cast<double>(eq[j-1]) + boost::lexical_cast<double>(eq[j+1]));
                else if (!symbols[math].compare("-"))
                    eq2[j-1] = boost::lexical_cast<std::string>(boost::lexical_cast<double>(eq[j-1]) - boost::lexical_cast<double>(eq[j+1]));
                else if (!symbols[math].compare("e")){
                    if (!eq[j+1].compare("-")){
                        eq2[j-1] = boost::lexical_cast<std::string>(boost::lexical_cast<double>(eq[j-1]) * pow(10,-1*boost::lexical_cast<double>(eq[j+2])));
                        ++j;
                    }
                    else if (!eq[j+1].compare("+")){
                        eq2[j-1] = boost::lexical_cast<std::string>(boost::lexical_cast<double>(eq[j-1]) * pow(10,boost::lexical_cast<double>(eq[j+2])));
                        ++j;
                    }
                    else
                        eq2[j-1] = boost::lexical_cast<std::string>(boost::lexical_cast<double>(eq[j-1]) * pow(10,boost::lexical_cast<double>(eq[j+1])));

                }
            }

            j+=2;
            continuer = false;
        }
        if (j<eq.size())
            eq2.push_back(eq[j]);
    }

    if (continuer)
        return resolveEquation(eq2, ++math);
    else
        return resolveEquation(eq2, math);
}

double biorbd::utils::Equation::resolveEquation(
        biorbd::utils::Equation wholeEq,
        const std::map<biorbd::utils::Equation, double>& variables)
{
    return resolveEquation(splitIntoEquation(wholeEq, variables));
}
double biorbd::utils::Equation::resolveEquation(
        biorbd::utils::Equation wholeEq)
{
    std::map<biorbd::utils::Equation, double> dumb;
    return resolveEquation(splitIntoEquation(wholeEq, dumb));
}


/* Copyright (C) 2019-2020 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include <ros/ros.h>

/* Include standard libraries */
#include <string>
#include <boost/bind.hpp>
#include <cmath>

/* Include ROS libraries */
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/LinearMath/Quaternion.h>

/* Include message types */
#include "std_msgs/String.h"
#include "std_msgs/Float64.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Vector3.h"
#include "nav_msgs/Odometry.h"
#include "ackermann_msgs/AckermannDrive.h"

/* Include generated Services */

/* Include generated Message Types */
#include <jetsoncar_msgs/Encoders.h>

/* Include generated Dynamic Reconfigure parameters */

// For CLion to update/capture the changes made to generated services, message types and parameters, open the "build" folder and run "make"

/* This node Subscribes to the /encoders message and integrates that using the Bicycle model into
 * odometry messages (/odom) and odometry transformations (odom frame) */

/* Global variables */
std::string tfPrefix;
double min_steering = 0.003;
double max_steering = 1.0;
double min_omega = 0.01;
double wheel_radius = 1;
double front_to_rear_wheel_distance = 1;
double left_to_right_wheel_distance = 1;

ros::Time prev_odometry_time(0);
double prev_front_angle = 0;
double prev_rear_angle = 0;
double prev_steering_angle = 0;
double x_prev = 0;
double y_prev = 0;
double theta_prev = 0;

void PublishOdometry(double v, double rho, double dt, ros::Time time, tf::TransformBroadcaster& tfBroadcaster, ros::Publisher& pubOdom)
{
    // Use the Bicycle Kinematic model with the origin (position) defined in the center of mass
    // which is assumed to be the midpoint between front and rear wheels (base_link frame)
    // https://www.coursera.org/lecture/intro-self-driving-cars/lesson-2-the-kinematic-bicycle-model-Bi8yE
    // https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf
    // dx = v * cos(theta + beta)
    // dy = v * sin(theta + beta)
    // dtheta = v * cos(beta) * tan(rho) / L
    // beta = atan(l_r/L * tan(rho))
    // Where
    //   L   : distance between front to rear wheels
    //   l_r : distance between rear wheels to center of mass (midpoint)
    //   rho : steering angle
    // l_r is assumed to be L/2

    // To discretize this we assume the velocity and steering angle is constant during the discretization interval, dt
    // In that case the Coordinated Turn Model can be used
    // See Chapter 5.2.2 of http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.142.6137&rep=rep1&type=pdf
    // x[k+1] = x[k] + 2*v[k]/omega[k] * sin(dt/2*omega[k]) * cos(theta[k] + beta[k] + dt/2*omega[k])
    // y[k+1] = x[k] + 2*v[k]/omega[k] * sin(dt/2*omega[k]) * sin(theta[k] + beta[k] + dt/2*omega[k])
    // theta[k+1] = theta[k] + dt*omega[k]
    // Where
    //    omega[k] = v[k] * cos(beta[k]) * tan(rho[k]) / L
    // A different form can be found in equation 5.9 on page 101 of Probabilistic Robotics by Sebastian Thrun

    double x = x_prev;
    double y = y_prev;
    double theta = theta_prev;
    double omega = 0;
    std::cout << "v = " << v << std::endl;

    if (std::abs(rho) > min_steering) {
        double beta = std::atan((front_to_rear_wheel_distance / 2) / front_to_rear_wheel_distance * std::tan(rho));
        omega = v * std::cos(beta) * std::tan(rho) / front_to_rear_wheel_distance;

        std::cout << "rho = " << rho << std::endl;
        std::cout << "beta = " << beta << std::endl;
        std::cout << "omega = " << omega << std::endl;

        if (std::abs(omega) > min_omega) {
            // Compute predicted odometry position
            x += 2 * v / omega * std::sin(dt / 2 * omega) * std::cos(theta_prev + beta + dt / 2 * omega);
            y += 2 * v / omega * std::sin(dt / 2 * omega) * std::sin(theta_prev + beta + dt / 2 * omega);
            theta += dt * omega;
        } else {
            x += dt * cos(theta) * v;
            y += dt * sin(theta) * v;
        }
    } else {
        x += dt * cos(theta) * v;
        y += dt * sin(theta) * v;
    }

    double dx_est, dy_est;
    dx_est = (x - x_prev) / dt;
    dy_est = (y - y_prev) / dt;

    // Construct odometry transform and publish
    tf::Transform odom_transform;
    odom_transform.setOrigin(tf::Vector3(x, y, 0));
    odom_transform.setRotation(tf::createQuaternionFromYaw(theta));
    tfBroadcaster.sendTransform(tf::StampedTransform(odom_transform, time, "odom", "footprint"));

    std::cout.precision(3);
    std::cout.setf(std::ios::fixed,std::ios::floatfield);
    std::cout << "At time " << time.toSec() << std::endl;
    double yaw, pitch, roll;
    odom_transform.getBasis().getRPY(roll, pitch, yaw);
    tf::Quaternion q = odom_transform.getRotation();
    tf::Vector3 tv = odom_transform.getOrigin();
    std::cout << "- Translation: [" << tv.getX() << ", " << tv.getY() << ", " << tv.getZ() << "]" << std::endl;
    std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", "
              << q.getZ() << ", " << q.getW() << "]" << std::endl
              << "            in RPY (radian) [" <<  roll << ", " << pitch << ", " << yaw << "]" << std::endl
              << "            in RPY (degree) [" <<  roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl;

    /* Send odometry message */
    //   This represents an estimate of a position and velocity in free space.
    //   The pose in this message should be specified in the coordinate frame given by header.frame_id
    //   The twist in this message should be specified in the coordinate frame given by the child_frame_id
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "footprint";
    odom_msg.pose.pose.position.x = x; // inertial frame position
    odom_msg.pose.pose.position.y = y;
    odom_msg.pose.pose.position.z = 0;
    odom_msg.pose.pose.orientation.w = odom_transform.getRotation().w();
    odom_msg.pose.pose.orientation.x = odom_transform.getRotation().x();
    odom_msg.pose.pose.orientation.y = odom_transform.getRotation().y();
    odom_msg.pose.pose.orientation.z = odom_transform.getRotation().z();
    odom_msg.twist.twist.linear.x = v; // body frame velocity
    odom_msg.twist.twist.linear.y = 0;
    odom_msg.twist.twist.linear.z = 0;
    odom_msg.twist.twist.angular.x = 0;
    odom_msg.twist.twist.angular.y = 0;
    odom_msg.twist.twist.angular.z = omega;

    odom_msg.pose.covariance[0] = 0.001;
    odom_msg.pose.covariance[7] = 0.001;
    odom_msg.pose.covariance[14] = 1000000000000.0;
    odom_msg.pose.covariance[21] = 1000000000000.0;
    odom_msg.pose.covariance[28] = 1000000000000.0;

    odom_msg.pose.covariance[35] = 0.01; // std::abs(angular_vel.z) < 0.0001
    //odom_msg.pose.covariance[35] = 100.0;

    odom_msg.twist.covariance[0] = 0.001;
    odom_msg.twist.covariance[7] = 0.001;
    odom_msg.twist.covariance[14] = 0.001;
    odom_msg.twist.covariance[21] = 1000000000000.0;
    odom_msg.twist.covariance[28] = 1000000000000.0;
    odom_msg.twist.covariance[35] = 0.01; // std::abs(angular_vel.z) < 0.0001
    //odom_msg.twist.covariance[35] = 100.0;

    pubOdom.publish(odom_msg);

    // Store updated states for next iteration
    x_prev = x;
    y_prev = y;
    theta_prev = theta;
}

void ROS_Callback_encoders(const jetsoncar_msgs::Encoders::ConstPtr& msg, tf::TransformBroadcaster& tfBroadcaster, ros::Publisher& pubOdom)
{
    auto time_diff = msg->receive_time - prev_odometry_time;
    double dt = time_diff.toSec();

    if (dt > 0 && prev_odometry_time.toSec() != 0) {
        // Since we want to use and integrate the encoder measurements, we will compute the instantaneous velocity by
        // differentiating the encoder readings
        double front_velocity = (msg->front_angle - prev_front_angle) / dt;
        double rear_velocity = (msg->front_angle - prev_front_angle) / dt;
        double wheel_velocity = (front_velocity + rear_velocity) / 2.0;

        // Compute necessary variables
        double v = wheel_radius * wheel_velocity; // translational velocity
        double rho = msg->steering_angle;

        PublishOdometry(v, rho, dt, msg->receive_time, tfBroadcaster, pubOdom);
    }

    prev_odometry_time = msg->receive_time;
    prev_front_angle = msg->front_angle;
    prev_rear_angle = msg->rear_angle;
    prev_steering_angle = msg->steering_angle;
}

void ROS_Callback_cmd_ackermann(const ackermann_msgs::AckermannDrive::ConstPtr& msg, tf::TransformBroadcaster& tfBroadcaster, ros::Publisher& pubOdom)
{
    ros::Time current_time = ros::Time::now();

    // Convert cmd_vel command into actuation commends sent to the Gazebo simulated wheel controllers
    double speed = msg->speed;
    double steering_angle = msg->steering_angle; // misuse of using this to capture the steering angle

    auto time_diff = current_time - prev_odometry_time;
    double dt = time_diff.toSec();

    if (dt > 0 && prev_odometry_time.toSec() != 0) {
        PublishOdometry(speed, steering_angle, dt, current_time, tfBroadcaster, pubOdom);
    }

    prev_odometry_time = current_time;
}

int main(int argc, char **argv) {
	std::string nodeName = "odometry_node";
	ros::init(argc, argv, nodeName.c_str());
	ros::NodeHandle n; // default/current namespace node handle
	ros::NodeHandle nParam("~"); // default/current namespace node handle for parameters

    tfPrefix = tf::getPrefixParam(n);
	tf::TransformBroadcaster tfBroadcaster;

	bool use_encoders = true;
    if (!nParam.getParam("use_encoders", use_encoders)) {
        ROS_WARN_STREAM("Use encoders flag not set (Parameter: use_encoders). Defaults to: " << use_encoders);
    }

    if (!nParam.getParam("wheel_radius", wheel_radius)) {
        ROS_WARN_STREAM("Wheel radius not set (Parameter: wheel_radius). Defaults to: " << wheel_radius);
    }
    if (!nParam.getParam("front_to_rear_wheel_distance", front_to_rear_wheel_distance)) {
        ROS_WARN_STREAM("Wheel distance not set (Parameter: front_to_rear_wheel_distance). Defaults to: " << front_to_rear_wheel_distance);
    }
    if (!nParam.getParam("left_to_right_wheel_distance", left_to_right_wheel_distance)) {
        ROS_WARN_STREAM("Wheel distance not set (Parameter: front_to_rear_wheel_distance). Defaults to: " << left_to_right_wheel_distance);
    }
    if (!nParam.getParam("min_steering", min_steering)) {
        ROS_WARN_STREAM("Minimum steering angle not set (Parameter: min_steering). Defaults to: " << min_steering);
    }
    if (!nParam.getParam("max_steering", max_steering)) {
        ROS_WARN_STREAM("Maximum steering angle not set (Parameter: max_steering). Defaults to: " << max_steering);
    }
    if (!nParam.getParam("min_omega", min_omega)) {
        ROS_WARN_STREAM("Minimum angular velocity set (Parameter: min_omega). Defaults to: " << min_omega);
    }

    /* Configure publishers */
    ros::Publisher pub_odom = n.advertise<nav_msgs::Odometry>("odom", 50);

    /* Configure subscribers */
    ros::Subscriber sub;
    if (use_encoders)
        sub = n.subscribe<jetsoncar_msgs::Encoders>("encoders", 50, boost::bind(&ROS_Callback_encoders, _1, boost::ref(tfBroadcaster), boost::ref(pub_odom)));
    else
        sub = n.subscribe<ackermann_msgs::AckermannDrive>("cmd_ackermann", 50, boost::bind(&ROS_Callback_cmd_ackermann, _1, boost::ref(tfBroadcaster), boost::ref(pub_odom)));

    ros::spin();
}