/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file geometry_core.hpp
 * @author Ondrej Prochazka <ondrej.prochazka@citationtech.net>
 * @author Vaclav Blazek <vaclav.blazek@citationtech.net>
 *
 * Basic structures for geometric modeling
 */

#ifndef MATH_GEOMETRY_CORE_HPP
#define MATH_GEOMETRY_CORE_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/qi_match.hpp>
#include <boost/spirit/include/qi_match_auto.hpp>
#include <boost/spirit/include/qi_alternative.hpp>

#include <boost/rational.hpp>

#include <vector>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <stdexcept>
#include <limits>

#ifdef MATH_HAS_OPENCV
#include <opencv2/core/core.hpp>
#endif

#include "utility/streams.hpp"

namespace math {

namespace ublas = boost::numeric::ublas;

struct GeometryError : std::runtime_error {
    GeometryError(const std::string &msg) : std::runtime_error(msg) {}
};

struct NoIntersectError : GeometryError {
    NoIntersectError(const std::string &msg) : GeometryError(msg) {}
};

/* sizes */

template <class T>
struct Size2_ {
    typedef T value_type;

    Size2_() : width( 0 ), height( 0 ) {};

    Size2_( const T & width, const T & height )
        : width( width ), height( height ) {};

    T width, height;

    template <typename U>
    explicit Size2_(const Size2_<U> &s)
        : width(s.width), height(s.height)
    {}

    template <typename U>
    explicit Size2_(const Size2_<boost::rational<U> > &s)
        : width(boost::rational_cast<T>(s.width))
        , height(boost::rational_cast<T>(s.height))
    {}

    bool operator== (const Size2_<T>& s) const {
        return width == s.width && height == s.height;
    }
    bool operator!= (const Size2_<T>& s) const {
        return !operator==(s);
    }
};

typedef Size2_<int> Size2i;
typedef Size2_<long long> Size2ll;
typedef Size2_<double> Size2f;
typedef Size2i Size2;
typedef Size2_<boost::rational<long long> > Size2r;

namespace detail {

/** Area type. Must be wide enough to hold result of (width * height).
 *
 *  Defaults to long long (should hold any sane area of integral sizes.
 *
 *  Floating types have identical area type.
 */
template <typename T> struct AreaType { typedef long long type; };
template<> struct AreaType<float> { typedef float type; };
template<> struct AreaType<double> { typedef double type; };
template<> struct AreaType<long double> { typedef long double type; };

} // namespace detail

template <typename T>
typename detail::AreaType<T>::type area(const Size2_<T> &size)
{
    return ((typename detail::AreaType<T>::type)(size.width) * size.height);
}

template <typename T>
bool empty(const Size2_<T> &size)
{
    return (size.width <= 0) || (size.height <= 0);
}

template <typename T> struct Size2SimpleReader { math::Size2_<T> *value; };

/** Used to safely read Size2_<T> in format WxH or A (equivalent to AxA) from
 *  input stream.
 */
template <typename T>
Size2SimpleReader<T> size2SimpleReader(math::Size2_<T> &value);

template <class T>
struct Size3_ {
    typedef T value_type;

    Size3_() : width( 0 ), height( 0 ), depth( 0 ) {};

    Size3_( const T & width, const T & height, const T & depth)
        : width( width ), height( height ), depth(depth) {};

    T width, height, depth;

    template <typename U>
    explicit Size3_(const Size3_<U> &s)
        : width(s.width), height(s.height), depth(s.depth)
    {}

    template <typename U>
    explicit Size3_(const Size3_<boost::rational<U> > &s)
        : width(boost::rational_cast<T>(s.width))
        , height(boost::rational_cast<T>(s.height))
        , depth(boost::rational_cast<T>(s.depth))
    {}

    bool operator== (const Size3_<T>& s) const {
        return width == s.width && height == s.height && depth == s.depth;
    }
    bool operator!= (const Size3_<T>& s) const {
        return !operator==(s);
    }

    T & operator()(int idx) {
        switch(idx) {
            case 0 : return width;
            case 1 : return height;
            case 2 : return depth;
            default :
                throw GeometryError("Bad index to Size3.");
        }
    }

    const T & operator()(int idx) const {
        switch(idx) {
            case 0 : return width;
            case 1 : return height;
            case 2 : return depth;
            default :
                throw GeometryError("Bad index to Size3.");
        }
    }
};

typedef Size3_<int> Size3i;
typedef Size3_<long long> Size3ll;
typedef Size3_<double> Size3f;
typedef Size3i Size3;
typedef Size3_<boost::rational<long long> > Size3r;

template <typename T> struct Size3SimpleReader { math::Size3_<T> *value; };

/** Used to safely read Size3_<T> in format WxHxD or WxA (equivalent WxAxA) or A
 *  (equivalent to AxAxA) from input stream.
 */
template <typename T>
Size3SimpleReader<T> size3SimpleReader(math::Size3_<T> &value);

/* viewports */

template <typename T>
struct Viewport2_ {
    typedef T value_type;
    typedef Size2_<T> size_type;

    value_type width;
    value_type height;
    value_type x;
    value_type y;

    Viewport2_() : width(1000), height(1000), x(0), y(0) {}

    Viewport2_(value_type width, value_type height
               , value_type x = 0, value_type y = 0)
        : width(width), height(height), x(x), y(y) {};

    Viewport2_(const size_type &size, value_type x = 0, value_type y = 0)
        : width( size.width ), height( size.height ), x( x ), y( y ) {};

    size_type size() const { return math::Size2i(width, height); }

    template <typename U>
    explicit Viewport2_(const Viewport2_<U> &v)
        : width(v.width), height(v.height), x(v.x), y(v.y)
    {}
};

typedef Viewport2_<int> Viewport2i;
typedef Viewport2_<double> Viewport2f;
typedef Viewport2i Viewport2;

template <class T> class Point2_;

template <typename T1, typename T2>
inline bool inside(const Viewport2_<T1> &v, const Point2_<T2> &p)
{
    return ((p(0) >= v.x) && (p(0) <= (v.x + v.width))
            && (p(1) >= v.y) && (p(1) <= (v.y + v.height)));
}

template <typename T1, typename T2>
inline bool inside(const Viewport2_<T1> &v, T2 x, T2 y)
{
    return ((x >= v.x) && (x <= (v.x + v.width))
            && (y >= v.y) && (y <= (v.y + v.height)));
}

/* points and point vectors */

template <class T>
class Point2_ : public ublas::fixed_vector<T, 2>  {
    using Base = ublas::fixed_vector<T, 2>;
public:
    Point2_( const T & x = 0.0, const T & y = 0.0 )
        : Base() {
        (*this)(0) = x; (*this)(1) = y;
    }

    template <class AE>
    Point2_( const ublas::vector_expression<AE> & op )
        : Base() {
        ublas::vector_assign<ublas::scalar_assign>(*this, op );
    }

    Point2_( const ublas::vector<T> & op )
        : Base() {
        ublas::vector_assign<ublas::scalar_assign>( *this, op );
    }

#ifdef MATH_HAS_OPENCV
    Point2_( const cv::Point_<T> & op )
        : Base() {
        (*this)(0) = op.x; (*this)(1) = op.y;
    }

    Point2_(const cv::Vec<T,2>& op)
        : Base() {
        (*this)(0) = op[0]; (*this)(1) = op[1];
    }
#endif

#if 0
    // quite dangerous, imho, should be phased out, use euclidian instead
    Point2_( const cv::Point3_<T> & op ) {
        (*this)(0) = op.x / op.z; (*this)(1) = op.y / op.z;
    }
#endif

    template <typename U>
    explicit Point2_(const Point2_<U> &op)
        : Base()
    {
        (*this)(0) = op(0); (*this)(1) = op(1);
    }

    bool operator== (const Point2_<T>& p) const {
        return (*this)(0) == p(0) && (*this)(1) == p(1);
    }
    bool operator!= (const Point2_<T>& p) const {
        return !operator==(p);
    }
};

template <class T>
class Point3_ : public ublas::fixed_vector<T, 3> {
    using Base = ublas::fixed_vector<T, 3>;
public:
    Point3_( const T & x = 0.0, const T & y = 0.0, const T & z = 0.0 )
        : Base() {
        (*this)(0) = x; (*this)(1) = y; (*this)(2) = z;
    }

    template <class AE>
    Point3_( const ublas::vector_expression<AE> & op )
        : Base() {
        ublas::vector_assign<ublas::scalar_assign>( *this, op );
    }

    Point3_( const ublas::vector<T> & op )
        : Base() {
        ublas::vector_assign<ublas::scalar_assign>( *this, op );
    }

#ifdef MATH_HAS_OPENCV
    // convieniece for reading points from 3-channel OpenCV matrices
    Point3_(const cv::Vec<T,3>& op)
        : Base() {
        (*this)(0) = op[0]; (*this)(1) = op[1]; (*this)(2) = op[2];
    }

    Point3_(const cv::Point3_<T>& op)
        : Base() {
        (*this)(0) = op.x; (*this)(1) = op.y; (*this)(2) = op.z;
    }
#endif

    Point3_(const Point3_<T> &) = default;
    Point3_ & operator=(const Point3_<T> &) = default;

    // make point movable
    Point3_( Point3_<T> && op )
        : Base() {
        ublas::vector_assign<ublas::scalar_assign>( *this, op );
    }

    Point3_ & operator=(Point3_<T> &&op) {
        ublas::vector_assign<ublas::scalar_assign>( *this, op );
        return *this;
    }

    template <typename U>
    explicit Point3_(const Point3_<U> &op)
        : Base()
    {
        (*this)(0) = op(0); (*this)(1) = op(1); (*this)(2) = op(2);
    }

    bool operator== (const Point3_<T>& p) const {
        return (*this)(0) == p(0) && (*this)(1) == p(1) && (*this)(2) == p(2);
    }
    bool operator!= (const Point3_<T>& p) const {
        return !operator==(p);
    }
};

template <class T>
class Point4_ : public ublas::fixed_vector<T, 4> {
    using Base = ublas::fixed_vector<T, 4>;
public:
    Point4_( const T & x = 0.0, const T & y = 0.0
           , const T & z = 0.0, const T & w = 0.0 )
        : Base() {
        (*this)(0) = x; (*this)(1) = y; (*this)(2) = z; (*this)(3) = w;
    }

    template <class AE>
    Point4_( const ublas::vector_expression<AE> & op )
        : Base() {
        ublas::vector_assign<ublas::scalar_assign>( *this, op );
    }

    Point4_( const ublas::vector<T> & op )
        : Base() {
        ublas::vector_assign<ublas::scalar_assign>( *this, op );
    }

