/**
 * @file Transform.hpp
 * @author bwu
 * @brief Model of transform vector, matrix and quaternion concept
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_TRANSFORM_HPP
#define GENERIC_GEOMETRY_TRANSFORM_HPP
#include <boost/qvm/quat_vec_operations.hpp>
#include <boost/qvm/vec_mat_operations.hpp>
#include <boost/qvm/quat_operations.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/vec_operations.hpp>
#include <boost/qvm/vec_traits.hpp>
#include <boost/qvm/quat.hpp>
#include <boost/qvm/mat.hpp>
#include <boost/qvm/vec.hpp>
#include "generic/math/Numbers.hpp"
#include "Geometries.hpp"
#include "Vector.hpp"
namespace boost {
namespace qvm {

using namespace generic::geometry;
template <typename num_type>
struct vec_traits<Vector3D<num_type> >
{
    static const int dim = 3;
    using scalar_type = num_type;

    template <int I>
    static inline scalar_type & write_element(Vector3D<num_type> & vec) { return vec[I]; }

    template <int I>
    static inline scalar_type read_element(const Vector3D<num_type> & vec) { return vec[I]; }

    static inline scalar_type & write_element_idx(int i, Vector3D<num_type> & vec) { return vec[i]; }

    static inline scalar_type read_element_idx(int i, const Vector3D<num_type> & vec) { return vec[i]; }
};

template <typename num_type, size_t N>
struct vec_traits<VectorN<num_type, N> >
{
    static const int dim = static_cast<int>(N);
    using scalar_type = num_type;

    template <int I>
    static inline scalar_type & write_element(VectorN<num_type, N> & vec) { return vec[I]; }

    template <int I>
    static inline scalar_type read_element(const VectorN<num_type, N> & vec) { return vec[I]; }

    static inline scalar_type & write_element_idx(int i, VectorN<num_type, N> & vec) { return vec[i]; }

    static inline scalar_type read_element_idx(int i, const VectorN<num_type, N> & vec) { return vec[i]; }
};
}//namespace qvm
}//namespace boost

namespace generic {
namespace geometry{

template <typename num_type>
using Matrix2x2 = boost::qvm::mat<num_type, 2, 2>;

template <typename num_type>
using Matrix3x3 = boost::qvm::mat<num_type, 3, 3>;

template <typename num_type>
using Matrix4x4 = boost::qvm::mat<num_type, 4, 4>;

template <typename float_t> class Transform2D;
template <typename float_t> class Transform3D;
template <typename float_t> class Quaternion;

///@brief makes a 2d shift transform matrix by a vector2d
template <typename float_t, typename num_type>
inline Transform2D<float_t> makeShiftTransform2D(const Vector2D<num_type> & shift)
{
    Transform2D<float_t> trans;
    trans(0, 2) = float_t(shift[0]);
    trans(1, 2) = float_t(shift[1]);
    return trans;
}

///@brief makes a 2d rotation transform matrix by radian, unit: rad
template <typename float_t>
inline Transform2D<float_t> makeRotateTransform2D(float_t rot)
{
    float_t c = std::cos(rot);
    float_t s = std::sin(rot);
    Transform2D<float_t> trans;
    trans(0, 0) = c; trans(0, 1) = -s;
    trans(1, 0) = s; trans(1, 1) =  c;
    return trans;
}

///@brief makes a 2d scale transform matrix by scale factor
template <typename float_t>
inline Transform2D<float_t> makeScaleTransform2D(float_t scale)
{
    Transform2D<float_t> trans;
    trans(0, 0) = scale; trans(1,1) = scale;
    return trans;
}

///@brief makes a 2d mirror transform matrix by given axis
template <typename float_t>
inline Transform2D<float_t> makeMirroredTransform2D(Axis axis)
{
    Transform2D<float_t> trans;
    if(axis == Axis::X) trans(1, 1) *= -1;
    else if(axis == Axis::Y) trans(0, 0) *= -1;
    return trans;
}

///@brief makes a 3d shift transform matrix by a vector2d
template <typename float_t, typename num_type>
inline Transform3D<float_t> makeShiftTransform3D(const Vector3D<num_type> & shift)
{
    Transform3D<float_t> trans;
    trans(0, 3) = float_t(shift[0]);
    trans(1, 3) = float_t(shift[1]);
    trans(2, 3) = float_t(shift[2]);
    return trans;
}

///@brief makes a 3d scale transform matrix by scale factor
template <typename float_t>
inline Transform3D<float_t> makeScaleTransform3D(float_t scale)
{
    Transform3D<float_t> trans;
    trans(0, 0) = scale; trans(1,1) = scale; trans(2, 2) = scale;
    return trans;
}

template <typename float_t>
class Transform2D
{
    static_assert(std::is_floating_point<float_t>::value, "only floating point type support in transform construct!");
public:
    static const size_t dim = 2;
    Transform2D();
    ///@brief constructs a transform2d model by matrix 3x3
    explicit Transform2D(const Matrix3x3<float_t> & m);

    ///@brief accesses matrix element by row and col index 0-2
    float_t & operator() (size_t row, size_t col);
    const float_t & operator() (size_t row, size_t col) const;

    ///@brief inverses the transform matrix, return false if the matrix det = 0
    bool Inverse();

    ///@brief gets result transform of this * trans
    Transform2D<float_t> operator * (const Transform2D<float_t> & trans) const;
    ///@brief gets result transform of trans * this
    Transform2D<float_t> & Prod(const Transform2D<float_t> & trans);

    ///@brief gets transformed point of input
    template <typename num_type>
    Point2D<num_type> operator * (const Point2D<num_type> & point) const;

    ///@brief gets transformed segment of input
    template <typename num_type>
    Segment2D<num_type> operator * (const Segment2D<num_type> & segment) const;

    ///@brief gets transformed triangle of input
    template <typename num_type>
    Triangle2D<num_type> operator * (const Triangle2D<num_type> & triangle) const;

    ///@brief gets transformed box of input
    template <typename num_type>
    Polygon2D<num_type> operator * (const Box2D<num_type> & box) const;

    ///@brief gets transformed polyline of input
    template <typename num_type>
    Polyline2D<num_type> operator * (const Polyline2D<num_type> & polyline) const;

    ///@brief gets transformed polygon of input
    template <typename num_type>
    Polygon2D<num_type> operator * (const Polygon2D<num_type> & polygon) const;

    ///@brief gets transformed polygon with holes of input
    template <typename num_type>
    PolygonWithHoles2D<num_type> operator * (const PolygonWithHoles2D<num_type> & pwh) const;

    ///@brief accesses internal matrix data
    Matrix3x3<float_t> & GetMatrix() { return m_matrix; }
    const Matrix3x3<float_t> & GetMatrix() const { return m_matrix; }

    ///@brief gets matrix elements in array, adapt for OpenGL API
    void GetCoeffs(float_t coeffs[], bool rowMajor = true);

private:
    Matrix3x3<float_t> m_matrix;
};

template <typename float_t>
class Transform3D
{
    static_assert(std::is_floating_point<float_t>::value, "only floating point type support in transform construct!");
public:
    static const size_t dim = 3;
    Transform3D();
    ///@brief constructs a transform3d model from transform2d
    explicit Transform3D(const Transform2D<float_t> & trans2d);
    ///@brief constructs a transform3d model by matrix 4x4
    explicit Transform3D(const Matrix4x4<float_t> & m);

    ///@brief accesses matrix element by row and col index 0-2
    float_t & operator() (size_t row, size_t col);
    const float_t & operator() (size_t row, size_t col) const;

    ///@brief inverses the transform matrix, return false if the matrix det = 0
    bool Inverse();
    ///@brief gets transform2d(x, y) from transform3d(x, y, z)
    Transform2D<float_t> GetTransfrom2D() const;

    ///@brief gets result transform of this * trans
    Transform3D<float_t> operator * (const Transform3D<float_t> & trans) const;
    ///@brief gets result transform of trans * this
    Transform3D<float_t> & Prod(const Transform3D<float_t> & tran);

    ///@brief gets transformed point of input
    template <typename num_type>
    Point3D<num_type> operator * (const Point3D<num_type> & point) const;

    ///@brief gets transformed segment of input
    template <typename num_type>
    Segment3D<num_type> operator * (const Segment3D<num_type> & segment) const;

    ///@brief gets transformed triangle of input
    template <typename num_type>
    Triangle3D<num_type> operator * (const Triangle3D<num_type> & triangle) const;

    ///@brief accesses internal matrix data
    Matrix4x4<float_t> & GetMatrix() { return m_matrix; }
    const Matrix4x4<float_t> & GetMatrix() const { return m_matrix; }

    ///@brief gets matrix elements in array, adapt for OpenGL API
    void GetCoeffs(float_t coeffs[], bool rowMajor = true);

private:
    Matrix4x4<float_t> m_matrix;
};

template <typename float_t>
class Quaternion
{
    static_assert(std::is_floating_point<float_t>::value, "only floating point type support in transform construct!");
public:
    static const size_t dim = 3;
    Quaternion();
    Quaternion(float_t q0, float_t q1, float_t q2, float_t q3);
    Quaternion(const Vector3D<float_t> & axis, float_t angle);
    Quaternion(const boost::qvm::quat<float_t> & q);

    ///@brief accesses quaternion element by index 0-3
    float_t & operator[](size_t dim);
    const float_t & operator[](size_t dim) const;
    ///@brief multiples this with q
    Quaternion<float_t> & operator *= (const Quaternion<float_t> & q);
    ///@brief gets multiplie result of this * q
    Quaternion<float_t> operator * (const Quaternion<float_t> & q) const;

    ///@brief performs a rotation around the axis at angle radians
    void SetAxisAndAngle(const Vector3D<float_t> & axis, float angle);
    ///@brief gets axis reprented by this quaternion
    Vector3D<float_t> Axis() const;
    ///@brief gets angle reprented by this quaternion
    float_t Angle() const;

    ///@brief multiplicative inverse of this quaternion
    void Invert();
    ///@brief megates all the coefficients of this quaternion
    void Negate();
    ///@brief normalizes the quaternion oefficients with unit quaternions
    void Normalize();
    ///@brief returns the magnitude of this
    float_t Mag() const;
    ///@brief returns the squared magnitude of this
    float_t MagSqrt() const;
    Quaternion<float_t> Log() const;
    Quaternion<float_t> Exp() const;
    Quaternion<float_t> Inverse() const;
    Vector3D<float_t> Rotate(const Vector3D<float_t> & vec) const;
    Vector3D<float_t> InverseRotate(const Vector3D<float_t> & vec) const;

    ///@brief returns dot product of this and q
    float_t Dot(const Quaternion<float_t> & q) const;

    ///@brief accesses internal quat data
    boost::qvm::quat<float_t> & q() { return m_q; }
    const boost::qvm::quat<float_t> & q() const { return m_q; }

    static Quaternion<float_t> LnDif(const Quaternion<float_t> &a, const Quaternion<float_t> & b);
    static Quaternion<float_t> SquadTangent(const Quaternion<float_t> & before,
                                            const Quaternion<float_t> & center,
                                            const Quaternion<float_t> & after);
    static Quaternion<float_t> Squad(const Quaternion<float_t> & a, const Quaternion<float_t> & tgA,
                                     const Quaternion<float_t> & b, const Quaternion<float_t> & tgB, float_t t);

    ///@brief returns the result of spherical linear interpolation of the input quat `a`, `b` and interpolation parameter `t`
    static Quaternion<float_t> Slerp(const Quaternion<float_t> & a, const Quaternion<float_t> & b, float_t t);

private:
    boost::qvm::quat<float_t> m_q;
};

template <typename float_t>
inline Transform2D<float_t>::Transform2D()
{
    boost::qvm::set_identity(m_matrix);
}

template <typename float_t>
inline Transform2D<float_t>::Transform2D(const Matrix3x3<float_t> & m)
{
    boost::qvm::assign(m_matrix, m);
}

template <typename float_t>
inline float_t & Transform2D<float_t>::operator() (size_t row, size_t col)
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline const float_t & Transform2D<float_t>::operator() (size_t row, size_t col) const
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline bool Transform2D<float_t>::Inverse()
{
    float_t det = boost::qvm::determinant(m_matrix);
    if(math::EQ(det, float_t(0))) return false;
    m_matrix = boost::qvm::inverse(m_matrix, det);
    return true;
}

template <typename float_t>
inline Transform2D<float_t> Transform2D<float_t>::operator * (const Transform2D<float_t> & trans) const
{
    using namespace boost::qvm;
    return Transform2D<float_t>(m_matrix * trans.GetMatrix());
}

template <typename float_t>
inline Transform2D<float_t> & Transform2D<float_t>::Prod(const Transform2D<float_t> & trans)
{
    m_matrix = trans.GetMatrix() * m_matrix;
    return *this;
}

template <typename float_t>
template <typename num_type>
inline Point2D<num_type> Transform2D<float_t>::operator * (const Point2D<num_type> & point) const
{
    Point2D<num_type> res(point);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Segment2D<num_type> Transform2D<float_t>::operator * (const Segment2D<num_type> & segment) const
{
    Segment2D<num_type> res(segment);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Triangle2D<num_type> Transform2D<float_t>::operator * (const Triangle2D<num_type> & triangle) const
{
    Triangle2D<num_type> res(triangle);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Polygon2D<num_type> Transform2D<float_t>::operator * (const Box2D<num_type> & box) const
{
    Point2D<num_type> p[4] = { box[0], {box[1][0], box[0][1]}, box[1], {box[0][0], box[1][1]} };
    Polygon2D<num_type> res;
    for(size_t i = 0; i < 4; ++i){
        Transform(p[i], *this);
        res << p[i];
    }
    return res;
}

template <typename float_t>
template <typename num_type>
inline Polyline2D<num_type> Transform2D<float_t>::operator * (const Polyline2D<num_type> & polyline) const
{
    Polyline2D<num_type> res = polyline;
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Polygon2D<num_type>  Transform2D<float_t>::operator * (const Polygon2D<num_type> & polygon) const
{
    Polygon2D<num_type> res(polygon);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline PolygonWithHoles2D<num_type> Transform2D<float_t>::operator * (const PolygonWithHoles2D<num_type> & pwh) const
{
    PolygonWithHoles2D<num_type> res(pwh);
    Transform(res, *this);
    return res;
}

template <typename float_t>
inline void Transform2D<float_t>::GetCoeffs(float_t coeffs[], bool rowMajor)
{
    if(rowMajor){
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[i][j];
    }
    else{
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[j][i];
    }
}

template <typename float_t>
inline Transform3D<float_t>::Transform3D()
{
    boost::qvm::set_identity(m_matrix);
}

template <typename float_t>
inline Transform3D<float_t>::Transform3D(const Transform2D<float_t> & trans2d)
{
    boost::qvm::set_identity(m_matrix);
    for(size_t i = 0; i < 2; ++i)
        for(size_t j = 0; j < 2; ++j)
            m_matrix.a[i][j] = trans2d(i, j);
    m_matrix.a[0][3] = trans2d(0, 2);
    m_matrix.a[1][3] = trans2d(1, 2);
}

template <typename float_t>
inline Transform3D<float_t>::Transform3D(const Matrix4x4<float_t> & m)
{
    boost::qvm::assign(m_matrix, m);
}

template <typename float_t>
inline float_t & Transform3D<float_t>::operator() (size_t row, size_t col)
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline const float_t & Transform3D<float_t>::operator() (size_t row, size_t col) const
{
    return m_matrix.a[row][col];
}

template <typename float_t>
inline bool Transform3D<float_t>::Inverse()
{
    float_t det = boost::qvm::determinant(m_matrix);
    if(math::EQ(det, float_t(0))) return false;
    m_matrix = boost::qvm::inverse(m_matrix, det);
    return true;
}

template <typename float_t>
inline Transform2D<float_t> Transform3D<float_t>::GetTransfrom2D() const
{
    Transform2D<float_t> trans;
    for(size_t i = 0; i < 2; ++i)
        for(size_t j = 0; j < 2; ++j)
            trans(i, j) = m_matrix.a[i][j];
    trans(0, 2) = m_matrix.a[0][3];
    trans(1, 2) = m_matrix.a[1][3];
    return trans;
}

template <typename float_t>
inline Transform3D<float_t> Transform3D<float_t>::operator * (const Transform3D<float_t> & trans) const
{
    using namespace boost::qvm;
    return Transform3D<float_t>(m_matrix * trans.GetMatrix());
}

template <typename float_t>
inline Transform3D<float_t> & Transform3D<float_t>::Prod(const Transform3D<float_t> & trans)
{
    m_matrix = trans.GetMatrix() * m_matrix;
    return *this;
}

template <typename float_t>
template <typename num_type>
inline Point3D<num_type> Transform3D<float_t>::operator * (const Point3D<num_type> & point) const
{
    Point3D<num_type> res(point);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Segment3D<num_type> Transform3D<float_t>::operator * (const Segment3D<num_type> & segment) const
{
    Segment3D<num_type> res(segment);
    Transform(res, *this);
    return res;
}

template <typename float_t>
template <typename num_type>
inline Triangle3D<num_type> Transform3D<float_t>::operator * (const Triangle3D<num_type> & triangle) const
{
    Triangle3D<num_type> res(triangle);
    Transform(res, *this);
    return res;
}

template <typename float_t>
inline void Transform3D<float_t>::GetCoeffs(float_t coeffs[], bool rowMajor)
{
    if(rowMajor){
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[i][j];
    }
    else{
        for(size_t i = 0; i <= dim; ++i)
            for(size_t j = 0; j <= dim; ++j)
                coeffs[(dim + 1) * i + j] = m_matrix.a[j][i];    }
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion()
{
    m_q.a[0] = 1;
    m_q.a[1] = m_q.a[2] = m_q.a[3] = 0;
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion(float_t q0, float_t q1, float_t q2, float_t q3)
{
    m_q.a[0] = q0;
    m_q.a[1] = q1; m_q.a[2] = q2; m_q.a[3] = q3;
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion(const Vector3D<float_t> & axis, float_t angle)
{
    SetAxisAndAngle(axis, angle);
}

template <typename float_t>
inline Quaternion<float_t>::Quaternion(const boost::qvm::quat<float_t> & q)
{
    boost::qvm::assign(m_q, q);
}

template <typename float_t>
inline float_t & Quaternion<float_t>::operator[](size_t dim)
{
    return m_q.a[dim];
}

template <typename float_t>
inline const float_t & Quaternion<float_t>::operator[](size_t dim) const
{
    return m_q.a[dim];
}

template <typename float_t>
inline Quaternion<float_t> & Quaternion<float_t>::operator *= (const Quaternion<float_t> & q)
{
    *this = (*this) * q;
    return *this;
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::operator * (const Quaternion<float_t> & q) const
{
    return Quaternion<float_t>(m_q * q.q());
}

template <typename float_t>
inline void Quaternion<float_t>::SetAxisAndAngle(const Vector3D<float_t> & axis, float angle)
{
    boost::qvm::set_rot(m_q, axis, angle);
}

template <typename float_t>
inline Vector3D<float_t> Quaternion<float_t>::Axis() const
{
    Vector3D<float_t> res(m_q.a[1], m_q.a[2], m_q.a[3]);
    float_t sinus = res.Norm2();
    if(sinus > std::numeric_limits<float_t>::epsilon()) res /= sinus;
    return math::LE(std::acos(m_q.a[0]), float_t(math::pi_half)) ? res : -res;
}

template <typename float_t>
float_t Quaternion<float_t>::Angle() const
{
    float_t angle = 2.0 * std::acos(m_q.a[0]);
    return math::LE(angle, float_t(math::pi)) ? angle : math::pi_2 - angle;
}

template <typename float_t>
inline void Quaternion<float_t>::Invert()
{
    boost::qvm::assign(m_q, boost::qvm::inverse(m_q));
}

template <typename float_t>
inline void Quaternion<float_t>::Negate()
{
    Invert();
    m_q.a[0] = -m_q.a[0];
}

template <typename float_t>
inline void Quaternion<float_t>::Normalize()
{
    boost::qvm::normalize(m_q);
}

template <typename float_t>
inline float_t Quaternion<float_t>::Mag() const
{
    return boost::qvm::mag(m_q);
}

template <typename float_t>
inline float_t Quaternion<float_t>::MagSqrt() const
{
    return boost::qvm::mag_sqr(m_q);
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Log() const
{
    float_t len = std::sqrt(m_q.a[1] * m_q.a[1] + m_q.a[2] * m_q.a[2] + m_q.a[3] * m_q.a[3]);
    if(len < std::numeric_limits<float_t>::epsilon())
        return Quaternion<float_t>(0, m_q.a[1], m_q.a[2], m_q.a[3]);
    else{
        float_t coef = std::acos(m_q.a[0]) / len;
        return Quaternion<float_t>(0, coef * m_q.a[1], coef * m_q.a[2], coef * m_q.a[3]);
    }
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Exp() const
{
    float_t theta = std::sqrt(m_q.a[1] * m_q.a[1] + m_q.a[2] * m_q.a[2] + m_q.a[3] * m_q.a[3]);
    if(theta < std::numeric_limits<float_t>::epsilon())
        return Quaternion<float_t>(std::cos(theta), m_q.a[1], m_q.a[2], m_q.a[3]);
    else{
        float_t coef = std::sin(theta) / theta;
        return Quaternion<float_t>(std::cos(theta), coef * m_q.a[1], coef * m_q.a[2], coef * m_q.a[3]);
    }
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Inverse() const
{
    return Quaternion<float_t>(boost::qvm::inverse(m_q));
}

template <typename float_t>
inline Vector3D<float_t> Quaternion<float_t>::Rotate(const Vector3D<float_t> & vec) const
{
   using namespace boost::qvm;
   return m_q * vec;
}

template <typename float_t>
inline Vector3D<float_t> Quaternion<float_t>::InverseRotate(const Vector3D<float_t> & vec) const
{
    return Inverse().Rotate(vec);
}

template <typename float_t>
inline float_t Quaternion<float_t>::Dot(const Quaternion<float_t> & q) const
{
    return boost::qvm::dot(m_q, q.q());
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::LnDif(const Quaternion<float_t> &a, const Quaternion<float_t> & b)
{
    auto dif = a.Inverse() * b;
    dif.Normalize();
    return dif.Log();
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::SquadTangent(const Quaternion<float_t> & before,
                                                                const Quaternion<float_t> & center,
                                                                    const Quaternion<float_t> & after)
{
    auto l1 = LnDif(center, before);
    auto l2 = LnDif(center, after);
    Quaternion<float_t> e;
    for(size_t i = 0; i < 4; ++i)
        e[i] = -0.25 * (l1[i] + l2[2]);
    e = center * e.Exp();
    return e;
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Squad(const Quaternion<float_t> & a, const Quaternion<float_t> & tgA,
                                                        const Quaternion<float_t> & b, const Quaternion<float_t> & tgB, float_t t)
{
    auto ab = Slerp(a, b, t);
    auto tg = Slerp(tgA, tgB, t);
    return Slerp(ab, tg, 2.0 * t * (1.0 - t));
}

template <typename float_t>
inline Quaternion<float_t> Quaternion<float_t>::Slerp(const Quaternion<float_t> & a, const Quaternion<float_t> & b, float_t t)
{
    using namespace boost::qvm;
    return Quaternion<float_t>(slerp(a.q(), b.q(), t));
}

///@brief transforms the point by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Point2D<num_type> & point, const Transform2D<float_t> & trans)
{
    VectorN<float_t, 3> vec = trans.GetMatrix() * VectorN<float_t, 3>(point[0], point[1], float_t(1));
    point = Point2D<num_type>(vec[0], vec[1]);
}

///@brief transforms the segment by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Segment2D<num_type> & segment, const Transform2D<float_t> & trans)
{
    Transform(segment[0], trans);
    Transform(segment[1], trans);
}

///@brief transforms the triangle by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Triangle2D<num_type> & triangle, const Transform2D<float_t> & trans)
{
    Transform(triangle[0], trans);
    Transform(triangle[1], trans);
    Transform(triangle[2], trans);
}

///@brief transforms the polyline by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Polyline2D<num_type> & polyline, const Transform2D<float_t> & trans)
{
    for(auto & point : polyline)
        Transform(point, trans);
}

///@brief transforms the polygon by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Polygon2D<num_type> & polygon, const Transform2D<float_t> & trans)
{
    for(auto iter = polygon.Begin(); iter != polygon.End(); ++iter)
        Transform(*iter, trans);
}

///@brief transforms the polygon with holes by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(PolygonWithHoles2D<num_type> & pwh, const Transform2D<float_t> & trans)
{
    Transform(pwh.outline, trans);
    auto iter = pwh.BeginHoles();
    for(; iter != pwh.EndHoles(); ++iter)
        Transform(*iter, trans);
}

///@brief transforms the point by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Point3D<num_type> & point, const Transform3D<float_t> & trans)
{
    VectorN<float_t, 4> vec(trans.GetMatrix() * VectorN<float_t, 4>(point[0], point[1], point[2], float_t(1)));
    point = Point3D<num_type>(vec[0], vec[1], vec[2]);
}

///@brief transforms the segment by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Segment3D<num_type> & segment, const Transform3D<float_t> & trans)
{
    Transform(segment[0], trans);
    Transform(segment[1], trans);
}

///@brief transforms the triangle by the transform matrix
template <typename num_type, typename float_t>
inline void Transform(Triangle3D<num_type> & triangle, const Transform3D<float_t> & trans)
{
    Transform(triangle[0], trans);
    Transform(triangle[1], trans);
    Transform(triangle[2], trans);
}

///@brief transfroms a collection of geometries by the transform matrix
template <typename geometry_t, typename iterator, typename transform_t,
          typename std::enable_if<geometry_t::dim == transform_t::dim && std::is_same<geometry_t,
          typename std::iterator_traits<iterator>::value_type>::value, bool>::type = true>
inline void Transform(iterator begin, iterator end, const transform_t & trans)
{
    for(auto iter = begin; iter != end; ++iter){
        geometry_t & geom = *iter;
        Transform(geom, trans);
    }
}
}//namespace geometry
}//namespace generic
#endif//GENERIC_GEOMETRY_TRANSFORM_HPP


//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is supplied under the BSD license agreement (see license.txt)

#include <math.h>

#include <Eigen/Eigenvalues>

#include <boost/timer.hpp>

#include "AbstractMatrix.h"

namespace abstract{

/// Constructs an empty buffer
template <class scalar>
AbstractMatrix<scalar>::AbstractMatrix(int dimension) : AccelMatrix<scalar>(dimension),
  //m_roundDimension(dimension),
  m_abstractDynamics(dimension),
  m_abstractInputDynamics(dimension),
  m_abstractRoundDynamics(dimension)
  {
    m_abstractDynamics.setName("Abstract Dynamics");
    m_abstractRoundDynamics.setName("Round Dynamics");
  }

/// Changes the default dimension of the system
template <class scalar>
void AbstractMatrix<scalar>::changeDimensions(const int dimension)
{
  AccelMatrix<scalar>::changeDimensions(dimension);
  m_abstractDynamics.changeDimension(dimension);
  m_abstractInputDynamics.changeDimension(dimension);
  m_abstractRoundDynamics.changeDimension(dimension);
}

template <class scalar>
void AbstractMatrix<scalar>::swapValues(scalar &low,scalar &high)
{
  scalar temp=low;
  low=high;
  high=temp;
}

template <class scalar>
inline void AbstractMatrix<scalar>::checkRange(const int row,const powerS iteration,const scalar mag,scalar &min,scalar &max)
{
  if (func::isPositive(min-max)) {
    scalar temp=min;
    min=max;
    max=temp;
  }
  if ((m_zeniths[row]>0) && (m_zeniths[row]<=iteration)) {
    scalar mid=condPow(mag,m_zeniths[row],row);
    if (func::isPositive(mid-max)) max=mid;
    if (m_zeniths[row]<iteration) {
      mid=condPow(mag,m_zeniths[row]+1,row);
      if (func::toLower(mid)>func::toUpper(max)) max=mid;
    }
  }
  if (m_freq[row]>0) {
    if (m_freq[row]<=iteration) {
    }
    else if (row>m_dimension) {
      if (func::toUpper(min)<-func::toLower(max)) min=-max;
      else max=-min;
    }
  }
}

template <class scalar>
typename JordanMatrix<scalar>::complexS AbstractMatrix<scalar>::condPow(const complexS &coef,const powerS n,int row)
{
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      scalar mag=ms_one-func::pow(func::norm2(coef),n);
      if (func::isPositive(mag)) {
        return complexS(mag*this->m_foldedBinomialMultipliers.coeff(row-m_dimension),0);
      }
    }
    if (m_jordanIndex[row]==0) {
      if (func::isZero(func::norm2(coef-ms_complexOne))) return complexS(n,0);
      return this->ms_complexOne-func::c_pow(coef,n);
    }
    if (n<m_jordanIndex[row]) return complexS(0,0);
    if (func::isZero(func::norm2(coef-ms_complexOne))) {
      if (n<=m_jordanIndex[row]) return complexS(0,0);
      complexS mult(binomial(n,m_jordanIndex[row]+1),0);
      return mult;
    }
    complexS mult(binomial(n,m_jordanIndex[row]),0);
    return -mult*func::c_pow(coef,n-m_jordanIndex[row]);
  }
  if (m_jordanIndex[row]==0) {
    if (n==1) return coef;
    return func::c_pow(coef,n);
  }
  if (n<m_jordanIndex[row]) return complexS(0,0);
  complexS mult(binomial(n,m_jordanIndex[row]),0);
  return mult*func::c_pow(coef,n-m_jordanIndex[row]);
}

template <class scalar>
scalar AbstractMatrix<scalar>::condPow(const scalar &coef,const powerS n,int row)
{
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      scalar mag=ms_one-func::pow(coef,n);
      if (func::isPositive(mag)) {
        return mag*this->m_foldedBinomialMultipliers.coeff(row-m_dimension);
      }
    }
    if (m_jordanIndex[row]==0) {
      if (abs(coef-ms_one)<m_zero) return scalar(n);
      return ms_one-func::pow(coef,n);
    }
    if (n<m_jordanIndex[row])  return 0;
    if (abs(coef-ms_one)<m_zero) {
      if (n<=m_jordanIndex[row])  return 0;
      return binomial(n,m_jordanIndex[row]+1);
    }
    return -binomial(n,m_jordanIndex[row])*func::pow(coef,n-m_jordanIndex[row]);
  }

  if (m_jordanIndex[row]==0) return func::pow(coef,n);
  if (n<m_jordanIndex[row])  return 0;
  return binomial(n,m_jordanIndex[row])*func::pow(coef,n-m_jordanIndex[row]);
}

template <class scalar>
scalar AbstractMatrix<scalar>::diffPow(const scalar &coef,const powerS n,int row)
{
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      scalar mag=ms_one-func::pow(coef,n);
      if (func::isPositive(mag)) {
        mag=func::pow(coef,n)-func::pow(coef,n+1);
        return mag*this->m_foldedBinomialMultipliers.coeff(row-m_dimension);
      }
    }
    if (m_jordanIndex[row]==0) {
      if (abs(coef-ms_one)<m_zero) return ms_one;
      return func::pow(coef,n)-func::pow(coef,n+1);
    }
    if (n<m_jordanIndex[row])  return 0;
    if (abs(coef-ms_one)<m_zero) {
      if (n<m_jordanIndex[row])  return 0;
      return binomial(n,m_jordanIndex[row]);
    }
    return (binomial(n,m_jordanIndex[row])-binomial(n+1,m_jordanIndex[row])*coef)*func::pow(coef,n-m_jordanIndex[row]);
  }
  if (m_jordanIndex[row]==0) return (coef-ms_one)*func::pow(coef,n);
  if (n<m_jordanIndex[row])  return 0;
  return (binomial(n+1,m_jordanIndex[row])*coef-binomial(n,m_jordanIndex[row]))*func::pow(coef,n-m_jordanIndex[row]);
}

/// Calculates the expected iteration of coefficient to obtain value
template <class scalar>
typename AbstractMatrix<scalar>::powerS AbstractMatrix<scalar>::condLog(const refScalar &coef,refScalar value,int row,powerS &range)
{
  range=0;
  if (row>=m_dimension) {
    if (m_inputType==eParametricInputs) row-=m_dimension;
    else {
      if (func::isPositive(value)) value/=func::toCentre(this->m_foldedBinomialMultipliers.coeff(row-m_dimension));
      value=func::ms_1-value;
      if (func::isNegative(value)) return 0;
      return func::toInt(func::log(coef,value));
    }
    if (m_jordanIndex[row]==0) {
      if (abs(coef-ms_one)<m_zero) return func::toInt(value);
      value=func::ms_1-value;
      if (func::isNegative(value)) return 0;
      return func::toInt(func::log(coef,value));
    }
    range=m_jordanIndex[row]-1;
    if (func::isZero(value)) return 1;
    if (abs(coef-ms_one)<m_zero) {
      for (int i=2;i<=m_jordanIndex[row]+1;i++) value*=i;
      refScalar expected=pow(value,1/m_jordanIndex[row]+1);
      return func::toInt(expected);
    }
    value=-value;
    return binomialCondLog(coef,value,row,range);
  }
  if (m_jordanIndex[row]==0) return func::toInt(func::log(coef,value));
  range=m_jordanIndex[row]-1;
  if (func::isZero(value)) return 1;
  return binomialCondLog(coef,value,row,range);
}

/// Calculates the expected iteration of coefficient to obtain value for a jordan diagonal
template <class scalar>
typename AbstractMatrix<scalar>::powerS AbstractMatrix<scalar>::binomialCondLog(const refScalar &coef,refScalar value,int row,powerS &range)
{
  for (int i=2;i<=m_jordanIndex[row];i++) value*=i;
  value/=func::pow(coef,m_jordanIndex[row]);
  powerS low,high;
  if (coef>func::ms_1) {
    high=func::toInt(func::log(coef,value));
    refScalar bin=func::toCentre(binomial(high,m_jordanIndex[row]));
    low=func::toInt(func::log(coef,value/bin));
  }
  else {
    low=func::toInt(pow(value,1/m_jordanIndex[row]));
    refScalar bin=func::toCentre(binomial(low,m_jordanIndex[row]));
    high=func::toInt(func::log(coef,value/bin));
  }
  range=high-low;
  return low;
}

template <class scalar>
void AbstractMatrix<scalar>::fillDirection(const int row,const scalar &min,const scalar &max)
{
  m_directions.coeffRef(row,m_pos)=1;
  m_supports.coeffRef(m_pos++,0)=max;
  m_directions.coeffRef(row,m_pos)=-1;
  m_supports.coeffRef(m_pos++,0)=-min;
}

template <class scalar>
void AbstractMatrix<scalar>::fillDirections(const int row1,const int row2,const scalar &dir1,const scalar &dir2,const scalar &min,const scalar &max)
{
  m_supports.coeffRef(m_pos,0)=-min;
  m_directions.coeffRef(row1,m_pos)=-dir1;
  m_directions.coeffRef(row2,m_pos++)=-dir2;
  m_supports.coeffRef(m_pos,0)=max;
  m_directions.coeffRef(row1,m_pos)=dir1;
  m_directions.coeffRef(row2,m_pos++)=dir2;
}

template <class scalar>
void AbstractMatrix<scalar>::fillConjugateSupport(const int row1,const int row2,const scalar &angle,const scalar &max)
{
  m_directions.coeffRef(row1,m_pos)=func::cosine(angle);
  m_directions.coeffRef(row2,m_pos)=func::sine(angle);
  m_supports.coeffRef(m_pos++)=max;
}

template <class scalar>
void AbstractMatrix<scalar>::fillSupportFromPoints(const int row1,const int row2,const scalar &x1,const scalar &y1,const scalar &x2,const scalar &y2,const scalar &xRef,const scalar &yRef)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=yRef*func::sine(angle)+xRef*func::cosine(angle);
  out-=max;
  if (func::isPositive(out)) {
    angle+=func::const_pi(ms_one);
    max=-max;
  }
  fillConjugateSupport(row1,row2,angle,max);
}

template <class scalar>
void AbstractMatrix<scalar>::fillTangentSupport(const int row1,const int row2,const scalar &mag1,const scalar &mag2,powerS iter)
{
  scalar angle=func::const_pi(this->ms_half);
  scalar quotient=mag2/mag1;
  scalar slope=iter;
  slope*=func::pow(quotient,iter-1);
  char sign=func::hardSign(mag1);
  if (sign==0) return;
  if (sign<0)  angle+=atan(slope)+func::ms_pi;
  else         angle+=atan(slope);
  scalar max=func::pow(mag2,iter)*func::sine(angle)+func::pow(mag1,iter)*func::cosine(angle);
  fillConjugateSupport(row1,row2,angle,max);
}

template <class scalar>
void AbstractMatrix<scalar>::fillSupportFromIter(const int row1,const int row2,const scalar &mag1,const scalar &mag2,powerS iter)
{
  scalar xdif=diffPow(mag1,iter,row1);
  scalar ydif=diffPow(mag2,iter,row2);
  scalar angle=func::invtan(ydif,xdif)+func::const_pi(this->ms_half);
  scalar x1=condPow(mag1,iter,row1);
  scalar y1=condPow(mag2,iter,row2);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=mag2*func::sine(angle)+mag1*func::cosine(angle);
  out-=max;
  if (func::isPositive(out)) {
    angle+=func::const_pi(ms_one);
    max=-max;
  }
  fillConjugateSupport(row1,row2,angle,max);
}

template <class scalar>
void AbstractMatrix<scalar>::fillSemiConjugateSupportFromIter(const int row1,const int row2,const scalar &mag1,const scalar &mag2,powerS iter)
{
  powerS iter2=iter+1;
  scalar x1=condPow(mag1,iter,row1);
  scalar y1=condPow(mag2,iter,row2);
  scalar x2=condPow(mag1,iter2,row1);
  scalar y2=condPow(mag2,iter2,row2);
  fillSupportFromPoints(row1,row2,x1,y1,x2,y2,mag1,mag2);
  fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,-mag1,mag2);
}

template <class scalar>
bool AbstractMatrix<scalar>::fillLastConjugateSupportFromPoints(int row1,int row2,scalar x1,scalar y1,scalar x2,scalar y2,scalar xRef,scalar yRef,bool hasLastQuarter)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=yRef*func::sine(angle)+xRef*func::cosine(angle);
  char sign=func::hardSign(max);
  if (sign<0) {//the vector is in the wrong direction rotate 180deg
    angle+=func::const_pi(ms_one);
    max=-max;
    out=-out;
  }
  else if (sign==0) {// Undecided, overapproximate by error.
    max+=m_zero;
    out+=m_zero;
  }
  out-=max;
  sign=func::hardSign(out);
  if (hasLastQuarter) {
    //If the next to last point would be excluded by the restriction, ignore
    if (sign>0) return false;
    //If the next to last point is undecided around the restriction, make sure it remains inside
    if (sign==0) max+=m_zero;
  }
  else if (sign>0) {
    //Revert the bound to close the half-circle
    angle+=func::const_pi(ms_one);
    max=y1*func::sine(angle)+x1*func::cosine(angle);
  }
  else if (sign==0) {
    //should never happen
    func::imprecise(out,func::ms_hardZero);
    return false;
  }
  else {
    //should never happen but no error
    return false;
  }
  fillConjugateSupport(row1,row2,angle,max);
  return true;
}

template <class scalar>
bool AbstractMatrix<scalar>::testConjugateSupportFromPoints(const scalar &x1,const scalar &y1,const scalar &x2,const scalar &y2,const scalar &xExt,const scalar &yExt)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar ext=yExt*func::sine(angle)+xExt*func::cosine(angle);
  char sign=func::hardSign(max);
  if (sign<0) {
    angle+=func::const_pi(ms_one);
    max=-max;
    ext=-ext;
  }
  else if (sign==0) {
    //should never happen
    func::imprecise(max,func::ms_hardZero);
  }
  ext-=max;
  sign=func::hardSign(ext);
  if (sign>0) return false;
  //if ext is zero, the case is dealt when calculating the support function
  return true;
}

template <class scalar>
bool AbstractMatrix<scalar>::fillConjugateSupportFromPoints(const int row1,const int row2,const scalar &x1,const scalar &y1,const scalar &x2,const scalar &y2,const scalar &xExt,const scalar &yExt)
{
  scalar angle=func::invtan(y2-y1,x2-x1)+func::const_pi(this->ms_half);
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar ext=yExt*func::sine(angle)+xExt*func::cosine(angle);
  char sign=func::hardSign(max);
  if (sign<0) {
    angle+=func::const_pi(ms_one);
    max=-max;
    ext=-ext;
  }
  else if (sign==0) {
    //should never happen
    func::imprecise(max,func::ms_hardZero);
  }
  ext-=max;
  sign=func::hardSign(ext);
  if (sign>0) return false;
  if (sign==0) max+=m_zero;
  fillConjugateSupport(row1,row2,angle,max);
  return true;
}

template <class scalar>//template <>
void AbstractMatrix<scalar>::fillLinearSupportFromPoints(const int row1,const int row2,const scalar &min1,const scalar &max1,const scalar &min2,const scalar &max2)
{
  scalar angle=func::invtan(max2-min2,max1-min1)+func::const_pi(this->ms_half);
  scalar min=min2*func::sine(angle)+min1*func::cosine(angle);
  scalar max=max2*func::sine(angle)+max1*func::cosine(angle);
  max=func::getHull(min,max);
  min=-func::toLower(max)+this->m_largeZero;
  max=func::toUpper(max)+this->m_largeZero;
  fillConjugateSupport(row1,row2,angle,max);
  angle+=func::const_pi(ms_one);
  fillConjugateSupport(row1,row2,angle,min);
}

template <class scalar>
void AbstractMatrix<scalar>::fillQuadraticSupport(int row1,int row2,scalar mag1,scalar mag2,powerS iteration,int precision)
{
  precision=(1<<precision)-2;//Number of faces grows geometrically
  if (precision<1) return;
  refScalar iterationStep=iteration/precision;
  if (iterationStep<1) {
    iterationStep=1;
    precision=iteration;
  }
  int firstIter=1;
  if (m_jordanIndex[row1%m_dimension]>0) firstIter=m_jordanIndex[row1%m_dimension];
  if (m_jordanIndex[row2%m_dimension]>0) firstIter=m_jordanIndex[row2%m_dimension];
  for (int i=firstIter;i<precision;i++) {
    powerS iter1=func::toInt(floor(i*iterationStep));
    if (iter1==1) {
      powerS iter2=iter1+1;
      scalar x1=condPow(mag1,iter1,row1);
      scalar y1=condPow(mag2,iter1,row2);
      scalar x2=condPow(mag1,iter2,row1);
      scalar y2=condPow(mag2,iter2,row2);
      scalar x3=condPow(mag1,3,row1);
      scalar y3=condPow(mag2,3,row2);
      fillSupportFromPoints(row1,row2,x1,y1,x2,y2,x3,y3);
    }
    else fillSupportFromIter(row1,row2,mag1,mag2,iter1);
  //    else fillSupportFromPoints(row1,row2,x1,y1,x2,y2,mag1,mag2);
  }
}

template <class scalar>
void AbstractMatrix<scalar>::fillQuadraticConjugateSupport(int row1,int row2,scalar mag1,scalar mag2,powerS iteration,int precision)
{
  scalar x1=condPow(mag1,1,row1);
  scalar y1=condPow(mag2,1,row2);
  scalar x2=condPow(mag1,iteration,row1);
  scalar y2=condPow(mag2,iteration,row2);
  scalar x3=condPow(mag1,2,row1);
  scalar y3=condPow(mag2,2,row2);
  scalar angle=func::invtan(y2-y1,x2-x1)-func::const_pi(this->ms_half);//invtan returns positive values (0-pi)
  scalar max=y1*func::sine(angle)+x1*func::cosine(angle);
  scalar out=y3*func::sine(angle)+x3*func::cosine(angle);
  max-=out;
  if (func::isNegative(max)) {//TODO: need to split for jordan blocks
    precision=(1<<precision)-2;//Number of faces grows geometrically
    if (precision<1) return;
    scalar mag=condPow(mag1,iteration,row1);
    while(func::isZero(mag)) {
      iteration>>=2;
      mag=condPow(mag1,iteration,row1);
    }
    refScalar iterationStep=iteration/precision;
    if (iterationStep<1) {
      iterationStep=1;
      precision=iteration;
    }
    for (int i=1;i<precision;i++) {
      powerS iter1=func::toInt(floor(i*iterationStep));
      powerS iter2=iter1+1;
      scalar x1=condPow(mag1,iter1,row1);
      scalar y1=condPow(mag2,iter1,row2);
      scalar x2=condPow(mag1,iter2,row1);
      scalar y2=condPow(mag2,iter2,row2);
      if (iter1==1) {
        scalar x3=condPow(mag1,3,row1);
        scalar y3=condPow(mag2,3,row2);
        fillSupportFromPoints(row1,row2,x1,y1,x2,y2,x3,y3);
        fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,-x3,y3);
      }
      else {
        fillSupportFromPoints(row1,row2,x1,y1,x2,y2,mag1,mag2);
        fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,-mag1,mag2);
      }
    }
    return;
  }
  fillSupportFromPoints(row1,row2,x1,y1,x2,y2,0,y1);
  fillSupportFromPoints(row1,row2,-x1,y1,-x2,y2,0,y1);
}

template <class scalar>
void AbstractMatrix<scalar>::findConjugateSupports(const int row,const complexS &coef,const powerS iteration,int precision)
{
  if (m_conjugatePair[row]<row) return;
  precision=(1<<precision);//Number of faces grows geometrically
  int row2=row+1;
  if (iteration<=2) {
    complexS p1=condPow(coef,1,row);
    scalar minR=p1.real();
    scalar minI=p1.imag();
    if (iteration==2) {
      complexS p2=condPow(coef,2,row);
      scalar maxR=p2.real();
      scalar maxI=p2.imag();
      fillLinearSupportFromPoints(row,row2,minR,maxR,minI,maxI);
      if (func::isPositive(minR-maxR)) swapValues(minR,maxR);
      if (func::isPositive(minI-maxI)) swapValues(minI,maxI);
      fillDirection(row,minR,maxR);
      fillDirection(row2,minI,maxI);
    }
    else {
      fillDirection(row,minR,minR);
      fillDirection(row2,minI,minI);
    }
    return;
  }
  scalar mag=func::norm2(coef);
  scalar angle=func::invtan(coef.imag(),coef.real());
  scalar quartFreq=abs(func::const_pi(this->ms_half)/angle);
  powerS offset=func::toInt(trunc(4*func::toLower(quartFreq)));
  offset+=m_jordanIndex[row]+1;
  powerS start=(func::isPositive(mag-ms_one)) ? iteration : 1;
  powerS end=func::isPositive(mag-ms_one) ? ((iteration>offset) ? iteration-offset : 0) : ((iteration>offset) ? offset : iteration);
  int dir=((end-start)>0) ? 1 : -1;
  powerS step=(end-start)/precision;
  if (step==0) step=dir;
  complexS ext=condPow(coef,start+2*dir,row);
  complexS p1=condPow(coef,start,row);
  complexS p2=condPow(coef,start+dir,row);
  fillConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag());
  start+=step;
  ext=condPow(coef,func::isPositive(mag-ms_one) ? iteration : 1,row2);
  while (abs(end-start)>=abs(step)) {
    p1=condPow(coef,start,row);
    p2=condPow(coef,start+dir,row);
    if (!fillConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) break;
    start+=step;
  }
  if (abs(end-start)<abs(step)) {
    p1=condPow(coef,end,row);
    p2=condPow(coef,end-dir,row);
    if ((abs(end-start)<=abs(dir)) || fillConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) {
      start=end;
      p1=condPow(coef,start,row);
      p2=ext;
      ext=condPow(coef,start-dir,row);
      if (fillLastConjugateSupportFromPoints(row,row2,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag(),iteration>func::toInt(trunc(3*func::toLower(quartFreq))))) return;
      ext=p2;
    }
  }
  powerS base=start-step;
  end=start;
  while (abs(end-base)>1) {
    start=(base+end)/2;
    p1=condPow(coef,start,row);
    p2=condPow(coef,start+dir,row);
    if (testConjugateSupportFromPoints(p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) {
      base=start;
    }
    else {
      end=start;
    }
  }
  p1=condPow(coef,base,row);
  p2=condPow(coef,base-dir,row);
  if (testConjugateSupportFromPoints(p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag())) base+=dir;
  p1=condPow(coef,base,row);
  fillSupportFromPoints(row,row2,p1.real(),p1.imag(),ext.real(),ext.imag(),0,0);
}

template <class scalar>
void AbstractMatrix<scalar>::wrapUp()
{
  m_directions.conservativeResize(m_directions.rows(),m_pos);
  m_supports.conservativeResize(m_pos,1);
  m_pos=0;
}

/// Finds the point at which each jordan column is maximum
template <class scalar>
void AbstractMatrix<scalar>::findZeniths()
{
  //y_n=binomial(n,k)x^n-k = y_{n-1}*nx/(n-k) -> zenith=k/(1-x)
  m_zeniths.resize(2*m_dimension);
  for (int row=0;row<m_dimension;row++) {
    scalar mag=func::norm2(m_eigenValues.coeff(row,row));
    if ((m_jordanIndex[row]>0) && (func::isNegative(mag-ms_one))) {
      scalar zenith=m_jordanIndex[row];
      zenith/=(ms_one-mag);
      m_zeniths[row]=func::toInt(func::toLower(zenith));
    }
    else m_zeniths[row]=0;
  }
  for (int row=m_dimension;row<2*m_dimension;row++) m_zeniths[row]=0;
}

/// lists the first source row for a folded set of dimensions
template <class scalar>
void AbstractMatrix<scalar>::findUnfolded()
{
  m_unfolded.resize(m_dimension);
  int pos=0;
  for (int i=0;i<m_dimension;i++) {
    if (m_jordanIndex[i]>0) continue;
    if (m_conjugatePair[i]>=0) {
      m_unfolded[pos++]=i;
      i++;
    }
    else if (m_jordanIndex[i+1]>0) m_unfolded[pos++]=i;
  }
  m_unfolded.resize(pos);
}

/// Marks the round indices in a rounded vector array
template <class scalar>
void AbstractMatrix<scalar>::findRoundIndices(std::vector<bool> &isRoundIndex)
{
  int pos=0,mult;
  isRoundIndex.resize(m_dimension);
  for (int row=0;row<m_dimension;row+=mult,pos++) {
    mult=(m_conjugatePair[row]<0) ? 1 : 2;
    if (m_jordanIndex[row+mult]==0) isRoundIndex[pos]=(m_conjugatePair[row]>=0);
    else isRoundIndex[pos]=(m_jordanIndex[row+mult]!=0);
    while (m_jordanIndex[row+mult]!=0) row+=mult;
  }
  isRoundIndex.resize(pos);
}

/// Finds the frequency of rotation of each conjugate pair
template <class scalar>
void AbstractMatrix<scalar>::findFrequencies()
{
  m_freq.assign(2*m_dimension,0);
  for (int row=0;row<m_dimension;row++) {
    if (m_conjugatePair[row]>row) {
      complexS coef=m_eigenValues.coeff(row,row);
      scalar angle=func::invtan(coef.imag(),coef.real());
      m_freq[row++]=func::toInt(2*func::toUpper(abs(func::const_pi(ms_one)/angle)));
      m_freq[row]=m_freq[row-1];
    }
  }
}

/// Indicates if the matrix dynamics are divergent
/// @param strict if true returns only true if no eigenvalues are convergent
template <class scalar>
bool AbstractMatrix<scalar>::isDivergent(const bool strict)
{
  for (int i=0;i<m_dimension;i++) {
    scalar eigenNorm=func::norm2(m_eigenValues.coeff(i,i));
    char sign=func::hardSign(eigenNorm-ms_one);
    if (strict && (sign<0)) return false;
    else if (!strict && (sign>=0)) return true;
  }
  return strict;
}

/// Finds the coefficient, magnitude, maximum, and minimum for the abstract vector at row
template <class scalar>
void AbstractMatrix<scalar>::findCoeffBounds(int row,powerS iteration,complexS &coef,scalar &mag,scalar &min,scalar &max)
{
  if (row<m_dimension) coef=m_eigenValues.coeff(row,row);
  else {
    int transRow=(m_inputType==eVariableInputs) ? m_unfolded[row-m_dimension] : row-m_dimension;
    coef=m_eigenValues.coeff(transRow,transRow);
    if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef)-ms_one))) {
      coef=m_foldedEigenValues(transRow,transRow);
    }
  }
  if ((row>=m_dimension) && (m_inputType==eVariableInputs)) {
    scalar rad=func::norm2(coef);
    mag=condPow(rad,1,row);
    max=condPow(rad,iteration,row);
  }
  else if (m_conjugatePair[row]<0) {
    mag=condPow(coef.real(),1,row);
    max=condPow(coef.real(),iteration,row);
  }
  else if (iteration>2) {
    mag=func::norm2(condPow(coef,1,row));
    max=func::norm2(condPow(coef,iteration,row));
  }
  else if (m_conjugatePair[row]>row) {
    mag=condPow(coef,1,row).real();
    max=condPow(coef,iteration,row).real();
  }
  else {
    mag=-condPow(coef,1,row).imag();
    max=-condPow(coef,iteration,row).imag();
  }
  min=mag;
  if ((m_conjugatePair[row]<0) && ((row<m_dimension) || (m_inputType!=eVariableInputs)) && (func::isNegative(coef.real()))) {
    scalar sum=min+max;
    char sign=func::hardSign(sum);
    if (sign>0) min=-max;
    else if (sign<0) max=-min;
  }
}

/// Retrieves the last calculated dynamics
template <class scalar>
AbstractPolyhedra<scalar>& AbstractMatrix<scalar>::getAbstractDynamics(const inputType_t inputType)
{
  if (inputType==eVariableInputs) return m_abstractRoundDynamics;
  if (this->m_hasOnes && inputType==eParametricInputs) return m_abstractInputDynamics;
  return m_abstractDynamics;
}

/// retrieves the abstract dynamics matrix for a given iteration (n)
template <class scalar>
AbstractPolyhedra<scalar>& AbstractMatrix<scalar>::getAbstractDynamics(const powerS iteration,int precision,const inputType_t inputType,const bool normalised)
{
  boost::timer timer;
  if (precision<1) precision=1;
  int dimension=m_dimension;
  m_inputType=inputType;
  if (inputType==eVariableInputs) dimension+=m_foldedEigenValues.rows();
  else if (this->m_hasOnes && (inputType==eParametricInputs)) dimension+=m_dimension;
  m_maxIterations=iteration;
  if (m_hasMultiplicities) this->calculateBoundedEigenError(calculateMaxIterations(iteration));
  findUnfolded();
  findZeniths();
  findFrequencies();
  AbstractPolyhedra<scalar>& result=getAbstractDynamics(inputType);
  result.changeDimension(dimension);
  m_supports.resize((1<<precision)*dimension*dimension,1);
  m_directions=MatrixS::Zero(dimension,(1<<precision)*dimension*dimension);
  m_pos=0;
  complexS coef,coef2;
  for (int row=0;row<dimension;row++)
  {
    scalar mag,min1,max1;
    findCoeffBounds(row,iteration,coef,mag,min1,max1);
    scalar min=min1;
    scalar max=max1;
    checkRange(row,iteration,func::norm2(coef),min,max);
    if (m_conjugatePair[row]>=0) findConjugateSupports(row,coef,iteration,precision);
    else {
      if (func::isNegative(coef.real())) {
        scalar sum=min+max;
        char sign=func::hardSign(sum);
        if (sign>0) min=-max;
        else if (sign<0) max=-min;
      }
      fillDirection(row,min,max);
    }
    if ((iteration<2) || (precision<2)) continue;
    for (int row2=row+((m_conjugatePair[row]>row) ? 2 : 1);row2<dimension;row2++) {
      if (row2<m_dimension) coef2=m_eigenValues.coeff(row2,row2);
      else {
        int transRow2=(inputType==eVariableInputs) ? m_unfolded[row2-m_dimension]: row2-m_dimension;
        coef2=m_eigenValues.coeff(transRow2,transRow2);//TODO: the rotations and dilations are wrong at the row value
        if ((inputType==eVariableInputs) && (func::isPositive(func::norm2(coef2)-ms_one))) {
          coef=m_foldedEigenValues(transRow2,transRow2);
        }
      }
      scalar mag2,min2,max2;
      findCoeffBounds(row2,iteration,coef2,mag2,min2,max2);
      if (m_conjugatePair[row]>=0) {
        if (m_conjugatePair[row2]<0) {
          fillQuadraticConjugateSupport(row,row2,func::norm2(coef),func::norm2(coef2),iteration,precision);
        }
      }
      else if (m_conjugatePair[row2]>=0) {
        fillQuadraticConjugateSupport(row2,row,func::norm2(coef2),func::norm2(coef),iteration,precision);
      }
      else if ((iteration==2) || func::isZero(max1-max2)) {
        fillLinearSupportFromPoints(row,row2,min1,max1,min2,max2);
      }
      else {
        scalar out1=condPow(min1,2,row);
        scalar out2=condPow(min2,2,row2);
        fillSupportFromPoints(row,row2,min1,min2,max1,max2,out1,out2);
        fillQuadraticSupport(row,row2,func::norm2(coef),func::norm2(coef2),iteration,precision);
      }
    }
  }
  wrapUp();
  if (m_hasMultiplicities) {
    scalar high=func::pow(ms_one+m_error,iteration);
    scalar low=func::pow(ms_one-m_error,iteration);
    scalar relaxation=func::getHull(high,low);
    m_supports*=relaxation;
  }
  result.load(m_directions,m_supports,true);
  if (normalised) result.normalise();
  result.setCalculationTime(timer.elapsed()*1000);
  if (ms_trace_dynamics>=eTraceAbstraction) {
    std::stringstream stream;
    stream << "s=" << iteration << ",l=" << precision;
    result.logTableau(stream.str());
  }
  return result;
}

/// Adds a set of supports at the given iteration in order to refine the abstraction
template <class scalar>
bool AbstractMatrix<scalar>::addSupportsAtIteration(AbstractPolyhedra<scalar>& dynamics,powerS iteration,powerS max)
{
  boost::timer timer;
  complexS coef,coef2;
  int dimension=m_dimension;
  if (m_inputType==eVariableInputs) dimension+=m_foldedEigenValues.rows();
  else if (this->m_hasOnes && (m_inputType==eParametricInputs)) dimension+=m_dimension;

  m_supports.resize(dimension*dimension,1);
  m_directions=MatrixS::Zero(dimension,dimension*dimension);
  m_pos=0;
  for (int row=0;row<dimension;row++)
  {
    if (row<m_dimension) coef=m_eigenValues.coeff(row,row);
    else {
      int transRow=(m_inputType==eVariableInputs) ? m_unfolded[row-m_dimension] : row-m_dimension;
      coef=m_eigenValues.coeff(transRow,transRow);
      if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef)-ms_one))) {
        coef=m_foldedEigenValues(transRow,transRow);
      }
    }
    if (m_conjugatePair[row]>row) {
      scalar mag=func::norm2(coef);
      int dir=(func::isPositive(mag-ms_one)) ? -1 : 1;
      complexS ext=condPow(coef,func::isPositive(mag-ms_one) ? max : 1,row+1);
      complexS p1=condPow(coef,iteration,row);
      complexS p2=condPow(coef,iteration+dir,row);
      fillConjugateSupportFromPoints(row,row+1,p1.real(),p1.imag(),p2.real(),p2.imag(),ext.real(),ext.imag());
    }
    for (int row2=row+((m_conjugatePair[row]>row) ? 2 : 1);row2<dimension;row2++) {
        if (row2<m_dimension) coef2=m_eigenValues.coeff(row2,row2);
        else {
          int transRow2=(m_inputType==eVariableInputs) ? m_unfolded[row2-m_dimension]: row2-m_dimension;
          coef2=m_eigenValues.coeff(transRow2,transRow2);//TODO: the rotations and dilations are wrong at the row value
          if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef2)-ms_one))) {
            coef2=m_foldedEigenValues(transRow2,transRow2);
          }
        }
        if (m_conjugatePair[row]>=0) {
          if (m_conjugatePair[row2]<0) {
            fillSemiConjugateSupportFromIter(row,row2,func::norm2(coef),func::norm2(coef2),iteration);
          }
        }
        else if (m_conjugatePair[row2]>=0) {
          fillSemiConjugateSupportFromIter(row2,row,func::norm2(coef2),func::norm2(coef),iteration);
        }
        else {
          fillSupportFromIter(row,row2,func::norm2(coef),func::norm2(coef2),iteration);
        }
      }
  }
  wrapUp();
  if (m_hasMultiplicities) {
    scalar high=func::pow(ms_one+m_error,iteration);
    scalar low=func::pow(ms_one-m_error,iteration);
    scalar relaxation=func::getHull(high,low);
    m_supports*=relaxation;
  }
  dynamics.addDirection(m_directions,m_supports);
  dynamics.addCalculationTime(timer.elapsed()*1000);
  if (ms_trace_dynamics>=eTraceAbstraction) {
    std::stringstream stream;
    stream << "abs supports n=" << iteration;
    dynamics.logTableau(stream.str());
  }
  return true;
}

/// Calculates the number of iterations necessary to reacha fixpoint
template <class scalar>
typename AbstractMatrix<scalar>::powerS AbstractMatrix<scalar>::calculateMaxIterations(powerS max)
{
  powerS result=0;
  findZeniths();
  findFrequencies();
  for (int i=0;i<m_dimension;i++) {
    complexS coef=m_eigenValues.coeff(i,i);
    if (func::isPositive(func::norm2(coef)-ms_one)) return (max<func::ms_infPower) ? max: func::ms_infPower;
    if (m_zeniths[i]+m_freq[i]>result) result=m_zeniths[i]+m_freq[i];
  }
  return (max<result) ? max : result;
}

/// Finds the corresponding iteration that generates dynamics close to the given point
template <class scalar>
void AbstractMatrix<scalar>::findIterations(MatrixS &point,powerList &iterations)
{
  complexS coef;
  powerS range;
  for (int col=0;col<point.cols();col++) {
    if (col<m_dimension) coef=m_eigenValues.coeff(col,col);
    else {
      int transCol=(m_inputType==eVariableInputs) ? m_unfolded[col-m_dimension] : col-m_dimension;
      coef=m_eigenValues.coeff(transCol,transCol);
      if ((m_inputType==eVariableInputs) && (func::isPositive(func::norm2(coef)-ms_one))) {
        coef=m_foldedEigenValues(transCol,transCol);
      }
    }
    refScalar rad=func::toCentre(func::norm2(coef));
    powerS iteration;
    if (m_conjugatePair[col]>=0) {
      scalar coeffAngle=func::invtan(coef.imag(),coef.real());
      scalar angle=func::invtan(point.coeff(0,col+1),point.coeff(0,col));
      char dir=func::hardSign(coeffAngle);
      char sign=func::hardSign(angle);
      if (sign!=dir) {
        if (sign<0) angle+=func::ms_2_pi;
        else        angle-=func::ms_2_pi;
      }
      refScalar finalSteps=func::toCentre(angle/coeffAngle);
      iteration=func::toInt(finalSteps);
      if (rad>func::ms_1) {
        refScalar tau=func::toCentre(coeffAngle/func::ms_2_pi);
        tau*=m_maxIterations;
        powerS mult=func::toInt(tau);
        mult*=m_freq[col];
        iteration+=mult;
      }
      col++;
    }
    else if ((rad>func::ms_1) || (!func::isZero(point.coeff(0,col)))) {
      iteration=condLog(rad,func::toCentre(point.coeff(0,col)),col,range);
    }
    else {
      iteration=0;
      range=0;
    }
    while ((iteration<2) && (range>0)) {
      range--;
      iteration++;
    }
    if (iteration>1) iterations[iteration]=range;//TODO: should merge when there is equality
  }
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class AbstractMatrix<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class AbstractMatrix<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class AbstractMatrix<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class AbstractMatrix<mpinterval>;
  #endif
#endif

}


/*
agnanowire_modeling_v1.cpp
(Based on agnanowire_modeling_v3.py)
Random resitor network model for Ag nanowire network in oxide matrix
Using data from Will Scheideler

Created by Jeremy Smith on 2015-07-17
University of California, Berkeley
j-smith@eecs.berkeley.edu

Version 1.0
*/

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <unistd.h>
#include <string>
#include <vector>
#include <thread>
#include <ctime>
#include <Eigen/Dense>
#include "nwnet.h"

