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

// Header <coordinate> synopsis

#if !defined(HPX_UTIL_COORDINATE_NOV_03_2014_0227PM)
#define HPX_UTIL_COORDINATE_NOV_03_2014_0227PM

#include <hpx/hpx_fwd.hpp>
#include <hpx/util/assert.hpp>

#include <algorithm>
#include <numeric>
#include <cstddef>
#if !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX)
#include <initializer_list>
#endif

#include <boost/static_assert.hpp>

namespace hpx { namespace util
{
    ///////////////////////////////////////////////////////////////////////////
    // forward declaration
    template <int Rank> class index;
    template <int Rank> class bounds;
    template <int Rank> class bounds_iterator;

    ///////////////////////////////////////////////////////////////////////////
    // [coord.index], class template index
    template <int Rank>
    class index
    {
        BOOST_STATIC_ASSERT_MSG(Rank > 0, "Rank shall be greater than 0");

    public:
        // constants and types
        static BOOST_CONSTEXPR_OR_CONST int rank = Rank;

        typedef std::ptrdiff_t& reference;
        typedef std::ptrdiff_t const& const_reference;
        typedef std::size_t size_type;
        typedef std::ptrdiff_t value_type;

        // [coord.index.cnstr], index construction
        //! Effects: Zero-initializes each component.
        index() BOOST_NOEXCEPT
        {
            std::fill(vs_ + 0, vs_ + rank, 0);
        }

#if !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) && !defined(HPX_INTEL_VERSION)
        //! Requires: il.size() == Rank.
        //! Effects: For all i in the range [0, Rank), initializes the ith
        //! component of *this with *(il.begin() + i).
        index(std::initializer_list<value_type> const& il)
        {
            HPX_ASSERT(il.size() == std::size_t(rank) &&
                "il.size() must be equal to Rank");
            std::copy(il.begin(), il.end(), vs_ + 0);
        }
#else
        index(value_type const (&il)[Rank])
        {
            std::copy(il, il + Rank, vs_ + 0);
        }

        index(value_type const* il, std::size_t size)
        {
            HPX_ASSERT(size == std::size_t(rank) && "size must be equal to Rank");
            std::copy(il, il + size, vs_ + 0);
        }
#endif

        // [coord.index.eq], index equality
        //! Returns: true if (*this)[i] == rhs[i] for all i in the range
        //! [0, Rank), otherwise false.
        bool operator==(index const& rhs) const BOOST_NOEXCEPT
        {
            return std::equal(vs_ + 0, vs_ + rank, rhs.vs_ + 0);
        }

        //! Returns: !(*this == rhs).
        bool operator!=(index const& rhs) const BOOST_NOEXCEPT
        {
            return !(*this == rhs);
        }

        // [coord.index.cmpt], index component access
        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        reference operator[](size_type n)
        {
            HPX_ASSERT(n < std::size_t(rank) && "n must be less than Rank");
            return vs_[n];
        }

        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        const_reference operator[](size_type n) const
        {
            HPX_ASSERT(n < std::size_t(rank) && "n must be less than Rank");
            return vs_[n];
        }

        // [coord.index.arith], index arithmetic
        //! Returns: index<Rank>{*this} += rhs.
        index operator+(index const& rhs) const
        {
            return index(*this) += rhs;
        }

        //! Returns: index<Rank>{*this} -= rhs.
        index operator-(index const& rhs) const
        {
            return index(*this) -= rhs;
        }