    bool operator== (const Point4_<T>& p) const {
        return (*this)(0) == p(0) && (*this)(1) == p(1) &&
               (*this)(2) == p(2) && (*this)(3) == p(3);
    }
    bool operator!= (const Point4_<T>& p) const {
        return !operator==(p);
    }
};

typedef Point2_<int> Point2i;
typedef Point2_<long long> Point2ll;
typedef Point2_<float> Point2f;
typedef Point2_<double> Point2d;
typedef Point2d Point2;

typedef Point3_<int> Point3i;
typedef Point3_<long long> Point3ll;
typedef Point3_<float> Point3f;
typedef Point3_<double> Point3d;
typedef Point3d Point3;

typedef Point4_<int> Point4i;
typedef Point4_<long long> Point4ll;
typedef Point4_<float> Point4f;
typedef Point4_<double> Point4d;
typedef Point4d Point4;

typedef std::vector<Point2_<int> > Points2i;
typedef std::vector<Point2_<long long> > Points2ll;
typedef std::vector<Point2_<float> > Points2f;
typedef std::vector<Point2_<double> > Points2d;
typedef std::vector<Point2> Points2;

typedef std::vector<Point3_<int> > Points3i;
typedef std::vector<Point3_<long long> > Points3ll;
typedef std::vector<Point3_<float> > Points3f;
typedef std::vector<Point3_<double> > Points3d;
typedef std::vector<Point3> Points3;

typedef ublas::matrix<double,ublas::row_major,
                      ublas::bounded_array<double, 4> > Matrix2;
typedef ublas::matrix<double,ublas::row_major,
                      ublas::bounded_array<double, 9> > Matrix3;
typedef ublas::matrix<double,ublas::row_major,
                      ublas::bounded_array<double, 16> > Matrix4;

// handy point->size conversion
template<typename T>
inline Size2_<T> size(const Point2_<T> &p) {
    return { p(0), p(1) };
}

// handy size->point conversion
template<typename T>
inline Point2_<T> point(const Size2_<T> &s) {
    return { s.width, s.height };
}

// handy point->size conversion for point3
template<typename T>
inline Size3_<T> size(const Point3_<T> &p) {
    return { p(0), p(1), p(2) };
}

// handy size->point conversion
template<typename T>
inline Point3_<T> point(const Size3_<T> &s) {
    return { s.width, s.height, s.depth };
}

/** Helper tag struct for invalid value extents instantiation.
 */
struct InvalidExtents {};

/** Helper tag struct for creating infinite extents
 */
struct InfiniteExtents {};

/** Helper tag struct for inclusive size computation:
 *
 *  size(Extents<integral>(x, y, x, y)) is (0, 0)
 *  size(Extents<integral>(x, y, x, y), Inclusive{}) is (1, 1)
 */
struct Inclusive {};

template <typename T>
struct Extents2_ {
    typedef T value_type;
    typedef Point2_<T> point_type;

    point_type ll;
    point_type ur;

    Extents2_() : ll(), ur() {}

    explicit Extents2_(InvalidExtents)
        : ll(std::numeric_limits<T>::max()
             , std::numeric_limits<T>::max())
        , ur(std::numeric_limits<T>::lowest()
             , std::numeric_limits<T>::lowest())
    {}

    explicit Extents2_(InfiniteExtents)
        : ll(std::numeric_limits<T>::lowest()
             , std::numeric_limits<T>::lowest())
        , ur(std::numeric_limits<T>::max()
             , std::numeric_limits<T>::max())
    {}

    explicit Extents2_(const point_type &p)
        : ll(p), ur(p)
    {}

    Extents2_(const point_type &ll, const point_type &ur)
        : ll(ll), ur(ur)
    {}

    Extents2_(const value_type &xll, const value_type &yll
              , const value_type &xur, const value_type &yur)
        : ll(xll, yll), ur(xur, yur)
    {}

    template <typename U>
    explicit Extents2_(const Extents2_<U> &e)
        : ll(e.ll), ur(e.ur)
    {}

    Extents2_<T>& operator=(InvalidExtents) {
        ll(0) = std::numeric_limits<T>::max();
        ll(1) = std::numeric_limits<T>::max();
        ur(0) = std::numeric_limits<T>::lowest();
        ur(1) = std::numeric_limits<T>::lowest();
        return *this;
    }

    T area() const {
        if ( ur[1] < ll[1] || ur[0] < ll[0] ) return 0;
        return ( ur[1] - ll[1] ) * ( ur[0] - ll[0] ); }

    T size() const {
        return std::max(ur[0] - ll[0], ur[1] - ll[1]);
    }
};

typedef Extents2_<int> Extents2i;
typedef Extents2_<long long> Extents2ll;
typedef Extents2_<double> Extents2f;
typedef Extents2f Extents2;

template <typename T1, typename T2>
inline bool inside(const Extents2_<T1> &e, const T2 &p0, const T2 &p1)
{
    return ((p0 >= e.ll(0)) && (p0 <= e.ur(0))
            && (p1 >= e.ll(1)) && (p1 <= e.ur(1)));
}

template <typename T1, typename T2>
inline bool inside(const Extents2_<T1> &e, const Point2_<T2> &p)
{
    return ((p(0) >= e.ll(0)) && (p(0) <= e.ur(0))
            && (p(1) >= e.ll(1)) && (p(1) <= e.ur(1)));
}

/** Checks only for x and y components of Point3
 */
template <typename T1, typename T2>
inline bool inside(const Extents2_<T1> &e, const Point3_<T2> &p)
{
    return ((p(0) >= e.ll(0)) && (p(0) <= e.ur(0))
            && (p(1) >= e.ll(1)) && (p(1) <= e.ur(1)));
}

template <typename T1, typename T2>
inline Extents2_<T1> operator+(const Extents2_<T1> &e, const T2 &diff)
{
    return Extents2_<T1>
        (e.ll(0) - diff, e.ll(1) - diff
         , e.ur(0) + diff, e.ur(1) + diff);
}

template <typename T1, typename T2>
inline Extents2_<T1> operator+(const Extents2_<T1> &e, const Size2_<T2> &diff)
{
    return Extents2_<T1>
        (e.ll(0) - diff.width, e.ll(1) - diff.height
         , e.ur(0) + diff.width, e.ur(1) + diff.height);
}

template <typename T>
Viewport2_<T> viewport(const Extents2_<T> &e)
{
    return Viewport2_<T>(e.ur(0) - e.ll(0), e.ur(1) - e.ll(1)
                         , e.ll(0), e.ll(1));
}

template <typename T>
Extents2_<T> extents(const Viewport2_<T> &v)
{
    return Extents2_<T>(v.x, v.y, v.x + v.width, v.y + v.height);
}

template <typename T>
const typename Extents2_<T>::point_type& ll(const Extents2_<T> &e) {
    return e.ll;
}

template <typename T>
const typename Extents2_<T>::point_type& ur(const Extents2_<T> &e) {
    return e.ur;
}

template <typename T>
typename Extents2_<T>::point_type ul(const Extents2_<T> &e) {
    return typename Extents2_<T>::point_type(e.ll(0), e.ur(1));
}

template <typename T>
typename Extents2_<T>::point_type lr(const Extents2_<T> &e) {
    return typename Extents2_<T>::point_type(e.ur(0), e.ll(1));
}

template<typename T>
inline Point2_<T> clip(const Extents2_<T> &e, const Point2_<T> &p) {
    return Point2_<T>(std::max(e.ll(0), std::min(e.ur(0), p(0)))
                      , std::max(e.ll(1), std::min(e.ur(1), p(1))));
}

template<typename T>
inline Point2_<T> center(const Extents2_<T> &e) {
    return (e.ur + e.ll) / 2;
}

template<typename T>
inline Size2_<T> size(const Extents2_<T> &e) {
    return Size2_<T>(e.ur(0) - e.ll(0), e.ur(1) - e.ll(1));
}

template<typename T>
inline Size2_<T> size(const Extents2_<T> &e, const Inclusive&) {
    return Size2_<T>(e.ur(0) - e.ll(0) + T(1), e.ur(1) - e.ll(1) + T(1));
}

template<typename T>
inline T area(const Extents2_<T> &e) {
    return ((e.ur[1] < e.ll[1]) || (e.ur[0] < e.ll[0]))
        ? 0
        : (e.ur[1] - e.ll[1]) * (e.ur[0] - e.ll[0]);
}

template<typename T>
inline bool empty(const Extents2_<T> &e) {
    return !area(e);
}

template<typename T>
inline bool valid(const Extents2_<T> &e) {
    return (e.ll(0) <= e.ur(0)) && (e.ll(1) <= e.ur(1));
}

template <typename T>
inline Extents2_<T> unite( const Extents2_<T> &a, const Extents2_<T> &b ) {

    return Extents2_<T>(
        typename Extents2_<T>::point_type (
            std::min( a.ll[0], b.ll[0] ),
            std::min( a.ll[1], b.ll[1] ) ),
        typename Extents2_<T>::point_type (
            std::max( a.ur[0], b.ur[0] ),
            std::max( a.ur[1], b.ur[1] ) ) );
}

template <typename T1, typename T2>
inline bool overlaps(const Extents2_<T1> &a, const Extents2_<T2> &b)
{
    return ((a.ll(0) < b.ur(0)) && (b.ll(0) < a.ur(0))
             && (a.ll(1) < b.ur(1)) && (b.ll(1) < a.ur(1)));
}

template <typename T>
inline Extents2_<T> intersect( const Extents2_<T> &a, const Extents2_<T> &b ) {
    if (!overlaps(a, b)) {
        throw NoIntersectError
            ("Extents do not overlap, cannot compute intersection");
    }

    return Extents2_<T>(
        typename Extents2_<T>::point_type (
            std::max( a.ll[0], b.ll[0] ),
            std::max( a.ll[1], b.ll[1] ) ),
        typename Extents2_<T>::point_type (
            std::min( a.ur[0], b.ur[0] ),
            std::min( a.ur[1], b.ur[1] ) ) );
}

template <typename T>
inline double overlap( const Extents2_<T> & a, const Extents2_<T> & b ) {
    try {
        return ( intersect( a, b ).area() / unite( a, b ).area() );
    } catch (const NoIntersectError &) {
        return .0;
    }
}

template <typename T>
inline void update(Extents2_<T> &e, const T &x, const T &y) {
    e.ll(0) = std::min(e.ll(0), x);
    e.ll(1) = std::min(e.ll(1), y);

    e.ur(0) = std::max(e.ur(0), x);
    e.ur(1) = std::max(e.ur(1), y);
}

template <typename T>
inline void update(Extents2_<T> &e, const Point2_<T> &p) {
    e.ll(0) = std::min(e.ll(0), p(0));
    e.ll(1) = std::min(e.ll(1), p(1));

    e.ur(0) = std::max(e.ur(0), p(0));
    e.ur(1) = std::max(e.ur(1), p(1));
}

template <typename T>
inline void update(Extents2_<T> &e, const Point3_<T> &p) {
    e.ll(0) = std::min(e.ll(0), p(0));
    e.ll(1) = std::min(e.ll(1), p(1));

    e.ur(0) = std::max(e.ur(0), p(0));
    e.ur(1) = std::max(e.ur(1), p(1));
}

template <typename T>
inline void update(Extents2_<T> &e, const Extents2_<T> &other) {
    update(e, other.ll);
    update(e, other.ur);
}

template <typename P, typename R, typename T>
inline P snapToGrid(const P & point, const P & origin, T step, R roundFcn) {
    P res;

    for (std::size_t i(0); i < res.size(); ++i) {
        res(i) = roundFcn((point(i) - origin(i))/step) * step + origin(i);
    }

    return res;
}

template <typename E, typename T>
inline E snapToGrid( const E &ext, const typename E::point_type &origin
                   , T step, bool inscribe = false)
{
    typedef typename E::value_type VT;
    if (inscribe) {
        return { snapToGrid(ext.ll, origin, step
                            , [](VT value) { return std::ceil(value); })
                , snapToGrid(ext.ur, origin, step
                             , [](VT value) { return std::floor(value); }) };
    }

    return { snapToGrid(ext.ll, origin, step
                        , [](VT value) { return std::floor(value); })
            , snapToGrid(ext.ur, origin, step
                         , [](VT value) { return std::ceil(value); }) };
}

template <typename T>
struct Extents3_ {
    typedef T value_type;
    typedef Point3_<T> point_type;

    point_type ll;
    point_type ur;

    Extents3_() : ll(), ur() {}