using namespace std;
using namespace Eigen;

void stats(std::vector<double>& v, double& average, double& stderr, double& median){
	double sum = 0;
	for(int i = 0; i < v.size(); i++){
		sum += v[i];
	}
	average = sum/v.size();
	double dev = 0;
	for(int i = 0; i < v.size(); i++){
		dev += (v[i] - average)*(v[i] - average);
	}
	stderr = std::sqrt(dev/(v.size()*(v.size()-1)));
	std::sort(v.begin(), v.end());
	median = v[v.size()/2];
}

int main(){
	/*
	Setup parameters
	Change values here for different nanowire networks
	*/
	std::srand(time(NULL) | (getpid() << 4));
	double substratesize = 50.0;
	// Points to test resistance as fraction of substrate size
	ArrayXXd testpoints(12, 2);
	testpoints << 0.250, 0.750, 0.750, 0.250,
	              0.250, 0.250, 0.750, 0.750,
	              0.375, 0.625, 0.625, 0.375,
	              0.375, 0.375, 0.625, 0.625,
	              0.323, 0.500, 0.677, 0.500,
	              0.500, 0.323, 0.500, 0.677;
	testpoints *= substratesize;
	double nwlength = 14.0;                                              // Nanowire length
	double nwlength_sd = 4.0;                                            // Standard deviation of wire lengths
	double nwdiameter = 0.033;                                           // Nanowire diameter
	double agresistivity = 1.59e-2;                                      // Ag resistivity

	double nwdensity[] = {0.02, 0.04, 0.05};                             // Nanowires per sq micron
	double nwinterres[] = {1.0, 10.0, 1000.0};                           // Resistance between wires

	double matrixrsheet = 1.0e8;                                         // Sheet resistance of matrix
	int runs = 4;                                                        // Number of runs per condition

	double nwresistance = 4*agresistivity/(M_PI*nwdiameter*nwdiameter);  // Nanowire resistance per unit length

	std::vector<std::string> summaryList;
	std::ofstream outfile;

	cout << "\n=================\n";
	cout << "Ag Nanowire Model\n";
	cout << "Jeremy Smith\n";
	cout << "=================\n\n";

	for(int j = 0; j < 3; j++){
		for(int k = 0; k < 3; k++){
			std::vector<WireNet> nets;
			std::vector<std::thread> procs;
			int nwnumber = nwdensity[k]*substratesize*substratesize;

			for(int i = 0; i < runs; i++){
				unsigned seed = rand()%10000;
				WireNet n(nwnumber, nwlength, nwlength_sd, substratesize, nwresistance, nwinterres[j], matrixrsheet, seed, false);
				nets.push_back(n);
			}
			for(int i = 0; i < runs; i++){
				procs.push_back(std::thread(&WireNet::solve, &nets[i]));
			}
			for(int i = 0; i < runs; i++){
				procs[i].join();
			}
			cout << endl;

			std::vector<double> rList;
			std::vector<double> tList;
			for(int i = 0; i < runs; i++){
				nets[i].parameters();
				for(int p = 0; p < testpoints.rows()/2 - 1; p++){
					Vector2d xy1 = testpoints.row(2*p);
					Vector2d xy2 = testpoints.row(2*p+1);
					int node1 = findnode(nets[i].nodeCoords, xy1);
					int node2 = findnode(nets[i].nodeCoords, xy2);
					double r = two_point_resistance(nets[i].eigenvalues, nets[i].eigenvectors, node1, node2);
					cout << "  R: " << r << "    Between nodes: " << node1 << "," << node2 << endl;
					rList.push_back(std::log10(r));
				}
				double t = 100*(1 - nets[i].areal_coverage(nwdiameter));
				cout << "  T: " << t << " " << char(37) << endl;
				tList.push_back(t);

				nets[i].output_files("data_" + std::to_string(j) + std::to_string(k) + char(65+i));    // Test conditions j and k and run number i
				cout << endl;
			}
			double r_average;
			double r_stderr;
			double r_median;
			double t_average;
			double t_stderr;
			double t_median;
			stats(rList, r_average, r_stderr, r_median);
			stats(tList, t_average, t_stderr, t_median);
			summaryList.push_back("data_" + std::to_string(j) + std::to_string(k) + '\t'
				                  + to_string(nwlength) + '\t' + to_string(nwlength_sd) + '\t' + to_string(nwdiameter) + '\t'
				                  + to_string(agresistivity) + '\t' + to_string(nwresistance) + '\t'
				                  + to_string(nwdensity[k]) + '\t' + to_string(nwnumber) + '\t'
				                  + to_string(nwinterres[j]) + '\t' + to_string(matrixrsheet) + '\t'
				                  + to_string(r_average) + '\t' + to_string(r_median) + '\t' + to_string(r_stderr) + '\t'
				                  + to_string(t_average) + '\t' + to_string(t_stderr) + '\n');
		}
	}

	cout << "Writing out summary file..." << endl;
    outfile.open("summary.txt");
    outfile << "filename\tnwlength\tnwstd\tnwdiameter\tagresistivity\tnwresistance\t"
            << "nwdensity\tnwnumber\tnwinterres\tmatrixrsheet\t"
            << "resistance\tmedresistance\tresistanceerr\ttransmission\ttransmissionerr\n";
    for(int i = 0; i < summaryList.size(); i++){
        outfile << summaryList[i];
    }
    outfile.close();
    cout << "DONE" << endl;
}


