//
//  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_STORAGE_SPARSE_H
#define BOOST_UBLAS_STORAGE_SPARSE_H

#include <algorithm>
#include <map>
#include <set>

#include <boost/numeric/ublas/config.hpp>
#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/iterator.hpp>
#include <boost/numeric/ublas/storage.hpp>

namespace boost { namespace numeric { namespace ublas {

    namespace detail {

        template<class I, class T, class C>
        BOOST_UBLAS_INLINE
        I lower_bound (const I &begin, const I &end, const T &t, C compare) {
            // t <= *begin <=> ! (*begin < t)
            if (begin == end || ! compare (*begin, t))
                return begin;
            if (compare (*(end - 1), t))
                return end;
            return std::lower_bound (begin, end, t, compare);
        }
        template<class I, class T, class C>
        BOOST_UBLAS_INLINE
        I upper_bound (const I &begin, const I &end, const T &t, C compare) {
            if (begin == end || compare (t, *begin))
                return begin;
            // (*end - 1) <= t <=> ! (t < *end)
            if (! compare (t, *(end - 1)))
                return end;
            return std::upper_bound (begin, end, t, compare);
        }

        template<class P>
        struct less_pair {
            BOOST_UBLAS_INLINE
            bool operator () (const P &p1, const P &p2) {
                return p1.first < p2.first;
            }
        };
        template<class T>
        struct less_triple {
            BOOST_UBLAS_INLINE
            bool operator () (const T &t1, const T &t2) {
                return t1.first.first < t2.first.first ||
                       (t1.first.first == t2.first.first && t1.first.second < t2.first.second);
            }
        };

    }

#ifdef BOOST_UBLAS_STRICT_STORAGE_SPARSE
    template<class D>
    struct sparse_storage_element_traits {
        typedef typename D::index_type index_type;
        typedef typename D::data_const_reference data_const_reference;
        typedef typename D::data_reference data_reference;
    };
    template<>
    struct sparse_storage_element_traits<float> {
        typedef std::size_t index_type;
        typedef void data_const_reference;
        typedef void data_reference;
    };
    template<>
    struct sparse_storage_element_traits<double> {
        typedef std::size_t index_type;
        typedef void data_const_reference;
        typedef void data_reference;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct sparse_storage_element_traits<long double> {
        typedef std::size_t index_type;
        typedef void data_const_reference;
        typedef void data_reference;
    };
#endif
    template<>
    struct sparse_storage_element_traits<std::complex<float> > {
        typedef std::size_t index_type;
        typedef void data_const_reference;
        typedef void data_reference;
    };
    template<>
    struct sparse_storage_element_traits<std::complex<double> > {
        typedef std::size_t index_type;
        typedef void data_const_reference;
        typedef void data_reference;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct sparse_storage_element_traits<std::complex<long double> > {
        typedef std::size_t index_type;
        typedef void data_const_reference;
        typedef void data_reference;
    };
#endif

    template<class A>
    class sparse_storage_element:
       public container_reference<A> {
    public:
        typedef A array_type;
        typedef typename A::index_type index_type;
        typedef typename A::data_value_type data_value_type;
        // typedef const data_value_type &data_const_reference;
        typedef typename type_traits<data_value_type>::const_reference data_const_reference;
        typedef data_value_type &data_reference;
        typedef std::pair<index_type, data_value_type> value_type;
        typedef std::pair<index_type, data_value_type> *pointer;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        sparse_storage_element (array_type &a, pointer it):
            container_reference<array_type> (a), it_ (it), i_ (it->first), d_ (it->second) {}
        BOOST_UBLAS_INLINE
        sparse_storage_element (array_type &a, index_type i):
            container_reference<array_type> (a), it_ (), i_ (i), d_ () {
            pointer it = (*this) ().find (i_);
            if (it == (*this) ().end ())
                it = (*this) ().insert ((*this) ().end (), value_type (i_, d_));
            d_ = it->second;
        }
        BOOST_UBLAS_INLINE
        ~sparse_storage_element () {
            if (! it_)
                it_ = (*this) ().find (i_);
            BOOST_UBLAS_CHECK (it_ != (*this) ().end (), internal_logic ());
            it_->second = d_;
        }

        // Element access
        // FIXME: GCC 3.1 warn's, if enabled
#ifndef __GNUC__
        BOOST_UBLAS_INLINE
        typename sparse_storage_element_traits<data_value_type>::data_const_reference
        operator [] (typename sparse_storage_element_traits<data_value_type>::index_type i) const {
            return d_ [i];
        }
#endif
        BOOST_UBLAS_INLINE
        typename sparse_storage_element_traits<data_value_type>::data_reference
        operator [] (typename sparse_storage_element_traits<data_value_type>::index_type i) {
            return d_ [i];
        }

        // Assignment
        template<class D>
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator = (const D &d) {
            d_ = d;
            return *this;
        }
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator = (const sparse_storage_element &p) {
            d_ = p.d_;
            return *this;
        }
        template<class D>
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator += (const D &d) {
            d_ += d;
            return *this;
        }
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator += (const sparse_storage_element &p) {
            d_ += p.d_;
            return *this;
        }
        template<class D>
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator -= (const D &d) {
            d_ -= d;
            return *this;
        }
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator -= (const sparse_storage_element &p) {
            d_ -= p.d_;
            return *this;
        }
        template<class D>
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator *= (const D &d) {
            d_ *= d;
            return *this;
        }
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator *= (const sparse_storage_element &p) {
            d_ *= p.d_;
            return *this;
        }
        template<class D>
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator /= (const D &d) {
            d_ /= d;
            return *this;
        }
        BOOST_UBLAS_INLINE
        sparse_storage_element &operator /= (const sparse_storage_element &p) {
            d_ /= p.d_;
            return *this;
        }

        // Conversion
        // FIXME: GCC 3.1 warn's, if enabled
#ifndef __GNUC__
        BOOST_UBLAS_INLINE
        operator data_const_reference () const {
            return d_;
        }
#endif
        BOOST_UBLAS_INLINE
        operator data_reference () {
            return d_;
        }

    private:
        pointer it_;
        index_type i_;
        data_value_type d_;
    };
#endif

    // Map array
    template<class I, class T>
    class map_array {
    public:
        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;
        typedef I index_type;
        typedef T data_value_type;
        // typedef const T &data_const_reference;
        typedef typename type_traits<T>::const_reference data_const_reference;
#ifndef BOOST_UBLAS_STRICT_STORAGE_SPARSE
        typedef T &data_reference;
#else
        typedef sparse_storage_element<map_array> data_reference;
#endif
        typedef std::pair<I, T> value_type;
        typedef const std::pair<I, T> &const_reference;
        typedef std::pair<I, T> &reference;
        typedef const std::pair<I, T> *const_pointer;
        typedef std::pair<I, T> *pointer;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        map_array ():
            capacity_ (0), data_ (new value_type [0]), size_ (0) {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
        }
        BOOST_UBLAS_EXPLICIT BOOST_UBLAS_INLINE
        map_array (size_type size):
            capacity_ (size), data_ (new value_type [size]), size_ (0) {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
        }
        BOOST_UBLAS_INLINE
        map_array (const map_array &a):
            capacity_ (a.size_), data_ (new value_type [a.size_]), size_ (a.size_) {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
            *this = a;
        }
        BOOST_UBLAS_INLINE
        ~map_array () {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
            delete [] data_;
        }

        // Resizing
        BOOST_UBLAS_INLINE
        void resize (size_type size) {
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
            if (size > capacity_) {
                pointer data = new value_type [size << 1];
                // Assuming std compliant allocator as requested during review.
                // if (! data)
                //     throw std::bad_alloc ();
                // if (! data_)
                //     throw std::bad_alloc ();
                std::copy (data_, data_ + size_, data);
                delete [] data_;
                capacity_ = size << 1;
                data_ = data;
            }
            size_ = size;
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
        }

        // Reserving
        BOOST_UBLAS_INLINE
        void reserve (size_type capacity) {
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
            if (capacity > capacity_) {
                pointer data = new value_type [capacity];
                // Assuming std compliant allocator as requested during review.
                // if (! data)
                //     throw std::bad_alloc ();
                // if (! data_)
                //     throw std::bad_alloc ();
                std::copy (data_, data_ + size_, data);
                delete [] data_;
                capacity_ = capacity;
                data_ = data;
            }
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
        }

        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }

        // Element access
        BOOST_UBLAS_INLINE
        data_reference operator [] (index_type i) {
#ifndef BOOST_UBLAS_STRICT_STORAGE_SPARSE
            pointer it = find (i);
            if (it == end ())
                it = insert (end (), value_type (i, data_value_type ()));
            BOOST_UBLAS_CHECK (it != end (), internal_logic ());
            return it->second;
#else
            return data_reference (*this, i);
#endif
        }