    explicit Extents3_(InvalidExtents)
        : ll(std::numeric_limits<T>::max(), std::numeric_limits<T>::max()
             , std::numeric_limits<T>::max())
        , ur(std::numeric_limits<T>::lowest(), std::numeric_limits<T>::lowest()
             , std::numeric_limits<T>::lowest())
    {}

    explicit Extents3_(const point_type &p)
        : ll(p), ur(p)
    {}

    Extents3_(const point_type &ll, const point_type &ur)
        : ll(ll), ur(ur)
    {}

    Extents3_(const value_type &xll, const value_type &yll
              , const value_type &zll
              , const value_type &xur, const value_type yur
              , const value_type &zur)
        : ll(xll, yll, zll), ur(xur, yur, zur)
    {}

    template <typename U>
    explicit Extents3_(const Extents3_<U> &e)
        : ll(e.ll), ur(e.ur)
    {}

    Extents3_<T>& operator=(InvalidExtents) {
        ll(0) = std::numeric_limits<T>::max();
        ll(1) = std::numeric_limits<T>::max();
        ll(2) = std::numeric_limits<T>::max();
        ur(0) = std::numeric_limits<T>::lowest();
        ur(1) = std::numeric_limits<T>::lowest();
        ur(2) = std::numeric_limits<T>::lowest();
        return *this;
    }
};

typedef Extents3_<int> Extents3i;
typedef Extents3_<long long> Extents3ll;
typedef Extents3_<double> Extents3f;
typedef Extents3f Extents3;

template<typename T>
inline Point3_<T> center(const Extents3_<T> &e) {
    return (e.ur + e.ll) / 2;
}

template<typename T>
inline Point3_<T> clip(const Extents3_<T> &e, const Point3_<T> &p) {
    return Point3_<T>(std::max(e.ll(0), std::min(e.ur(0), p(0)))
                      , std::max(e.ll(1), std::min(e.ur(1), p(1)))
                      , std::max(e.ll(2), std::min(e.ur(2), p(2))));
}

template<typename T>
inline Size3_<T> size(const Extents3_<T> &e) {
    return Size3_<T>(e.ur(0) - e.ll(0), e.ur(1) - e.ll(1), e.ur(2) - e.ll(2));
}

template<typename T>
inline Size3_<T> size(const Extents3_<T> &e, const Inclusive&) {
    return Size3_<T>(e.ur(0) - e.ll(0) + T(1)
                     , e.ur(1) - e.ll(1) + T(1)
                     , e.ur(2) - e.ll(2) + T(1));
}

template<typename T>
inline Size2_<T> size2(const Extents3_<T> &e) {
    return Size2_<T>(e.ur(0) - e.ll(0), e.ur(1) - e.ll(1));
}

template<typename T>
inline Size2_<T> size2(const Extents3_<T> &e, const Inclusive&) {
    return Size2_<T>(e.ur(0) - e.ll(0) + T(1)
                     , e.ur(1) - e.ll(1) + T(1));
}

template<typename T>
inline T volume(const Extents3_<T> &e) {
    return ((e.ur[2] < e.ll[2]) || (e.ur[1] < e.ll[1]) || (e.ur[0] < e.ll[0]))
        ? 0
        : (e.ur[2] - e.ll[2]) * (e.ur[1] - e.ll[1]) * (e.ur[0] - e.ll[0]);
}

template<typename T>
inline bool empty(const Extents3_<T> &e) {
    return !volume(e);
}

template<typename T>
inline bool valid(const Extents3_<T> &e) {
    return ((e.ll(0) <= e.ur(0))
            && (e.ll(1) <= e.ur(1))
            && (e.ll(2) <= e.ur(2)));
}

template <typename T>
inline void update(Extents3_<T> &e, const Point3_<T> &p) {
    e.ll(0) = std::min(e.ll(0), p(0));
    e.ll(1) = std::min(e.ll(1), p(1));
    e.ll(2) = std::min(e.ll(2), p(2));

    e.ur(0) = std::max(e.ur(0), p(0));
    e.ur(1) = std::max(e.ur(1), p(1));
    e.ur(2) = std::max(e.ur(2), p(2));
}

template <typename T>
inline void update(Extents3_<T> &e, const Extents3_<T> &other) {
    update(e, other.ll);
    update(e, other.ur);
}

template <typename T>
inline Extents3_<T> unite(const Extents3_<T> &a, const Extents3_<T> &b ) {
    Extents3_<T> res(a);
    update(res, b.ll);
    update(res, b.ur);
    return res;
}

template <typename T1, typename T3>
inline bool overlaps(const Extents3_<T1> &a, const Extents3_<T3> &b)
{
    return ((a.ll(0) < b.ur(0)) && (b.ll(0) < a.ur(0))
             && (a.ll(1) < b.ur(1)) && (b.ll(1) < a.ur(1))
             && (a.ll(2) < b.ur(2)) && (b.ll(2) < a.ur(2)));
}

template <typename T>
inline Extents3_<T> intersect( const Extents3_<T> &a, const Extents3_<T> &b ) {
    if (!overlaps(a, b)) {
        throw NoIntersectError
            ("Extents do not overlap, cannot compute intersection");
    }

    typedef Extents3_<T> E3;
    typedef typename E3::point_type point_type;
    return Extents3(point_type(std::max(a.ll[0], b.ll[0])
                               , std::max(a.ll[1], b.ll[1])
                               , std::max(a.ll[2], b.ll[2]))
                    , point_type(std::min(a.ur[0], b.ur[0])
                                 , std::min(a.ur[1], b.ur[1])
                                 , std::min(a.ur[2], b.ur[2])));
}

template <typename T>
inline double overlap(const Extents3_<T> & a, const Extents3_<T> & b) {
    try {
        return (volume(intersect(a, b)) / volume(unite(a, b)));
    } catch (const NoIntersectError&) {
        return .0;
    }
}

template<typename CharT, typename Traits, typename T>
inline std::basic_ostream<CharT, Traits>&
operator<<(std::basic_ostream<CharT, Traits> &os, const Viewport2_<T> &v)
{
    std::ios::fmtflags flags(os.flags());
    os << v.width << "x" << v.height << std::showpos << v.x << v.y;
    os.flags(flags);
    return os;
}

template<typename CharT, typename Traits, typename T>
inline std::basic_istream<CharT, Traits>&
operator>>(std::basic_istream<CharT, Traits> &is, Viewport2_<T> &v)
{
    using boost::spirit::qi::auto_;
    using boost::spirit::qi::char_;
    using boost::spirit::qi::omit;
    using boost::spirit::qi::match;

    char sign1, sign2;

    is >> match((auto_ >> omit['x'] >> auto_
                 >> (char_('+') | char_('-')) >> auto_
                 >> (char_('+') | char_('-')) >> auto_)
                , v.width, v.height, sign1, v.x, sign2, v.y);

    if (sign1 == '-') { v.x = -v.x; }
    if (sign2 == '-') { v.y = -v.y; }
    return is;
}

#if 0
// original unsafe code
template <class UblasContainer>
inline bool operator < (
    const UblasContainer & op1, const UblasContainer & op2 ) {

    return std::lexicographical_compare(
                op1.data().begin(), op1.data().end(),
                op2.data().begin(), op2.data().end() );
}

#else

inline bool operator < (
    const Matrix4 & op1, const Matrix4 & op2 ) {
    return std::lexicographical_compare(
                op1.data().begin(), op1.data().end(),
                op2.data().begin(), op2.data().end() );
}

template <typename T1, typename T2>
inline bool operator < (
    const Point3_<T1> & op1, const Point3_<T2> & op2 ) {

    return std::lexicographical_compare(
                op1.data().begin(), op1.data().end(),
                op2.data().begin(), op2.data().end() );
}

template <typename T1, typename T2>
inline bool operator < (
    const Point2_<T1> & op1, const Point2_<T2> & op2 ) {

    return std::lexicographical_compare(
                op1.data().begin(), op1.data().end(),
                op2.data().begin(), op2.data().end() );
}

#endif

template<typename CharT, typename Traits, typename T>
inline std::basic_ostream<CharT, Traits>&
operator<<(std::basic_ostream<CharT, Traits> &os, const Extents2_<T> &e)
{
    os << e.ll(0) << ',' << e.ll(1) << ':' << e.ur(0) << ',' << e.ur(1);
    return os;
}

template<typename CharT, typename Traits, typename T>
inline std::basic_istream<CharT, Traits>&
operator>>(std::basic_istream<CharT, Traits> &is, Extents2_<T> &e)
{
    using boost::spirit::qi::auto_;
    using boost::spirit::qi::char_;
    using boost::spirit::qi::omit;
    using boost::spirit::qi::match;

    is >> match((auto_ >> omit[','] >> auto_ >> omit[':']
                 >> auto_ >> omit[','] >> auto_)
                , e.ll(0), e.ll(1), e.ur(0), e.ur(1));

    return is;
}

template<typename CharT, typename Traits, typename T>
inline std::basic_ostream<CharT, Traits>&
operator<<(std::basic_ostream<CharT, Traits> &os, const Extents3_<T> &e)
{
    os << e.ll(0) << ',' << e.ll(1) << ',' << e.ll(2) << ':'
       << e.ur(0) << ',' << e.ur(1) << ',' << e.ur(2);
    return os;
}

template<typename CharT, typename Traits, typename T>
inline std::basic_istream<CharT, Traits>&
operator>>(std::basic_istream<CharT, Traits> &is, Extents3_<T> &e)
{
    using boost::spirit::qi::auto_;
    using boost::spirit::qi::char_;
    using boost::spirit::qi::omit;
    using boost::spirit::qi::match;

    is >> match((auto_ >> omit[','] >> auto_ >> omit[','] >> auto_ >> omit[':']
                 >> auto_ >> omit[','] >> auto_ >> omit[','] >> auto_)
                , e.ll(0), e.ll(1), e.ll(2), e.ur(0), e.ur(1), e.ur(2));

    return is;
}

template <typename T1, typename T2>
inline bool inside(const Extents3_<T1> &e, const T2 &p0, const T2 &p1
                   , const T2 &p2)
{
    return ((p0 >= e.ll(0)) && (p0 <= e.ur(0))
            && (p1 >= e.ll(1)) && (p1 <= e.ur(1))
            && (p2 >= e.ll(2)) && (p2 <= e.ur(2)));
}

template <typename T1, typename T2>
inline bool inside(const Extents3_<T1> &e, const Point3_<T2> &p)
{
    return ((p(0) >= e.ll(0)) && (p(0) <= e.ur(0))
            && (p(1) >= e.ll(1)) && (p(1) <= e.ur(1))
            && (p(2) >= e.ll(2)) && (p(2) <= e.ur(2)));
}

template <typename T1, typename T2>
inline Extents3_<T1> operator+(const Extents3_<T1> &e, const T2 &diff)
{
    return Extents3_<T1>
        (e.ll(0) - diff, e.ll(1) - diff, e.ll(2) - diff
        , e.ur(0) + diff, e.ur(1) + diff, e.ur(2) + diff);
}

template <typename T1, typename T2>
inline Extents3_<T1> operator+(const Extents3_<T1> &e, const Size3_<T2> &diff)
{
    return Extents3_<T1>
        (e.ll(0) - diff.width, e.ll(1) - diff.height
         , e.ll(2) - diff.depth
         , e.ur(0) + diff.width, e.ur(1) + diff.height
         , e.ur(2) + diff.depth);
}

template <typename T>
const typename Extents3_<T>::point_type& bll(const Extents3_<T> &e) {
    return e.ll;
}

template <typename T>
typename Extents3_<T>::point_type bul(const Extents3_<T> &e) {
    return typename Extents3_<T>::point_type(e.ll(0), e.ur(1), e.ll(2));
}

template <typename T>
typename Extents3_<T>::point_type bur(const Extents3_<T> &e) {
    return typename Extents3_<T>::point_type(e.ur(0), e.ur(1), e.ll(2));
}

template <typename T>
typename Extents3_<T>::point_type blr(const Extents3_<T> &e) {
    return typename Extents3_<T>::point_type(e.ur(0), e.ll(1), e.ll(2));
}

template <typename T>
const typename Extents3_<T>::point_type tll(const Extents3_<T> &e) {
    return typename Extents3_<T>::point_type(e.ll(0), e.ll(1), e.ur(2));
}

template <typename T>
typename Extents3_<T>::point_type tul(const Extents3_<T> &e) {
    return typename Extents3_<T>::point_type(e.ll(0), e.ur(1), e.ur(2));
}

template <typename T>
const typename Extents3_<T>::point_type& tur(const Extents3_<T> &e) {
    return e.ur;
}

template <typename T>
typename Extents3_<T>::point_type tlr(const Extents3_<T> &e) {
    return typename Extents3_<T>::point_type(e.ur(0), e.ll(1), e.ur(2));
}

/** Get all 4 vertices from Extents2 as a vector of points.
 */
template <typename T>
const std::vector<typename Extents2_<T>::point_type>
vertices(const Extents2_<T> &e)
{
    return { ll(e), ul(e), ur(e), lr(e) };
}

/** Get all 8 vertices from Extents3 as a vector of points.
 */
template <typename T>
const std::vector<typename Extents3_<T>::point_type>
vertices(const Extents3_<T> &e)
{
    return { bll(e), bul(e), bur(e), blr(e), tll(e), tul(e), tur(e), tlr(e) };
}

/** Helper functions to convert between 2d and 3d entities
 */

// Size2_ <-> Size3_

template <typename T>
Size2_<T> size2(const Size3_<T> &s)
{
    return Size2_<T>(s.width, s.height);
}

template <typename T>
const Size2_<T>& size2(const Size2_<T> &s) { return s; }

template <typename T>
Size3_<T> size3(const Size2_<T> &s)
{
    return Size3_<T>(s.width, s.height, T(0));
}

template <typename T>
const Size3_<T>& size3(const Size3_<T> &s) { return s; }

// Extents2_ <-> Extents3_

template <typename T>
Extents2_<T> extents2(const Extents3_<T> &e)
{
    return Extents2_<T>(e.ll(0), e.ll(1), e.ur(0), e.ur(1));
}

template <typename T>
const Extents2_<T>& extents2(const Extents2_<T> &e) { return e; }

template <typename T>
Extents3_<T> extents3(const Extents2_<T> &e)
{
    return Extents3_<T>(e.ll(0), e.ll(1), T(0), e.ur(0), e.ur(1), T(0));
}

template <typename T>
const Extents3_<T>& extents3(const Extents3_<T> &e) { return e; }

// Point2_ <-> Point3_

template <typename T>
Point2_<T> point2(const Point3_<T> &p)
{
    return Point2_<T>(p(0), p(1));
}

template <typename T>
const Point2_<T>& point2(const Point2_<T> &p) { return p; }

template <typename T>
Point3_<T> point3(const Point2_<T> &p)
{
    return Point3_<T>(p(0), p(1), T(0));
}

template <typename T>
const Point3_<T>& point3(const Point3_<T> &p) { return p; }

#define MATH_GEOMETRY_CORE_HPP_INLINES_
#include "detail/point.inline.hpp"
#include "detail/size2.inline.hpp"
#include "detail/size3.inline.hpp"
#include "detail/extents2.inline.hpp"
#undef MATH_GEOMETRY_CORE_HPP_INLINES_

} // namespace math