/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Social Robotics Lab, University of Freiburg
*  Copyright (c) 2006-2012, Matthias Luber, Luciano Spinello and Kai O. Arras, Social Robotics Laboratory and
*    Oscar Martinez, Autonomous Intelligent Systems, University of Freiburg
*  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 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.
*/

#include <srl_laser_features/features/feature06.h>

#include <Eigen/LU>
#include <Eigen/QR>
#include <float.h>

#define MATRIX_LN_EPS -1e8

namespace srl_laser_features {

Feature06::Feature06(bool extended) : Feature(), m_extended(extended)
{
}

void Feature06::evaluate(const Segment& segment, Eigen::VectorXd& result) const
{
	result = Eigen::VectorXd::Zero(getNDimensions());

	const size_t numPoints = segment.points.size();
	if (numPoints > 2) {

		double px[numPoints];
		double py[numPoints];

		// copy
		for (size_t pIndex = 0; pIndex < numPoints; ++pIndex) {
			px[pIndex] = segment.points[pIndex](0);
			py[pIndex] = segment.points[pIndex](1);
		}

		double xc, yc, rc;
		fitCircle(numPoints, px, py, xc, yc, rc);

		result(1) = rc;
		if (m_extended) {
			result(3) = DBL_MAX;
			result(4) = 0.0;
		}

		// residual sum
		Eigen::Vector2d centerOfCircle(xc, yc);

		double residual;
		for (size_t pIndex = 0; pIndex < numPoints; ++pIndex) {
			double dist = (centerOfCircle - segment.points[pIndex]).norm();

			if (m_extended) {
				if (dist < result(3)) {
					result(3) = dist;
				}
				if (dist > result(4)) {
					result(4) = dist;
				}
			}

			residual = rc - dist;
			result(0) += residual * residual;
		}

		if (m_extended) {
			result(2) = result(0) / (numPoints - 1);
			if (result(4) != 0.0) {
				result(5) = result(3) / result(4);
			}
		}
	}
}

int Feature06::fitCircle(int n, double *x_vec, double *y_vec, double& xc, double& yc, double& r)
{
	int i;

	if (n > 3) {
		Eigen::MatrixXd A(n, 3);
		Eigen::MatrixXd X(3, 1);
		Eigen::MatrixXd B(n, 3);
		Eigen::MatrixXd P(3, n);
		Eigen::MatrixXd T(3, 3);

		// We want the overdetermined linear equation system Ax = b
		// Fill in matrix A
		for (i = 0; i < n; i++) {
			A(i, 0) = -2 * x_vec[i];
			A(i, 1) = -2 * y_vec[i];
			A(i, 2) = 1.0;
		}

		// Fill in vector b
		for (i = 0; i < n; i++) {
			B(i, 0) = -(x_vec[i] * x_vec[i] + y_vec[i] * y_vec[i]);
		}

		// Now we have the equation system with n equations and 3 unknowns.
		// The LSQ-solution is given by the pseudo-inverse:
		P = A.transpose();
		T = P * A;
		Eigen::FullPivLU<Eigen::MatrixXd> lu(T);
		if (lu.isInvertible()) {
			if (log(lu.determinant()) > MATRIX_LN_EPS) {
				X = lu.inverse() * P * B;
				// Extract circle center and radius
				xc = X(0, 0);
				yc = X(1, 0);
				r = sqrt(-X(2, 0) + xc * xc + yc * yc);
				return 1;
			}
			else {
				// Points badly conditioned (n > 3)
				return -3;
			}
		}
		else {
			// try QR
			Eigen::FullPivHouseholderQR<Eigen::MatrixXd> QR(A);
			X = QR.solve(B);

			// Extract circle center and radius
			xc = X(0, 0);
			yc = X(1, 0);
			r = sqrt(-X(2, 0) + xc * xc + yc * yc);
			return 1;
		}
	}
	else if (n == 3) {
		double a, b, c, d, e, f, g;

		a = x_vec[1] - x_vec[0];
		b = y_vec[1] - y_vec[0];
		c = x_vec[2] - x_vec[0];
		d = y_vec[2] - y_vec[0];
		e = a * (x_vec[0] + x_vec[1]) + b * (y_vec[0] + y_vec[1]);
		f = c * (x_vec[0] + x_vec[2]) + d * (y_vec[0] + y_vec[2]);
		g = 2 * (a * (y_vec[2] - y_vec[1]) - b * (x_vec[2] - x_vec[1]));

		if (g != 0.0) {
			xc = (d * e - b * f) / g;
			yc = (a * f - c * e) / g;
			r = sqrt((x_vec[0] - xc) * (x_vec[0] - xc) + (y_vec[0] - yc) * (y_vec[0] - yc));
			return 2;
		}
		else {
			// Points badly conditioned (n == 3)
			return -2;
		}
	}
	else {
		// Too few points
		xc = 0.0;
		yc = 0.0;
		r = -1.0;
		return -1;
	}
}

} // end of namespace srl_laser_features