        // Assignment
        BOOST_UBLAS_INLINE
        map_array &operator = (const map_array &a) {
            // Too unusual semantic.
            // Thanks to Michael Stevens for spotting this.
            // BOOST_UBLAS_CHECK (this != &a, external_logic ());
            if (this != &a) {
                resize (a.size_);
                std::copy (a.data_, a.data_ + a.size_, data_);
            }
            return *this;
        }
        BOOST_UBLAS_INLINE
        map_array &assign_temporary (map_array &a) {
            swap (a);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (map_array &a) {
            // Too unusual semantic.
            // BOOST_UBLAS_CHECK (this != &a, external_logic ());
            if (this != &a) {
                std::swap (capacity_, a.capacity_);
                std::swap (data_, a.data_);
                std::swap (size_, a.size_);
            }
        }
#ifndef BOOST_UBLAS_NO_MEMBER_FRIENDS
        BOOST_UBLAS_INLINE
        friend void swap (map_array &a1, map_array &a2) {
            a1.swap (a2);
        }
#endif

        // Element insertion and deletion
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer push_back (pointer it, const value_type &p) {
            if (size () == 0 || (it = end () - 1)->first < p.first) {
                resize (size () + 1);
                *(it = end () - 1) = p;
                return it;
            }
            // Raising exceptions abstracted as requested during review.
            // throw external_logic ();
            external_logic ().raise ();
            return it;
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer insert (pointer it, const value_type &p) {
            it = detail::lower_bound (begin (), end (), p, detail::less_pair<value_type> ());
            difference_type n = it - begin ();
            BOOST_UBLAS_CHECK (size () == 0 || size () == size_type (n) || it->first != p.first, external_logic ());
            resize (size () + 1);
            it = begin () + n;
            std::copy_backward (it, end () - 1, end ());
            *it = p;
            return it;
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void insert (pointer it, pointer it1, pointer it2) {
#ifdef BOOST_UBLAS_BOUNDS_CHECK
            while (it1 != it2) {
                insert (it, *it1);
                ++ it1;
            }
#else
            difference_type n = it - begin ();
            resize (size () + it2 - it1);
            it = begin () + n;
            std::copy (it1, it2, it);
            std::sort (begin (), end (), detail::less_pair<value_type> ());
#endif
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void erase (pointer it) {
            BOOST_UBLAS_CHECK (begin () <= it && it < end (), bad_index ());
            // Fixed by George Katsirelos.
            // (*it).second = data_value_type ();
            std::copy (it + 1, end (), it);
            resize (size () - 1);
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void erase (pointer it1, pointer it2) {
            BOOST_UBLAS_CHECK (begin () <= it1 && it1 < it2 && it2 <= end (), bad_index ());
            // Fixed by George Katsirelos.
            // while (it1 != it2) {
            //     BOOST_UBLAS_CHECK (begin () <= it1 && it1 < end (), bad_index ());
            //     (*it1).second = data_value_type ();
            //     ++ it1;
            // }
            std::copy (it2, end (), it1);
            resize (size () - (it2 - it1));
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void clear () {
            resize (0);
        }

        // Element lookup
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        const_pointer find (index_type i) const {
#ifdef BOOST_UBLAS_DEPRECATED
            std::pair<const_pointer, const_pointer> pit;
            pit = std::equal_range (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ());
            if (pit.first->first == i)
                return pit.first;
            else if (pit.second->first == i)
                return pit.second;
            else
                return end ();
#else
            const_pointer it (detail::lower_bound (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ()));
            if (it == end () || it->first != i)
                it = end ();
            return it;
#endif
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer find (index_type i) {
#ifdef BOOST_UBLAS_DEPRECATED
            std::pair<pointer, pointer> pit;
            pit = std::equal_range (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ());
            if (pit.first->first == i)
                return pit.first;
            else if (pit.second->first == i)
                return pit.second;
            else
                return end ();
#else
            pointer it (detail::lower_bound (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ()));
            if (it == end () || it->first != i)
                it = end ();
            return it;
#endif
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        const_pointer lower_bound (index_type i) const {
            return detail::lower_bound (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ());
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer lower_bound (index_type i) {
            return detail::lower_bound (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ());
        }
#ifdef BOOST_UBLAS_DEPRECATED
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        const_pointer upper_bound (index_type i) const {
            return detail::upper_bound (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ());
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer upper_bound (index_type i) {
            return detail::upper_bound (begin (), end (), value_type (i, data_value_type ()), detail::less_pair<value_type> ());
        }
#endif

        // Iterators simply are pointers.

        typedef const_pointer const_iterator;

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return data_;
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return data_ + size_;
        }

        typedef pointer iterator;

        BOOST_UBLAS_INLINE
        iterator begin () {
            return data_;
        }
        BOOST_UBLAS_INLINE
        iterator end () {
            return data_ + size_;
        }

        // Reverse iterators

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef std::reverse_iterator<const_iterator, value_type, const_reference> const_reverse_iterator;
#else
        typedef std::reverse_iterator<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 std::reverse_iterator<iterator, value_type, reference> reverse_iterator;
#else
        typedef std::reverse_iterator<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 capacity_;
        pointer data_;
        size_type size_;
    };

    template<class I, class T>
    BOOST_UBLAS_INLINE
    map_array<I, T> &assign_temporary (map_array<I, T> &a1, map_array<I, T> &a2) {
        return a1.assign_temporary (a2);
    }

    template<class I, class T, class F>
    BOOST_UBLAS_INLINE
    std::map<I, T, F> &assign_temporary (std::map<I, T, F> &a1, std::map<I, T, F> &a2) {
        // Too unusual semantic.
        // BOOST_UBLAS_CHECK (&a1 != &a2, external_logic ());
        if (&a1 != &a2)
            a1.swap (a2);
        return  a1;
    }
    // This specialization is missing in Dinkumware's STL?!
    template<class I, class T, class F>
    BOOST_UBLAS_INLINE
    void swap (std::map<I, T, F> &a1, std::map<I, T, F> &a2) {
        // Too unusual semantic.
        // BOOST_UBLAS_CHECK (&a1 != &a2, external_logic ());
        if (&a1 != &a2)
            a1.swap (a2);
    }

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    template<class A>
    struct map_traits {};
    template<class I, class T>
    struct map_traits<std::map<I, T> > {
        typedef typename std::map<I, T>::size_type size_type;
        typedef typename std::map<I, T>::mapped_type &reference;

        static void reserve (std::map<I, T> &a, size_type capacity) {}

        static reference make_reference (std::map<I, T> &a, typename std::map<I, T>::iterator it) {
            return (*it).second;
        }
    };
    template<class I, class T>
    struct map_traits<map_array<I, T> > {
        typedef typename map_array<I, T>::size_type size_type;
        typedef typename map_array<I, T>::data_reference reference;

        static void reserve (map_array<I, T> &a, size_type capacity) {
            a.reserve (capacity);
        }

        static reference make_reference (map_array<I, T> &a, typename map_array<I, T>::iterator it) {
            return reference (a, it);
        }
    };
#endif

    // Set array
    template<class I>
    class set_array {
    public:
        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;
        typedef I index_type;
        typedef I value_type;
        typedef const I &const_reference;
        typedef I &reference;
        typedef const I *const_pointer;
        typedef I *pointer;

        // Construction and destruction
        BOOST_UBLAS_INLINE
        set_array ():
            capacity_ (0), data_ (new value_type [0]), size_ (0) {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
        }
        BOOST_UBLAS_EXPLICIT BOOST_UBLAS_INLINE
        set_array (size_type size):
            capacity_ (size), data_ (new value_type [size]), size_ (0) {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
        }
        BOOST_UBLAS_INLINE
        set_array (const set_array &a):
            capacity_ (a.size_), data_ (new value_type [a.size_]), size_ (a.size_) {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
            *this = a;
        }
        BOOST_UBLAS_INLINE
        ~set_array () {
            // Assuming std compliant allocator as requested during review.
            // if (! data_)
            //     throw std::bad_alloc ();
            delete [] data_;
        }

        // Resizing
        BOOST_UBLAS_INLINE
        void resize (size_type size) {
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
            if (size > capacity_) {
                pointer data = new value_type [size << 1];
                // Assuming std compliant allocator as requested during review.
                // if (! data)
                //     throw std::bad_alloc ();
                // if (! data_)
                //     throw std::bad_alloc ();
                std::copy (data_, data_ + size_, data);
                delete [] data_;
                capacity_ = size << 1;
                data_ = data;
            }
            size_ = size;
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
        }

        // Reserving
        BOOST_UBLAS_INLINE
        void reserve (size_type capacity) {
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
            if (capacity > capacity_) {
                pointer data = new value_type [capacity];
                // Assuming std compliant allocator as requested during review.
                // if (! data)
                //     throw std::bad_alloc ();
                // if (! data_)
                //     throw std::bad_alloc ();
                std::copy (data_, data_ + size_, data);
                delete [] data_;
                capacity_ = capacity;
                data_ = data;
            }
            BOOST_UBLAS_CHECK (size_ <= capacity_, internal_logic ());
        }

        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }

        // Element access
        BOOST_UBLAS_INLINE
        const_reference operator [] (index_type i) {
            pointer it = find (i);
            if (it == end ())
                it = insert (end (), i);
            BOOST_UBLAS_CHECK (it != end (), internal_logic ());
            return *it;
        }

        // Assignment
        BOOST_UBLAS_INLINE
        set_array &operator = (const set_array &a) {
            // Too unusual semantic.
            // Thanks to Michael Stevens for spotting this.
            // BOOST_UBLAS_CHECK (this != &a, external_logic ());
            if (this != &a) {
                resize (a.size_);
                std::copy (a.data_, a.data_ + a.size_, data_);
            }
            return *this;
        }
        BOOST_UBLAS_INLINE
        set_array &assign_temporary (set_array &a) {
            swap (a);
            return *this;
        }

        // Swapping
        BOOST_UBLAS_INLINE
        void swap (set_array &a) {
            // Too unusual semantic.
            // BOOST_UBLAS_CHECK (this != &a, external_logic ());
            if (this != &a) {
                std::swap (capacity_, a.capacity_);
                std::swap (data_, a.data_);
                std::swap (size_, a.size_);
            }
        }
#ifndef BOOST_UBLAS_NO_MEMBER_FRIENDS
        BOOST_UBLAS_INLINE
        friend void swap (set_array &a1, set_array &a2) {
            a1.swap (a2);
        }
#endif

        // Element insertion and deletion
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer push_back (pointer it, const value_type &p) {
            if (size () == 0 || (*(it = end () - 1)) < p) {
                resize (size () + 1);
                *(it = end () - 1) = p;
                return it;
            }
            // Raising exceptions abstracted as requested during review.
            // throw external_logic ();
            external_logic ().raise ();
            return it;
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer insert (pointer it, const value_type &p) {
            it = detail::lower_bound (begin (), end (), p, std::less<value_type> ());
            difference_type n = it - begin ();
            BOOST_UBLAS_CHECK (size () == 0 || size () == size_type (n) || *it != p, external_logic ());
            resize (size () + 1);
            it = begin () + n;
            std::copy_backward (it, end () - 1, end ());
            *it = p;
            return it;
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void insert (pointer it, pointer it1, pointer it2) {
#ifdef BOOST_UBLAS_BOUNDS_CHECK
            while (it1 != it2) {
                insert (it, *it1);
                ++ it1;
            }
#else
            difference_type n = it - begin ();
            resize (size () + it2 - it1);
            it = begin () + n;
            std::copy (it1, it2, it);
            std::sort (begin (), end (), std::less<value_type> ());
#endif
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void erase (pointer it) {
            BOOST_UBLAS_CHECK (begin () <= it && it < end (), bad_index ());
            std::copy (it + 1, end (), it);
            resize (size () - 1);
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void erase (pointer it1, pointer it2) {
            BOOST_UBLAS_CHECK (begin () <= it1 && it1 < it2 && it2 <= end (), bad_index ());
            std::copy (it2, end (), it1);
            resize (size () - (it2 - it1));
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        void clear () {
            resize (0);
        }

        // Element lookup
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        const_pointer find (index_type i) const {
#ifdef BOOST_UBLAS_DEPRECATED
            std::pair<const_pointer, const_pointer> pit;
            pit = std::equal_range (begin (), end (), i, std::less<value_type> ());
            if (*pit.first == i)
                return pit.first;
            else if (*pit.second == i)
                return pit.second;
            else
                return end ();
#else
            const_pointer it (detail::lower_bound (begin (), end (), i, std::less<value_type> ()));
            if (it == end () || *it != i)
                it = end ();
            return it;
#endif
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer find (index_type i) {
#ifdef BOOST_UBLAS_DEPRECATED
            std::pair<pointer, pointer> pit;
            pit = std::equal_range (begin (), end (), i, std::less<value_type> ());
            if (*pit.first == i)
                return pit.first;
            else if (*pit.second == i)
                return pit.second;
            else
                return end ();
#else
            pointer it (detail::lower_bound (begin (), end (), i, std::less<value_type> ()));
            if (it == end () || *it != i)
                it = end ();
            return it;
#endif
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        const_pointer lower_bound (index_type i) const {
            return detail::lower_bound (begin (), end (), i, std::less<value_type> ());
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer lower_bound (index_type i) {
            return detail::lower_bound (begin (), end (), i, std::less<value_type> ());
        }
#ifdef BOOST_UBLAS_DEPRECATED
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        const_pointer upper_bound (index_type i) const {
            return detail::upper_bound (begin (), end (), i, std::less<value_type> ());
        }
        // This function seems to be big. So we do not let the compiler inline it.
        // BOOST_UBLAS_INLINE
        pointer upper_bound (index_type i) {
            return detail::upper_bound (begin (), end (), i, std::less<value_type> ());
        }
#endif

        // Iterators simply are pointers.

        typedef const_pointer const_iterator;

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return data_;
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return data_ + size_;
        }

        typedef pointer iterator;

        BOOST_UBLAS_INLINE
        iterator begin () {
            return data_;
        }
        BOOST_UBLAS_INLINE
        iterator end () {
            return data_ + size_;
        }

        // Reverse iterators

#ifdef BOOST_MSVC_STD_ITERATOR
        typedef std::reverse_iterator<const_iterator, value_type, const_reference> const_reverse_iterator;
#else
        typedef std::reverse_iterator<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 std::reverse_iterator<iterator, value_type, reference> reverse_iterator;
#else
        typedef std::reverse_iterator<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 capacity_;
        pointer data_;
        size_type size_;
    };

    template<class I>
    BOOST_UBLAS_INLINE
    set_array<I> &assign_temporary (set_array<I> &a1, set_array<I> &a2) {
        return a1.assign_temporary (a2);
    }

    template<class I, class F>
    BOOST_UBLAS_INLINE
    std::set<I, F> &assign_temporary (std::set<I, F> &a1, std::set<I, F> &a2) {
        // Too unusual semantic.
        // BOOST_UBLAS_CHECK (&a1 != &a2, external_logic ());
        if (&a1 != &a2)
            a1.swap (a2);
        return  a1;
    }
    // This specialization is missing in Dinkumware's STL?!
    template<class I, class F>
    BOOST_UBLAS_INLINE
    void swap (std::set<I, F> &a1, std::set<I, F> &a2) {
        // Too unusual semantic.
        // BOOST_UBLAS_CHECK (&a1 != &a2, external_logic ());
        if (&a1 != &a2)
            a1.swap (a2);
    }

}}}

#endif


/*****************************************************************************
*
* Copyright (c) 2003-2018 by The University of Queensland
* http://www.uq.edu.au
*
* Primary Business: Queensland, Australia
* Licensed under the Apache License, version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Development until 2012 by Earth Systems Science Computational Center (ESSCC)
* Development 2012-2013 by School of Earth Sciences
* Development from 2014 by Centre for Geoscience Computing (GeoComp)
*
*****************************************************************************/

#include <dudley/DomainFactory.h>

#include <escript/index.h>
#include <escript/SubWorld.h>

#ifdef ESYS_HAVE_NETCDF
#ifdef NETCDF4
  #include <ncDim.h>
  #include <ncVar.h>
  #include <ncFile.h>
  #include <escript/NCHelper.h>
#else
#include <netcdfcpp.h>
#endif
#endif

#include <boost/python/extract.hpp>
#include <boost/scoped_array.hpp>

#include <sstream>

using namespace std;
using namespace escript;
namespace bp = boost::python;

#ifdef NETCDF4
using namespace netCDF;
#endif

namespace dudley {

#ifdef ESYS_HAVE_NETCDF
#ifdef NETCDF4

// A convenience method to retrieve an integer attribute from a NetCDF file
template<typename T>
T ncReadAtt(NcFile& dataFile, const string& fName, const string& attrName)
{
    NcGroupAtt attr = dataFile.getAtt(attrName.c_str());
    if (attr.isNull()) {
        stringstream msg;
        msg << "loadMesh: Error retrieving integer attribute '" << attrName
            << "' from NetCDF file '" << fName << "'";
        throw IOError(msg.str());
    }
    T value;
    attr.getValues(&value);
    return value;
}
#else
// A convenience method to retrieve an integer attribute from a NetCDF file
template<typename T>
T ncReadAtt(NcFile* dataFile, const string& fName, const string& attrName)
{
    NcAtt* attr = dataFile->get_att(attrName.c_str());
    if (!attr) {
        stringstream msg;
        msg << "loadMesh: Error retrieving integer attribute '" << attrName
            << "' from NetCDF file '" << fName << "'";
        throw IOError(msg.str());
    }
    T value = (sizeof(T) > 4 ? attr->as_long(0) : attr->as_int(0));
    delete attr;
    return value;
}
#endif
#endif

inline void cleanupAndThrow(DudleyDomain* dom, string msg)
{
    delete dom;
    string msgPrefix("loadMesh: NetCDF operation failed - ");
    throw IOError(msgPrefix+msg);
}

#ifdef NETCDF4

Domain_ptr DudleyDomain::load(const string& fileName)
{
#ifdef ESYS_HAVE_NETCDF
    JMPI mpiInfo = makeInfo(MPI_COMM_WORLD);
    const string fName(mpiInfo->appendRankToFileName(fileName));

    // Open NetCDF file for reading
    NcGroupAtt attr;
    NcVar nc_var_temp;
    NcFile dataFile;
    if (!openNcFile(dataFile, fileName))
    {
        throw IOError("load: opening of netCDF file for input failed.");
    }

    // Read NetCDF integer attributes

    // index_size was only introduced with 64-bit index support so fall back
    // to 32 bits if not found.
    int index_size;
    try {
        index_size = ncReadAtt<int>(dataFile, fName, "index_size");
    } catch (IOError& e) {
        index_size = 4;
    }
    // technically we could cast if reading 32-bit data on 64-bit escript
    // but cost-benefit analysis clearly favours this implementation for now
    if (sizeof(index_t) != index_size) {
        throw IOError("loadMesh: size of index types at runtime differ from dump file");
    }

    int mpi_size = ncReadAtt<int>(dataFile, fName, "mpi_size");
    int mpi_rank = ncReadAtt<int>(dataFile, fName, "mpi_rank");
    int numDim = ncReadAtt<int>(dataFile, fName, "numDim");
    dim_t numNodes = ncReadAtt<dim_t>(dataFile, fName, "numNodes");
    dim_t num_Elements = ncReadAtt<dim_t>(dataFile, fName, "num_Elements");
    dim_t num_FaceElements = ncReadAtt<dim_t>(dataFile, fName, "num_FaceElements");
    dim_t num_Points = ncReadAtt<dim_t>(dataFile, fName, "num_Points");
    int num_Elements_numNodes = ncReadAtt<int>(dataFile, fName, "num_Elements_numNodes");
    int Elements_TypeId = ncReadAtt<int>(dataFile, fName, "Elements_TypeId");
    int num_FaceElements_numNodes = ncReadAtt<int>(dataFile, fName, "num_FaceElements_numNodes");
    int FaceElements_TypeId = ncReadAtt<int>(dataFile, fName, "FaceElements_TypeId");
    int Points_TypeId = ncReadAtt<int>(dataFile, fName, "Points_TypeId");
    int num_Tags = ncReadAtt<int>(dataFile, fName, "num_Tags");

    // Verify size and rank
    if (mpiInfo->size != mpi_size) {
        stringstream msg;
        msg << "loadMesh: The NetCDF file '" << fName
            << "' can only be read on " << mpi_size
            << " CPUs. Currently running: " << mpiInfo->size;
        throw DudleyException(msg.str());
    }
    if (mpiInfo->rank != mpi_rank) {
        stringstream msg;
        msg << "loadMesh: The NetCDF file '" << fName
            << "' should be read on CPU #" << mpi_rank
            << " and NOT on #" << mpiInfo->rank;
        throw DudleyException(msg.str());
    }

    // Read mesh name
    if ((attr=dataFile.getAtt("Name")), attr.isNull() ) {
        stringstream msg;
        msg << "loadMesh: Error retrieving mesh name from NetCDF file '"
            << fName << "'";
        throw IOError(msg.str());
    }
    string name;
    attr.getValues(name);

    // allocate mesh
    DudleyDomain* dom = new DudleyDomain(name.c_str(), numDim, mpiInfo);

    // read nodes
    NodeFile* nodes = dom->getNodes();
    nodes->allocTable(numNodes);
    // Nodes_Id
    if (( nc_var_temp = dataFile.getVar("Nodes_Id")), nc_var_temp.isNull() )
        cleanupAndThrow(dom, "get_var(Nodes_Id)");
    nc_var_temp.getVar(&nodes->Id[0]);    // numNodes) )
    // Nodes_Tag
    if (( nc_var_temp = dataFile.getVar("Nodes_Tag")), nc_var_temp.isNull() )
        cleanupAndThrow(dom, "get_var(Nodes_Tag)");
    nc_var_temp.getVar(&nodes->Tag[0]);   // numNodes
    // Nodes_gDOF
    if (( nc_var_temp = dataFile.getVar("Nodes_gDOF")), nc_var_temp.isNull() )
        cleanupAndThrow(dom, "get_var(Nodes_gDOF)");
    nc_var_temp.getVar(&nodes->globalDegreesOfFreedom[0]);  // numNodes
    // Nodes_gNI
    if (( nc_var_temp = dataFile.getVar("Nodes_gNI")), nc_var_temp.isNull() )
        cleanupAndThrow(dom, "get_var(Nodes_gNI)");
    nc_var_temp.getVar(&nodes->globalNodesIndex[0]);    // numNodes
    // Nodes_Coordinates
    if ((nc_var_temp = dataFile.getVar("Nodes_Coordinates")), nc_var_temp.isNull())
        cleanupAndThrow(dom, "get_var(Nodes_Coordinates)");
    nc_var_temp.getVar(&nodes->Coordinates[0]); // numNodes, numDim

    nodes->updateTagList();

    // read elements
    ElementFile* elements = new ElementFile((ElementTypeId)Elements_TypeId, mpiInfo);
    dom->setElements(elements);
    elements->allocTable(num_Elements);
    elements->minColor = 0;
    elements->maxColor = num_Elements-1;
    if (num_Elements > 0) {
       // Elements_Id
       if (( nc_var_temp = dataFile.getVar("Elements_Id")), nc_var_temp.isNull() )
           cleanupAndThrow(dom, "get_var(Elements_Id)");
       nc_var_temp.getVar(&elements->Id[0]);    // num_Elements
       // Elements_Tag
       if (( nc_var_temp = dataFile.getVar("Elements_Tag")), nc_var_temp.isNull())
           cleanupAndThrow(dom, "get_var(Elements_Tag)");
       nc_var_temp.getVar(&elements->Tag[0]);   // num_Elements
       // Elements_Owner
       if (( nc_var_temp = dataFile.getVar("Elements_Owner")), nc_var_temp.isNull() )
           cleanupAndThrow(dom, "get_var(Elements_Owner)");
       nc_var_temp.getVar(&elements->Owner[0]); // num_Elements
       // Elements_Color
       if (( nc_var_temp = dataFile.getVar("Elements_Color")), nc_var_temp.isNull() )
           cleanupAndThrow(dom, "get_var(Elements_Color)");
       nc_var_temp.getVar(&elements->Color[0]); // num_Elements
       // Elements_Nodes
       int* Elements_Nodes = new int[num_Elements*num_Elements_numNodes];
       if ((nc_var_temp = dataFile.getVar("Elements_Nodes")), nc_var_temp.isNull()) {
           delete[] Elements_Nodes;
           cleanupAndThrow(dom, "get_var(Elements_Nodes)");
       }
       nc_var_temp.getVar(&Elements_Nodes[0]);  // num_Elements, num_Elements_numNodes
       // Copy temp array into elements->Nodes
       for (index_t i = 0; i < num_Elements; i++) {
           for (int j = 0; j < num_Elements_numNodes; j++) {
               elements->Nodes[INDEX2(j,i,num_Elements_numNodes)]
                    = Elements_Nodes[INDEX2(j,i,num_Elements_numNodes)];
           }
       }
       delete[] Elements_Nodes;
    } // num_Elements > 0
    elements->updateTagList();

    // get the face elements
    ElementFile* faces = new ElementFile((ElementTypeId)FaceElements_TypeId, mpiInfo);
    dom->setFaceElements(faces);
    faces->allocTable(num_FaceElements);
    faces->minColor = 0;
    faces->maxColor = num_FaceElements-1;
    if (num_FaceElements > 0) {
        // FaceElements_Id
        if (( nc_var_temp = dataFile.getVar("FaceElements_Id")), nc_var_temp.isNull() )
            cleanupAndThrow(dom, "get_var(FaceElements_Id)");
        nc_var_temp.getVar(&faces->Id[0]);  // num_FaceElements
        // FaceElements_Tag
        if (( nc_var_temp = dataFile.getVar("FaceElements_Tag")), nc_var_temp.isNull() )
            cleanupAndThrow(dom, "get_var(FaceElements_Tag)");
        nc_var_temp.getVar(&faces->Tag[0]); // num_FaceElements) )
        // FaceElements_Owner
        if (( nc_var_temp = dataFile.getVar("FaceElements_Owner")), nc_var_temp.isNull() )
            cleanupAndThrow(dom, "get_var(FaceElements_Owner)");
        nc_var_temp.getVar(&faces->Owner[0]);   //, num_FaceElements) )
        // FaceElements_Color
        if (( nc_var_temp = dataFile.getVar("FaceElements_Color")), nc_var_temp.isNull() )
            cleanupAndThrow(dom, "get_var(FaceElements_Color)");
        nc_var_temp.getVar(&faces->Color[0]);   //, num_FaceElements) )
        // FaceElements_Nodes
        int* FaceElements_Nodes = new int[num_FaceElements*num_FaceElements_numNodes];
        if ((nc_var_temp = dataFile.getVar("FaceElements_Nodes")), nc_var_temp.isNull()) {
            delete[] FaceElements_Nodes;
            cleanupAndThrow(dom, "get_var(FaceElements_Nodes)");
        }
        nc_var_temp.getVar(&(FaceElements_Nodes[0]));   // num_FaceElements, num_FaceElements_numNodes
        // Copy temp array into faces->Nodes
        for (index_t i = 0; i < num_FaceElements; i++) {
            for (int j = 0; j < num_FaceElements_numNodes; j++) {
                faces->Nodes[INDEX2(j,i,num_FaceElements_numNodes)] = FaceElements_Nodes[INDEX2(j,i,num_FaceElements_numNodes)];
            }
        }
        delete[] FaceElements_Nodes;
    } // num_FaceElements > 0
    faces->updateTagList();

    // get the Points (nodal elements)
    ElementFile* points = new ElementFile((ElementTypeId)Points_TypeId, mpiInfo);
    dom->setPoints(points);
    points->allocTable(num_Points);
    points->minColor = 0;
    points->maxColor = num_Points-1;
    if (num_Points > 0) {
        // Points_Id
        if (( nc_var_temp = dataFile.getVar("Points_Id")), nc_var_temp.isNull())
            cleanupAndThrow(dom, "get_var(Points_Id)");
        nc_var_temp.getVar(&points->Id[0]); // num_Points
        // Points_Tag
        if (( nc_var_temp = dataFile.getVar("Points_Tag")), nc_var_temp.isNull())
            cleanupAndThrow(dom, "get_var(Points_Tag)");
        nc_var_temp.getVar(&points->Tag[0]);    // num_Points
        // Points_Owner
        if (( nc_var_temp = dataFile.getVar("Points_Owner")), nc_var_temp.isNull())
            cleanupAndThrow(dom, "get_var(Points_Owner)");
        nc_var_temp.getVar(&points->Owner[0]);   // num_Points
        // Points_Color
        if (( nc_var_temp = dataFile.getVar("Points_Color")), nc_var_temp.isNull())
            cleanupAndThrow(dom, "get_var(Points_Color)");
        nc_var_temp.getVar(&points->Color[0]);  // num_Points
        // Points_Nodes
        int* Points_Nodes = new int[num_Points];
        if ((nc_var_temp = dataFile.getVar("Points_Nodes")), nc_var_temp.isNull()) {
            delete[] Points_Nodes;
            cleanupAndThrow(dom, "get_var(Points_Nodes)");
        }
        nc_var_temp.getVar(&Points_Nodes[0]);   // num_Points
        // Copy temp array into points->Nodes
        for (index_t i = 0; i < num_Points; i++) {
            points->Id[points->Nodes[INDEX2(0,i,1)]] = Points_Nodes[i];
        }
        delete[] Points_Nodes;
    } // num_Points > 0
    points->updateTagList();

    // get the tags
    if (num_Tags > 0) {
        // Temp storage to gather node IDs
        int *Tags_keys = new int[num_Tags];
        char name_temp[4096];
        int i;

        // Tags_keys
        if (( nc_var_temp = dataFile.getVar("Tags_keys")), nc_var_temp.isNull() ) {
            delete[] Tags_keys;
            cleanupAndThrow(dom, "get_var(Tags_keys)");
        }
        nc_var_temp.getVar(&Tags_keys[0]);  // num_Tags
        for (i=0; i<num_Tags; i++) {
          // Retrieve tag name
          sprintf(name_temp, "Tags_name_%d", i);
          if ((attr=dataFile.getAtt(name_temp)), attr.isNull() ) {
              delete[] Tags_keys;
              stringstream msg;
              msg << "get_att(" << name_temp << ")";
              cleanupAndThrow(dom, msg.str());
          }
          string name;
          attr.getValues(name);
          dom->setTagMap(name.c_str(), Tags_keys[i]);
        }
        delete[] Tags_keys;
    }

    // Nodes_DofDistribution
    IndexVector first_DofComponent(mpi_size+1);
    if ((nc_var_temp = dataFile.getVar("Nodes_DofDistribution")), nc_var_temp.isNull() ) {
        cleanupAndThrow(dom, "get_var(Nodes_DofDistribution)");
    }
    nc_var_temp.getVar(&first_DofComponent[0]); // mpi_size+1

    // Nodes_NodeDistribution
    IndexVector first_NodeComponent(mpi_size+1);
    if ((nc_var_temp = dataFile.getVar("Nodes_NodeDistribution")), nc_var_temp.isNull() ) {
        cleanupAndThrow(dom, "get_var(Nodes_NodeDistribution)");
    }
    nc_var_temp.getVar(&first_NodeComponent[0]); // mpi_size+1
    dom->createMappings(first_DofComponent, first_NodeComponent);

    return dom->getPtr();
#else
    throw DudleyException("loadMesh: not compiled with NetCDF. Please contact your installation manager.");
#endif // ESYS_HAVE_NETCDF
}

#else

Domain_ptr DudleyDomain::load(const string& fileName)
{
#ifdef ESYS_HAVE_NETCDF
    JMPI mpiInfo = makeInfo(MPI_COMM_WORLD);
    const string fName(mpiInfo->appendRankToFileName(fileName));

    // Open NetCDF file for reading
    NcAtt *attr;
    NcVar *nc_var_temp;
    // netCDF error handler
    NcError err(NcError::silent_nonfatal);
    // Create the NetCDF file.
    NcFile dataFile(fName.c_str(), NcFile::ReadOnly);
    if (!dataFile.is_valid()) {
        stringstream msg;
        msg << "loadMesh: Opening NetCDF file '" << fName << "' for reading failed.";
        throw IOError(msg.str());
    }

    // Read NetCDF integer attributes

    // index_size was only introduced with 64-bit index support so fall back
    // to 32 bits if not found.
    int index_size;
    try {
        index_size = ncReadAtt<int>(&dataFile, fName, "index_size");
    } catch (IOError& e) {
        index_size = 4;
    }
    // technically we could cast if reading 32-bit data on 64-bit escript
    // but cost-benefit analysis clearly favours this implementation for now
    if (sizeof(index_t) != index_size) {
        throw IOError("loadMesh: size of index types at runtime differ from dump file");
    }

    int mpi_size = ncReadAtt<int>(&dataFile, fName, "mpi_size");
    int mpi_rank = ncReadAtt<int>(&dataFile, fName, "mpi_rank");
    int numDim = ncReadAtt<int>(&dataFile, fName, "numDim");
    dim_t numNodes = ncReadAtt<dim_t>(&dataFile, fName, "numNodes");
    dim_t num_Elements = ncReadAtt<dim_t>(&dataFile, fName, "num_Elements");
    dim_t num_FaceElements = ncReadAtt<dim_t>(&dataFile, fName, "num_FaceElements");
    dim_t num_Points = ncReadAtt<dim_t>(&dataFile, fName, "num_Points");
    int num_Elements_numNodes = ncReadAtt<int>(&dataFile, fName, "num_Elements_numNodes");
    int Elements_TypeId = ncReadAtt<int>(&dataFile, fName, "Elements_TypeId");
    int num_FaceElements_numNodes = ncReadAtt<int>(&dataFile, fName, "num_FaceElements_numNodes");
    int FaceElements_TypeId = ncReadAtt<int>(&dataFile, fName, "FaceElements_TypeId");
    int Points_TypeId = ncReadAtt<int>(&dataFile, fName, "Points_TypeId");
    int num_Tags = ncReadAtt<int>(&dataFile, fName, "num_Tags");

    // Verify size and rank
    if (mpiInfo->size != mpi_size) {
        stringstream msg;
        msg << "loadMesh: The NetCDF file '" << fName
            << "' can only be read on " << mpi_size
            << " CPUs. Currently running: " << mpiInfo->size;
        throw DudleyException(msg.str());
    }
    if (mpiInfo->rank != mpi_rank) {
        stringstream msg;
        msg << "loadMesh: The NetCDF file '" << fName
            << "' should be read on CPU #" << mpi_rank
            << " and NOT on #" << mpiInfo->rank;
        throw DudleyException(msg.str());
    }

    // Read mesh name
    if (! (attr=dataFile.get_att("Name")) ) {
        stringstream msg;
        msg << "loadMesh: Error retrieving mesh name from NetCDF file '"
            << fName << "'";
        throw IOError(msg.str());
    }
    boost::scoped_array<char> name(attr->as_string(0));
    delete attr;

    // allocate mesh
    DudleyDomain* dom = new DudleyDomain(name.get(), numDim, mpiInfo);

    // read nodes
    NodeFile* nodes = dom->getNodes();
    nodes->allocTable(numNodes);
    // Nodes_Id
    if (! ( nc_var_temp = dataFile.get_var("Nodes_Id")) )
        cleanupAndThrow(dom, "get_var(Nodes_Id)");
    if (! nc_var_temp->get(&nodes->Id[0], numNodes) )
        cleanupAndThrow(dom, "get(Nodes_Id)");
    // Nodes_Tag
    if (! ( nc_var_temp = dataFile.get_var("Nodes_Tag")) )
        cleanupAndThrow(dom, "get_var(Nodes_Tag)");
    if (! nc_var_temp->get(&nodes->Tag[0], numNodes) )
        cleanupAndThrow(dom, "get(Nodes_Tag)");
    // Nodes_gDOF
    if (! ( nc_var_temp = dataFile.get_var("Nodes_gDOF")) )
        cleanupAndThrow(dom, "get_var(Nodes_gDOF)");
    if (! nc_var_temp->get(&nodes->globalDegreesOfFreedom[0], numNodes) )
        cleanupAndThrow(dom, "get(Nodes_gDOF)");
    // Nodes_gNI
    if (! ( nc_var_temp = dataFile.get_var("Nodes_gNI")) )
        cleanupAndThrow(dom, "get_var(Nodes_gNI)");
    if (! nc_var_temp->get(&nodes->globalNodesIndex[0], numNodes) )
        cleanupAndThrow(dom, "get(Nodes_gNI)");
    // Nodes_Coordinates
    if (!(nc_var_temp = dataFile.get_var("Nodes_Coordinates")))
        cleanupAndThrow(dom, "get_var(Nodes_Coordinates)");
    if (! nc_var_temp->get(&nodes->Coordinates[0], numNodes, numDim) )
        cleanupAndThrow(dom, "get(Nodes_Coordinates)");

    nodes->updateTagList();

    // read elements
    ElementFile* elements = new ElementFile((ElementTypeId)Elements_TypeId, mpiInfo);
    dom->setElements(elements);
    elements->allocTable(num_Elements);
    elements->minColor = 0;
    elements->maxColor = num_Elements-1;
    if (num_Elements > 0) {
       // Elements_Id
       if (! ( nc_var_temp = dataFile.get_var("Elements_Id")) )
           cleanupAndThrow(dom, "get_var(Elements_Id)");
       if (! nc_var_temp->get(&elements->Id[0], num_Elements) )
           cleanupAndThrow(dom, "get(Elements_Id)");
       // Elements_Tag
       if (! ( nc_var_temp = dataFile.get_var("Elements_Tag")) )
           cleanupAndThrow(dom, "get_var(Elements_Tag)");
       if (! nc_var_temp->get(&elements->Tag[0], num_Elements) )
           cleanupAndThrow(dom, "get(Elements_Tag)");
       // Elements_Owner
       if (! ( nc_var_temp = dataFile.get_var("Elements_Owner")) )
           cleanupAndThrow(dom, "get_var(Elements_Owner)");
       if (! nc_var_temp->get(&elements->Owner[0], num_Elements) )
           cleanupAndThrow(dom, "get(Elements_Owner)");
       // Elements_Color
       if (! ( nc_var_temp = dataFile.get_var("Elements_Color")) )
           cleanupAndThrow(dom, "get_var(Elements_Color)");
       if (! nc_var_temp->get(&elements->Color[0], num_Elements) )
           cleanupAndThrow(dom, "get(Elements_Color)");
       // Elements_Nodes
       int* Elements_Nodes = new int[num_Elements*num_Elements_numNodes];
       if (!(nc_var_temp = dataFile.get_var("Elements_Nodes"))) {
           delete[] Elements_Nodes;
           cleanupAndThrow(dom, "get_var(Elements_Nodes)");
       }
       if (! nc_var_temp->get(&Elements_Nodes[0], num_Elements, num_Elements_numNodes) ) {
           delete[] Elements_Nodes;
           cleanupAndThrow(dom, "get(Elements_Nodes)");
       }

       // Copy temp array into elements->Nodes
       for (index_t i = 0; i < num_Elements; i++) {
           for (int j = 0; j < num_Elements_numNodes; j++) {
               elements->Nodes[INDEX2(j,i,num_Elements_numNodes)]
                    = Elements_Nodes[INDEX2(j,i,num_Elements_numNodes)];
           }
       }
       delete[] Elements_Nodes;
    } // num_Elements > 0
    elements->updateTagList();

    // get the face elements
    ElementFile* faces = new ElementFile((ElementTypeId)FaceElements_TypeId, mpiInfo);
    dom->setFaceElements(faces);
    faces->allocTable(num_FaceElements);
    faces->minColor = 0;
    faces->maxColor = num_FaceElements-1;
    if (num_FaceElements > 0) {
        // FaceElements_Id
        if (! ( nc_var_temp = dataFile.get_var("FaceElements_Id")) )
            cleanupAndThrow(dom, "get_var(FaceElements_Id)");
        if (! nc_var_temp->get(&faces->Id[0], num_FaceElements) )
            cleanupAndThrow(dom, "get(FaceElements_Id)");
        // FaceElements_Tag
        if (! ( nc_var_temp = dataFile.get_var("FaceElements_Tag")) )
            cleanupAndThrow(dom, "get_var(FaceElements_Tag)");
        if (! nc_var_temp->get(&faces->Tag[0], num_FaceElements) )
            cleanupAndThrow(dom, "get(FaceElements_Tag)");
        // FaceElements_Owner
        if (! ( nc_var_temp = dataFile.get_var("FaceElements_Owner")) )
            cleanupAndThrow(dom, "get_var(FaceElements_Owner)");
        if (! nc_var_temp->get(&faces->Owner[0], num_FaceElements) )
            cleanupAndThrow(dom, "get(FaceElements_Owner)");
        // FaceElements_Color
        if (! ( nc_var_temp = dataFile.get_var("FaceElements_Color")) )
            cleanupAndThrow(dom, "get_var(FaceElements_Color)");
        if (! nc_var_temp->get(&faces->Color[0], num_FaceElements) )
            cleanupAndThrow(dom, "get(FaceElements_Color)");
        // FaceElements_Nodes
        int* FaceElements_Nodes = new int[num_FaceElements*num_FaceElements_numNodes];
        if (!(nc_var_temp = dataFile.get_var("FaceElements_Nodes"))) {
            delete[] FaceElements_Nodes;
            cleanupAndThrow(dom, "get_var(FaceElements_Nodes)");
        }
        if (! nc_var_temp->get(&(FaceElements_Nodes[0]), num_FaceElements, num_FaceElements_numNodes) ) {
            delete[] FaceElements_Nodes;
            cleanupAndThrow(dom, "get(FaceElements_Nodes)");
        }
        // Copy temp array into faces->Nodes
        for (index_t i = 0; i < num_FaceElements; i++) {
            for (int j = 0; j < num_FaceElements_numNodes; j++) {
                faces->Nodes[INDEX2(j,i,num_FaceElements_numNodes)] = FaceElements_Nodes[INDEX2(j,i,num_FaceElements_numNodes)];
            }
        }
        delete[] FaceElements_Nodes;
    } // num_FaceElements > 0
    faces->updateTagList();

    // get the Points (nodal elements)
    ElementFile* points = new ElementFile((ElementTypeId)Points_TypeId, mpiInfo);
    dom->setPoints(points);
    points->allocTable(num_Points);
    points->minColor = 0;
    points->maxColor = num_Points-1;
    if (num_Points > 0) {
        // Points_Id
        if (! ( nc_var_temp = dataFile.get_var("Points_Id")))
            cleanupAndThrow(dom, "get_var(Points_Id)");
        if (! nc_var_temp->get(&points->Id[0], num_Points))
            cleanupAndThrow(dom, "get(Points_Id)");
        // Points_Tag
        if (! ( nc_var_temp = dataFile.get_var("Points_Tag")))
            cleanupAndThrow(dom, "get_var(Points_Tag)");
        if (! nc_var_temp->get(&points->Tag[0], num_Points))
            cleanupAndThrow(dom, "get(Points_Tag)");
        // Points_Owner
        if (! ( nc_var_temp = dataFile.get_var("Points_Owner")))
            cleanupAndThrow(dom, "get_var(Points_Owner)");
        if (!nc_var_temp->get(&points->Owner[0], num_Points))
            cleanupAndThrow(dom, "get(Points_Owner)");
        // Points_Color
        if (! ( nc_var_temp = dataFile.get_var("Points_Color")))
            cleanupAndThrow(dom, "get_var(Points_Color)");
        if (!nc_var_temp->get(&points->Color[0], num_Points))
            cleanupAndThrow(dom, "get(Points_Color)");
        // Points_Nodes
        int* Points_Nodes = new int[num_Points];
        if (!(nc_var_temp = dataFile.get_var("Points_Nodes"))) {
            delete[] Points_Nodes;
            cleanupAndThrow(dom, "get_var(Points_Nodes)");
        }
        if (! nc_var_temp->get(&Points_Nodes[0], num_Points) ) {
            delete[] Points_Nodes;
            cleanupAndThrow(dom, "get(Points_Nodes)");
        }
        // Copy temp array into points->Nodes
        for (index_t i = 0; i < num_Points; i++) {
            points->Id[points->Nodes[INDEX2(0,i,1)]] = Points_Nodes[i];
        }
        delete[] Points_Nodes;
    } // num_Points > 0
    points->updateTagList();

    // get the tags
    if (num_Tags > 0) {
        // Temp storage to gather node IDs
        int *Tags_keys = new int[num_Tags];
        char name_temp[4096];
        int i;

        // Tags_keys
        if (! ( nc_var_temp = dataFile.get_var("Tags_keys")) ) {
            delete[] Tags_keys;
            cleanupAndThrow(dom, "get_var(Tags_keys)");
        }
        if (! nc_var_temp->get(&Tags_keys[0], num_Tags) ) {
            delete[] Tags_keys;
            cleanupAndThrow(dom, "get(Tags_keys)");
        }
        for (i=0; i<num_Tags; i++) {
          // Retrieve tag name
          sprintf(name_temp, "Tags_name_%d", i);
          if (! (attr=dataFile.get_att(name_temp)) ) {
              delete[] Tags_keys;
              stringstream msg;
              msg << "get_att(" << name_temp << ")";
              cleanupAndThrow(dom, msg.str());
          }
          boost::scoped_array<char> name(attr->as_string(0));
          delete attr;
          dom->setTagMap(name.get(), Tags_keys[i]);
        }
        delete[] Tags_keys;
    }

    // Nodes_DofDistribution
    IndexVector first_DofComponent(mpi_size+1);
    if (! (nc_var_temp = dataFile.get_var("Nodes_DofDistribution")) ) {
        cleanupAndThrow(dom, "get_var(Nodes_DofDistribution)");
    }
    if (!nc_var_temp->get(&first_DofComponent[0], mpi_size+1)) {
        cleanupAndThrow(dom, "get(Nodes_DofDistribution)");
    }

    // Nodes_NodeDistribution
    IndexVector first_NodeComponent(mpi_size+1);
    if (! (nc_var_temp = dataFile.get_var("Nodes_NodeDistribution")) ) {
        cleanupAndThrow(dom, "get_var(Nodes_NodeDistribution)");
    }
    if (!nc_var_temp->get(&first_NodeComponent[0], mpi_size+1)) {
        cleanupAndThrow(dom, "get(Nodes_NodeDistribution)");
    }
    dom->createMappings(first_DofComponent, first_NodeComponent);

    return dom->getPtr();
#else
    throw DudleyException("loadMesh: not compiled with NetCDF. Please contact your installation manager.");
#endif // ESYS_HAVE_NETCDF
}
#endif

Domain_ptr readMesh(const string& fileName, int /*integrationOrder*/,
                    int /*reducedIntegrationOrder*/, bool optimize)
{
    JMPI mpiInfo = makeInfo(MPI_COMM_WORLD);
    return DudleyDomain::read(mpiInfo, fileName, optimize);
}

Domain_ptr readGmsh(const string& fileName, int numDim,
                    int /*integrationOrder*/, int /*reducedIntegrationOrder*/,
                    bool optimize)
{
    JMPI mpiInfo = makeInfo(MPI_COMM_WORLD);
    return DudleyDomain::readGmsh(mpiInfo, fileName, numDim, optimize);
}

Domain_ptr brick(JMPI info, dim_t n0, dim_t n1, dim_t n2, int order,
                 double l0, double l1, double l2,
                 bool periodic0, bool periodic1, bool periodic2,
                 int integrationOrder, int reducedIntegrationOrder,
                 bool useElementsOnFace, bool useFullElementOrder,
                 bool optimize)
{
    // we don't support periodic boundary conditions
    if (periodic0 || periodic1)
        throw ValueError("Dudley does not support periodic boundary conditions.");

    if (integrationOrder > 3 || reducedIntegrationOrder > 1)
        throw ValueError("Dudley does not support the requested integration order.");

    if (useElementsOnFace || useFullElementOrder)
        throw ValueError("Dudley does not support useElementsOnFace or useFullElementOrder.");

    if (order > 1)
        throw ValueError("Dudley does not support element order greater than 1.");

    return DudleyDomain::create3D(n0, n1, n2, l0, l1, l2, optimize, info);
}

Domain_ptr brick_driver(const bp::list& args)
{
    bp::object pworld = args[15];
    JMPI info;
    if (!pworld.is_none()) {
        bp::extract<SubWorld_ptr> ex(pworld);
        if (!ex.check()) {
            throw ValueError("Invalid escriptWorld parameter.");
        }
        info = ex()->getMPI();
    } else {
        info = makeInfo(MPI_COMM_WORLD);
    }
    return brick(info, static_cast<dim_t>(bp::extract<float>(args[0])),
                 static_cast<dim_t>(bp::extract<float>(args[1])),
                 static_cast<dim_t>(bp::extract<float>(args[2])),
                 bp::extract<int>(args[3]), bp::extract<double>(args[4]),
                 bp::extract<double>(args[5]), bp::extract<double>(args[6]),
                 bp::extract<int>(args[7]), bp::extract<int>(args[8]),
                 bp::extract<int>(args[9]), bp::extract<int>(args[10]),
                 bp::extract<int>(args[11]), bp::extract<int>(args[12]),
                 bp::extract<int>(args[13]), bp::extract<int>(args[14])
                 );
}

Domain_ptr rectangle(JMPI info, dim_t n0, dim_t n1, int order,
                     double l0, double l1, bool periodic0, bool periodic1,
                     int integrationOrder, int reducedIntegrationOrder,
                     bool useElementsOnFace, bool useFullElementOrder,
                     bool optimize)
{
    if (periodic0 || periodic1) // we don't support periodic boundary conditions
        throw ValueError("Dudley does not support periodic boundary conditions.");
    if (integrationOrder > 3 || reducedIntegrationOrder > 1)
        throw ValueError("Dudley does not support the requested integrationorders.");
    if (useElementsOnFace || useFullElementOrder)
        throw ValueError("Dudley does not support useElementsOnFace or useFullElementOrder.");
    if (order > 1)
        throw ValueError("Dudley only supports first-order elements.");
    return DudleyDomain::create2D(n0, n1, l0, l1, optimize, info);
}

Domain_ptr rectangle_driver(const bp::list& args)
{
    bp::object pworld = args[12];
    JMPI info;
    if (!pworld.is_none()) {
        bp::extract<SubWorld_ptr> ex(pworld);
        if (!ex.check()) {
            throw ValueError("Invalid escriptWorld parameter.");
        }
        info = ex()->getMPI();
    } else {
        info = makeInfo(MPI_COMM_WORLD);
    }

    return rectangle(info, static_cast<dim_t>(bp::extract<float>(args[0])),
                     static_cast<dim_t>(bp::extract<float>(args[1])),
                     bp::extract<int>(args[2]), bp::extract<double>(args[3]),
                     bp::extract<double>(args[4]), bp::extract<int>(args[5]),
                     bp::extract<int>(args[6]), bp::extract<int>(args[7]),
                     bp::extract<int>(args[8]), bp::extract<int>(args[9]),
                     bp::extract<int>(args[10]), bp::extract<int>(args[11])
                     );
}

} // namespace dudley


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#include "A_StarPublicTransitShortestPathImpl.hpp"

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/unordered_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/utility.hpp>
#include <boost/thread.hpp>
#include <map>
#include <ostream>
#include <string>
#include <vector>

#include "geospatial/network/Point.hpp"
#include "util/GeomHelpers.hpp"
#include "util/LangHelpers.hpp"
#include "StreetDirectory.hpp"
#include "util/Utils.hpp"

using std::map;
using std::vector;
using std::list;

namespace {
void printPath(const std::vector<sim_mob::PT_NetworkEdge>& path, double cost, bool addedtoB) {
    if (addedtoB) {
        std::cout << "Added to B: ";
    } else {
        std::cout << "Added to Q: ";
    }
    for (std::vector<sim_mob::PT_NetworkEdge>::const_iterator it = path.begin();
            it != path.end(); it++) {
        std::cout << it->getEdgeId() << "-->";
    }
    std::cout << "pathcost=" << cost;
    std::cout << "\n";
}

struct TempPathSet {
private:
    std::vector<std::vector<sim_mob::PT_NetworkEdge> > paths;
    std::vector<double> pathCosts;

public:
    void addPathIfUnavailable(const std::vector<sim_mob::PT_NetworkEdge>& path, double cost) {
        if (std::find(paths.begin(), paths.end(), path) == paths.end()) {
            paths.push_back(path);
            pathCosts.push_back(cost);
            //printPath(path,cost,true);
        }
    }

    void deletePath(size_t index)
    {
        paths.erase(paths.begin() + index);
        pathCosts.erase(pathCosts.begin() + index);
    }

    uint32_t getMinElementIdx() const
    {
        return std::distance(pathCosts.begin(), std::min_element(pathCosts.begin(), pathCosts.end()));
    }

    const std::vector<sim_mob::PT_NetworkEdge>& getPath(int index) const
    {
        if (index >= paths.size()) {
            throw std::runtime_error("invalid index");
        }
        return paths.at(index);
    }

    bool empty() const
    {
        return paths.empty();
    }
};

}

sim_mob::A_StarPublicTransitShortestPathImpl::A_StarPublicTransitShortestPathImpl(
        const std::map<int, PT_NetworkEdge>& ptEdgeMap,
        const std::map<std::string, PT_NetworkVertex>& ptVertexMap)
{
    initPublicNetwork(ptEdgeMap, ptVertexMap);
}

void sim_mob::A_StarPublicTransitShortestPathImpl::initPublicNetwork(
        const std::map<int, PT_NetworkEdge>& ptEdgeMap, const std::map<std::string, PT_NetworkVertex>& ptVertexMap)
{
    for (std::map<std::string, PT_NetworkVertex>::const_iterator itVertex = ptVertexMap.begin(); itVertex != ptVertexMap.end(); ++itVertex) {
        procAddPublicNetworkVertices(publicTransitMap, (itVertex->second));
    }

    for (std::map<int, PT_NetworkEdge>::const_iterator itEdge = ptEdgeMap.begin(); itEdge != ptEdgeMap.end(); ++itEdge) {
        procAddPublicNetworkEdges(publicTransitMap, itEdge->second);
    }
}
void sim_mob::A_StarPublicTransitShortestPathImpl::procAddPublicNetworkVertices(
        StreetDirectory::PublicTransitGraph& graph, const PT_NetworkVertex& ptVertex)
{
    StreetDirectory::PT_Vertex v = boost::add_vertex(const_cast<StreetDirectory::PublicTransitGraph &>(graph));
    boost::put(boost::vertex_name,  const_cast<StreetDirectory::PublicTransitGraph &>(graph), v, ptVertex.getRoadItemId());
    vertexMap[ptVertex.getRoadItemId()] = v;
}

void sim_mob::A_StarPublicTransitShortestPathImpl::procAddPublicNetworkEdges(
        StreetDirectory::PublicTransitGraph& graph, const PT_NetworkEdge& ptEdge)
{
    StreetDirectory::PT_Vertex from = vertexMap.find(ptEdge.getStartStop())->second;
    StreetDirectory::PT_Vertex to = vertexMap.find(ptEdge.getEndStop())->second;
    bool ok;
    StreetDirectory::PT_Edge edge;
    boost::tie(edge, ok) = boost::add_edge(from, to, graph);
    edgeMap[ptEdge.getEdgeId()] = edge;
    procAddPublicNetworkEdgeWeights(edge, publicTransitMap, ptEdge);
}
double sim_mob::A_StarPublicTransitShortestPathImpl::getK_ShortestPathCost(const PT_NetworkEdge& ptEdge)
{
    return ptEdge.getDayTransitTimeSecs() + ptEdge.getTransferPenaltySecs() + ptEdge.getWalkTimeSecs() + ptEdge.getWaitTimeSecs();
}

double sim_mob::A_StarPublicTransitShortestPathImpl::getLabelingApproachWeights(int Label, PT_NetworkEdge ptEdge)
{
    double cost=0.0;
    switch (Label) {
    case LabelingApproach1: {
        ptEdge.setWalkTimeSecs(0);
        ptEdge.setWaitTimeSecs(0);
        break;
    }
    case LabelingApproach2: {
        ptEdge.setWalkTimeSecs(0);
        ptEdge.setWaitTimeSecs(0);
        ptEdge.setDayTransitTimeSecs(0);
        ptEdge.setTransferPenaltySecs(1);
        break;
    }
    case LabelingApproach3: {
        if (ptEdge.getType() == sim_mob::WALK_EDGE) {
            ptEdge.setWalkTimeSecs(100000000);
        }
        break;
    }
    case LabelingApproach4: {
        if (ptEdge.getType() != sim_mob::TRAIN_EDGE) {
            ptEdge.setTransferPenaltySecs(100000000);
        }
        break;
    }
    case LabelingApproach5: {
        if (ptEdge.getType() != sim_mob::BUS_EDGE) {
            ptEdge.setTransferPenaltySecs(100000000);
        }
        break;
    }
    case LabelingApproach6: {
        ptEdge.setDayTransitTimeSecs(0);
        ptEdge.setWalkTimeSecs(0);
        break;
    }
    case LabelingApproach7:
        // DO Nothing
        break;
    case LabelingApproach8: {
        double walkTime = ptEdge.getWalkTimeSecs();
        double waitTime = ptEdge.getWaitTimeSecs();
        ptEdge.setWalkTimeSecs(5 * walkTime);
        ptEdge.setWaitTimeSecs(10 * waitTime);
        break;
    }
    case LabelingApproach9: {
        double walkTime = ptEdge.getWalkTimeSecs();
        double waitTime = ptEdge.getWaitTimeSecs();
        ptEdge.setWalkTimeSecs(10 * walkTime);
        ptEdge.setWaitTimeSecs(10 * waitTime);
        break;
    }
    case LabelingApproach10: {
        double walkTime = ptEdge.getWalkTimeSecs();
        double waitTime = ptEdge.getWaitTimeSecs();
        ptEdge.setWalkTimeSecs(20 * walkTime);
        ptEdge.setWaitTimeSecs(10 * waitTime);
        break;
    }
    default:
        // Do Nothing
        break;
    }
    //Cost function for labeling approach. Cost = day_transit_time + walk_time + wait_time + transfer_penalty
    cost = ptEdge.getDayTransitTimeSecs() + ptEdge.getWalkTimeSecs() + ptEdge.getWaitTimeSecs() + ptEdge.getTransferPenaltySecs();
    return cost;
}

double sim_mob::A_StarPublicTransitShortestPathImpl::getSimulationApproachWeights(PT_NetworkEdge ptEdge)
{
    double cost = ptEdge.getDayTransitTimeSecs() + ptEdge.getWalkTimeSecs() + ptEdge.getWaitTimeSecs() + ptEdge.getTransferPenaltySecs();
    return abs(Utils::nRandom(cost, 5 * cost));
}
void sim_mob::A_StarPublicTransitShortestPathImpl::procAddPublicNetworkEdgeWeights( const StreetDirectory::PT_Edge& edge,
        StreetDirectory::PublicTransitGraph& graph, const PT_NetworkEdge& ptEdge)
{
    graph[edge].kShortestPathWeight = getK_ShortestPathCost(ptEdge);
    graph[edge].labelingApproach1Weight = getLabelingApproachWeights(LabelingApproach1, ptEdge);
    graph[edge].labelingApproach2Weight = getLabelingApproachWeights(LabelingApproach2, ptEdge);
    graph[edge].labelingApproach3Weight = getLabelingApproachWeights(LabelingApproach3, ptEdge);
    graph[edge].labelingApproach4Weight = getLabelingApproachWeights(LabelingApproach4, ptEdge);
    graph[edge].labelingApproach5Weight = getLabelingApproachWeights(LabelingApproach5, ptEdge);
    graph[edge].labelingApproach6Weight = getLabelingApproachWeights(LabelingApproach6, ptEdge);
    graph[edge].labelingApproach7Weight = getLabelingApproachWeights(LabelingApproach7, ptEdge);
    graph[edge].labelingApproach8Weight = getLabelingApproachWeights(LabelingApproach8, ptEdge);
    graph[edge].labelingApproach9Weight = getLabelingApproachWeights(LabelingApproach9, ptEdge);
    graph[edge].labelingApproach10Weight = getLabelingApproachWeights(LabelingApproach10, ptEdge);
    graph[edge].simulationApproach1Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach2Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach3Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach4Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach5Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach6Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach7Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach8Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach9Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].simulationApproach10Weight = getSimulationApproachWeights(ptEdge);
    graph[edge].edge_id = ptEdge.getEdgeId();
}

vector<sim_mob::PT_NetworkEdge> sim_mob::A_StarPublicTransitShortestPathImpl::searchShortestPath(
        const StreetDirectory::PT_VertexId& fromNode,const StreetDirectory::PT_VertexId& toNode, const PT_CostLabel cost)
{
    vector<PT_NetworkEdge> res = vector<sim_mob::PT_NetworkEdge>();
    StreetDirectory::PT_Vertex from;
    StreetDirectory::PT_Vertex to;
    if (vertexMap.find(fromNode) != vertexMap.end()) {
        from = vertexMap.find(fromNode)->second;
    } else {
        std::cout << "From Node " << fromNode << " not found in the graph\n";
        return vector<sim_mob::PT_NetworkEdge>();
    }
    if (vertexMap.find(toNode) != vertexMap.end()) {
        to = vertexMap.find(toNode)->second;
    } else {
        std::cout << "To Node " << toNode << " not found in the graph\n";
        return vector<sim_mob::PT_NetworkEdge>();
    }
    StreetDirectory::PublicTransitGraph& graph = publicTransitMap;
    list<StreetDirectory::PT_Vertex> partialRes;
    vector<StreetDirectory::PT_Vertex> p(boost::num_vertices(graph));
    vector<double> d(boost::num_vertices(graph));
    boost::property_map<StreetDirectory::PublicTransitGraph,double (StreetDirectory::PT_EdgeProperties::*)>::type edgeWeightMap;

    switch (cost) {
    case KshortestPath:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::kShortestPathWeight,graph);
        break;
    case LabelingApproach1:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach1Weight,graph);
        break;
    case LabelingApproach2:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach2Weight,graph);
        break;
    case LabelingApproach3:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach3Weight,graph);
        break;
    case LabelingApproach4:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach4Weight,graph);
        break;
    case LabelingApproach5:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach5Weight,graph);
        break;
    case LabelingApproach6:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach6Weight,graph);
        break;
    case LabelingApproach7:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach7Weight,graph);
        break;
    case LabelingApproach8:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach8Weight,graph);
        break;
    case LabelingApproach9:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach9Weight,graph);
        break;
    case LabelingApproach10:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::labelingApproach10Weight,graph);
        break;
    case SimulationApproach1:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach1Weight,graph);
        break;
    case SimulationApproach2:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach2Weight,graph);
        break;
    case SimulationApproach3:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach3Weight,graph);
        break;
    case SimulationApproach4:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach4Weight,graph);
        break;
    case SimulationApproach5:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach5Weight,graph);
        break;
    case SimulationApproach6:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach6Weight,graph);
        break;
    case SimulationApproach7:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach7Weight,graph);
        break;
    case SimulationApproach8:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach8Weight,graph);
        break;
    case SimulationApproach9:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach9Weight,graph);
        break;
    case SimulationApproach10:
        edgeWeightMap = boost::get(&StreetDirectory::PT_EdgeProperties::simulationApproach10Weight, graph);
        break;
    }
    try {
        boost::astar_search(graph, from, PT_HeuristicGraph(&graph, to),
                boost::weight_map(edgeWeightMap).predecessor_map(&p[0]).distance_map(&d[0]).visitor(PT_GoalVisitor(to)));

    } catch (PT_FoundGoal& goal) {
        //Build backwards.
        for (StreetDirectory::PT_Vertex v = to;; v = p[v]) {
            partialRes.push_front(v);
            if (p[v] == v) {
                break;
            }
        }
        //Now build forwards.
        std::list<StreetDirectory::PT_Vertex>::const_iterator prev = partialRes.end();
        for (std::list<StreetDirectory::PT_Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++) {
            //Add this edge.
            if (prev != partialRes.end()) {
                //This shouldn't fail.
                std::pair<StreetDirectory::PT_Edge, bool> edge = boost::edge(*prev, *it, graph);
                if (!edge.second) {
                    Warn() << "ERROR: Boost can't find an edge that it should know about."
                            << std::endl;
                    return vector<sim_mob::PT_NetworkEdge>();
                }
                StreetDirectory::PT_EdgeId edge_id = get(&StreetDirectory::PT_EdgeProperties::edge_id, graph,edge.first);
                res.push_back(PT_NetworkCreater::getInstance().PT_NetworkEdgeMap.find(edge_id)->second);
            }
            //Save for later.
            prev = it;
        }
    }
    return res;
}
vector<sim_mob::PT_NetworkEdge> sim_mob::A_StarPublicTransitShortestPathImpl::searchShortestPathWithBlacklist(
        const StreetDirectory::PT_VertexId& fromNode,const StreetDirectory::PT_VertexId& toNode,const std::set<StreetDirectory::PT_EdgeId>& blacklist,  double &pathCost)
{
    StreetDirectory::PublicTransitGraph& graph = publicTransitMap;
    StreetDirectory::PT_Vertex from;
    StreetDirectory::PT_Vertex to;
    if (vertexMap.find(fromNode) != vertexMap.end()) {
        from = vertexMap.find(fromNode)->second;
    } else {
        std::cout << "From Node not found in the graph";
        return vector<sim_mob::PT_NetworkEdge>();
    }
    if (vertexMap.find(toNode) != vertexMap.end()) {
        to = vertexMap.find(toNode)->second;
    } else {
        std::cout << "TO Node not found in the graph";
        return vector<sim_mob::PT_NetworkEdge>();
    }
    std::set<StreetDirectory::PT_Edge> blEdge = std::set<StreetDirectory::PT_Edge>();
    for (std::set<StreetDirectory::PT_EdgeId>::const_iterator blIt =blacklist.begin(); blIt != blacklist.end(); blIt++) {
        blEdge.insert(edgeMap[*blIt]);
    }

    PT_EdgeConstraint filter(blEdge);
    boost::filtered_graph<StreetDirectory::PublicTransitGraph, PT_EdgeConstraint> filtered(graph, filter);
    vector<sim_mob::PT_NetworkEdge> res;
    list<StreetDirectory::PT_Vertex> partialRes;
    vector<StreetDirectory::PT_Vertex> p(boost::num_vertices(graph));
    vector<double> d(boost::num_vertices(graph));
    try {
        boost::astar_search(filtered, from, PT_HeuristicGraph(&graph, to),
                boost::weight_map(get(&StreetDirectory::PT_EdgeProperties::kShortestPathWeight, graph)).predecessor_map(&p[0]).
                distance_map(&d[0]).visitor(PT_GoalVisitor(to)));
    } catch (PT_FoundGoal& goal) {
        pathCost = d[to];
        //Build backwards.
        for (StreetDirectory::PT_Vertex v = to;; v = p[v]) {
            partialRes.push_front(v);
            if (p[v] == v) {
                break;
            }
        }
        //Now build forwards.
        std::list<StreetDirectory::PT_Vertex>::const_iterator prev =partialRes.end();
        for (std::list<StreetDirectory::PT_Vertex>::const_iterator it = partialRes.begin(); it != partialRes.end(); it++) {
            //Add this edge.
            if (prev != partialRes.end()) {
                //This shouldn't fail.
                std::pair<StreetDirectory::PT_Edge, bool> edge = boost::edge(*prev, *it, graph);
                if (!edge.second) {
                    Warn()  << "ERROR: Boost can't find an edge that it should know about."
                            << std::endl;
                    return vector<sim_mob::PT_NetworkEdge>();
                }

                StreetDirectory::PT_EdgeId edge_id = get(&StreetDirectory::PT_EdgeProperties::edge_id, graph,edge.first);
                res.push_back(PT_NetworkCreater::getInstance().PT_NetworkEdgeMap.find(edge_id)->second);
            }
            //Save for later.
            prev = it;
        }
    }
    return res;
}