#endif // MATH_GEOMETRY_CORE_HPP


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_PairProductionPhotoatomicReaction.hpp
//! \author Alex Robinson
//! \brief  The pair production photoatomic reaction class decl.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_PAIR_PRODUCTION_PHOTOATOMIC_REACTION_HPP
#define MONTE_CARLO_PAIR_PRODUCTION_PHOTOATOMIC_REACTION_HPP

// Boost Includes
#include <boost/function.hpp>

// FRENSIE Includes
#include "MonteCarlo_StandardReactionBaseImpl.hpp"
#include "MonteCarlo_PhotoatomicReaction.hpp"
#include "Utility_InterpolatedFullyTabularBasicBivariateDistribution.hpp"

namespace MonteCarlo{

//! The pair production photoatomic reaction class
template<typename InterpPolicy, bool processed_cross_section = true>
class PairProductionPhotoatomicReaction : public StandardReactionBaseImpl<PhotoatomicReaction,InterpPolicy,processed_cross_section>
{
  // Typedef for the base class type
  typedef StandardReactionBaseImpl<PhotoatomicReaction,InterpPolicy,processed_cross_section> BaseType;

public:

  //! Basic constructor
  PairProductionPhotoatomicReaction(
       const std::shared_ptr<const std::vector<double> >& incoming_energy_grid,
       const std::shared_ptr<const std::vector<double> >& cross_section,
       const size_t threshold_energy_index,
       const bool use_detailed_electron_emission_physics = true );

  //! Constructor
  PairProductionPhotoatomicReaction(
    const std::shared_ptr<const std::vector<double> >& incoming_energy_grid,
    const std::shared_ptr<const std::vector<double> >& cross_section,
    const size_t threshold_energy_index,
    const std::shared_ptr<const Utility::HashBasedGridSearcher<double> >&
    grid_searcher,
    const bool use_detailed_electron_emission_physics = true );

  //! Destructor
  ~PairProductionPhotoatomicReaction()
  { /* ... */ }

  //! Return the number of photons emitted from the rxn at the given energy
  unsigned getNumberOfEmittedPhotons( const double energy ) const override;

  //! Return the number of electrons emitted from the rxn at the given energy
  unsigned getNumberOfEmittedElectrons( const double energy ) const override;

  //! Return the number of positrons emitted from the rxn at the given energy
  unsigned getNumberOfEmittedPositrons( const double energy ) const override;

  //! Return the reaction type
  PhotoatomicReactionType getReactionType() const override;

  //! Simulate the reaction
  void react( PhotonState& photon,
	      ParticleBank& bank,
	      Data::SubshellType& shell_of_interaction ) const override;

protected:

  //! The basic pair production model
  static void basicInteraction( PhotonState& photon,
                                ParticleBank& bank );

  //! The detailed pair production model
  static void detailedInteraction( PhotonState& photon,
                                   ParticleBank& bank );

private:

  // Sample the polar angle of the emitted electron/positron
  static double sampleEmittedPolarAngle( const double energy );

  // Create the secondary energy distribution
  static Utility::FullyTabularBasicBivariateDistribution*
  initializeSecondaryEnergyDistribution();

  // Initialize interaction models
  void initializeInteractionModels(
                           const bool use_detailed_electron_emission_physics );

  // The pair production secondary energy distribution (in a ratio form)
  static std::unique_ptr<const Utility::FullyTabularBasicBivariateDistribution>
  s_secondary_energy_distribution;

  // Check if a detailed model is being used
  bool d_detailed_electron_emission_model;

  // The pair production model
  boost::function<void (PhotonState&,ParticleBank&)> d_interaction_model;