#include "poisson_fem.hpp"
#include <boost/python.hpp>
//#include "vectorize.hpp"

namespace gd {

using namespace boost::python;
using namespace std;
using namespace dealii;

void py_export_poisson_fem() {
	class_<PoissonFEM<2>, boost::noncopyable >("PoissonFEM2d", init<int>())
		.def("make_grid", &PoissonFEM<2>::make_grid)
		.def("setup_system", &PoissonFEM<2>::setup_system)
		.def("refine_grid", &PoissonFEM<2>::refine_grid)
		.def("assemble_system", &PoissonFEM<2>::assemble_system)
		.def("solve", &PoissonFEM<2>::solve)
		.def("output_results", &PoissonFEM<2>::output_results)
		.def("output_grid", &PoissonFEM<2>::output_grid)
		.def("eval", (&PoissonFEM<2>::eval))
		.def("n_active_cells", (&PoissonFEM<2>::n_active_cells))
		.def("global_refine", (&PoissonFEM<2>::global_refine))
	;
	class_<PoissonFEM<1>, boost::noncopyable >("PoissonFEM1d", init<int>())
		.def("make_grid", &PoissonFEM<1>::make_grid)
		.def("setup_system", &PoissonFEM<1>::setup_system)
		.def("refine_grid", &PoissonFEM<1>::refine_grid)
		.def("assemble_system", &PoissonFEM<1>::assemble_system)
		.def("solve", &PoissonFEM<1>::solve)
		.def("output_results", &PoissonFEM<1>::output_results)
		.def("output_grid", &PoissonFEM<1>::output_grid)
		.def("eval", (&PoissonFEM<1>::eval))
		.def("getpoints", (&PoissonFEM<1>::getpoints))
		.def("n_active_cells", (&PoissonFEM<1>::n_active_cells))
		.def("grad", (&PoissonFEM<1>::grad))
		.def("global_refine", (&PoissonFEM<1>::global_refine))

	;
}

template <int dim>
class RightHandSide : public Function<dim>
{
public:
	RightHandSide () : Function<dim>() {}