void sim_mob::A_StarPublicTransitShortestPathImpl::searchK_ShortestPaths(uint32_t kPaths, const StreetDirectory::PT_VertexId& from,
        const StreetDirectory::PT_VertexId& to, vector<vector<sim_mob::PT_NetworkEdge> >& res)
{
    StreetDirectory::PublicTransitGraph& graph = publicTransitMap;

    double pathCost = 0.0;
    // Define Q to hold the k shortest paths
    vector<vector<sim_mob::PT_NetworkEdge> >& Q = res; //another reference to be consistent with Tan Rui's algorithm specification
    // Define B to hold the intermediate paths found (Only subset from this set goes to Q)
    TempPathSet B;

    //step 1: Search shortest path p_0
    std::set<StreetDirectory::PT_EdgeId> blackList = std::set<StreetDirectory::PT_EdgeId>();
    std::vector<PT_NetworkEdge> p0 = searchShortestPathWithBlacklist(from, to,blackList, pathCost);

    if (p0.empty()) {
        return;
    }
    Q.push_back(p0);
    //printPath(p0,pathCost,false);

    //step 2: Each iteration of the below loop gives the kth shortest path
    // For k in 1:K-1 (K - number of paths required )
    for (int k = 1; k < kPaths; k++) {
        //black list first edge from each path found already (p_0,p_1,p_2,...,p_(k-1))
        blackList.clear();
        for (int p = 0; p < k; p++) {
            StreetDirectory::PT_EdgeId firstEdgeId = Q[p].front().getEdgeId();
            blackList.insert(firstEdgeId);
        }
        //Search shortest path, denote p_temp, if p_temp doesn't exist in Q or B, B = (B union {p_temp})
        std::vector<PT_NetworkEdge> pTemp = searchShortestPathWithBlacklist(from, to, blackList, pathCost);
        blackList.clear();
        //Check if p_temp present in Q or B already
        if (!pTemp.empty() && std::find(Q.begin(), Q.end(), pTemp) == Q.end()) {
            B.addPathIfUnavailable(pTemp, pathCost);
        }

        size_t prevPathLength = Q[k - 1].size();
        if (prevPathLength - 1 >= 1) // Enter loop only if q(k-1) > 1 ;
        {
            const vector<PT_NetworkEdge>& prevPath = Q[k - 1];
            // For i in 1:q(k-1)
            for (int i = 0; i < prevPathLength - 1; i++) {
                vector<PT_NetworkEdge> rootpath(prevPath.begin(),prevPath.begin() + (i + 1));
                // For j in 1:k-1
                for (int j = 0; j <= k - 1; j++) {
                    const std::vector<PT_NetworkEdge>& Q_j = Q[j];
                    // Check if (e_1,e_2,...e_(i))in p_(k-1) is same as (e_1,e_2,...e(i)) in p_j
                    if (std::equal(Q_j.begin(), Q_j.begin() + (i + 1),  rootpath.begin())) {
                        // Block e(i+1) from path p_j in graph
                        if (Q_j.size() > i + 1) {
                            PT_NetworkEdge removeEdge = Q_j.at(i + 1);
                            blackList.insert(removeEdge.getEdgeId());
                        }
                    }
                }

                // Denote (e_1,e_2,...e_(i-1)) as S_i. Find the shortest route R_i from ending vertex of e_i and same destination
                vector<PT_NetworkEdge> S_i(Q[k - 1].begin(),Q[k - 1].begin() + (i));
                StreetDirectory::PT_VertexId start;
                //Finding the ending vertex of the edge e_i in Q[k-1]
                if (S_i.empty()) {
                    start = from;
                } else {
                    StreetDirectory::PT_Vertex startVertex = boost::target(edgeMap[S_i.back().getEdgeId()], graph);
                    start = boost::get(boost::vertex_name, graph, startVertex);
                }
                double s_i_cost = 0.0;
                for (std::vector<sim_mob::PT_NetworkEdge>::const_iterator itEdge = S_i.begin(); itEdge != S_i.end(); itEdge++) {
                    s_i_cost += graph[sim_mob::A_StarPublicTransitShortestPathImpl::edgeMap[itEdge->getEdgeId()]].kShortestPathWeight;
                }
                vector<PT_NetworkEdge> R_i = searchShortestPathWithBlacklist(start, to, blackList, pathCost);
                blackList.clear();
                if (!R_i.empty()) {
                    // Concatenate S_i and R_i to obtain A_i_k
                    vector<PT_NetworkEdge> A_i_k;
                    A_i_k.insert(A_i_k.end(), S_i.begin(), S_i.end());
                    A_i_k.insert(A_i_k.end(), R_i.begin(), R_i.end());
                    // If A_i_k not exist in B not in Q
                    // B = Union(B,A_i_k)
                    if (std::find(Q.begin(), Q.end(), A_i_k) == Q.end()) {
                        B.addPathIfUnavailable(A_i_k, (pathCost + s_i_cost));
                    }
                }
            }
        }
        // If B is empty Break the whole loop
        if (B.empty()) {
            break;
        }

        // Remove the least cost path A_j in B and put it in Q as p_k
        uint32_t index = B.getMinElementIdx();
        Q.push_back(B.getPath(index));
        //printPath(B.getPath(index),0.0,false);
        B.deletePath(index);
    }
    return;
}