  // pair production secondary energy distribution (in a ratio form)
  std::shared_ptr<Utility::FullyTabularBasicBivariateDistribution> d_secondary_energy_distribution;
};

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// Template Includes.
//---------------------------------------------------------------------------//

#include "MonteCarlo_PairProductionPhotoatomicReaction_def.hpp"

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

#endif // end MONTE_CARLO_PAIR_PRODUCTION_PHOTOATOMIC_REACTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_PairProductionPhotoatomicReaction.hpp
//---------------------------------------------------------------------------//


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "Input.hpp"

#include <iostream>
#include <map>
#include <vector>
#include <string>

#include "Config.hpp"

#include <Eigen/Core>
#include "utils/getkw/Getkw.h"

#include <boost/algorithm/string.hpp>

#include "cavity/CavityData.hpp"
#include "green/GreenData.hpp"
#include "solver/SolverData.hpp"
#include "PCMInput.h"
#include "utils/Solvent.hpp"
#include "utils/Sphere.hpp"

using boost::algorithm::to_upper_copy;
using boost::algorithm::trim;

Input::Input(const std::string & filename)
{
    reader(filename);
    semanticCheck();
}

Input::Input(const PCMInput & host_input)
{
    reader(host_input);
    semanticCheck();
}

void Input::reader(const std::string & filename)
{
    Getkw input_ = Getkw(filename, false, true);
    units_      = input_.getStr("UNITS");
    CODATAyear_ = input_.getInt("CODATA");
    initBohrToAngstrom(bohrToAngstrom, CODATAyear_);

    const Section & cavity = input_.getSect("CAVITY");

    type_ = cavity.getStr("TYPE");
    area_ = cavity.getDbl("AREA");
    patchLevel_ = cavity.getInt("PATCHLEVEL");
    coarsity_ = cavity.getDbl("COARSITY");
    minDistance_ = cavity.getDbl("MINDISTANCE");
    derOrder_ = cavity.getInt("DERORDER");
    if (type_ == "RESTART") {
        cavFilename_ = cavity.getStr("NPZFILE");
    }
    dyadicFilename_ = cavity.getStr("DYADICFILE");

    scaling_ = cavity.getBool("SCALING");
    radiiSet_ = to_upper_copy(cavity.getStr("RADIISET"));
    minimalRadius_ = cavity.getDbl("MINRADIUS");
    mode_ = to_upper_copy(cavity.getStr("MODE"));
    if (mode_ == "EXPLICIT") {
        std::vector<double> spheresInput = cavity.getDblVec("SPHERES");
        int j = 0;
        int nAtoms = int(spheresInput.size() / 4);
        for (int i = 0; i < nAtoms; ++i) {
            Eigen::Vector3d center;
            center << spheresInput[j], spheresInput[j+1], spheresInput[j+2];
            Sphere sph(center, spheresInput[j+3]);
            spheres_.push_back(sph);
            j += 4;
        }
        // Initialize molecule from spheres only when molecule section is absent
        if (!input_.getSect("MOLECULE").isDefined()) molecule_ = Molecule(spheres_);
    } else if (mode_ == "ATOMS") {
        atoms_ = cavity.getIntVec("ATOMS");
        radii_ = cavity.getDblVec("RADII");
    }

    // Get the contents of the Medium section
    const Section & medium = input_.getSect("MEDIUM");
    // Get the name of the solvent
    std::string name = medium.getStr("SOLVENT");
    if (name == "EXPLICIT") {
        hasSolvent_ = false;
        // Get the probe radius
        probeRadius_ = medium.getDbl("PROBERADIUS");
        // Get the contents of the Green<inside> section...
        const Section & inside = medium.getSect("GREEN<INSIDE>");
        // ...and initialize the data members
        greenInsideType_ = inside.getStr("TYPE");
        derivativeInsideType_ = derivativeTraits(inside.getStr("DER"));
        epsilonInside_ = inside.getDbl("EPS");
        // Get the contents of the Green<outside> section...
        const Section & outside = medium.getSect("GREEN<OUTSIDE>");
        // ...and initialize the data members
        greenOutsideType_ = outside.getStr("TYPE");
        derivativeOutsideType_ = derivativeTraits(outside.getStr("DER"));
        epsilonStaticOutside_ = outside.getDbl("EPS");
        epsilonDynamicOutside_ = outside.getDbl("EPSDYN");
        // This will be needed for the metal sphere only
        if (greenOutsideType_ == "METALSPHERE") {
            epsilonReal_ = outside.getDbl("EPSRE");
            epsilonImaginary_ = outside.getDbl("EPSIMG");
            spherePosition_ = outside.getDblVec("SPHEREPOSITION");
            sphereRadius_ = outside.getDbl("SPHERERADIUS");
        }
        epsilonStatic1_ = outside.getDbl("EPS1");
        epsilonDynamic1_ = outside.getDbl("EPSDYN1");
        epsilonStatic2_ = outside.getDbl("EPS2");
        epsilonDynamic2_ = outside.getDbl("EPSDYN2");
        center_ = outside.getDbl("CENTER");
        width_ = outside.getDbl("WIDTH");
        origin_ = outside.getDblVec("INTERFACEORIGIN");
        profileType_ = profilePolicy(outside.getStr("PROFILE"));
        maxL_ = outside.getInt("MAXL");
    } else { // This part must be reviewed!! Some data members are not initialized...
        // Just initialize the solvent object in this class
        hasSolvent_ = true;
        if (solvents().find(name) == solvents().end()) {
            PCMSOLVER_ERROR("Solvent " + name + " NOT found!", BOOST_CURRENT_FUNCTION);
        } else {
            solvent_ = solvents()[name];
        }
        probeRadius_ = solvent_.probeRadius * angstromToBohr();
        // Specification of the solvent by name means isotropic PCM
        // We have to initialize the Green's functions data here, Solvent class
        // is an helper class and should not be used in the core classes.
        greenInsideType_ = "VACUUM";
        derivativeInsideType_ = derivativeTraits("DERIVATIVE");
        epsilonInside_ = 1.0;
        greenOutsideType_ = "UNIFORMDIELECTRIC";
        derivativeOutsideType_ = derivativeTraits("DERIVATIVE");
        epsilonStaticOutside_ = solvent_.epsStatic;
        epsilonDynamicOutside_ = solvent_.epsDynamic;
    }
    integratorType_ = integratorPolicy(medium.getStr("DIAGONALINTEGRATOR"));
    integratorScaling_ = medium.getDbl("DIAGONALSCALING");

    solverType_ = medium.getStr("SOLVERTYPE");
    equationType_ = integralEquation(medium.getStr("EQUATIONTYPE"));
    correction_ = medium.getDbl("CORRECTION");
    hermitivitize_ = medium.getBool("MATRIXSYMM");
    isDynamic_ = medium.getBool("NONEQUILIBRIUM");

    if (input_.getSect("MOLECULE").isDefined()) {
      TIMER_ON("Initializing molecule");
      geometry_ = input_.getSect("MOLECULE").getDblVec("GEOMETRY");
      initMolecule();
      TIMER_OFF("Initializing molecule");
    }

    providedBy_ = std::string("API-side");
}

std::string trim(const char * src)
{
    std::string tmp(src);
    trim(tmp);
    return tmp;
}

std::string trim_and_upper(const char * src)
{
    return to_upper_copy(trim(src));
}

void Input::reader(const PCMInput & host_input)
{
    CODATAyear_ = 2010;
    initBohrToAngstrom(bohrToAngstrom, CODATAyear_);

    type_ = trim_and_upper(host_input.cavity_type);
    area_ = host_input.area * angstrom2ToBohr2();
    patchLevel_ = host_input.patch_level;
    coarsity_ = host_input.coarsity * angstromToBohr();
    minDistance_ = host_input.min_distance * angstromToBohr();
    derOrder_ = host_input.der_order;
    if (type_ == "RESTART") {
        cavFilename_ = trim(host_input.restart_name); // No case conversion here!
    }

    scaling_ = host_input.scaling;
    radiiSet_ = trim_and_upper(host_input.radii_set);
    minimalRadius_ = host_input.min_radius * angstromToBohr();
    mode_ = std::string("IMPLICIT");

    std::string name = trim_and_upper(host_input.solvent);
    if (name.empty()) {
        hasSolvent_ = false;
        // Get the probe radius
        probeRadius_ = host_input.probe_radius * angstromToBohr();
        // Get the contents of the Green<inside> section...
        // ...and initialize the data members
        greenInsideType_ = trim_and_upper(host_input.inside_type);
        derivativeInsideType_ = derivativeTraits("DERIVATIVE");
        epsilonInside_ = 1.0;
        // Get the contents of the Green<outside> section...
        // ...and initialize the data members
        greenOutsideType_ = trim_and_upper(host_input.outside_type);
        derivativeOutsideType_ = derivativeTraits("DERIVATIVE");
        epsilonStaticOutside_ = host_input.outside_epsilon;
        epsilonDynamicOutside_ = host_input.outside_epsilon;
    } else { // This part must be reviewed!! Some data members are not initialized...
        // Just initialize the solvent object in this class
        hasSolvent_ = true;
        solvent_ = solvents()[name];
        probeRadius_ = solvent_.probeRadius* angstromToBohr();
        // Specification of the solvent by name means isotropic PCM
        // We have to initialize the Green's functions data here, Solvent class
        // is an helper class and should not be used in the core classes.
        greenInsideType_ = std::string("VACUUM");
        derivativeInsideType_ = derivativeTraits("DERIVATIVE");
        epsilonInside_ = 1.0;
        greenOutsideType_ = std::string("UNIFORMDIELECTRIC");
        derivativeOutsideType_ = derivativeTraits("DERIVATIVE");
        epsilonStaticOutside_ = solvent_.epsStatic;
        epsilonDynamicOutside_ = solvent_.epsDynamic;
    }
    integratorType_ = integratorPolicy("COLLOCATION");
    integratorScaling_ = 1.07;

    solverType_ = trim_and_upper(host_input.solver_type);
    std::string inteq = trim_and_upper(host_input.equation_type);
    equationType_ = integralEquation(inteq);
    correction_ = host_input.correction;
    hermitivitize_ = true;
    isDynamic_ = false;

    providedBy_ = std::string("host-side");
}

void Input::semanticCheck()
{
}

void Input::initMolecule()
{
  // Gather information necessary to build molecule_
  // 1. number of atomic centers
  int nuclei = int(geometry_.size() / 4);
  // 2. position and charges of atomic centers
  Eigen::Matrix3Xd centers = Eigen::Matrix3Xd::Zero(3, nuclei);
  Eigen::VectorXd charges  = Eigen::VectorXd::Zero(nuclei);
  int j = 0;
  for (int i = 0; i < nuclei; ++i) {
    centers.col(i) << geometry_[j], geometry_[j+1], geometry_[j+2];
    charges(i) = geometry_[j+3];
    j += 4;
  }
  // 3. list of atoms and list of spheres
  double factor = angstromToBohr();
  std::vector<Atom> radiiSet;
  std::vector<Atom> atoms;
  if ( radiiSet_ == "UFF" ) {
    radiiSet = initUFF();
    radiiSetName_ = "UFF";
  } else if ( radiiSet_ == "BONDI" ) {
    radiiSet = initBondi();
    radiiSetName_ = "Bondi-Mantina";
  } else {
    radiiSet = initAllinger();
    radiiSetName_ = "Allinger's MM3";
  }
  for (int i = 0; i < charges.size(); ++i) {
    int index = int(charges(i)) - 1;
    atoms.push_back(radiiSet[index]);
    if (scaling_) atoms[index].radiusScaling = 1.2;
  }
  // Based on the creation mode (Implicit or Atoms)
  // the spheres list might need postprocessing
  if ( mode_ == "IMPLICIT" || mode_ == "ATOMS") {
    for (int i = 0; i < charges.size(); ++i) {
      int index = int(charges(i)) - 1;
      double radius = radiiSet[index].radius * factor;
      if (scaling_) radius *= 1.2;
      spheres_.push_back(Sphere(centers.col(i), radius));
    }
    if (mode_ == "ATOMS") {
      // Loop over the atomsInput array to get which atoms will have a user-given radius
      for (size_t i = 0; i < atoms_.size(); ++i) {
        int index = atoms_[i] - 1; // -1 to go from human readable to machine readable
        // Put the new Sphere in place of the implicit-generated one
        spheres_[index] = Sphere(centers.col(index), radii_[i]);
      }
    }
  }

  // 4. masses
  Eigen::VectorXd masses = Eigen::VectorXd::Zero(nuclei);
  for (int i = 0; i < masses.size(); ++i) {
    masses(i) = atoms[i].mass;
  }
  // 5. molecular point group
  // FIXME currently hardcoded to C1

  // OK, now get molecule_
  molecule_ = Molecule(nuclei, charges, masses, centers, atoms, spheres_);
  // Check that all atoms have a radius attached
  std::vector<Atom>::const_iterator res =
    std::find_if(atoms.begin(), atoms.end(), invalid);
  if (res != atoms.end()) {
    std::cout << molecule_ << std::endl;
    PCMSOLVER_ERROR("Some atoms do not have a radius attached. Please specify a radius for all atoms (see http://pcmsolver.readthedocs.org/en/latest/users/input.html)!", BOOST_CURRENT_FUNCTION);
  }
}

cavityData Input::cavityParams()
{
    if (cavData_.empty) {
        cavData_ = cavityData(molecule_, area_, probeRadius_, minDistance_, derOrder_, minimalRadius_,
                        patchLevel_, coarsity_, cavFilename_, dyadicFilename_);
    }
    return cavData_;
}

greenData Input::insideGreenParams()
{
    if (insideGreenData_.empty) {
        int profile = profilePolicy("UNIFORM");
        insideGreenData_ = greenData(derivativeInsideType_, integratorType_, profile, epsilonInside_, integratorScaling_);
    }
    return insideGreenData_;
}

greenData Input::outsideStaticGreenParams()
{
    if (outsideStaticGreenData_.empty) {
        int profile = profilePolicy("UNIFORM");
        outsideStaticGreenData_ = greenData(derivativeOutsideType_,
                                            integratorType_, profile,  epsilonStaticOutside_, integratorScaling_);
        if (not hasSolvent_) {
           outsideStaticGreenData_.howProfile = profileType_;
           outsideStaticGreenData_.epsilon1 = epsilonStatic1_;
           outsideStaticGreenData_.epsilon2 = epsilonStatic2_;
           outsideStaticGreenData_.center   = center_;
           outsideStaticGreenData_.width    = width_;
           outsideStaticGreenData_.origin   << origin_[0], origin_[1], origin_[2];
           outsideStaticGreenData_.maxL = maxL_;
        }
    }
    return outsideStaticGreenData_;
}

greenData Input::outsideDynamicGreenParams()
{
    if (outsideDynamicGreenData_.empty) {
        int profile = profilePolicy("UNIFORM");
        outsideDynamicGreenData_ = greenData(derivativeOutsideType_,
                                             integratorType_, profile, epsilonDynamicOutside_, integratorScaling_);
        if (not hasSolvent_) {
           outsideDynamicGreenData_.howProfile  = profileType_;
           outsideDynamicGreenData_.epsilon1 = epsilonDynamic1_;
           outsideDynamicGreenData_.epsilon2 = epsilonDynamic2_;
           outsideDynamicGreenData_.center   = center_;
           outsideDynamicGreenData_.width    = width_;
           outsideDynamicGreenData_.origin   << origin_[0], origin_[1], origin_[2];
           outsideDynamicGreenData_.maxL = maxL_;
        }
    }
    return outsideDynamicGreenData_;
}

solverData Input::solverParams()
{
    if (solverData_.empty) {
        solverData_ = solverData(correction_, equationType_, hermitivitize_);
    }
    return solverData_;
}

int derivativeTraits(const std::string & name)
{
    static std::map<std::string, int> mapStringToInt;
    mapStringToInt.insert(std::map<std::string, int>::value_type("NUMERICAL", 0));
    mapStringToInt.insert(std::map<std::string, int>::value_type("DERIVATIVE", 1));
    mapStringToInt.insert(std::map<std::string, int>::value_type("GRADIENT", 2));
    mapStringToInt.insert(std::map<std::string, int>::value_type("HESSIAN", 3));

    return mapStringToInt.find(name)->second;
}

int integratorPolicy(const std::string & name)
{
    static std::map<std::string, int> mapStringToInt;
    mapStringToInt.insert(std::map<std::string, int>::value_type("COLLOCATION", 0));
    mapStringToInt.insert(std::map<std::string, int>::value_type("NUMERICAL", 1));
    mapStringToInt.insert(std::map<std::string, int>::value_type("PURISIMA", 2));

    return mapStringToInt.find(name)->second;
}

int profilePolicy(const std::string & name)
{
    static std::map<std::string, int> mapStringToInt;
    mapStringToInt.insert(std::map<std::string, int>::value_type("TANH", 0));
    mapStringToInt.insert(std::map<std::string, int>::value_type("ERF", 1));

    return mapStringToInt.find(name)->second;
}

int integralEquation(const std::string & name)
{
    static std::map<std::string, int> mapStringToInt;
    mapStringToInt.insert(std::map<std::string, int>::value_type("FIRSTKIND",0));
    mapStringToInt.insert(std::map<std::string, int>::value_type("SECONDKIND", 1));
    mapStringToInt.insert(std::map<std::string, int>::value_type("FULL", 2));

    return mapStringToInt.find(name)->second;
}


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

///////////////////////////////////////////////////////////////////////////////
//
// Defines the InvertedPendulumState struct.
//
///////////////////////////////////////////////////////////////////////////////

#ifndef RL_ENVIRONMENT_INVERTED_PENDULUM_STATE_H
#define RL_ENVIRONMENT_INVERTED_PENDULUM_STATE_H

#include "../util/types.hpp"

#include <glog/logging.h>
#include <boost/functional/hash.hpp>
#include <stddef.h>
#include <math.h>
#include <random>

#ifdef SYSTEM_OSX
#include <GLUT/glut.h>
#endif

#ifdef SYSTEM_LINUX
#include <GL/glut.h>
#endif

namespace rl {