        //! Effects: For all i in the range [0, Rank), adds the ith component
        //! of rhs to the ith component of *this and stores the sum in the ith
        //! component of *this.
        //! Returns: *this.
        index& operator+=(index const& rhs)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] += rhs.vs_[i];
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), subtracts the ith
        //! component of rhs from the ith component of *this and stores the
        //! difference in the ith component of *this.
        //! Returns: *this.
        index& operator-=(index const& rhs)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] -= rhs.vs_[i];
            return *this;
        }

        //! Returns: *this.
        index  operator+() const BOOST_NOEXCEPT
        {
            return index(*this);
        }

        //! Returns: A copy of *this with each component negated.
        index  operator-() const
        {
            index r;
            for (std::size_t i = 0; i < rank; ++i)
                r.vs_[i] = -vs_[i];
            return r;
        }

        //! Returns: index<Rank>{*this} *= v.
        index  operator*(value_type v) const
        {
            return index(*this) *= v;
        }

        //! Returns: index<Rank>{*this} /= v.
        index  operator/(value_type v) const
        {
            return index(*this) /= v;
        }

        //! Effects: For all i in the range [0, Rank), multiplies the ith
        //! component of *this by v and stores the product in the ith component
        //! of *this.
        //! Returns: *this.
        index& operator*=(value_type v)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] *= v;
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), divides the ith component
        //! of *this by v and stores the quotient in the ith component of *this.
        //! Returns: *this.
        index& operator/=(value_type v)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] /= v;
            return *this;
        }

    private:
        value_type vs_[Rank];
    };

    ///////////////////////////////////////////////////////////////////////////
    // [coord.bounds], class template bounds
    template <int Rank>
    class bounds
    {
        BOOST_STATIC_ASSERT_MSG(Rank > 0, "Rank shall be greater than 0");

    public:
        // constants and types
        static BOOST_CONSTEXPR_OR_CONST int rank = Rank;

        typedef std::ptrdiff_t& reference;
        typedef std::ptrdiff_t const& const_reference;
        typedef bounds_iterator<Rank> iterator;
        typedef bounds_iterator<Rank> const_iterator;
        typedef std::size_t size_type;
        typedef std::ptrdiff_t value_type;

        // [coord.bounds.cnstr], bounds construction

        //! Effects: Zero-initializes each component.
        bounds() BOOST_NOEXCEPT
        {
            std::fill(vs_ + 0, vs_ + rank, 0);
        }

#if !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) && !defined(HPX_INTEL_VERSION)
        //! Requires: il.size() == Rank.
        //! Effects: For all i in the range [0, Rank), initializes the ith
        //! component of *this with *(il.begin() + i).
        bounds(std::initializer_list<value_type> const& il)
        {
            HPX_ASSERT(il.size() == std::size_t(rank) &&
                "il.size() must be equal to Rank");
            std::copy(il.begin(), il.end(), vs_ + 0);
        }
#else
        bounds(value_type const (&il)[Rank])
        {
            std::copy(il, il + Rank, vs_ + 0);
        }

        bounds(value_type const* il, std::size_t size)
        {
            HPX_ASSERT(size == std::size_t(rank) && "size must be equal to Rank");
            std::copy(il, il + size, vs_ + 0);
        }
#endif

        // [coord.bounds.eq], bounds equality
        //! Returns: true if (*this)[i] == rhs[i] for all i in the range
        //! [0, Rank), otherwise false.
        bool operator==(bounds const& rhs) const BOOST_NOEXCEPT
        {
            return std::equal(vs_ + 0, vs_ + rank, rhs.vs_ + 0);
        }

        //! Returns: !(*this == rhs).
        bool operator!=(bounds const& rhs) const BOOST_NOEXCEPT
        {
            return !(*this == rhs);
        }

        // [coord.bounds.obs], bounds observers
        //! Returns: The product of all components of *this.
        size_type size() const BOOST_NOEXCEPT
        {
            return std::accumulate(vs_ + 0, vs_ + rank, 1,
                std::multiplies<value_type>());
        }

        //! Returns: true if 0 <= idx[i] and idx[i] < (*this)[i] for all i in
        //! the range [0, Rank), otherwise false.
        bool contains(index<Rank> const& idx) const BOOST_NOEXCEPT
        {
            for (std::size_t i = 0; i < rank; ++i)
            {
                if (!(idx[i] >= 0 && idx[i] < vs_[i]))
                    return false;
            }
            return true;
        }

        // [coord.bounds.iter], bounds iterators
        //! Returns: A bounds_iterator referring to the first element of the
        //! space defined by *this such that *begin() == index<Rank>{} if
        //! size() != 0, begin() == end() otherwise.
        const_iterator begin() const BOOST_NOEXCEPT
        {
            return const_iterator(*this, index<Rank>());
        }

        //! Returns: A bounds_iterator which is the past-the-end iterator for
        //! the space defined by *this.
        const_iterator end() const BOOST_NOEXCEPT
        {
            index<Rank> idx;
            idx[0] = vs_[0];
            return const_iterator(*this, idx);
        }

        // [coord.bounds.cmpt], bounds component access
        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        reference operator[](size_type n)
        {
            HPX_ASSERT(n < std::size_t(rank) && "n must be less than Rank");
            return vs_[n];
        }

        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        const_reference operator[](size_type n) const
        {
            HPX_ASSERT(n < std::size_t(rank) && "n must be less than Rank");
            return vs_[n];
        }

        // [coord.bounds.arith], bounds arithmetic
        //! Returns: bounds<Rank>{*this} += rhs.
        bounds operator+(const index<Rank>& rhs) const
        {
            return bounds(*this) += rhs;
        }

        //! Returns: bounds<Rank>{*this} -= rhs.
        bounds operator-(const index<Rank>& rhs) const
        {
            return bounds(*this) -= rhs;
        }

        //! Effects: For all i in the range [0, Rank), adds the ith component
        //! of rhs to the ith component of *this and stores the sum in the ith
        //! component of *this.
        //! Returns: *this.
        bounds& operator+=(const index<Rank>& rhs)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] += rhs[i];
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), subtracts the ith
        //! component of rhs from the ith component of *this and stores the
        //! difference in the ith component of *this.
        //! Returns: *this.
        bounds& operator-=(const index<Rank>& rhs)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] -= rhs[i];
            return *this;
        }

        //! Returns: bounds<Rank>{*this} *= v.
        bounds  operator*(value_type v) const
        {
            return bounds(*this) /= v;
        }

        //! Returns: bounds<Rank>{*this} /= v.
        bounds  operator/(value_type v) const
        {
            return bounds(*this) /= v;
        }

        //! Effects: For all i in the range [0, Rank), multiplies the ith
        //! component of *this by v and stores the product in the ith component
        //! of *this.
        //! Returns: *this.
        bounds& operator*=(value_type v)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] += v;
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), divides the ith component
        //! of *this by v and stores the quotient in the ith component of *this.
        //! Returns: *this.
        bounds& operator/=(value_type v)
        {
            for (std::size_t i = 0; i < rank; ++i)
                vs_[i] += v;
            return *this;
        }

    private:
        value_type vs_[Rank];
    };

    ///////////////////////////////////////////////////////////////////////////
    namespace detail
    {
        template <int Rank>
        class bounds_iterator_pointer
        {
        public:
            explicit bounds_iterator_pointer(index<Rank> const& idx)
              : index_(idx)
            {}

            index<Rank> const& operator*() const
            {
                return index_;
            }

            index<Rank> const* operator->() const
            {
                return &index_;
            }

        private:
            index<Rank> index_;
        };
    }

    // [coord.bounds.iterator], class template bounds_iterator
    template <int Rank>
    class bounds_iterator
    {
    public:
        typedef std::random_access_iterator_tag iterator_category;
        typedef index<Rank> value_type;
        typedef std::ptrdiff_t difference_type;
        typedef typename detail::bounds_iterator_pointer<Rank> pointer;  // unspecified
        typedef index<Rank> const reference;

        explicit bounds_iterator(bounds<Rank> const& bnd, index<Rank> const& idx)
          : bounds_(bnd)
          , index_(idx)
        {}

        //! Requires: *this and rhs are iterators over the same bounds object.
        //! Returns: index_ == rhs.index_.
        bool operator==(bounds_iterator const& rhs) const
        {
            return index_ == rhs.index_;
        }

        bool operator!=(bounds_iterator const& rhs) const
        {
            return !(*this == rhs);
        }

        bool operator<(bounds_iterator const& rhs) const
        {
            for (std::size_t i = 0; i < Rank; ++i)
            {
                if (index_[i] < rhs.index_[i])
                    return true;
                else if (index_[i] > rhs.index_[i])
                    return false;
            }
            return false;
        }

        bool operator<=(bounds_iterator const& rhs) const
        {
            return !(rhs < *this);
        }

        bool operator>(bounds_iterator const& rhs) const
        {
            return rhs < *this;
        }

        bool operator>=(bounds_iterator const& rhs) const
        {
            return !(*this < rhs);
        }

        //! Requires: *this is not the past-the-end iterator.
        //! Effects: Equivalent to:
        //!          for (auto i = Rank - 1; i >= 0; --i) {
        //!              if (++index_[i] < bounds_[i])
        //!                   return *this;
        //!              index_[i] = 0;
        //!          }
        //!          index_ = unspecified past-the-end value;
        //! Returns: *this.
        bounds_iterator& operator++()
        {
            for (int i = Rank - 1; i >= 0; --i)
            {
                if (++index_[i] < bounds_[i])
                    return *this;
                index_[i] = 0;
            }
            index_[0] = bounds_[0];
            return *this;
        }

        bounds_iterator operator++(int)
        {
            bounds_iterator r(*this);
            ++(*this);
            return r;
        }

        //! Requires: There exists a bounds_iterator<Rank> it such that
        //! *this == ++it.
        //! Effects: *this = it.
        //! Returns: *this.
        bounds_iterator& operator--()
        {
            for (int i = Rank - 1; i >= 0; --i)
            {
                if (--index_[i] >= 0)
                    return *this;
                index_[i] = bounds_[i] - 1;
            }
            // index_[Rank - 1] == -1;
            return *this;
        }

        bounds_iterator operator--(int)
        {
            bounds_iterator r(*this);
            --(*this);
            return r;
        }

        bounds_iterator operator+(difference_type n) const
        {
            return bounds_iterator(*this) += n;
        }

        bounds_iterator& operator+=(difference_type n)
        {
            for (int i = Rank - 1; i >= 0 && n != 0; --i)
            {
                std::ptrdiff_t nx = index_[i] + n;

                if (nx >= bounds_[i])
                {
                    n = nx / bounds_[i];
                    index_[i] = nx % bounds_[i];
                }
                else
                {
                    index_[i] = nx;
                    return *this;
                }
            }

            index_[0] = bounds_[0];
            return *this;
        }

        bounds_iterator operator-(difference_type n) const
        {
            return bounds_iterator(*this) -= n;
        }

        bounds_iterator& operator-=(difference_type n)
        {
            return (*this += (-n));
        }

        difference_type operator-(bounds_iterator const& rhs) const
        {
            difference_type r = 0;
            difference_type flat_bounds = 1;
            for (int i = Rank - 1; i >= 0; --i)
            {
                r += (index_[i] - rhs.index_[i]) * flat_bounds;
                flat_bounds *= bounds_[i];
            }
            return r;
        }

        //! Returns: index_.
        reference operator*() const
        {
            return index_;
        }

        pointer operator->() const
        {
            return pointer(index_);
        }

        reference operator[](difference_type n) const
        {
            return *(*this + n);
        }

    protected:
        bounds<Rank> bounds_;
        index<Rank> index_;
    };

    template <int Rank>
    bounds_iterator<Rank> operator+(
        typename bounds_iterator<Rank>::difference_type n,
        bounds_iterator<Rank> const& rhs)
    {
        return rhs + n;
    }

    ///////////////////////////////////////////////////////////////////////////
    template <>
    class index<1>
    {
    public:
        // constants and types
        static BOOST_CONSTEXPR_OR_CONST int rank = 1;

        typedef std::ptrdiff_t& reference;
        typedef std::ptrdiff_t const& const_reference;
        typedef std::size_t size_type;
        typedef std::ptrdiff_t value_type;

        // [coord.index.cnstr], index construction
        //! Effects: Zero-initializes each component.
        index() BOOST_NOEXCEPT
          : vs_(0)
        {
        }

        //! Effects: Initializes the 0th component of *this with v.
        //! Remarks: This constructor shall not participate in overload
        //! resolution unless Rank is 1.
        index(value_type v) BOOST_NOEXCEPT
          : vs_(v)
        {}

        // [coord.index.eq], index equality
        //! Returns: true if (*this)[i] == rhs[i] for all i in the range
        //! [0, Rank), otherwise false.
        bool operator==(index const& rhs) const BOOST_NOEXCEPT
        {
            return vs_ == rhs.vs_;
        }

        //! Returns: !(*this == rhs).
        bool operator!=(index const& rhs) const BOOST_NOEXCEPT
        {
            return !(*this == rhs);
        }

        // [coord.index.cmpt], index component access
        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        reference operator[](size_type n)
        {
            HPX_ASSERT(n == 0 && "n must be less than Rank (1)");
            return vs_;
        }

        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        const_reference operator[](size_type n) const
        {
            HPX_ASSERT(n == 0 && "n must be less than Rank (1)");
            return vs_;
        }

        // [coord.index.arith], index arithmetic
        //! Returns: index<Rank>{*this} += rhs.
        index operator+(index const& rhs) const
        {
            return index(*this) += rhs;
        }

        //! Returns: index<Rank>{*this} -= rhs.
        index operator-(index const& rhs) const
        {
            return index(*this) -= rhs;
        }

        //! Effects: For all i in the range [0, Rank), adds the ith component
        //! of rhs to the ith component of *this and stores the sum in the ith
        //! component of *this.
        //! Returns: *this.
        index& operator+=(index const& rhs)
        {
            vs_ += rhs.vs_;
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), subtracts the ith
        //! component of rhs from the ith component of *this and stores the
        //! difference in the ith component of *this.
        //! Returns: *this.
        index& operator-=(index const& rhs)
        {
            vs_ -= rhs.vs_;
            return *this;
        }

        //! Requires: Rank == 1.
        //! Effects: ++(*this)[0].
        //! Returns: *this.
        index& operator++()
        {
            return ++vs_, *this;
        }

        //! Requires: Rank == 1.
        //! Returns: index<Rank>{(*this)[0]++}.
        index  operator++(int)
        {
            return index(vs_++);
        }

        //! Requires: Rank == 1.
        //! Effects: --(*this)[0].
        //! Returns: *this.
        index& operator--()
        {
            return --vs_, *this;
        }

        //! Requires: Rank == 1.
        //! Returns: index<Rank>{(*this)[0]--}.
        index  operator--(int)
        {
            return index(vs_--);
        }

        //! Returns: *this.
        index  operator+() const BOOST_NOEXCEPT
        {
            return index(*this);
        }

        //! Returns: A copy of *this with each component negated.
        index  operator-() const
        {
            index r;
            r.vs_ = -vs_;
            return r;
        }

        //! Returns: index<Rank>{*this} *= v.
        index  operator*(value_type v) const
        {
            return index(*this) *= v;
        }

        //! Returns: index<Rank>{*this} /= v.
        index  operator/(value_type v) const
        {
            return index(*this) /= v;
        }

        //! Effects: For all i in the range [0, Rank), multiplies the ith
        //! component of *this by v and stores the product in the ith component
        //! of *this.
        //! Returns: *this.
        index& operator*=(value_type v)
        {
            vs_ *= v;
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), divides the ith component
        //! of *this by v and stores the quotient in the ith component of *this.
        //! Returns: *this.
        index& operator/=(value_type v)
        {
            vs_ /= v;
            return *this;
        }

    private:
        value_type vs_;
    };

    // [coord.index.arith], index arithmetic
    //! Returns: index<Rank>{rhs} *= v.
    template <int Rank>
    index<Rank> operator*(std::ptrdiff_t v, index<Rank> const& rhs)
    {
        return index<Rank>(rhs) *= v;
    }

    ///////////////////////////////////////////////////////////////////////////
    template <>
    class bounds<1>
    {
    public:
        // constants and types
        static BOOST_CONSTEXPR_OR_CONST int rank = 1;

        typedef std::ptrdiff_t& reference;
        typedef std::ptrdiff_t const& const_reference;
        typedef bounds_iterator<1> iterator;
        typedef bounds_iterator<1> const_iterator;
        typedef std::size_t size_type;
        typedef std::ptrdiff_t value_type;

        // [coord.bounds.cnstr], bounds construction

        //! Effects: Zero-initializes each component.
        bounds() BOOST_NOEXCEPT
          : vs_ (0)
        {}

        //! Effects: Initializes the 0th component of *this with v.
        //! Remarks: This constructor shall not participate in overload
        //! resolution unless Rank is 1.
        bounds(value_type v) BOOST_NOEXCEPT
          : vs_(v)
        {}

        bounds(value_type il[1])
        {
            vs_ = il[0];
        }

        bounds(value_type const* il, std::size_t size)
        {
            HPX_ASSERT(size == 1 && "size must be equal to Rank (1)");
            vs_ = il[0];
        }

        // [coord.bounds.eq], bounds equality
        //! Returns: true if (*this)[i] == rhs[i] for all i in the range
        //! [0, Rank), otherwise false.
        bool operator==(bounds const& rhs) const BOOST_NOEXCEPT
        {
            return vs_ == rhs.vs_;
        }

        //! Returns: !(*this == rhs).
        bool operator!=(bounds const& rhs) const BOOST_NOEXCEPT
        {
            return !(*this == rhs);
        }

        // [coord.bounds.obs], bounds observers
        //! Returns: The product of all components of *this.
        size_type size() const BOOST_NOEXCEPT
        {
            return vs_;
        }

        //! Returns: true if 0 <= idx[i] and idx[i] < (*this)[i] for all i in
        //! the range [0, Rank), otherwise false.
        bool contains(index<1> const& idx) const BOOST_NOEXCEPT
        {
            return (idx[0] >= 0 && idx[0] < vs_);
        }

        // [coord.bounds.iter], bounds iterators
        //! Returns: A bounds_iterator referring to the first element of the
        //! space defined by *this such that *begin() == index<Rank>{} if
        //! size() != 0, begin() == end() otherwise.
        const_iterator begin() const BOOST_NOEXCEPT
        {
            return const_iterator(*this, index<1>());
        }

        //! Returns: A bounds_iterator which is the past-the-end iterator for
        //! the space defined by *this.
        const_iterator end() const BOOST_NOEXCEPT
        {
            return const_iterator(*this, index<1>(vs_));
        }

        // [coord.bounds.cmpt], bounds component access
        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        reference operator[](size_type n)
        {
            HPX_ASSERT(n == 0 && "n must be less than Rank (1)");
            return vs_;
        }

        //! Requires: n < Rank.
        //! Returns: A reference to the nth component of *this.
        const_reference operator[](size_type n) const
        {
            HPX_ASSERT(n == 0 && "n must be less than Rank (1)");
            return vs_;
        }

        // [coord.bounds.arith], bounds arithmetic
        //! Returns: bounds<Rank>{*this} += rhs.
        bounds operator+(const index<1>& rhs) const
        {
            return bounds(*this) += rhs;
        }

        //! Returns: bounds<Rank>{*this} -= rhs.
        bounds operator-(const index<1>& rhs) const
        {
            return bounds(*this) -= rhs;
        }

        //! Effects: For all i in the range [0, Rank), adds the ith component
        //! of rhs to the ith component of *this and stores the sum in the ith
        //! component of *this.
        //! Returns: *this.
        bounds& operator+=(const index<1>& rhs)
        {
            vs_ += rhs[0];
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), subtracts the ith
        //! component of rhs from the ith component of *this and stores the
        //! difference in the ith component of *this.
        //! Returns: *this.
        bounds& operator-=(const index<1>& rhs)
        {
            vs_ -= rhs[0];
            return *this;
        }

        //! Returns: bounds<Rank>{*this} *= v.
        bounds  operator*(value_type v) const
        {
            return bounds(*this) /= v;
        }

        //! Returns: bounds<Rank>{*this} /= v.
        bounds  operator/(value_type v) const
        {
            return bounds(*this) /= v;
        }

        //! Effects: For all i in the range [0, Rank), multiplies the ith
        //! component of *this by v and stores the product in the ith component
        //! of *this.
        //! Returns: *this.
        bounds& operator*=(value_type v)
        {
            vs_ += v;
            return *this;
        }

        //! Effects: For all i in the range [0, Rank), divides the ith component
        //! of *this by v and stores the quotient in the ith component of *this.
        //! Returns: *this.
        bounds& operator/=(value_type v)
        {
            vs_ += v;
            return *this;
        }

    private:
        value_type vs_;
    };

    // [coord.bounds.arith], bounds arithmetic
    //! Returns: bounds<Rank>{rhs} += lhs.
    template <int Rank>
    bounds<Rank> operator+(index<Rank> const& lhs, bounds<Rank> const& rhs)
    {
        return bounds<Rank>(rhs) += lhs;
    }

    //! Returns: bounds<Rank>{rhs} *= lhs.
    template <int Rank>
    bounds<Rank> operator*(std::ptrdiff_t v, bounds<Rank> const& rhs)
    {
        return bounds<Rank>(rhs) *= v;
    }
}}