	virtual double value (const Point<dim>   &p,
						const unsigned int  component = 0) const;
};
template <int dim>
double RightHandSide<dim>::value (const Point<dim> &p,
								const unsigned int /* component */) const
{
	double rsq = 0;
	for (unsigned int i=0; i<dim; ++i)
		rsq += std::pow(p(i), 2);
	double r = sqrt(rsq);
	double x = p(0);
	double y = p(1);
	double phi = fmod(atan2(y, x)+2*M_PI, 2*M_PI);
	//return -exp(-pow(r/4-phi, 2));
	//return ((r > 10) && (r < 15.0)) * 1.; //3/(4*M_PI) * pow(1+pow(r,2), -5./2);
	//return -1;
	//return -1/(1+x/20.);
	return -3/(4*M_PI) * pow(1+pow(r/20,2), -5./2);
}

template <int dim>
PoissonFEM<dim>::PoissonFEM (int order) :
                 fe (order),
                dof_handler (triangulation)
 {}

template <int dim>
void PoissonFEM<dim>::make_grid()
{
	const Point<dim> center;

	if(dim == 1) {
		GridGenerator::hyper_cube(triangulation, 0, 40) ;//, 10);
	} else {
		GridGenerator::hyper_ball(triangulation, center, 40.) ;//, 10);
		//const HyperShellBoundary<dim> boundary_description(center);
		//triangulation.set_boundary (0, boundary_description);
	}
	//const HyperShellBoundary<dim> boundary_description(center);
	//const HyperShellBoundary<2> boundary_description2(center);
	//triangulation.set_boundary (0, boundary_description);
	//triangulation.set_boundary (1, boundary_description2);

	triangulation.refine_global (2);
	//triangulation.set_boundary (0);

	std::cout << "Number of active cells: "
				<< triangulation.n_active_cells()
				<< std::endl;
	std::cout << "Total number of cells: "
				<< triangulation.n_cells()
				<< std::endl;

}

template <int dim>
void  PoissonFEM<dim>::setup_system() {
	dof_handler.distribute_dofs (fe);
	std::cout << "Number of degrees of freedom: "
			<< dof_handler.n_dofs()
			<< std::endl;

	sparsity_pattern.reinit (dof_handler.n_dofs(),
							dof_handler.n_dofs(),
							dof_handler.max_couplings_between_dofs());
	DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
	solution.reinit (dof_handler.n_dofs());
	system_rhs.reinit (dof_handler.n_dofs());

	hanging_node_constraints.clear ();
	DoFTools::make_hanging_node_constraints (dof_handler,
                                            hanging_node_constraints);
	hanging_node_constraints.condense (sparsity_pattern);

	hanging_node_constraints.close ();

   sparsity_pattern.compress();

   system_matrix.reinit (sparsity_pattern);

}

template <int dim>
void PoissonFEM<dim>::assemble_system ()
{
	QGauss<dim>  quadrature_formula(2);
	FEValues<dim> fe_values (fe, quadrature_formula,
							update_values | update_gradients | update_JxW_values | update_quadrature_points);

	const RightHandSide<dim> right_hand_side;

	const unsigned int   dofs_per_cell = fe.dofs_per_cell;
	const unsigned int   n_q_points    = quadrature_formula.size();

	FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
	Vector<double>       cell_rhs (dofs_per_cell);

	std::vector<unsigned int> local_dof_indices (dofs_per_cell);

	typename DoFHandler<dim>::active_cell_iterator
		cell = dof_handler.begin_active(),
		endc = dof_handler.end();
	for (; cell!=endc; ++cell)  {
		fe_values.reinit (cell);

		cell_matrix = 0;
		cell_rhs = 0;

	   for (unsigned int i=0; i<dofs_per_cell; ++i) {
			for (unsigned int j=0; j<dofs_per_cell; ++j) {
				for (unsigned int q_point=0; q_point<n_q_points; ++q_point) {
					//cout << "q: " << fe_values.quadrature_point (q_point) << endl;
					double r = fe_values.quadrature_point (q_point)(0);
					cell_matrix(i,j) += ( fe_values.shape_grad (i, q_point) *
									fe_values.shape_grad (j, q_point) *
					                      fe_values.JxW (q_point)) ;/* r * r; //*/
					cout << "-> " <<fe_values.JxW (q_point) << " " << r << " " << fe_values.shape_grad (i, q_point) << endl;
				}
			}
		}

		for (unsigned int i=0; i<dofs_per_cell; ++i) {
			for (unsigned int q_point=0; q_point<n_q_points; ++q_point) {
				double r = fe_values.quadrature_point (q_point)(0);
				cell_rhs(i) += (fe_values.shape_value (i, q_point) *
							right_hand_side.value (fe_values.quadrature_point (q_point)) *
				                fe_values.JxW (q_point)) ;/* r * r; //*/

				cell->get_dof_indices (local_dof_indices);

			}
		}

		for (unsigned int i=0; i<dofs_per_cell; ++i)
		for (unsigned int j=0; j<dofs_per_cell; ++j)
			system_matrix.add (local_dof_indices[i],
								local_dof_indices[j],
								cell_matrix(i,j));

		for (unsigned int i=0; i<dofs_per_cell; ++i)
		system_rhs(local_dof_indices[i]) += cell_rhs(i);
		}
	//cout << system_matrix << endl;
	system_matrix.print(cout);
	cout << "next" << endl;
	system_rhs.print(cout);
	hanging_node_constraints.condense (system_matrix);
	hanging_node_constraints.condense (system_rhs);

	std::map<unsigned int,double> boundary_values;
	//boundary_values[0] = 0;
	if(dim == 1) {
		VectorTools::interpolate_boundary_values (dof_handler,
												1,
												ConstantFunction<dim>(0),
												boundary_values);
	} else {
		VectorTools::interpolate_boundary_values (dof_handler,
				0,
				ConstantFunction<dim>(0),
				boundary_values);
	}
	MatrixTools::apply_boundary_values (boundary_values,
										system_matrix,
										solution,
										system_rhs);
	cout << "boundary:" << endl;
	system_matrix.print(cout);
	cout << "next" << endl;
	system_rhs.print(cout);
}

template <int dim>
Point<dim> pointr(double r);

template<>
Point<1> pointr<1>(double r) {
	return Point<1>(r);
}

template<>
Point<2> pointr<2>(double r) {
	return Point<2>(r, 0);
}

template<>
Point<3> pointr(double r) {
	return Point<3>(r, 0, 0);
}

template <int dim>
 void PoissonFEM<dim>::solve ()
 {
   SolverControl           solver_control (5000, 1e-12);
   SolverCG<>              cg (solver_control);

   cg.solve (system_matrix, solution, system_rhs,
            PreconditionIdentity());
hanging_node_constraints.distribute (solution);

	std::cout << VectorTools::point_value (dof_handler, solution, pointr<dim>(0)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, pointr<dim>(5)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, pointr<dim>(10)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, pointr<dim>(15)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, pointr<dim>(20)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, pointr<dim>(40)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, pointr<dim>(0.1)) << std::endl;
	/*std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(40, 0)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 40)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 20)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(1,0)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0,1)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(2,0)) << std::endl;
	double a = VectorTools::point_value (dof_handler, solution, Point<2>(1,0))-VectorTools::point_value (dof_handler, solution, Point<2>(0,0));
	double b = VectorTools::point_value (dof_handler, solution, Point<2>(2,0))-VectorTools::point_value (dof_handler, solution, Point<2>(1,0));
	std::cout << "a = " << a << std::endl;
	std::cout << "b = " << b << std::endl;
	std::cout << "a/b = " << (a/b) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 0)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 5)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 10)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 15)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 20)) << std::endl;
	std::cout << VectorTools::point_value (dof_handler, solution, Point<2>(0, 25)) << std::endl;*/

}

template <int dim>
void PoissonFEM<dim>::getpoints(double_vector p) // for 1d only
{
	double* pp = p.data().begin();
	/*Triangulation<2>::active_line_iterator
		cell = triangulation.begin_active_line(),
		endc = triangulation.end_line();

	double nextx;
	const Point<dim> center;
	cout << "starting.. " << (cell==endc) << endl;
	for (; cell!=endc; ++cell) {
		cout << "-> " << cell->vertex(0)(0) << endl;
		*pp++ = (cell->vertex(0))(0);
		nextx =  (cell->vertex(1))(0);
	}
	*pp++ = nextx; // only include last point of last cell
	*/
	auto vertices = triangulation.get_vertices();
	auto used = triangulation.get_used_vertices();
	for(int i = 0; i < vertices.size(); i++) {
		if(used[i]) {
			*pp++ = vertices[i](0);
		}
	}
}

template <int dim>
double PoissonFEM<dim>::eval(double r) {
	Functions::FEFieldFunction<dim> ff(dof_handler, solution);
	return ff.value(pointr<dim>(r));
	//return VectorTools::point_value (dof_handler, solution, pointr<dim>(r));
}

template <int dim>
double PoissonFEM<dim>::grad(double r) {
	Functions::FEFieldFunction<dim> ff(dof_handler, solution);
	auto tensor = ff.gradient(pointr<dim>(r));
	return tensor[0];
}

template <int dim>
void PoissonFEM<dim>::refine_grid ()
{
   Vector<float> estimated_error_per_cell (triangulation.n_active_cells());

   KellyErrorEstimator<dim>::estimate (dof_handler,
                                       QGauss<dim-1>(3),
                                       typename FunctionMap<dim>::type(),
                                       solution,
                                       estimated_error_per_cell);
   GridRefinement::refine_and_coarsen_fixed_number (triangulation,
                                                    estimated_error_per_cell,
                                                    0.3, 0.03);
triangulation.execute_coarsening_and_refinement ();

}

template <int dim>
void PoissonFEM<dim>::output_results () const
 {
   DataOut<dim> data_out;
   data_out.attach_dof_handler (dof_handler);
   data_out.add_data_vector (solution, "solution");
   data_out.build_patches ();

   std::ofstream output (dim == 2 ? "solution2d.gpl" : "solution3d.gpl" );
   data_out.write_gnuplot (output);

	std::ofstream outgrid (dim == 2 ? "grid2d.eps" : "grid3d.eps");
	GridOut grid_out;
	grid_out.write_eps (triangulation, outgrid);

   std::ofstream output2(dim == 2 ? "solution2d.vtk" : "solution3d.vtk" );
   data_out.write_vtk (output2);

 }
template <int dim>
void PoissonFEM<dim>::output_grid(char* filename) const
{
	//DataOut<dim> data_out;
	//data_out.attach_dof_handler (dof_handler);
	//data_out.add_data_vector (solution, "solution");
	//data_out.build_patches ();

	std::ofstream outgrid(filename);
	GridOut grid_out;
	grid_out.write_eps(triangulation, outgrid);

}

template <int dim>
int PoissonFEM<dim>::n_active_cells() const
{
	return triangulation.n_active_cells();
}
int refines = 1;

template <int dim>
void PoissonFEM<dim>::global_refine(int refine)
{
	return triangulation.refine_global(refine);
}

/*
template <int dim>
void PoissonFEM<dim>::run ()
{
	make_grid();
	setup_system();
	assemble_system ();
	solve ();

	for(int i = 0; i < refines; i++) {

		refine_grid();
		setup_system();
		assemble_system ();
		solve ();
	}
	output_results ();
 }
*/
 /*int main (int argc, char** argv)
 {
	if(argc < 2) {
		printf("usage: %s <refinements>\n", argv[0]);
		exit(-1);
	}
	refines = atoi(argv[1]);
	LaplaceProblem<2> laplace_problem;
	laplace_problem.run ();

	LaplaceProblem<3> laplace_problem3d;
	laplace_problem3d.run ();
   return 0;
 }*/

}