  struct InvertedPendulumState {
    // Angle from the positive x-axis, and its derivative.
    double theta_;
    double omega_;

    // Random number generator for initialization.
    static std::random_device rd_;
    static std::default_random_engine rng_;

    // Constructor/destructor.
    ~InvertedPendulumState() {}
    InvertedPendulumState(double theta, double omega)
      : theta_(theta), omega_(omega) {}
    InvertedPendulumState()
      : theta_(M_PI_2), omega_(0.0) {
      // Reset randomly.
      std::uniform_real_distribution<double> unif_theta(0.1 * M_PI, 0.9 * M_PI);
      std::uniform_real_distribution<double> unif_omega(-0.25, 0.25);

      theta_ = unif_theta(rng_);
      omega_ = unif_omega(rng_);
    }

    // Static number of dimensions.
    static constexpr size_t FeatureDimension() { return 2; }

    // Get a feature vector for this state.
    void Features(VectorXd& features) const {
      CHECK_EQ(features.size(), FeatureDimension());

      features(0) = theta_;
      features(1) = omega_;
    }

    // (In)equality operators. Note that these are not exactly transitive,
    // but rather are intended to simplify routine comparisons.
    bool operator==(const InvertedPendulumState& rhs) const {
      return (std::abs(theta_ - rhs.theta_) < 1e-8 &&
              std::abs(omega_ - rhs.omega_) < 1e-8);
    }

    bool operator!=(const InvertedPendulumState& rhs) const {
      return (std::abs(theta_ - rhs.theta_) >= 1e-8 ||
              std::abs(omega_ - rhs.omega_) >= 1e-8);
    }

    // OpenGL visualization. Must provide pendulum arm length and ball radius.
    void Visualize(double arm_length, double ball_radius) const {
      const size_t kNumVertices = 100;

      // Extract current position on xy plane.
      const GLfloat current_x = arm_length * std::cos(theta_);
      const GLfloat current_y = arm_length * std::sin(theta_);

      // Draw the arm first. Arm will be drawn as a narrow rectangle from the
      // origin to the center of the ball.
      const GLfloat arm_width = 0.25 * ball_radius;
      glBegin(GL_POLYGON);
      glColor4f(0.3, 0.3, 0.3, 0.5);
      // Bottom left, bottom right, top right, top left.
      glVertex2f(-0.5 * arm_width * std::sin(theta_),
                 0.5 * arm_width * std::cos(theta_));
      glVertex2f(0.5 * arm_width * std::sin(theta_),
                 -0.5 * arm_width * std::cos(theta_));
      glVertex2f(current_x + 0.5 * arm_width * std::sin(theta_),
                 current_y - 0.5 * arm_width * std::cos(theta_));
      glVertex2f(current_x - 0.5 * arm_width * std::sin(theta_),
                 current_y + 0.5 * arm_width * std::cos(theta_));
      glEnd();

      // Now draw the ball.
      glBegin(GL_POLYGON);
      glColor4f(0.0, 0.8, 0.2, 0.5);
      for (size_t ii = 0; ii < kNumVertices; ii++) {
        const GLfloat angle = 2.0 * M_PI *
          static_cast<GLfloat>(ii) / static_cast<GLfloat>(kNumVertices);
        glVertex2f(current_x + ball_radius * std::cos(angle),
                   current_y + ball_radius * std::sin(angle));
      }
      glEnd();
    }

    // Hash functor. Should not really ever need to hash a pendulum state,
    // but we provide this functor just in case.
    struct Hash {
      size_t operator()(const InvertedPendulumState& state) const {
        size_t seed = 0;
        boost::hash_combine(seed, boost::hash_value(state.theta_));
        boost::hash_combine(seed, boost::hash_value(state.omega_));

        return seed;
      }
    }; //\struct Hash
  }; //\struct InvertedPendulumState
}  //\namespace rl

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MORTON_DENSE_INCLUDE
#define MTL_MORTON_DENSE_INCLUDE

#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>

#include <boost/numeric/mtl/utility/common_include.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/crtp_base_matrix.hpp>
#include <boost/numeric/mtl/matrix/base_sub_matrix.hpp>
#include <boost/numeric/mtl/detail/contiguous_memory_block.hpp>
#include <boost/numeric/mtl/detail/dilated_int.hpp>
#include <boost/numeric/mtl/utility/iterator_adaptor.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/matrix/mat_expr.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/compute_factors.hpp>
#include <boost/numeric/mtl/operation/clone.hpp>

#ifdef MTL_WITH_INITLIST
# include <initializer_list>
#endif

namespace mtl { namespace matrix {

// Helper type
struct morton_dense_sub_ctor {};

template <std::size_t BitMask>
struct morton_dense_key
{
    typedef std::size_t                               size_type;
    typedef dilated_int<std::size_t, BitMask, true>   dilated_row_t;
    typedef dilated_int<std::size_t, ~BitMask, true>  dilated_col_t;
    typedef morton_dense_key                          self;

    morton_dense_key(size_type my_row, size_type my_col)
	: my_row(my_row), my_col(my_col), dilated_row(my_row), dilated_col(my_col)
    {}

    bool operator== (self const& x) const
    {
	return my_row == x.my_row && my_col == x.my_col;
    }

    bool operator!= (self const& x)
    {
	return !(*this == x);
    }

    size_type row() const
    {
	return my_row;
    }

    size_type col() const
    {
	return my_col;
    }

    self& advance_row(int row_inc)
    {
	dilated_row.advance(row_inc);
	// potential addition of signed and unsigned
	my_row+= row_inc;
	return *this;
    }

    self& advance_col(int col_inc)
    {
	dilated_col.advance(col_inc);
	// potential addition of signed and unsigned
	my_col+= col_inc;
	return *this;
    }