#endif


// ======================================================================
/*!
 * \file
 * \brief Regression tests for Fmi::LatLonTree
 */
// ======================================================================

#include "LatLonTree.h"
#include <boost/lexical_cast.hpp>
#include <regression/tframe.h>
#include <iostream>
#include <sstream>
#include <string>

using namespace std;

struct Point
{
  Point() : longitude(0), latitude(0), name() {}
  Point(double x, double y, const std::string& n = "") : longitude(x), latitude(y), name(n) {}
  bool operator==(const Point& other) const
  {
    return (longitude == other.longitude && latitude == other.latitude);
  }

  bool operator!=(const Point& other) const { return !(operator==(other)); }
  double longitude;
  double latitude;
  std::string name;
};

template <typename T>
void print(const T& points)
{
  for (const auto& p : points)
    std::cout << p.second.name << " " << p.first << std::endl;
  std::cout << std::endl;
}

Fmi::LatLonTree<Point> latlontree;

void prepare()
{
  latlontree.insert(Point(24.9354, 60.1695, "Helsinki"));
  latlontree.insert(Point(23.8702, 61.6074, "Tampere"));
  latlontree.insert(Point(22.3333, 60.5333, "Turku"));
  latlontree.insert(Point(25.9694, 65.2173, "Oulu"));
  latlontree.insert(Point(27.8957, 62.9472, "Kuopio"));
  latlontree.insert(Point(25.9525, 66.60, "Rovaniemi"));
  latlontree.insert(Point(22.95, 59.833, "Hanko"));
  latlontree.insert(Point(27.0284, 69.908, "Utsjoki"));
}