#pragma once
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <utility>
#include <cmath>
#include "ray.hpp"
#include "interaction.hpp"
#include "mathext.hpp"
#include "config.hpp"

extern Config conf;

class Light
{
public:
    Light(Eigen::Vector3f pos, Eigen::Vector3f color)
        : m_Pos(std::move(pos)),
        m_Color(std::move(color))
    {
    }
    virtual ~Light() = default;

    /* Sample a point on the light's surface, if light is not delta light
     * Set PDF and the surface position
     * Return color of light
     */
    virtual Eigen::Vector3f sampleSurfacePos(float* pdf = nullptr) = 0;

    virtual float sampleSurfacePdf() = 0;

    virtual bool isHit(Ray* ray, float* hit_t = nullptr) = 0;

    virtual Eigen::Vector3f emission(Eigen::Vector3f d) = 0;

    Eigen::Vector3f m_Pos;
    Eigen::Vector3f m_Color;
};

class AreaLight:public Light
{
public:
    float radius;
    Eigen::Vector3f normal;
    Eigen::Vector3f right;

    AreaLight(Eigen::Vector3f pos, Eigen::Vector3f color,
        float r = 1.0f, Eigen::Vector3f n = Eigen::Vector3f(0.0f, -1.0f, 0.0f))
    : Light(pos, color), radius(r), normal(n.normalized())
    {
        right = mathext::rot_align_normal(normal) * Eigen::Vector3f(1.0f, 0.0f, 0.0f);
    }