    self& advance(int row_inc, int col_inc)
    {
	advance_row(row_inc);
	advance_col(col_inc);
	return *this;
    }

public:
    size_type                    my_row, my_col;
    dilated_row_t                dilated_row;
    dilated_col_t                dilated_col;
};

template <std::size_t BitMask>
struct morton_dense_el_cursor
  : public morton_dense_key<BitMask>
{
    typedef std::size_t                               size_type;
    typedef dilated_int<std::size_t, ~BitMask, true>  dilated_col_t;
    typedef morton_dense_el_cursor                    self;
    typedef morton_dense_key<BitMask>                 base;
    typedef base                                      key_type;

    morton_dense_el_cursor(size_type my_row, size_type my_col, size_type num_cols)
	: base(my_row, my_col), num_cols(num_cols)
    {}

    self& operator++ ()
    {
	++this->my_col; ++this->dilated_col;
	if (this->my_col == num_cols) {
	    this->my_col= 0; this->dilated_col= dilated_col_t(0);
	    ++this->my_row; ++this->dilated_row;
	}
	return *this;
    }

    base& operator* ()
    {
	return *this;
    }

    const base& operator* () const
    {
	return *this;
    }

protected:
    size_t                       num_cols;
};

template <std::size_t BitMask>
struct morton_dense_row_cursor
    : public morton_dense_key<BitMask>
{
    typedef std::size_t                               size_type;
    typedef morton_dense_row_cursor                   self;
    typedef morton_dense_key<BitMask>                 base;
    typedef base                                      key_type;

    morton_dense_row_cursor(size_type my_row, size_type my_col)
	: base(my_row, my_col)
    {}

    self& operator++ ()
    {
	++this->my_row; ++this->dilated_row;
	return *this;
    }

    self& operator+=(int inc)
    {
	this->advance_row(inc);
	return *this;
    }

    self& operator-- ()
    {
	--this->my_row; --this->dilated_row;
	return *this;
    }

    self& operator-=(int dec)
    {
	this->advance_row(-dec);
	return *this;
    }

    self operator+ (int inc) const
    {
	self tmp(*this);
	tmp.advance_row(inc);
	return tmp;
    }

    base& operator* ()
    {
	return *this;
    }

    const base& operator* () const
    {
	return *this;
    }
};

template <std::size_t BitMask>
struct morton_dense_col_cursor
    : public morton_dense_key<BitMask>
{
    typedef std::size_t                               size_type;
    typedef morton_dense_col_cursor                   self;
    typedef morton_dense_key<BitMask>                 base;
    typedef base                                      key_type;

    morton_dense_col_cursor(size_type my_row, size_type my_col)
	: base(my_row, my_col)
    {}

    self& operator++ ()
    {
	++this->my_col; ++this->dilated_col;
	return *this;
    }

    self& operator+=(int inc)
    {
	this->advance_col(inc);
	return *this;
    }

    self& operator-- ()
    {
	--this->my_col; --this->dilated_col;
	return *this;
    }

    self& operator-=(int dec)
    {
	this->advance_col(-dec);
	return *this;
    }

    self operator+ (int inc) const
    {
	self tmp(*this);
	tmp.advance_col(inc);
	return tmp;
    }

    base& operator* ()
    {
	return *this;
    }

    const base& operator* () const
    {
	return *this;
    }
};

template <typename Matrix>
struct morton_dense_row_const_iterator
    : utilities::const_iterator_adaptor<typename mtl::traits::const_value<Matrix>::type, morton_dense_row_cursor<Matrix::mask>,
					typename Matrix::value_type>
{
    static const std::size_t                          mask= Matrix::mask;
    typedef morton_dense_row_cursor<mask>               cursor_type;
    typedef typename mtl::traits::const_value<Matrix>::type  map_type;
    typedef typename Matrix::value_type                 value_type;
    typedef typename Matrix::size_type                  size_type;
    typedef utilities::const_iterator_adaptor<map_type, cursor_type, value_type> base;

    morton_dense_row_const_iterator(const Matrix& matrix, size_type row, size_type col)
	: base(map_type(matrix), cursor_type(row, col))
    {}
};

template <typename Matrix>
struct morton_dense_row_iterator
    : utilities::iterator_adaptor<typename mtl::traits::value<Matrix>::type, morton_dense_row_cursor<Matrix::mask>,
				  typename Matrix::value_type>
{
    static const std::size_t                          mask= Matrix::mask;
    typedef morton_dense_row_cursor<mask>               cursor_type;
    typedef typename mtl::traits::value<Matrix>::type   map_type;
    typedef typename Matrix::value_type                 value_type;
    typedef typename Matrix::size_type                  size_type;
    typedef utilities::iterator_adaptor<map_type, cursor_type, value_type> base;

    morton_dense_row_iterator(Matrix& matrix, size_type row, size_type col)
	:  base(map_type(matrix), cursor_type(row, col))
    {}
};

template <typename Matrix>
struct morton_dense_col_const_iterator
    : utilities::const_iterator_adaptor<typename mtl::traits::const_value<Matrix>::type, morton_dense_col_cursor<Matrix::mask>,
					typename Matrix::value_type>
{
    static const std::size_t                          mask= Matrix::mask;
    typedef morton_dense_col_cursor<mask>               cursor_type;
    typedef typename mtl::traits::const_value<Matrix>::type  map_type;
    typedef typename Matrix::value_type                 value_type;
    typedef typename Matrix::size_type                  size_type;
    typedef utilities::const_iterator_adaptor<map_type, cursor_type, value_type> base;

    morton_dense_col_const_iterator(const Matrix& matrix, size_type row, size_type col)
	: base(map_type(matrix), cursor_type(row, col))
    {}
};

template <typename Matrix>
struct morton_dense_col_iterator
    : utilities::iterator_adaptor<typename mtl::traits::value<Matrix>::type, morton_dense_col_cursor<Matrix::mask>,
				  typename Matrix::value_type>
{
    static const std::size_t                          mask= Matrix::mask;
    typedef morton_dense_col_cursor<mask>               cursor_type;
    typedef typename mtl::traits::value<Matrix>::type   map_type;
    typedef typename Matrix::value_type                 value_type;
    typedef typename Matrix::size_type                  size_type;
    typedef utilities::iterator_adaptor<map_type, cursor_type, value_type> base;

    morton_dense_col_iterator(Matrix& matrix, size_type row, size_type col)
      : base(map_type(matrix), cursor_type(row, col))  {}
};

/// Dense Morton-order matrix
template <typename Elt, std::size_t BitMask, typename Parameters = mtl::matrix::parameters<> >
class morton_dense
  : public base_sub_matrix<Elt, Parameters>,
    public mtl::detail::contiguous_memory_block<Elt, false>,
    public crtp_base_matrix< morton_dense<Elt, BitMask, Parameters>, Elt, std::size_t >,
    public mat_expr< morton_dense<Elt, BitMask, Parameters> >
{
    typedef morton_dense                                               self;
    typedef base_sub_matrix<Elt, Parameters>                           super;
    typedef mtl::detail::contiguous_memory_block<Elt, false>           memory_base;
    typedef crtp_matrix_assign< self, Elt, std::size_t >               assign_base;
    typedef mat_expr< morton_dense<Elt, BitMask, Parameters> >         expr_base;

  public:

    typedef Parameters                        parameters;
    typedef typename Parameters::orientation  orientation;
    typedef typename Parameters::index        index_type;
    typedef typename Parameters::dimensions   dim_type;
    typedef Elt                               value_type;
    typedef const value_type&                 const_reference;
    typedef value_type&                       reference;
    typedef typename parameters::size_type    size_type;
    const static std::size_t                  mask= BitMask;

    //  implement cursor for morton matrix, somewhere
    //  also, morton indexer?

    typedef morton_dense_key<BitMask>          key_type;
    typedef morton_dense_el_cursor<BitMask>    el_cursor_type;

    typedef dilated_int<std::size_t, BitMask, true>   dilated_row_t;
    typedef dilated_int<std::size_t, ~BitMask, true>  dilated_col_t;

  protected:

    // ranges of rows and columns
    dilated_row_t            my_begin_row, my_end_row;
    dilated_col_t            my_begin_col, my_end_col;

    // Set ranges from begin_r to end_r and begin_c to end_c
    void set_ranges(size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
    {
	super::set_ranges(begin_r, end_r, begin_c, end_c);
	my_begin_row= begin_r; my_end_row= end_r;
	my_begin_col= begin_c; my_end_col= end_c;
	set_nnz();
    }

    // Set ranges to a num_row x num_col matrix, keeps indexing
    void set_ranges(size_type num_rows, size_type num_cols)
    {
	set_ranges(this->begin_row(), this->begin_row() + num_rows,
		   this->begin_col(), this->begin_col() + num_cols);
    }

    void init(size_type num_rows, size_type num_cols)
    {
	set_ranges(num_rows, num_cols);
	// set_to_zero(*this);
    }

  public:
    /// Default constructor
    /** If compile time matrix size allocate memory. **/
    morton_dense() : memory_base(memory_need(dim_type().num_rows(), dim_type().num_cols()))
    {
	init(dim_type().num_rows(), dim_type().num_cols());
    }

    /// Construction from run-time dimension type
    explicit morton_dense(mtl::non_fixed::dimensions d)
	: memory_base(memory_need(d.num_rows(), d.num_cols()))
    {
	init(d.num_rows(), d.num_cols());
    }

    /// Construction of matrix of dimension \p num_rows by \p num_cols
    morton_dense(size_type num_rows, size_type num_cols)
	: memory_base(memory_need(num_rows, num_cols))
    {
	init(num_rows, num_cols);
    }

    /// Construction of matrix with dimension \p d using pointer \p a to external data
    explicit morton_dense(mtl::non_fixed::dimensions d, value_type* a)
      : memory_base(a, memory_need(d.num_rows(), d.num_cols()))
    {
	set_ranges(d.num_rows(), d.num_cols());
    }

    /// Construction of \p num_rows by \p num_cols matrix with pointer \p a to external data
    explicit morton_dense(size_type num_rows, size_type num_cols, value_type* a)
      : memory_base(a, memory_need(num_rows, num_cols))
    {
	set_ranges(num_rows, num_cols);
    }

    /// Construction of matrix with static dimension using pointer \p a to external data
    explicit morton_dense(value_type* a)
	: memory_base(a, memory_need(dim_type().num_rows(), dim_type().num_cols()))
    {
	BOOST_ASSERT((dim_type::is_static));
	set_ranges(dim_type().num_rows(), dim_type().num_cols());
    }

    /// Copy constructor
    morton_dense(const self& m)
      : super(m), memory_base(m)
    {
	set_ranges(m.num_rows(), m.num_cols());
    }

    /// Clone constructor
    explicit morton_dense(const self& m, clone_ctor)
      : memory_base(m, clone_ctor())
    {
	init(m.num_rows(), m.num_cols());
	*this= m;
    }

    /// Templated copy constructor
    template <typename MatrixSrc>
    explicit morton_dense(const MatrixSrc& src)
      : memory_base(memory_need(dim_type().num_rows(), dim_type().num_cols()))
    {
	init(dim_type().num_rows(), dim_type().num_cols());
	*this= src;
    }

#if defined(MTL_WITH_INITLIST) && defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
    /// Constructor for initializer list \p values
    template <typename Value2>
    morton_dense(std::initializer_list<std::initializer_list<Value2> > values)
      : super(mtl::non_fixed::dimensions(values.size(), values.size()? values.begin()->size() : 0)),
    	memory_base(this->num_rows() * this->num_cols())
    {
    	init(this->num_rows(), this->num_cols());
	*this= values;
    }
#endif

    /// Construct a sub-matrix as a view
    explicit morton_dense(self& matrix, morton_dense_sub_ctor,
			  size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
      : memory_base(matrix.data, memory_need(end_r - begin_r, end_c - begin_c), true) // View constructor
    {
	matrix.check_ranges(begin_r, end_r, begin_c, end_c);

	if (begin_r >= end_r || begin_c >= end_c) {
	    set_ranges(0, 0);
	    return;
	}

	// Check whether sub-matrix is contigous memory block
	// by comparing the address of the last and the first element in the entire and the sub-matrix
	MTL_DEBUG_THROW_IF(&matrix[end_r-1][end_c-1] - &matrix[begin_r][begin_c]
			   != &matrix[end_r-begin_r-1][end_c-begin_c-1] - &matrix[0][0],
			   range_error("This sub-matrix cannot be used because it is split in memory"));
	// Check with David if this is a sufficient condition (it is a necessary at least)

	dilated_row_t  dilated_row(begin_r);
	dilated_col_t  dilated_col(begin_c);

	// Set new start address within masked matrix
	this->data += dilated_row.dilated_value() + dilated_col.dilated_value();
	set_ranges(end_r - begin_r, end_c - begin_c);
    }

    /// Change dimension to \p num_rows by \p num_cols
    void change_dim(size_type num_rows, size_type num_cols)
    {
	set_ranges(num_rows, num_cols);
	this->realloc(memory_need(num_rows, num_cols));
    }

#if 0
    // Move assignment (emulation)
    self& operator=(self src)
    {
	// Self-copy would be an indication of an error
	assert(this != &src);

	this->check_dim(src.num_rows(), src.num_cols());
	if (this->category == memory_base::view || src.category == memory_base::view)
	    matrix_copy(src, *this);
	else
	    memory_base::move_assignment(src);
	return *this;
    }
#endif

#ifdef MTL_WITH_MOVE
    /// Move Assignment
    self& operator=(self&& src)
    {
	this->checked_change_dim(src.num_rows(), src.num_cols());
	if (this->category == memory_base::view || src.category == memory_base::view)
	    matrix_copy(src, *this);
	else
	    memory_base::move_assignment(src);
	std::cout << "In dense2D::move_assignment\n";
	return *this;
    }

    /// Copy Assignment
    self& operator=(const self& src)
    {
	this->checked_change_dim(src.num_rows(), src.num_cols());
	// this->check_dim(src.num_rows(), src.num_cols());
	matrix_copy(src, *this);
	return *this;
    }
#else
    /// Copy assignment (with move emulation)
    self& operator=(self src)
    {
	// Self-copy would be an indication of an error
	assert(this != &src);

	this->check_dim(src.num_rows(), src.num_cols());
	if (this->category == memory_base::view || src.category == memory_base::view)
	    matrix_copy(src, *this);
	else
	    memory_base::move_assignment(src);
	return *this;
    }
#endif

    using assign_base::operator=;

    value_type operator() (key_type const& key) const
    {
	return this->data[key.dilated_row.dilated_value() + key.dilated_col.dilated_value()];
    }

    void operator()(key_type const& key, value_type const& value)
    {
	this->data[key.dilated_row.dilated_value() + key.dilated_col.dilated_value()]= value;
    }

    /// Constant reference to A[row][col]
    const_reference operator() (size_type row, size_type col) const
    {
	MTL_DEBUG_THROW_IF(is_negative(row) || row >= this->num_rows() || is_negative(col) || col >= this->num_cols(), index_out_of_range());
	return this->data[dilated_row_t(row).dilated_value() + dilated_col_t(col).dilated_value()];
    }

    /// Mutable reference to A[row][col]
    value_type& operator() (size_type row, size_type col)
    {
	MTL_DEBUG_THROW_IF(is_negative(row) || row >= this->num_rows() || is_negative(col) || col >= this->num_cols(), index_out_of_range());
	return this->data[dilated_row_t(row).dilated_value() + dilated_col_t(col).dilated_value()];
    }

    void crop() {} ///< Delete structural zeros, only dummy here

  protected:
    void set_nnz()
    {
      this->my_nnz = this->num_rows() * this->num_cols();
    }

    size_type memory_need(size_type rows, size_type cols)
    {
        dilated_row_t n_rows(rows - 1);
        dilated_col_t n_cols(cols - 1);
        return (n_rows.dilated_value() + n_cols.dilated_value() + 1);
    }

    /// Swap matrices
    friend void swap(self& matrix1, self& matrix2)
    {
	swap(static_cast<memory_base&>(matrix1), static_cast<memory_base&>(matrix2));
	swap(static_cast<super&>(matrix1), static_cast<super&>(matrix2));
    }

    template <typename> friend struct sub_matrix_t;
};

// ================
// Free functions
// ================

/// Number of rows
template <typename Value, std::size_t Mask, typename Parameters>
typename morton_dense<Value, Mask, Parameters>::size_type
inline num_rows(const morton_dense<Value, Mask, Parameters>& matrix)
{
    return matrix.num_rows();
}

/// Number of columns
template <typename Value, std::size_t Mask, typename Parameters>
typename morton_dense<Value, Mask, Parameters>::size_type
inline num_cols(const morton_dense<Value, Mask, Parameters>& matrix)
{
    return matrix.num_cols();
}

/// Matrix size, i.e. number of rows times columns
template <typename Value, std::size_t Mask, typename Parameters>
typename morton_dense<Value, Mask, Parameters>::size_type
inline size(const morton_dense<Value, Mask, Parameters>& matrix)
{
    return matrix.num_cols() * matrix.num_rows();
}

}} // namespace mtl::matrix

// ================
// Range generators
// ================

namespace mtl { namespace traits {