namespace LatLonTreeTest
{
// ----------------------------------------------------------------------

void size()
{
  if (latlontree.size() != 8)
    TEST_FAILED("Test tree size should be 8, not " +
                boost::lexical_cast<string>(latlontree.size()));
  TEST_PASSED();
}

// ----------------------------------------------------------------------

void empty()
{
  if (latlontree.empty())
    TEST_FAILED("Test tree should not be empty");
  TEST_PASSED();
}

// ----------------------------------------------------------------------

void nearest()
{
  {
    auto ret = latlontree.nearest(Point(25, 60));
    if (!ret)
      TEST_FAILED("Failed to find nearest point for 25,60");
    if (ret->name != "Helsinki")
      TEST_FAILED("Closest point to 25,60 should be Helsinki, not " + ret->name);
  }

  {
    auto ret = latlontree.nearest(Point(0, 0));
    if (!ret)
      TEST_FAILED("Failed to find nearest point for 0,0");
    if (ret->name != "Hanko")
      TEST_FAILED("Closest point to 0,0 should be Hanko, not " + ret->name);
  }

  {
    auto ret = latlontree.nearest(Point(25, 80));
    if (!ret)
      TEST_FAILED("Failed to find nearest point for 25,80");
    if (ret->name != "Utsjoki")
      TEST_FAILED("Closest point to 25,80 should be Utsjoki, not " + ret->name);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------

void farthest()
{
  {
    auto ret = latlontree.farthest(Point(0, 0));
    if (!ret)
      TEST_FAILED("Failed to find farthest point for 0,0");
    if (ret->name != "Utsjoki")
      TEST_FAILED("Farthest point to 0,0 should be Utsjoki, not " + ret->name);
  }

  {
    auto ret = latlontree.farthest(Point(25, 80));
    if (!ret)
      TEST_FAILED("Failed to find farthest point for 25,80");
    if (ret->name != "Hanko")
      TEST_FAILED("Farthest point to 25,80 should be Hanko, not " + ret->name);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------

void nearest_many()
{
  auto ret = latlontree.nearestones(Point(25, 60), 200);
  if (ret.size() != 4)
  {
    // print(ret);
    TEST_FAILED("Failed to find 4 nearest points for 25,60 with radius 200 km");
  }

  ret = latlontree.nearestones(Point(25, 70), 400);
  if (ret.size() != 2)
  {
    // print(ret);
    TEST_FAILED("Failed to find 2 nearest points for 25,70 with radius 400");
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * The actual test suite
 */
// ----------------------------------------------------------------------

class tests : public tframe::tests
{
  virtual const char* error_message_prefix() const { return "\n\t"; }
  void test(void)
  {
    TEST(size);
    TEST(empty);
    TEST(nearest);
    TEST(farthest);
    TEST(nearest_many);
  }
};

}  // namespace LatLonTreeTest

//! The main program
int main(void)
{
  prepare();

  using namespace std;
  cout << endl << "LatLonTree" << endl << "==========" << endl;
  LatLonTreeTest::tests t;
  return t.run();
}

// ======================================================================


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

/**
 * \file
 * \brief C Interface implementation to Vital use of Eigen vector and matrix
 *        classes
 *
 * Eigen is Column-major by default, i.e. (rows, cols) indexing
 */

#include "eigen.h"

#include <Eigen/Core>
#include <vital/bindings/c/helpers/c_utils.h>

/// Define Eigen matrix interface functions for use with Vital
/**
 * \param T The data storage type like double or float
 * \param S The character suffix to use for naming of functions.
 * \param R Number of rows in the matrix. "Vector" types use this as the size
 *          parameter.
 * \param C Number of columns in the matrix. "Vector" types have a value of 1
 *          here.
 */
#define DEFINE_EIGEN_OPERATIONS( T, S, R, C ) \
/** Create a new Eigen type-based Matrix of the given shape */ \
vital_eigen_matrix##R##x##C##S##_t* \
vital_eigen_matrix##R##x##C##S##_new() \
{ \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".new.", 0, \
    return reinterpret_cast<vital_eigen_matrix##R##x##C##S##_t*>( \
      new Eigen::Matrix< T, R, C >() \
    ); \
  ); \
  return 0; \
} \
/** Create a new Eigen type-based Matrix of the given shape */ \
vital_eigen_matrix##R##x##C##S##_t* \
vital_eigen_matrix##R##x##C##S##_new_sized( ptrdiff_t rows, ptrdiff_t cols ) \
{ \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".new.", 0, \
    return reinterpret_cast<vital_eigen_matrix##R##x##C##S##_t*>( \
      new Eigen::Matrix< T, R, C >(rows, cols) \
    ); \
  ); \
  return 0; \
} \
\
/** Destroy a given Eigen matrix instance */ \
void \
vital_eigen_matrix##R##x##C##S##_destroy( vital_eigen_matrix##R##x##C##S##_t *m, \
                                          vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".destroy", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    if( mp ) \
    { \
      delete( mp ); \
    } \
  ); \
} \
\
/** Get the value at a location */ \
T \
vital_eigen_matrix##R##x##C##S##_get( vital_eigen_matrix##R##x##C##S##_t *m, \
                                      ptrdiff_t row, ptrdiff_t col, \
                                      vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".get", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return (*mp)(row, col); \
  ); \
  return 0; \
} \
\
/** Set the value at a location */ \
void \
vital_eigen_matrix##R##x##C##S##_set( vital_eigen_matrix##R##x##C##S##_t *m, \
                                      ptrdiff_t row, ptrdiff_t col, \
                                      T value, \
                                      vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".set", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    (*mp)(row, col) = value; \
  ); \
} \
\
/** Get the number of rows in the matrix */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_rows( vital_eigen_matrix##R##x##C##S##_t *m, \
                                       vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".rows", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->rows(); \
  ); \
  return 0; \
} \
\
/** Get the number of columns in the matrix */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_cols( vital_eigen_matrix##R##x##C##S##_t *m, \
                                       vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".cols", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->cols(); \
  ); \
  return 0; \
} \
\
/** Get the pointer increment betweentwo consecutive rows. */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_row_stride( vital_eigen_matrix##R##x##C##S##_t *m, \
                                             vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".row_stride", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->rowStride(); \
  ); \
  return 0; \
} \
\
/** Get the pointer increment betweentwo consecutive columns */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_col_stride( vital_eigen_matrix##R##x##C##S##_t *m, \
                                             vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".col_stride", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->colStride(); \
  ); \
  return 0; \
} \
\
/** Get the pointer to the vector's data array */ \
T* \
vital_eigen_matrix##R##x##C##S##_data( vital_eigen_matrix##R##x##C##S##_t *m, \
                                       vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".data", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->data(); \
  ); \
  return 0; \
}