    float sampleSurfacePdf()
    {
        return PI_INV / (radius * radius);
    }

    Eigen::Vector3f sampleSurfacePos(float* pdf = nullptr) override
    {
        Eigen::Vector2f sample = mathext::disk(radius);
        /* Convert polar to cartesian coordinate */
        Eigen::Vector3f sampled_light_pos = m_Pos + (Eigen::AngleAxisf(sample.y(), normal) * (sample.x() * right));

        /* Compute PDF of sampling a point in the area light */
        if (pdf) {
            *pdf = sampleSurfacePdf();
        }

        return sampled_light_pos;
    }

    Eigen::Vector3f emission(Eigen::Vector3f d) override
    {
        return conf.light_power * m_Color * abs(d.dot(normal));
    }

    /*
     * Reference: https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-plane-and-ray-disk-intersection
     */
    bool isHit(Ray* ray, float* hit_t = nullptr) override
    {
        float d = ray->m_Dir.dot(normal);

        if (abs(d) < EPSILON) {
            return false;
        }

        float t = (m_Pos - ray->m_Ori).dot(normal) / d;
        if (t < 0) {
            return false;
        }

        Eigen::Vector3f isect = ray->getPoint(t);
        bool is_hit = (isect - m_Pos).norm() <= radius;

        if (is_hit && hit_t) {
            *hit_t = t;
        }

        return is_hit;
    }
};