    // VC 8.0 finds ambiguity with mtl::tag::morton_dense (I wonder why)
    using mtl::matrix::morton_dense;
    using mtl::matrix::morton_dense_el_cursor;
    using mtl::matrix::morton_dense_col_cursor;
    using mtl::matrix::morton_dense_row_cursor;
    using mtl::matrix::morton_dense_col_const_iterator;
    using mtl::matrix::morton_dense_row_const_iterator;
    using mtl::matrix::morton_dense_col_iterator;
    using mtl::matrix::morton_dense_row_iterator;

    // ===========
    // For cursors
    // ===========

    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::all, morton_dense<Elt, BitMask, Parameters> >
    {
	typedef morton_dense<Elt, BitMask, Parameters>        Matrix;
	typedef complexity_classes::linear_cached             complexity;
	static int const                                      level = 1;
	typedef morton_dense_el_cursor<BitMask>  type;
	type begin(Matrix const& matrix)
	{
	    return type(matrix.begin_row(), matrix.begin_col(), matrix.num_cols());
	}
	type end(Matrix const& matrix)
	{
	    return type(matrix.end_row(), matrix.begin_col(), matrix.num_cols());
	}
    };

    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::nz, morton_dense<Elt, BitMask, Parameters> >
	: range_generator<glas::tag::all, morton_dense<Elt, BitMask, Parameters> >
    {};

    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::row, morton_dense<Elt, BitMask, Parameters> >
	: detail::all_rows_range_generator<morton_dense<Elt, BitMask, Parameters>, complexity_classes::linear_cached>
    {};

    // For a cursor pointing to some row give the range of elements in this row
    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::nz,
			   detail::sub_matrix_cursor<morton_dense<Elt, BitMask, Parameters>, glas::tag::row, 2> >
    {
	typedef morton_dense<Elt, BitMask, Parameters>                   matrix;
	typedef typename Collection<matrix>::size_type                   size_type;
	typedef detail::sub_matrix_cursor<matrix, glas::tag::row, 2>     cursor;
	typedef complexity_classes::linear_cached                        complexity;
	static int const                                                 level = 1;
	typedef morton_dense_col_cursor<BitMask>                         type;

	type begin(cursor const& c) const
	{
	    return type(c.key, c.ref.begin_col());
	}
	type end(cursor const& c) const
	{
	    return type(c.key, c.ref.end_col());
	}
	type lower_bound(cursor const& c, size_type position) const
	{
	    return type(c.key, std::min(c.ref.end_col(), position));
	}
    };

    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::all,
			   detail::sub_matrix_cursor<morton_dense<Elt, BitMask, Parameters>, glas::tag::row, 2> >
        : range_generator<glas::tag::nz,
			  detail::sub_matrix_cursor<morton_dense<Elt, BitMask, Parameters>, glas::tag::row, 2> >
    {};

    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::col, morton_dense<Elt, BitMask, Parameters> >
	: detail::all_cols_range_generator<morton_dense<Elt, BitMask, Parameters>, complexity_classes::linear_cached>
    {};

    // For a cursor pointing to some row give the range of elements in this row
    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::nz,
			   detail::sub_matrix_cursor<morton_dense<Elt, BitMask, Parameters>, glas::tag::col, 2> >
    {
	typedef morton_dense<Elt, BitMask, Parameters>                   matrix;
	typedef typename Collection<matrix>::size_type                   size_type;
	typedef detail::sub_matrix_cursor<matrix, glas::tag::col, 2>     cursor;
	typedef complexity_classes::linear_cached                        complexity;
	static int const                                                 level = 1;
	typedef morton_dense_row_cursor<BitMask>                         type;

	type begin(cursor const& c)
	{
	    return type(c.ref.begin_row(), c.key);
	}
	type end(cursor const& c)
	{
	    return type(c.ref.end_row(), c.key);
	}
	type lower_bound(cursor const& c, size_type position) const
	{
	    return type(std::min(c.ref.end_row(), position), c.key);
	}
    };

    template <class Elt, std::size_t BitMask, class Parameters>
    struct range_generator<glas::tag::all,
			   detail::sub_matrix_cursor<morton_dense<Elt, BitMask, Parameters>, glas::tag::col, 2> >
        : range_generator<glas::tag::nz,
			  detail::sub_matrix_cursor<morton_dense<Elt, BitMask, Parameters>, glas::tag::col, 2> >
    {};

// =============
// For iterators
// =============

    namespace detail {

        template <typename OuterTag, typename Matrix, bool is_const>
        struct morton_dense_iterator_range_generator
        {
	    typedef Matrix                                                                matrix_type;
	    typedef typename matrix_type::size_type                                       size_type;
	    typedef typename matrix_type::value_type                                      value_type;
	    typedef typename matrix_type::parameters                                      parameters;
	    typedef detail::sub_matrix_cursor<matrix_type, OuterTag, 2>                   cursor;

	    typedef complexity_classes::linear_cached                                     complexity;
	    static int const                                                              level = 1;

	    typedef typename boost::mpl::if_<
		boost::is_same<OuterTag, glas::tag::row>
	      , typename boost::mpl::if_c<
    	            is_const
		  , morton_dense_col_const_iterator<Matrix>
		  , morton_dense_col_iterator<Matrix>
		>::type
	      , typename boost::mpl::if_c<
    	            is_const
		  , morton_dense_row_const_iterator<Matrix>
		  , morton_dense_row_iterator<Matrix>
		>::type
	    >::type type;

        private:

	    typedef typename boost::mpl::if_c<is_const, const Matrix&, Matrix&>::type    mref_type;

	    type begin_dispatch(cursor const& c, glas::tag::row)
	    {
		return type(const_cast<mref_type>(c.ref), c.key, c.ref.begin_col());
	    }

	    type end_dispatch(cursor const& c, glas::tag::row)
	    {
		return type(const_cast<mref_type>(c.ref), c.key, c.ref.end_col());
	    }

	    type begin_dispatch(cursor const& c, glas::tag::col)
	    {
		return type(const_cast<mref_type>(c.ref), c.ref.begin_row(), c.key);
	    }

	    type end_dispatch(cursor const& c, glas::tag::col)
	    {
		return type(const_cast<mref_type>(c.ref), c.ref.end_row(), c.key);
	    }

        public:

	    type begin(cursor const& c)
	    {
		return begin_dispatch(c, OuterTag());
	    }

	    type end(cursor const& c)
	    {
		return end_dispatch(c, OuterTag());
	    }
        };

    } // namespace detail

    template <typename Value, std::size_t BitMask, typename Parameters, typename OuterTag>
    struct range_generator<tag::iter::nz,
			   detail::sub_matrix_cursor<morton_dense<Value, BitMask, Parameters>, OuterTag, 2> >
      : public detail::morton_dense_iterator_range_generator<OuterTag, morton_dense<Value, BitMask, Parameters>, false>
    {};

    template <typename Value, std::size_t BitMask, typename Parameters, typename OuterTag>
    struct range_generator<tag::iter::all,
			   detail::sub_matrix_cursor<morton_dense<Value, BitMask, Parameters>, OuterTag, 2> >
      : public detail::morton_dense_iterator_range_generator<OuterTag, morton_dense<Value, BitMask, Parameters>, false>
    {};

    template <typename Value, std::size_t BitMask, typename Parameters, typename OuterTag>
    struct range_generator<tag::const_iter::nz,
			   detail::sub_matrix_cursor<morton_dense<Value, BitMask, Parameters>, OuterTag, 2> >
      : public detail::morton_dense_iterator_range_generator<OuterTag, morton_dense<Value, BitMask, Parameters>, true>
    {};

    template <typename Value, std::size_t BitMask, typename Parameters, typename OuterTag>
    struct range_generator<tag::const_iter::all,
			   detail::sub_matrix_cursor<morton_dense<Value, BitMask, Parameters>, OuterTag, 2> >
      : public detail::morton_dense_iterator_range_generator<OuterTag, morton_dense<Value, BitMask, Parameters>, true>
    {};

}} // namespace mtl::traits

namespace mtl { namespace matrix {

    // ==========
    // Sub matrix
    // ==========

    template <typename Value, std::size_t BitMask, typename Parameters>
    struct sub_matrix_t<morton_dense<Value, BitMask, Parameters> >
    {
        typedef morton_dense<Value, BitMask, Parameters>    matrix_type;
        typedef matrix_type                     sub_matrix_type;
        typedef matrix_type const               const_sub_matrix_type;
        typedef typename matrix_type::size_type size_type;

        sub_matrix_type operator()(matrix_type& matrix, size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
        {
    	return sub_matrix_type(matrix, morton_dense_sub_ctor(), begin_r, end_r, begin_c, end_c);
        }

        const_sub_matrix_type
        operator()(matrix_type const& matrix, size_type begin_r, size_type end_r, size_type begin_c, size_type end_c)
        {
    	// To minimize code duplication, we use the non-const version
    	sub_matrix_type tmp((*this)(const_cast<matrix_type&>(matrix), begin_r, end_r, begin_c, end_c));
    	return tmp;
        }
    };

}} // mtl::matrix

namespace mtl {

	using matrix::morton_dense;

    // Enable cloning of dense matrices
    template <typename Value, std::size_t BitMask, typename Parameters>
    struct is_clonable< matrix::morton_dense<Value, BitMask, Parameters> > : boost::mpl::true_ {};

} // namespace mtl

#endif // MTL_MORTON_DENSE_INCLUDE