/// Define operations for both combinations of X and Y
#define DEFINE_EIGEN_RECTANGLES( T, S, X, Y )\
DEFINE_EIGEN_OPERATIONS( T, S, X, Y ) \
DEFINE_EIGEN_OPERATIONS( T, S, Y, X )

/// DEFINE operations for all shapes
/**
 * \param T Data type
 * \param S Type suffix
 */
#define DEFINE_EIGEN_ALL_SHAPES( T, S ) \
/* Vector shapes */                      \
DEFINE_EIGEN_RECTANGLES( T, S, 2, 1 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 3, 1 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 4, 1 )   \
DEFINE_EIGEN_RECTANGLES( T, S, X, 1 )   \
/* Square shapes */                      \
DEFINE_EIGEN_OPERATIONS( T, S, 2, 2 )   \
DEFINE_EIGEN_OPERATIONS( T, S, 3, 3 )   \
DEFINE_EIGEN_OPERATIONS( T, S, 4, 4 )   \
DEFINE_EIGEN_OPERATIONS( T, S, X, X )   \
/* Other Rectangular shapes */           \
DEFINE_EIGEN_RECTANGLES( T, S, 3, 2 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 4, 2 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 4, 3 )

// Set a constant X for convenience in struct/function naming
namespace {
int const X = Eigen::Dynamic;
}

DEFINE_EIGEN_ALL_SHAPES( double, d )
DEFINE_EIGEN_ALL_SHAPES( float,  f )

#undef DEFINE_EIGEN_OPERATIONS
#undef DEFINE_EIGEN_RECTANGLES
#undef DEFINE_EIGEN_ALL_SHAPES


/*
 * draw.cpp
 *
 *  Created on: Dec 16, 2015
 *      Author: jcassidy
 */

#include <iostream>
#include <fstream>

#include <string>
#include <cinttypes>
#include <utility>

#include "XercesUtils.hpp"
#include "LoadOSM.hpp"

#include "OSMDatabase.hpp"
#include "PathNetwork.hpp"

#include <boost/timer/timer.hpp>

#include <boost/archive/binary_oarchive.hpp>

#include "DrawPathNetwork.hpp"
#include "MidLatProjection.hpp"

using namespace std;
using namespace xercesc;

int main(int argc,char **argv)
{
	xercesc::XMLPlatformUtils::Initialize();

	string fn("hamilton_canada.osm.bz2");

	if (argc > 1)
		fn=argv[1];

	OSMDatabase db;

	db = loadOSM(fn);

	cout << "==== BUILDING NETWORK" << endl;

	OSMWayFilterRoads hwyFilt(db,db.nodes());

	auto G = buildNetwork(db,hwyFilt);

	cout << "==== DONE" << endl;

	// draw the paths to a .png surface

	std::array<unsigned,2> dims{2048,1536};

	Cairo::RefPtr<Cairo::ImageSurface> surface =
			Cairo::ImageSurface::create(Cairo::Format::FORMAT_ARGB32, dims[0], dims[1]);

	Cairo::RefPtr<Cairo::Context> cr = Cairo::Context::create(surface);

	cr->save();

	/** Cairo affine matrix is provided column-major as follows:
	 * [u] 		[	arg0	arg2	arg4]	[ x ]
	 * [v]	=	[	arg1	arg3	arg5]	[ y ]
	 * 										[ 1 ]
	 */

	/** Cairo graphics surface has +y downwards and (0,0) at upper left
	 *  Provide coordinate system in standard orientation:
	 * 		(0,0)	-> lower-left corner
	 * 		+x		-> rightwards
	 * 		+y		-> upwards
	 *
	 */

	Cairo::Matrix mat(1,0,0,-1,0,dims[1]);
	cr->set_matrix(mat);

	cr->set_source_rgb(1.0,1.0,1.0);			// paint it white
	cr->fill();

	cr->set_source_rgb(0.0,0.0,0.0);			// black 1px wide lines
	cr->set_line_width(1);

	DrawPathNetwork dpn;

	MidLatProjection proj;
	proj.centreWithin(db.bounds(),dims);		// centre bounding box within the drawing dimensions

	dpn.projection(&proj);						// set the projection
	dpn.network(&G);							// associate the network
	dpn.drawCurvePoints(true);					// draw intermediate points (not just intersection-intersection straight lines)

	dpn.draw(cr);								// draw it

	cr->set_source_rgb(1.0,0.0,0.0);			// draw without curve points
	cr->set_line_width(0.5);
	dpn.drawCurvePoints(false);
	dpn.draw(cr);

	cr->save();

	surface->write_to_png("network.png");

	// technically should close this up, but there are still some strings outstanding which will be deleted and cause trouble
	// program termination will free them anyway
	//xercesc::XMLPlatformUtils::Terminate();
}


#include "SPlisHSPlasH/Common.h"
#include "GL/glew.h"
#include "Visualization/MiniGL.h"
#include "GL/glut.h"
#include "SPlisHSPlasH/TimeManager.h"
#include <Eigen/Dense>
#include <iostream>
#include "SPlisHSPlasH/Utilities/Timing.h"
#include "Utilities/PartioReaderWriter.h"
#include "PositionBasedDynamicsWrapper/PBDRigidBody.h"
#include "Utilities/OBJLoader.h"
#include "SPlisHSPlasH/Utilities/PoissonDiskSampling.h"
#include "PositionBasedDynamicsWrapper/PBDWrapper.h"
#include "Demos/Common/DemoBase.h"
#include "Utilities/FileSystem.h"
#include "SPlisHSPlasH/DFSPH/DFSPH_CUDA.h"

//#define FFMPEG_RENDER
#ifdef FFMPEG_RENDER
FILE* ffmpeg = NULL;
//#define USE_MULTIPLES_SHADER
#endif

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
	#define new DEBUG_NEW
#endif
#endif

using namespace SPH;
using namespace Eigen;
using namespace std;

void timeStep ();
void initBoundaryData();
void render ();
void renderBoundary();
void reset();
void updateBoundaryParticles(const bool forceUpdate);
void updateBoundaryForces();
void simulationMethodChanged();
void partioExport();

DemoBase base;
PBDWrapper pbdWrapper;
Real nextFrameTime = 0.0;
unsigned int frameCounter = 1;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS;

	base.init(argc, argv, "DynamicBoundaryDemo");

	//////////////////////////////////////////////////////////////////////////
	// PBD
	//////////////////////////////////////////////////////////////////////////
	pbdWrapper.initShader();
	pbdWrapper.readScene(base.getSceneFile());

	initBoundaryData();
	base.buildModel();
	base.setSimulationMethodChangedFct(simulationMethodChanged);
	pbdWrapper.initGUI();

	pbdWrapper.initModel(TimeManager::getCurrent()->getTimeStepSize());

	MiniGL::setClientIdleFunc(50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setClientSceneFunc(render);

	glutMainLoop ();

	base.cleanup();

	Timing::printAverageTimes();
	Timing::printTimeSums();

	return 0;
}

void reset()
{
	Timing::printAverageTimes();
	Timing::reset();

	//////////////////////////////////////////////////////////////////////////
	// PBD
	//////////////////////////////////////////////////////////////////////////
	pbdWrapper.reset();

	updateBoundaryParticles(true);

	base.getSimulationMethod().simulation->reset();
	TimeManager::getCurrent()->setTime(0.0);
	base.getSelectedParticles().clear();

	nextFrameTime = 0.0;
	frameCounter = 1;
}