//
// Created by keszocze on 15.10.18.
//

#pragma once

#include <vector>
#include <cudd/cplusplus/cuddObj.hh>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include "number_representation.hpp"

namespace abo::error_metrics {

    using abo::util::NumberRepresentation;

    /**
     * @brief Computes the maximal value of a function represented by a vector of BDDs
     *
     * The function is assumed to return an natural number
     *
     * @param mgr
     * @param fun The function given by a vector of BDDs
     */
    boost::multiprecision::uint256_t get_max_value(const Cudd &mgr, const std::vector<BDD> &fun);

    /**
     * @brief Computes the maximum absolute difference between the f and f_hat for any input
     * The computation is performed symbolically using BDDs
     * @param mgr The BDD object manager
     * @param f The original function
     * @param f_hat The approximated function. Must have the same number of bits as f
     * @param num_rep The number representation for f and f_hat
     * @return The maximum absolute difference
     */
    boost::multiprecision::uint256_t
        worst_case_error(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                             const NumberRepresentation num_rep= NumberRepresentation::BaseTwo);

    /**
     * @brief Computes the maximum absolute difference between the f and f_hat for any input
     * divided by 2^n - 1 to normalize it to the range [0, 1] regardless of the function size (with n = f.size())
     * This is mainly intended to be a helper function to make the use of the worst case error easier
     * @param mgr The BDD object manager
     * @param f The original function
     * @param f_hat The approximated function. Must have the same number of bits as f
     * @param num_rep The number representation for f and f_hat
     * @return The maximum absolute difference normalized to [0, 1]
     */
    double worst_case_error_percent(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                         const NumberRepresentation num_rep= NumberRepresentation::BaseTwo);

    /**
     * @brief Computes the maximum absolute difference between the f and f_hat for any input
     * The computation is performed using BDDs and will be typically slow compared to the BDD based variant
     * @param mgr The BDD object manager
     * @param f The original function
     * @param f_hat The approximated function. Must have the same number of bits as f
     * @param num_rep The number representation for f and f_hat
     * @return The maximum absolute difference
     */
    boost::multiprecision::uint256_t
        worst_case_error_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                             const NumberRepresentation num_rep = NumberRepresentation::BaseTwo);

    /**
     * @brief approximate_worst_case_error
     *  Calculates the worst case error approximately, to a given relative error.
     *  The time and memory this function uses scales exponentially with the desired precision (n) (in the worst case).
     *  With m being the input size (f, f_hat), in the worst case, it lies in O(m ^ n).
     * @param mgr
     * @param f
     * @param f_hat
     * @param n The precision to calculate in number of bits, for more details see the return value. It must be greater than zero
     * @param num_rep The number representation for f and f_hat
     * @return The approximated worst case error. It is an upper bound and therefore guaranteed to be larger
     *  than the actual error. Let wc be the correct worst case error and x be the result of this function.
     *  Then it holds that wc <= x <= wc * (1 + 1 / (2 ^ (n + 1) - 1)))
     */
    boost::multiprecision::uint256_t approximate_worst_case_error(const Cudd &mgr, const std::vector<BDD> &f,
                                                                  const std::vector<BDD> &f_hat, int n,
                                                                  const NumberRepresentation num_rep = NumberRepresentation::BaseTwo);

}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_EXPONENTIAL_FUNCTION_SCALAR_IMPL_LOGS_D_LOG_HPP_INCLUDED
#define NT2_TOOLBOX_EXPONENTIAL_FUNCTION_SCALAR_IMPL_LOGS_D_LOG_HPP_INCLUDED

#include <boost/fusion/include/vector_tie.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/tofloat.hpp>
#include <nt2/include/functions/is_nan.hpp>
#include <nt2/include/functions/is_ltz.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/fast_frexp.hpp>
#include <nt2/include/functions/genmask.hpp>
#include <nt2/include/functions/amul.hpp>
#include <nt2/include/functions/minusone.hpp>
#include <nt2/include/functions/madd.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/constant/real.hpp>

namespace nt2
{
  namespace details
  {
    namespace internal
    {
      //////////////////////////////////////////////////////////////////////////////
      // math log functions
      //////////////////////////////////////////////////////////////////////////////

      template < class A0 >
      struct logarithm< A0, tag::not_simd_type, double>
      {
	static inline void kernel_log(const A0& a0,
				      A0& dk,
				      A0& hfsq,
				      A0& s,
				      A0& R,
				      A0& f)
	{
	  typedef typename meta::as_integer<A0, signed>::type int_type;
	  typedef typename meta::scalar_of<A0>::type               sA0;
	  A0 x;
	  int_type k;
	  boost::fusion::vector_tie(x, k) = fast_frexp(a0);
	  const int_type x_lt_sqrthf = -is_greater(Sqrt_2o_2<A0>(), x);
	  k += x_lt_sqrthf;
	  f = minusone(x+b_and(x, genmask(x_lt_sqrthf)));
	  dk = tofloat(k);
	  s = f/add(Two<A0>(),f);
	  A0 z = sqr(s);
	  A0 w = sqr(z);
	  A0 t1= w*horner<NT2_HORNER_COEFF_T(sA0, 3,
					 (0x3fc39a09d078c69fll,
					  0x3fcc71c51d8e78afll,
					  0x3fd999999997fa04ll)
					 )> (w);
	  A0 t2= z*horner<NT2_HORNER_COEFF_T(sA0, 4,
				       (0x3fc2f112df3e5244ll,
					0x3fc7466496cb03dell,
					0x3fd2492494229359ll,
					0x3fe5555555555593ll)
			)> (w);
	  R = t2+t1;
	  hfsq = mul(Half<A0>(), sqr(f));
	}

	static inline A0 log(const A0& a0)
	{
	  // ln(2)hi  =  6.93147180369123816490e-01  or  0x3fe62e42fee00000
	  // ln(2)lo  =  1.90821492927058770002e-10  or  0x3dea39ef35793c76
	  if (a0 == Inf<A0>()) return a0;
	  if (is_eqz(a0)) return Minf<A0>();
	  if (nt2::is_nan(a0)||is_ltz(a0)) return Nan<A0>();
	  A0 dk, hfsq, s, R, f;
	  kernel_log(a0, dk, hfsq, s, R, f);
	  return  mul(dk, double_constant<A0, 0x3fe62e42fee00000ll>())-
	    ((hfsq-(s*(hfsq+R)+mul(dk,double_constant<A0, 0x3dea39ef35793c76ll>())))-f);
	}
	static inline A0 log2(const A0& a0)
	{
	  if (a0 == Inf<A0>()) return a0;
	  if (is_eqz(a0)) return Minf<A0>();
	  if (nt2::is_nan(a0)||is_ltz(a0)) return Nan<A0>();
	  A0 dk, hfsq, s, R, f;
	  kernel_log(a0, dk, hfsq, s, R, f);
	  return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;
	}

	static inline A0 log10(const A0& a0)
	{
	  if (a0 == Inf<A0>()) return a0;
	  if (is_eqz(a0)) return Minf<A0>();
	  if (nt2::is_nan(a0)||is_ltz(a0)) return Nan<A0>();
	  A0 dk, hfsq, s, R, f;
	  kernel_log(a0, dk, hfsq, s, R, f);
	  return -(hfsq-(s*(hfsq+R))-f)*Invlog_10<A0>()+dk*Log_2olog_10<A0>();
	}
      };
    }
  }
}

#endif

// /////////////////////////////////////////////////////////////////////////////
// End of d_log.hpp
// /////////////////////////////////////////////////////////////////////////////