void timeStep ()
{
	{
		static bool firstTime = false;
		if (firstTime) {
			firstTime = false;

			MiniGL::setViewport(40.0, 0.1f, 500.0, Vector3r(-11.0, 3.0, 0.0), Vector3r(0.0, 2.0, 0.0));
		}
	}

	if ((base.getPauseAt() > 0.0) && (base.getPauseAt() < TimeManager::getCurrent()->getTime()))
		base.setPause(true);

	float pauseRbAfterTime = 0.72;
	if (pauseRbAfterTime > 0) {
		if (TimeManager::getCurrent()->getTime() > pauseRbAfterTime) {
			base.setRbPause(true);
		}
	}

	if (base.getSimulationMethod().simulationMethod == DemoBase::SimulationMethods::DFSPH_CUDA) {
		DFSPHCUDA* sim = dynamic_cast<DFSPHCUDA*>(base.getSimulationMethod().simulation);

		sim->handleDynamicBodiesPause(base.getRbPause());

		//save th simulation state if asked
		sim->handleSimulationSave(base.getSaveLiquid()|| base.getSaveSimulation(), base.getSaveSimulation(), base.getSaveSimulation());
		//I'll handle the save as token so I need to consume them
		base.setSaveLiquid(false);
		base.setSaveSimulation(false);

		//load the simulation state if asked
		//sim->handleSimulationLoad(base.getLoadLiquid() || base.getLoadSimulation(), true, base.getLoadSimulation(), true, base.getLoadSimulation(), true);
		sim->handleSimulationLoad(base.getLoadLiquid() || base.getLoadSimulation(), true, false, false, base.getLoadSimulation(), true);
		//I'll handle the save as token so I need to consume them
		base.setLoadLiquid(false);
		base.setLoadSimulation(false);

		if (base.getZeroVelocities()) {
			sim->zeroFluidVelocities();
		}
		base.setZeroVelocities(false);
	}

	if (base.getPause())
		return;

	/*
	static int k = 0;
	k++;
	std::cout << k << std::endl;

	if (k == 1) {
		MiniGL::rotateY(0.001*1454);
		MiniGL::rotateX(0.00001*4784);
		MiniGL::move(0.001*1305, 0, 0);
	}

	//MiniGL::move(0.001, 0, 0);
	//MiniGL::move(0, 0.001, 0);
	//MiniGL::move(0, 0, 0.001);
	//MiniGL::move();
	//MiniGL::rotateX(0.00001);
	//MiniGL::rotateY(-0.0001);
	//*/

	// Simulation code
	for (unsigned int i = 0; i < base.getNumberOfStepsPerRenderUpdate(); i++)
	{

		START_TIMING("SimStep");
		base.getSimulationMethod().simulation->step();
		STOP_TIMING_AVG;

		if (!base.getRbPause()) {
			bool use_external_rb_engine = false;
			if ((use_external_rb_engine)&& (base.getSimulationMethod().simulationMethod == DemoBase::SimulationMethods::DFSPH_CUDA)) {
				DFSPHCUDA* sim = dynamic_cast<DFSPHCUDA*>(base.getSimulationMethod().simulation);
				//send the information to the physics engine
				sim->updateRigidBodiesStateToFile();

				//read the information back (there is a wait in this function)
				sim->updateRigidBodiesStatefromFile();
			}
			else {
				updateBoundaryForces();

				bool controlBoat = false;
				if (controlBoat) {
					bool manualBoatVelocityControl = false;
					bool manualBoatOrientationControl = false;

					FluidModel::RigidBodyParticleObject *rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(1);
					RigidBodyObject *rbo = rbpo->m_rigidBody;
					if (rbo->isDynamic())
					{
						//std::cout << "test nbr particle boat: " << rbpo->numberOfParticles() << std::endl;
						//Vector3r angles= rbo->getRotation().eulerAngles(0, 1, 2);
						//std::cout << "boat euler angles: " << angles[0] << "  " << angles[1] << "  " << angles[2] << std::endl;

						//a system to counter gravity to debug the boat control
						if (false&&rbo->getPosition()[1] < 1.5) {
							Vector3r f_counter_grav(0, 1200, 0);
							rbo->addForce(f_counter_grav);
						}

						//a system that deactivate a boat control key when the oposite key is pressed
						static bool boatForward_old = false;
						static bool boatBackward_old = false;
						static bool boatLeft_old = false;
						static bool boatRight_old = false;

						if (boatForward_old&&base.getBoatBackward()) {
							base.setBoatForward(false);
						}

						if (boatBackward_old&&base.getBoatForward()) {
							base.setBoatBackward(false);
						}

						if (boatLeft_old&&base.getBoatRight()) {
							base.setBoatLeft(false);
						}

						if (boatRight_old&&base.getBoatLeft()) {
							base.setBoatRight(false);
						}

						//and save the keyboard state for next iter
						boatForward_old = base.getBoatForward();
						boatBackward_old = base.getBoatBackward();
						boatLeft_old = base.getBoatLeft();
						boatRight_old = base.getBoatRight();

						//some intermediary boleans to integrate the atomatic boat control easily
						bool boatForward = false;
						bool boatBackward = false;
						bool boatLeft = false;
						bool boatRight = false;

						float controlForceIntensity = base.getBoatForceIntensity();
						if (manualBoatVelocityControl) {
							boatForward = boatForward_old;
							boatBackward = boatBackward_old;
						}
						else {
							//here code the scenarios you want for the automatic control
							controlForceIntensity = 5000; // 800;
							boatForward = true;
						}

						//apply the boat control
						Matrix3r rot_marix=rbo->getRotation();
						//*
						//this should have the boat base direction as a default value
						Vector3r boatControlForce(-1,0,0);
						if (boatForward) {
							boatControlForce *= controlForceIntensity;
						}
						else if (boatBackward) {
							boatControlForce *= -controlForceIntensity;
						}
						else {
							boatControlForce = Vector3r(0, 0, 0);
						}
						if (boatControlForce.norm() > 1e-6) {
							//convert the force to global coordinates
							boatControlForce= rot_marix*boatControlForce;

							//in fact I want the force to be in the horizontal plane.
							// I'll go the easy and dirty way
							Real norm_f = boatControlForce.norm();
							boatControlForce[1] = 0;
							boatControlForce *= norm_f / boatControlForce.norm();

							//apply the force
							rbo->addForce(boatControlForce);
						}
						//*/

						//*

						float controlTorqueIntensity = base.getBoatTorqueIntensity();
						if (manualBoatOrientationControl) {
							boatLeft = boatLeft_old;
							boatRight = boatRight_old;
						}
						else {
							//here code the scenarios you want for the automatic control
							Vector3r boatDirLocal(-1, 0, 0);
							Vector3r boatDirGlobal=rbo->getRotation()*boatDirLocal;

							//I want the direction in the Horizontal plane
							boatDirGlobal[1] = 0;
							boatDirGlobal.normalize();

							Vector3r targetDir(-1, 0, 0);

							bool with_animation = true;
							if (with_animation) {
								if (TimeManager::getCurrent()->getTime() > 20) {
									targetDir = Vector3r(-0.25, 0, -0.75);
									targetDir.normalize();
								}else if (TimeManager::getCurrent()->getTime() > 10) {
									targetDir = Vector3r(-0.25, 0, 0.75);
									targetDir.normalize();
								}
							}

							//and now depending on the target direction activatethe left or right control
							//for the intensity I want the actual angle
							//most likely I need a pd-controler for that
							//float angleToTarget=acosf(targetDir.dot(boatDirGlobal));

							//currently using a fixed intesity since it works well
							//controlTorqueIntensity = 50;
							controlTorqueIntensity = 400;

							//and wealso need to know the direction
							float crossProdRes = targetDir.cross(boatDirGlobal).y();
							if (crossProdRes > 0) {
								boatRight = true;
							}
							else {
								boatLeft = true;
							}

							//std::cout << "automatic boat control internal values: " << angleToTarget << "   " << crossProdRes << std::endl;
						}

						bool apply_local_torque = false;
						Vector3r boatControlTorque(0, 1, 0);
						if (apply_local_torque) {
							boatControlTorque= Vector3r(0, 0, -1);
						}
						if (boatLeft) {
							boatControlTorque *= controlTorqueIntensity;
						}
						else if (boatRight) {
							boatControlTorque *= -controlTorqueIntensity;
						}
						else {
							boatControlTorque = Vector3r(0, 0, 0);
						}
						if (boatControlTorque.norm() > 1e-6) {
							//convert the torque to global coordinates
							if (apply_local_torque) {
								boatControlTorque = rbo->getRotation() * boatControlTorque;
							}

							//apply the torque
							rbo->addTorque(boatControlTorque);
						}
						//*/

						//std::cout << "control intensity force/torque: " << controlForceIntensity << "  " << controlTorqueIntensity << std::endl;

					}
				}

			//////////////////////////////////////////////////////////////////////////
			// PBD
			//////////////////////////////////////////////////////////////////////////
				START_TIMING("SimStep - PBD");
				pbdWrapper.timeStep();
				STOP_TIMING_AVG;

				updateBoundaryParticles(false);

				bool camera_follow_boat = false;
				if (camera_follow_boat) {
					FluidModel::RigidBodyParticleObject *rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(1);
					RigidBodyObject *rbo = rbpo->m_rigidBody;
					if (rbo->isDynamic())
					{
						Vector3r newBoatpos;
						newBoatpos = rbo->getPosition();

						//let's use the lookat procedure from the setViewport function since it's easier for now
						Vector3r eyePos = newBoatpos;
						Vector3r lookAt = newBoatpos;

						//the type of camera used
						//0: folow small boat
						//1: follow large boat
						int camera_type = 1;
						if (camera_type == 0) {
							eyePos[0] += 0.5;
							eyePos[1] = 3;
							eyePos[2] += 5;
							lookAt[1] = 1;
						}
						else if (camera_type == 1) {
							eyePos[0] += 0.5*1.66;
							eyePos[1] = 2*1.66;
							eyePos[2] += 5*1.66;
							lookAt[1] = 1;
						}

						MiniGL::setViewport(40.0, 0.1f, 500.0, eyePos, lookAt);
					}
				}
			}
		}

		if (base.getEnablePartioExport())
		{
			if (TimeManager::getCurrent()->getTime() >= nextFrameTime)
			{
				nextFrameTime += 1.0 / base.getFramesPerSecond();
				partioExport();
				frameCounter++;
			}
		}
	}

#ifdef FFMPEG_RENDER
	//the part to save to file
	if (ffmpeg != NULL) {
		if (TimeManager::getCurrent()->getTime()>20) {
			_pclose(ffmpeg);
			exit(0);
		}
	}
#endif
}

void simulationMethodChanged()
{
	pbdWrapper.initGUI();
	reset();
}

void renderBoundary()
{
	DemoBase::SimulationMethod &simulationMethod = base.getSimulationMethod();
	Shader &shader = base.getShader();
	Shader &meshShader = base.getMeshShader();
	SceneLoader::Scene &scene = base.getScene();
	const int renderWalls = base.getRenderWalls();

	float wallColor[4] = { 0.1f, 0.6f, 0.6f, 1.0f };
	if ((renderWalls == 1) || (renderWalls == 2))
	{
		if (MiniGL::checkOpenGLVersion(3, 3))
		{
			shader.begin();
			glUniform3fv(shader.getUniform("color"), 1, &wallColor[0]);

			if (base.isDFSPH()) {
				base.renderBoundariesDFSPH_CUDA(renderWalls == 1);
			}else{
				glEnableVertexAttribArray(0);
				for (int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
				{
					if ((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
					{
						FluidModel::RigidBodyParticleObject *rb = simulationMethod.model.getRigidBodyParticleObject(body);
						glVertexAttribPointer(0, 3, GL_DOUBLE, GL_FALSE, 0, &simulationMethod.model.getPosition(body + 1, 0));
						glDrawArrays(GL_POINTS, 0, rb->numberOfParticles());
					}
				}
				glDisableVertexAttribArray(0);
			}
			shader.end();
		}
		else
		{
			glDisable(GL_LIGHTING);
			glPointSize(4.0f);

			glBegin(GL_POINTS);
			for (int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
			{
				if ((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
				{
					FluidModel::RigidBodyParticleObject *rb = simulationMethod.model.getRigidBodyParticleObject(body);
					for (unsigned int i = 0; i < rb->numberOfParticles(); i++)
					{
						glColor3fv(wallColor);
						glVertex3v(&simulationMethod.model.getPosition(body + 1, i)[0]);
					}
				}
			}
			glEnd();
			glEnable(GL_LIGHTING);
		}
	}
}

void render()
{
	MiniGL::coordinateSystem();

	static int width = glutGet(GLUT_WINDOW_WIDTH);
	static int height = glutGet(GLUT_WINDOW_HEIGHT);

#ifdef FFMPEG_RENDER
	static int* buffer = new int[width*height];

	if (!base.getPause()) {
		if (ffmpeg == NULL) {
			int framerate = (1 / TimeManager::getCurrent()->getTimeStepSize());
			std::cout << "video framerate: " << framerate << std::endl;
			std::ostringstream oss;
			// start ffmpeg telling it to expect raw rgba 720p-60hz frames
			// -i - tells it to read frames from stdin
			oss << "D:\\ffmpeg-4.1.3-win64-static\\bin\\ffmpeg " <<
				" -r " << framerate << " -f rawvideo -pix_fmt rgba -s " << width << "x" << height << " -i - " <<
				"-threads 0 -preset fast -y -pix_fmt yuv420p -crf 21 -vf vflip output.mp4";

			// open pipe to ffmpeg's stdin in binary write mode
			ffmpeg = _popen(oss.str().c_str(), "wb");
		}
	}
#endif

	base.renderFluid();

	renderBoundary();

	//////////////////////////////////////////////////////////////////////////
	// PBD
	//////////////////////////////////////////////////////////////////////////

	PBD::SimulationModel &model = pbdWrapper.getSimulationModel();
	PBD::SimulationModel::RigidBodyVector &rb = model.getRigidBodies();

	const int renderWalls = base.getRenderWalls();
	SceneLoader::Scene &scene = base.getScene();
	if ((renderWalls == 3) || (renderWalls == 4))
	{
		for (size_t i = 0; i < rb.size(); i++)
		{
			const PBD::VertexData &vd = rb[i]->getGeometry().getVertexData();
			const PBD::IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
			if ((renderWalls == 3) || (!scene.boundaryModels[i]->isWall))
			{
				float *col = &scene.boundaryModels[i]->color[0];
				if (!scene.boundaryModels[i]->isWall)
				{
					base.meshShaderBegin(col);
					pbdWrapper.drawMesh(vd, mesh, 0, col);
					base.meshShaderEnd();
				}
				else
				{
					base.meshShaderBegin(col);
					pbdWrapper.drawMesh(vd, mesh, 0, col);
					base.meshShaderEnd();
				}
			}
		}
	}

	pbdWrapper.renderTriangleModels();
	pbdWrapper.renderTetModels();
	pbdWrapper.renderConstraints();
	pbdWrapper.renderBVH();

#ifdef FFMPEG_RENDER
	if (ffmpeg != NULL) {
		glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, buffer);

		fwrite(buffer, sizeof(int)*width*height, 1, ffmpeg);
	}

#endif

}

void initBoundaryData()
{
	std::string base_path = FileSystem::getFilePath(base.getSceneFile());
	SceneLoader::Scene &scene = base.getScene();
	const bool useCache = base.getUseParticleCaching();

	std::cout << "nbr of boundary models: "<< scene.boundaryModels.size() <<std::endl;

	for (unsigned int i = 0; i < scene.boundaryModels.size(); i++)
	{
		std::vector<Vector3r> boundaryParticles;
		if (scene.boundaryModels[i]->samplesFile != "")
		{
			string particleFileName = base_path + "/" + scene.boundaryModels[i]->samplesFile;
			PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), scene.boundaryModels[i]->scale[0], boundaryParticles);
		}

		PBD::SimulationModel &model = pbdWrapper.getSimulationModel();
		PBD::SimulationModel::RigidBodyVector &rigidBodies = model.getRigidBodies();
		PBDRigidBody *rb = new PBDRigidBody(rigidBodies[i]);
		PBD::RigidBodyGeometry &geo = rigidBodies[i]->getGeometry();
		PBD::IndexedFaceMesh &mesh = geo.getMesh();
		PBD::VertexData &vd = geo.getVertexData();

		if (scene.boundaryModels[i]->samplesFile == "")
		{
			// Cache sampling
			std::string mesh_base_path = FileSystem::getFilePath(scene.boundaryModels[i]->meshFile);
			std::string mesh_file_name = FileSystem::getFileName(scene.boundaryModels[i]->meshFile);
			std::string scene_path = FileSystem::getFilePath(base.getSceneFile());
			std::string scene_file_name = FileSystem::getFileName(base.getSceneFile());
			string cachePath = scene_path + "/" + mesh_base_path + "/Cache";
			string particleFileName = FileSystem::normalizePath(cachePath + "/" + scene_file_name + "_" + mesh_file_name + "_" + std::to_string(i) + ".bgeo");

			// check MD5 if cache file is available
			bool foundCacheFile = false;
			bool md5 = false;

			std::string md5FileName = FileSystem::normalizePath(cachePath + "/" + scene_file_name + ".md5");
			if (useCache)
			{
				foundCacheFile = FileSystem::fileExists(particleFileName);
				if (foundCacheFile)
				{
					string md5Str = FileSystem::getFileMD5(base.getSceneFile());
					md5 = FileSystem::checkMD5(md5Str, md5FileName);
				}
			}

			if (useCache && foundCacheFile && md5)
			{
				 PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), 1.0, boundaryParticles);
				std::cout << "Loaded cached boundary sampling: " << particleFileName << "\n";
			}

			if (!useCache || !foundCacheFile || !md5)
			{
				std::cout << "Surface sampling of " << scene.boundaryModels[i]->meshFile << "\n";
				START_TIMING("Poisson disk sampling");
				PoissonDiskSampling sampling;
				sampling.sampleMesh(mesh.numVertices(), &vd.getPosition(0), mesh.numFaces(), mesh.getFaces().data(), scene.particleRadius, 10, 1, boundaryParticles);
				STOP_TIMING_AVG;

				// Cache sampling
				if (useCache && (FileSystem::makeDir(cachePath) == 0))
				{
					std::cout << "Save particle sampling: " << particleFileName << "\n";
					PartioReaderWriter::writeParticles(particleFileName, (unsigned int)boundaryParticles.size(), boundaryParticles.data(), NULL, scene.particleRadius);

					FileSystem::writeMD5File(base.getSceneFile(), md5FileName);
				}
			}
			// transform back to local coordinates
			for (unsigned int j = 0; j < boundaryParticles.size(); j++) {
				boundaryParticles[j] = rb->getRotation().transpose() * (boundaryParticles[j] - rb->getPosition());
			}
		}
		base.getSimulationMethod().model.addRigidBodyObject(rb, static_cast<unsigned int>(boundaryParticles.size()), &boundaryParticles[0]);
	}
	updateBoundaryParticles(true);
}

void updateBoundaryParticles(const bool forceUpdate = false)
{
	SceneLoader::Scene &scene = base.getScene();
	const unsigned int nObjects = base.getSimulationMethod().model.numberOfRigidBodyParticleObjects();
	for (unsigned int i = 0; i < nObjects; i++)
	{
		FluidModel::RigidBodyParticleObject *rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(i);
		RigidBodyObject *rbo = rbpo->m_rigidBody;
		if (rbo->isDynamic() || forceUpdate)
		{
			#pragma omp parallel default(shared)
			{
				#pragma omp for schedule(static)
				for (int j = 0; j < (int)rbpo->numberOfParticles(); j++)
				{
					rbpo->m_x[j] = rbo->getRotation() * rbpo->m_x0[j] + rbo->getPosition();
					rbpo->m_v[j] = rbo->getAngularVelocity().cross(rbpo->m_x[j] - rbo->getPosition()) + rbo->getVelocity();
				}
			}
		}
	}
}

void updateBoundaryForces()
{
	Real h = TimeManager::getCurrent()->getTimeStepSize();
	SceneLoader::Scene &scene = base.getScene();
	const unsigned int nObjects = base.getSimulationMethod().model.numberOfRigidBodyParticleObjects();
	for (unsigned int i = 0; i < nObjects; i++)
	{
		FluidModel::RigidBodyParticleObject *rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(i);
		RigidBodyObject *rbo = rbpo->m_rigidBody;
		if (rbo->isDynamic())
		{
			((PBDRigidBody*)rbo)->updateTimeStepSize();
			Vector3r force, torque;
			force.setZero();
			torque.setZero();

			for (int j = 0; j < (int)rbpo->numberOfParticles(); j++)
			{
				force += rbpo->m_f[j];
				torque += (rbpo->m_x[j] - rbo->getPosition()).cross(rbpo->m_f[j]);
				rbpo->m_f[j].setZero();
			}
			rbo->addForce(force);
			rbo->addTorque(torque);
		}
	}
}

void partioExport()
{
	FluidModel &model = base.getSimulationMethod().model;
	std::string exportPath = FileSystem::normalizePath(base.getExePath() + "/PartioExport");
	FileSystem::makeDirs(exportPath);

	std::string fileName = "ParticleData";
	fileName = fileName + std::to_string(frameCounter) + ".bgeo";
	std::string exportFileName = FileSystem::normalizePath(exportPath + "/" + fileName);

	PartioReaderWriter::writeParticles(exportFileName, model.numActiveParticles(), &model.getPosition(0, 0), &model.getVelocity(0, 0), 0.0);
}


///////////////////////////////////////////////////////////////////////////////
// statistics::model::wrap::unary::covariates.hpp                            //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_WRAP_UNARY_COVARIATES_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_WRAP_UNARY_COVARIATES_HPP_ER_2009
#include <boost/statistics/model/wrap/unary/detail/base.hpp>

namespace boost{
namespace statistics{
namespace model{

    template<typename Rx>
    class covariates_wrapper : detail::base_<Rx>{
        typedef detail::base_<Rx> base_;
    public:
        typedef Rx                          covariates_type;
        typedef covariates_wrapper<Rx>      covariates_wrapper_type;

        // Construction
        covariates_wrapper();
        covariates_wrapper(const Rx& x);
        covariates_wrapper(const covariates_wrapper&);

        // Access
        const Rx& covariates()const;
    };

    // Implementation //

    template<typename Rx>
    covariates_wrapper<Rx>::covariates_wrapper() : base_(){}

    template<typename Rx>
    covariates_wrapper<Rx>::covariates_wrapper( const Rx& rx ) : base_(rx){}

    template<typename Rx>
    covariates_wrapper<Rx>::covariates_wrapper(const covariates_wrapper& that)
    :base_(that){}

    template<typename Rx>
    const Rx& covariates_wrapper<Rx>::covariates()const{
        return (this->value);
    }

    // Free functions

    template<typename Rx>
    covariates_wrapper<Rx>
    make_covariates_wrapper(const Rx& rx){
        typedef covariates_wrapper<Rx> result_type;
        return result_type(rx);
    }

}// model
}// statistics
}// boost

#endif


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

#include <boost/hana/ext/boost/mpl/integral_c.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/integral_constant.hpp>

#include <boost/mpl/bool.hpp>
#include <boost/mpl/char.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/long.hpp>
#include <boost/mpl/size_t.hpp>
#include <cstddef>
#include <type_traits>
using namespace boost::hana;
namespace mpl = boost::mpl;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // make sure the datatype is correct
    //////////////////////////////////////////////////////////////////////////
    {
        static_assert(std::is_same<
            datatype_t<mpl::bool_<true>>,
            ext::boost::mpl::IntegralC<bool>
        >::value, "");

        static_assert(std::is_same<
            datatype_t<mpl::int_<0>>,
            ext::boost::mpl::IntegralC<int>
        >::value, "");

        static_assert(std::is_same<
            datatype_t<mpl::long_<0>>,
            ext::boost::mpl::IntegralC<long>
        >::value, "");

        static_assert(std::is_same<
            datatype_t<mpl::size_t<0>>,
            ext::boost::mpl::IntegralC<std::size_t>
        >::value, "");

        static_assert(std::is_same<
            datatype_t<mpl::integral_c<int, 0>>,
            ext::boost::mpl::IntegralC<int>
        >::value, "");

        static_assert(std::is_same<
            datatype_t<mpl::char_<0>>,
            ext::boost::mpl::IntegralC<char>
        >::value, "");
    }

    //////////////////////////////////////////////////////////////////////////
    // interoperation with IntegralConstant
    //////////////////////////////////////////////////////////////////////////
    {
        BOOST_HANA_CONSTANT_CHECK(mpl::integral_c<int, 1>{} == int_<1>);
        BOOST_HANA_CONSTANT_CHECK(mpl::integral_c<int, 1>{} == long_<1>);

        BOOST_HANA_CONSTANT_CHECK(mpl::integral_c<int, 2>{} != int_<3>);
    }
}