///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1998-2011, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// 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 Industrial Light & Magic 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
// OWNER 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.
//
///////////////////////////////////////////////////////////////////////////

#define BOOST_PYTHON_MAX_ARITY 17

#include "PyImathMatrix.h"
#include "PyImathExport.h"
#include "PyImathDecorators.h"
#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/raw_function.hpp>
#include "PyImath.h"
#include "PyImathVec.h"
#include "PyImathMathExc.h"
#include <ImathVec.h>
#include <ImathMatrixAlgo.h>
#include <Iex.h>
#include "PyImathTask.h"

namespace PyImath {
template<> const char PYIMATH_EXPORT *PyImath::M44fArray::name() { return "M44fArray"; }
template<> const char PYIMATH_EXPORT *PyImath::M44dArray::name() { return "M44dArray"; }

using namespace boost::python;
using namespace IMATH_NAMESPACE;
using namespace PyImath;

template <class T, int len>
struct MatrixRow {
    explicit MatrixRow(T *data) : _data(data) {}
    T & operator [] (int i) { return _data[i]; }
    T *_data;

    static const char *name;
    static void register_class()
    {
        typedef PyImath::StaticFixedArray<MatrixRow,T,len> MatrixRow_helper;
        class_<MatrixRow> matrixRow_class(name,no_init);
        matrixRow_class
            .def("__len__", MatrixRow_helper::len)
            .def("__getitem__", MatrixRow_helper::getitem,return_value_policy<copy_non_const_reference>())
            .def("__setitem__", MatrixRow_helper::setitem)
            ;
    }
};

template <> const char *MatrixRow<float,4>::name = "M44fRow";
template <> const char *MatrixRow<double,4>::name = "M44dRow";

template <class Container, class Data, int len>
struct IndexAccessMatrixRow {
    typedef MatrixRow<Data,len> result_type;
    static MatrixRow<Data,len> apply(Container &c, int i) { return MatrixRow<Data,len>(c[i]); }
};

template <class T> struct Matrix44Name { static const char *value; };
template<> const char *Matrix44Name<float>::value  = "M44f";
template<> const char *Matrix44Name<double>::value = "M44d";

template <class T>
static std::string Matrix44_str(const Matrix44<T> &v)
{
    std::stringstream stream;
    stream << Matrix44Name<T>::value << "(";
    for (int row = 0; row < 4; row++)
    {
        stream << "(";
	for (int col = 0; col < 4; col++)
	{
	    stream << v[row][col];
            stream << (col != 3 ? ", " : "");
	}
        stream << ")" << (row != 3 ? ", " : "");
    }
    stream << ")";
    return stream.str();
}

// Non-specialized repr is same as str
template <class T>
static std::string Matrix44_repr(const Matrix44<T> &v)
{
    return Matrix44_str(v);
}

// Specialization for float to full precision
template <>
std::string Matrix44_repr(const Matrix44<float> &v)
{
    return (boost::format("%s((%.9g, %.9g, %.9g, %.9g), (%.9g, %.9g, %.9g, %.9g), (%.9g, %.9g, %.9g, %.9g), (%.9g, %.9g, %.9g, %.9g))")
                        % Matrix44Name<float>::value
                        % v[0][0] % v[0][1] % v[0][2] % v[0][3]
                        % v[1][0] % v[1][1] % v[1][2] % v[1][3]
                        % v[2][0] % v[2][1] % v[2][2] % v[2][3]
                        % v[3][0] % v[3][1] % v[3][2] % v[3][3]).str();
}

// Specialization for double to full precision
template <>
std::string Matrix44_repr(const Matrix44<double> &v)
{
    return (boost::format("%s((%.17g, %.17g, %.17g, %.17g), (%.17g, %.17g, %.17g, %.17g), (%.17g, %.17g, %.17g, %.17g), (%.17g, %.17g, %.17g, %.17g))")
                        % Matrix44Name<double>::value
                        % v[0][0] % v[0][1] % v[0][2] % v[0][3]
                        % v[1][0] % v[1][1] % v[1][2] % v[1][3]
                        % v[2][0] % v[2][1] % v[2][2] % v[2][3]
                        % v[3][0] % v[3][1] % v[3][2] % v[3][3]).str();
}

template <class T>
static const Matrix44<T> &
invert44 (Matrix44<T> &m, bool singExc = true)
{
    MATH_EXC_ON;
    return m.invert(singExc);
}

template <class T>
static Matrix44<T>
inverse44 (Matrix44<T> &m, bool singExc = true)
{
    MATH_EXC_ON;
    return m.inverse(singExc);
}

template <class T>
static const Matrix44<T> &
gjInvert44 (Matrix44<T> &m, bool singExc = true)
{
    MATH_EXC_ON;
    return m.gjInvert(singExc);
}

template <class T>
static Matrix44<T>
gjInverse44 (Matrix44<T> &m, bool singExc = true)
{
    MATH_EXC_ON;
    return m.gjInverse(singExc);
}

template <class T, class U>
static const Matrix44<T> &
iadd44(Matrix44<T> &m, const Matrix44<U> &m2)
{
    MATH_EXC_ON;
    Matrix44<T> m3;
    m3.setValue (m2);
    return m += m3;
}

template <class T>
static const Matrix44<T> &
iadd44T(Matrix44<T> &mat, T a)
{
    MATH_EXC_ON;
    return mat += a;
}

template <class T>
static Matrix44<T>
add44(Matrix44<T> &m, const Matrix44<T> &m2)
{
    MATH_EXC_ON;
    return m + m2;
}

template <class T, class U>
static const Matrix44<T> &
isub44(Matrix44<T> &m, const Matrix44<U> &m2)
{
    MATH_EXC_ON;
    Matrix44<T> m3;
    m3.setValue (m2);
    return m -= m3;
}

template <class T>
static const Matrix44<T> &
isub44T(Matrix44<T> &mat, T a)
{
    MATH_EXC_ON;
    return mat -= a;
}

template <class T>
static Matrix44<T>
sub44(Matrix44<T> &m, const Matrix44<T> &m2)
{
    MATH_EXC_ON;
    return m - m2;
}

template <class T>
static const Matrix44<T> &
negate44 (Matrix44<T> &m)
{
    MATH_EXC_ON;
    return m.negate();
}

template <class T>
static Matrix44<T>
neg44 (Matrix44<T> &m)
{
    MATH_EXC_ON;
    return -m;
}

template <class T>
static const Matrix44<T> &
imul44T(Matrix44<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m *= t;
}

template <class T>
static Matrix44<T>
mul44T(Matrix44<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m * t;
}

template <class T>
static Matrix44<T>
rmul44T(Matrix44<T> &m, const T &t)
{
    MATH_EXC_ON;
    return t * m;
}

template <class T>
static const Matrix44<T> &
idiv44T(Matrix44<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m /= t;
}

template <class T>
static Matrix44<T>
div44T(Matrix44<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m / t;
}

template <class T>
static void
extractAndRemoveScalingAndShear44(Matrix44<T> &mat, Vec3<T> &dstScl, Vec3<T> &dstShr, int exc = 1)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::extractAndRemoveScalingAndShear(mat, dstScl, dstShr, exc);
}

template <class T>
static void
extractEulerXYZ(Matrix44<T> &mat, IMATH_NAMESPACE::Vec3<T> &dst)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::extractEulerXYZ(mat, dst);
}

template <class T>
static void
extractEulerZYX(Matrix44<T> &mat, IMATH_NAMESPACE::Vec3<T> &dst)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::extractEulerZYX(mat, dst);
}

template <class T>
static int
extractSHRT44(Matrix44<T> &mat, Vec3<T> &s, Vec3<T> &h, Vec3<T> &r, Vec3<T> &t, int exc = 1)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::extractSHRT(mat, s, h, r, t, exc);
}

template <class T>
static void
extractScaling44(Matrix44<T> &mat, Vec3<T> &dst, int exc = 1)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::extractScaling(mat, dst, exc);
}

template <class T>
static void
extractScalingAndShear44(Matrix44<T> &mat, Vec3<T> &dstScl, Vec3<T> &dstShr, int exc = 1)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::extractScalingAndShear(mat, dstScl, dstShr, exc);
}

template <class TV,class TM>
static void
multDirMatrix44(Matrix44<TM> &mat, const Vec3<TV> &src, Vec3<TV> &dst)
{
    MATH_EXC_ON;
    mat.multDirMatrix(src, dst);
}

template <class TV,class TM>
static Vec3<TV>
multDirMatrix44_return_value(Matrix44<TM> &mat, const Vec3<TV> &src)
{
    MATH_EXC_ON;
    Vec3<TV> dst;
    mat.multDirMatrix(src, dst);
    return dst;
}

template <class T1, class T2>
struct op_multDirMatrix {

    static inline void apply(const Matrix44<T2>& m, const Vec3<T1>& src, Vec3<T1>& dst)
    {
        m.multDirMatrix(src,dst);
    }
};

template <class T1, class T2>
struct op_multVecMatrix {

    static inline void apply(const Matrix44<T2>& m, const Vec3<T1>& src, Vec3<T1>& dst)
    {
        m.multVecMatrix(src,dst);
    }
};

template <class T1,class T2, class Op>
struct MatrixVecTask : public Task
{
    const Matrix44<T2> &mat;
    const FixedArray<Vec3<T1> >& src;
    FixedArray<Vec3<T1> >& dst;

    MatrixVecTask(const Matrix44<T2> &m, const FixedArray<Vec3<T1> >& s, FixedArray<Vec3<T1> >& d)
        : mat(m), src(s), dst(d) {}

    void execute(size_t start, size_t end)
    {
        for(size_t p = start; p < end; ++p)
            Op::apply(mat,src[p],dst[p]);
    }
};

template <class TV,class TM>
static FixedArray<Vec3<TV> >
multDirMatrix44_array(Matrix44<TM> &mat, const FixedArray<Vec3<TV> >&src)
{
    MATH_EXC_ON;
    size_t len = src.len();
    FixedArray<Vec3<TV> > dst(len);

    MatrixVecTask<TV,TM,op_multDirMatrix<TV,TM> > task(mat,src,dst);
    dispatchTask(task,len);

    return dst;
}

template <class TV,class TM>
static void
multVecMatrix44(Matrix44<TM> &mat, const Vec3<TV> &src, Vec3<TV> &dst)
{
    MATH_EXC_ON;
    mat.multVecMatrix(src, dst);
}

template <class TV,class TM>
static Vec3<TV>
multVecMatrix44_return_value(Matrix44<TM> &mat, const Vec3<TV> &src)
{
    MATH_EXC_ON;
    Vec3<TV> dst;
    mat.multVecMatrix(src, dst);
    return dst;
}

template <class TV,class TM>
static FixedArray<Vec3<TV> >
multVecMatrix44_array(Matrix44<TM> &mat, const FixedArray<Vec3<TV> >&src)
{
    MATH_EXC_ON;
    size_t len = src.len();
    FixedArray<Vec3<TV> > dst(len);

    MatrixVecTask<TV,TM,op_multVecMatrix<TV,TM> > task(mat,src,dst);
    dispatchTask(task,len);

    return dst;
}

template <class T>
static int
removeScaling44(Matrix44<T> &mat, int exc = 1)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::removeScaling(mat, exc);
}

template <class T>
static int
removeScalingAndShear44(Matrix44<T> &mat, int exc = 1)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::removeScalingAndShear(mat, exc);
}

template <class T>
static Matrix44<T>
sansScaling44(const Matrix44<T> &mat, bool exc = true)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::sansScaling(mat, exc);
}

template <class T>
static Matrix44<T>
sansScalingAndShear44(const Matrix44<T> &mat, bool exc = true)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::sansScalingAndShear(mat, exc);
}

template <class T>
static const Matrix44<T> &
scaleSc44(Matrix44<T> &mat, const T &s)
{
    MATH_EXC_ON;
    Vec3<T> sVec(s, s, s);
    return mat.scale(sVec);
}

template <class T>
static const Matrix44<T> &
scaleV44(Matrix44<T> &mat, const Vec3<T> &s)
{
    MATH_EXC_ON;
    return mat.scale(s);
}

template <class T>
static const Matrix44<T> &
scale44Tuple(Matrix44<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 3)
    {
        Vec3<T> s;
        s.x = extract<T>(t[0]);
        s.y = extract<T>(t[1]);
        s.z = extract<T>(t[2]);

        return mat.scale(s);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.scale needs tuple of length 3");
}

template <class T>
static const Matrix44<T> &
rotateV44(Matrix44<T> &mat, const Vec3<T> &r)
{
    MATH_EXC_ON;
    return mat.rotate(r);
}

template <class T>
static const Matrix44<T> &
rotationMatrix44(Matrix44<T> &mat, const object &fromObj, const object &toObj)
{
    MATH_EXC_ON;
    Vec3<T> from, to;
    if (PyImath::V3<T>::convert (fromObj.ptr(), &from) &&
        PyImath::V3<T>::convert (toObj.ptr(), &to))
    {
        Matrix44<T> rot = IMATH_NAMESPACE::rotationMatrix(from, to);
        return mat.setValue(rot);
    }
    else
    {
        THROW(IEX_NAMESPACE::ArgExc, "m.rotationMatrix expected V3 arguments");
    }
}

template <class T>
static const Matrix44<T> &
rotationMatrixWithUp44(Matrix44<T> &mat, const object &fromObj, const object &toObj,
                       const object &upObj)
{
    MATH_EXC_ON;
    Vec3<T> from, to, up;
    if (PyImath::V3<T>::convert (fromObj.ptr(), &from) &&
        PyImath::V3<T>::convert (toObj.ptr(), &to) &
        PyImath::V3<T>::convert (upObj.ptr(), &up))
    {
        Matrix44<T> rot = IMATH_NAMESPACE::rotationMatrixWithUpDir(from, to, up);
        return mat.setValue(rot);
    }
    else
    {
        THROW(IEX_NAMESPACE::ArgExc, "m.rotationMatrix expected V3 arguments");
    }
}

template <class T>
static const Matrix44<T> &
setScaleSc44(Matrix44<T> &mat, const T &s)
{
    MATH_EXC_ON;
    Vec3<T> sVec(s, s, s);
    return mat.setScale(sVec);
}

template <class T>
static const Matrix44<T> &
setScaleV44(Matrix44<T> &mat, const Vec3<T> &s)
{
    MATH_EXC_ON;
    return mat.setScale(s);
}

template <class T>
static const Matrix44<T> &
setScale44Tuple(Matrix44<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 3)
    {
        Vec3<T> s;
        s.x = extract<T>(t[0]);
        s.y = extract<T>(t[1]);
        s.z = extract<T>(t[2]);

        return mat.setScale(s);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.translate needs tuple of length 3");
}

template <class T>
static const Matrix44<T> &
setShearV44(Matrix44<T> &mat, const Vec3<T> &sVec)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::Shear6<T> shear(sVec[0], sVec[1], sVec[2], T (0), T (0), T (0));
    return mat.setShear(shear);
}

template <class T>
static const Matrix44<T> &
setShearS44(Matrix44<T> &mat, const Shear6<T> &s)
{
    MATH_EXC_ON;
    return mat.setShear(s);
}

template <class T>
static const Matrix44<T> &
setShear44Tuple(Matrix44<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 3)
    {
        Vec3<T> s;
        s.x = extract<T>(t[0]);
        s.y = extract<T>(t[1]);
        s.z = extract<T>(t[2]);
        Shear6<T> shear(s);

        return mat.setShear(shear);
    }
    else if(t.attr("__len__")() == 6)
    {
        Shear6<T> shear;
        for(int i = 0; i < 6; ++i)
        {
            shear[i] = extract<T>(t[i]);
        }

        return mat.setShear(shear);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.setShear needs tuple of length 3 or 6");
}

template <class T>
static const Matrix44<T> &
setTranslation44(Matrix44<T> &mat, const Vec3<T> t)
{
    MATH_EXC_ON;
    return mat.setTranslation(t);
}

template <class T>
static const Matrix44<T> &
setTranslation44Tuple(Matrix44<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 3)
    {
        Vec3<T> trans;
        trans.x = extract<T>(t[0]);
        trans.y = extract<T>(t[1]);
        trans.z = extract<T>(t[2]);

        return mat.setTranslation(trans);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.translate needs tuple of length 3");
}

template <class T>
static const Matrix44<T> &
setTranslation44Obj(Matrix44<T> &mat, const object &o)
{
    MATH_EXC_ON;
    Vec3<T> v;
    if (PyImath::V3<T>::convert (o.ptr(), &v))
    {
        return mat.setTranslation(v);
    }
    else
    {
        THROW(IEX_NAMESPACE::ArgExc, "m.setTranslation expected V3 argument");
        return mat;
    }
}

template <class T>
static void
setValue44(Matrix44<T> &mat, const Matrix44<T> &value)
{
    MATH_EXC_ON;
    mat.setValue(value);
}

template <class T>
static const Matrix44<T> &
shearV44(Matrix44<T> &mat, const Vec3<T> &s)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::Shear6<T> shear(s[0], s[1], s[2], T (0), T (0), T (0));
    return mat.shear(shear);
}

template <class T>
static const Matrix44<T> &
shearS44(Matrix44<T> &mat, const Shear6<T> &s)
{
    MATH_EXC_ON;
    return mat.shear(s);
}

template <class T>
static const Matrix44<T> &
shear44Tuple(Matrix44<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 3)
    {
        Vec3<T> s;
        s.x = extract<T>(t[0]);
        s.y = extract<T>(t[1]);
        s.z = extract<T>(t[2]);
        Shear6<T> shear(s);

        return mat.shear(shear);
    }
    else if(t.attr("__len__")() == 6)
    {
        Shear6<T> shear;
        for(int i = 0; i < 6; ++i)
        {
            shear[i] = extract<T>(t[i]);
        }

        return mat.shear(shear);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.shear needs tuple of length 3 or 6");
}

template <class T>
static const Matrix44<T> &
translate44(Matrix44<T> &mat, const object &t)
{
    MATH_EXC_ON;
    Vec3<T> v;
    if (PyImath::V3<T>::convert (t.ptr(), &v))
    {
        return mat.translate(v);
    }
    else
    {
        THROW(IEX_NAMESPACE::ArgExc, "m.translate expected V3 argument");
        return mat;
    }
}
template <class T>
static const Matrix44<T> &
translate44Tuple(Matrix44<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 3)
    {
        Vec3<T> trans;
        trans.x = extract<T>(t[0]);
        trans.y = extract<T>(t[1]);
        trans.z = extract<T>(t[2]);

        return mat.translate(trans);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.translate needs tuple of length 3");
}

template <class T>
static Matrix44<T>
subtractTL44(Matrix44<T> &mat, T a)
{
    MATH_EXC_ON;
    Matrix44<T> m(mat.x);
    for(int i = 0; i < 4; ++i)
        for(int j = 0; j < 4; ++j)
            m.x[i][j] -= a;

    return m;
}

template <class T>
static Matrix44<T>
subtractTR44(Matrix44<T> &mat, T a)
{
    MATH_EXC_ON;
    Matrix44<T> m(mat.x);
    for(int i = 0; i < 4; ++i)
        for(int j = 0; j < 4; ++j)
            m.x[i][j] = a - m.x[i][j];

    return m;
}

template <class T>
static Matrix44<T>
add44T(Matrix44<T> &mat, T a)
{
    MATH_EXC_ON;
    Matrix44<T> m(mat.x);
    for(int i = 0; i < 4; ++i)
        for(int j = 0; j < 4; ++j)
            m.x[i][j] += a;

    return m;
}

template <class S, class T>
static Matrix44<T>
mul44(Matrix44<T> &mat1, Matrix44<S> &mat2)
{
    MATH_EXC_ON;
    Matrix44<T> mat2T;
    mat2T.setValue (mat2);
    return mat1 * mat2T;
}

template <class S, class T>
static Matrix44<T>
rmul44(Matrix44<T> &mat2, Matrix44<S> &mat1)
{
    MATH_EXC_ON;
    Matrix44<T> mat1T;
    mat1T.setValue (mat1);
    return mat1T * mat2;
}

template <class S, class T>
static const Matrix44<T> &
imul44(Matrix44<T> &mat1, Matrix44<S> &mat2)
{
    MATH_EXC_ON;
    Matrix44<T> mat2T;
    mat2T.setValue (mat2);
    return mat1 *= mat2T;
}

template <class T>
static bool
lessThan44(Matrix44<T> &mat1, const Matrix44<T> &mat2)
{
    for(int i = 0; i < 4; ++i){
        for(int j = 0; j < 4; ++j){
            if(mat1[i][j] > mat2[i][j]){
                return false;
            }
        }
    }

    return (mat1 != mat2);
}

template <class T>
static bool
lessThanEqual44(Matrix44<T> &mat1, const Matrix44<T> &mat2)
{
    for(int i = 0; i < 4; ++i){
        for(int j = 0; j < 4; ++j){
            if(mat1[i][j] > mat2[i][j]){
                return false;
            }
        }
    }

    return true;
}

template <class T>
static bool
greaterThan44(Matrix44<T> &mat1, const Matrix44<T> &mat2)
{
    for(int i = 0; i < 4; ++i){
        for(int j = 0; j < 4; ++j){
            if(mat1[i][j] < mat2[i][j]){
                return false;
            }
        }
    }

    return (mat1 != mat2);
}

template <class T>
static bool
greaterThanEqual44(Matrix44<T> &mat1, const Matrix44<T> &mat2)
{
    for(int i = 0; i < 4; ++i){
        for(int j = 0; j < 4; ++j){
            if(mat1[i][j] < mat2[i][j]){
                return false;
            }
        }
    }

    return true;
}

template <class T>
static tuple
singularValueDecomposition44(const Matrix44<T>& m, bool forcePositiveDeterminant = false)
{
    IMATH_NAMESPACE::Matrix44<T> U, V;
    IMATH_NAMESPACE::Vec4<T> S;
    IMATH_NAMESPACE::jacobiSVD (m, U, S, V, IMATH_NAMESPACE::limits<T>::epsilon(), forcePositiveDeterminant);
    return make_tuple (U, S, V);
}

BOOST_PYTHON_FUNCTION_OVERLOADS(invert44_overloads, invert44, 1, 2);
BOOST_PYTHON_FUNCTION_OVERLOADS(inverse44_overloads, inverse44, 1, 2);
BOOST_PYTHON_FUNCTION_OVERLOADS(gjInvert44_overloads, gjInvert44, 1, 2);
BOOST_PYTHON_FUNCTION_OVERLOADS(gjInverse44_overloads, gjInverse44, 1, 2);
BOOST_PYTHON_FUNCTION_OVERLOADS(extractAndRemoveScalingAndShear44_overloads, extractAndRemoveScalingAndShear44, 3, 4)
BOOST_PYTHON_FUNCTION_OVERLOADS(extractSHRT44_overloads, extractSHRT44, 5, 6)
BOOST_PYTHON_FUNCTION_OVERLOADS(extractScaling44_overloads, extractScaling44, 2, 3)
BOOST_PYTHON_FUNCTION_OVERLOADS(extractScalingAndShear44_overloads, extractScalingAndShear44, 3, 4)
BOOST_PYTHON_FUNCTION_OVERLOADS(removeScaling44_overloads, removeScaling44, 1, 2)
BOOST_PYTHON_FUNCTION_OVERLOADS(removeScalingAndShear44_overloads, removeScalingAndShear44, 1, 2)
BOOST_PYTHON_FUNCTION_OVERLOADS(sansScaling44_overloads, sansScaling44, 1, 2)
BOOST_PYTHON_FUNCTION_OVERLOADS(sansScalingAndShear44_overloads, sansScalingAndShear44, 1, 2)

template <class T>
static Matrix44<T> * Matrix4_tuple_constructor(const tuple &t0, const tuple &t1, const tuple &t2, const tuple &t3)
{
  if(t0.attr("__len__")() == 4 && t1.attr("__len__")() == 4 && t2.attr("__len__")() == 4 && t3.attr("__len__")() == 4)
  {
      return new Matrix44<T>(extract<T>(t0[0]),  extract<T>(t0[1]),  extract<T>(t0[2]),  extract<T>(t0[3]),
                             extract<T>(t1[0]),  extract<T>(t1[1]),  extract<T>(t1[2]),  extract<T>(t1[3]),
                             extract<T>(t2[0]),  extract<T>(t2[1]),  extract<T>(t2[2]),  extract<T>(t2[3]),
                             extract<T>(t3[0]),  extract<T>(t3[1]),  extract<T>(t3[2]),  extract<T>(t3[3]));
  }
  else
      THROW(IEX_NAMESPACE::LogicExc, "Matrix44 takes 4 tuples of length 4");
}

template <class T, class S>
static Matrix44<T> *Matrix4_matrix_constructor(const Matrix44<S> &mat)
{
    Matrix44<T> *m = new Matrix44<T>;

    for(int i = 0; i < 4; ++i)
        for(int j = 0; j < 4; ++j)
            m->x[i][j] = T (mat.x[i][j]);

    return m;
}

template <class T>
class_<Matrix44<T> >
register_Matrix44()
{
    typedef PyImath::StaticFixedArray<Matrix44<T>,T,4,IndexAccessMatrixRow<Matrix44<T>,T,4> > Matrix44_helper;

    MatrixRow<T,4>::register_class();

    class_<Matrix44<T> > matrix44_class(Matrix44Name<T>::value, Matrix44Name<T>::value,init<Matrix44<T> >("copy construction"));
    matrix44_class
        .def(init<>("initialize to identity"))
        .def(init<T>("initialize all entries to a single value"))
        .def("__init__", make_constructor(Matrix4_tuple_constructor<T>),"tuple constructor1")
        .def("__init__", make_constructor(Matrix4_matrix_constructor<T,float>))
        .def("__init__", make_constructor(Matrix4_matrix_constructor<T,double>))

        .def(init<T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T>("make from components"))
	//.def_readwrite("x00", &Matrix44<T>::x[0][0])
	//.def_readwrite("x01", &Matrix44<T>::x[0][1])
	//.def_readwrite("x02", &Matrix44<T>::x[0][2])
	//.def_readwrite("x03", &Matrix44<T>::x[0][3])
	//.def_readwrite("x10", &Matrix44<T>::x[1][0])
	//.def_readwrite("x11", &Matrix44<T>::x[1][1])
	//.def_readwrite("x12", &Matrix44<T>::x[1][2])
	//.def_readwrite("x13", &Matrix44<T>::x[1][3])
	//.def_readwrite("x20", &Matrix44<T>::x[2][0])
	//.def_readwrite("x21", &Matrix44<T>::x[2][1])
	//.def_readwrite("x22", &Matrix44<T>::x[2][2])
	//.def_readwrite("x23", &Matrix44<T>::x[2][3])
	//.def_readwrite("x30", &Matrix44<T>::x[3][0])
	//.def_readwrite("x31", &Matrix44<T>::x[3][1])
	//.def_readwrite("x32", &Matrix44<T>::x[3][2])
	//.def_readwrite("x33", &Matrix44<T>::x[3][3])
        .def("baseTypeEpsilon", &Matrix44<T>::baseTypeEpsilon,"baseTypeEpsilon() epsilon value of the base type of the vector")
        .staticmethod("baseTypeEpsilon")
        .def("baseTypeMax", &Matrix44<T>::baseTypeMax,"baseTypeMax() max value of the base type of the vector")
        .staticmethod("baseTypeMax")
        .def("baseTypeMin", &Matrix44<T>::baseTypeMin,"baseTypeMin() min value of the base type of the vector")
        .staticmethod("baseTypeMin")
        .def("baseTypeSmallest", &Matrix44<T>::baseTypeSmallest,"baseTypeSmallest() smallest value of the base type of the vector")
        .staticmethod("baseTypeSmallest")
        .def("equalWithAbsError", &Matrix44<T>::equalWithAbsError,
             "m1.equalWithAbsError(m2,e) true if the elements "
             "of v1 and v2 are the same with an absolute error of no more than e, "
             "i.e., abs(m1[i] - m2[i]) <= e")

        .def("equalWithRelError", &Matrix44<T>::equalWithRelError,
             "m1.equalWithAbsError(m2,e) true if the elements "
             "of m1 and m2 are the same with an absolute error of no more than e, "
             "i.e., abs(m1[i] - m2[i]) <= e * abs(m1[i])")

        // need a different version for matrix data access
        .def("__len__", Matrix44_helper::len)
        .def("__getitem__", Matrix44_helper::getitem)
	//.def("__setitem__", Matrix44_helper::setitem)
        .def("makeIdentity",&Matrix44<T>::makeIdentity,"makeIdentity() make this matrix the identity matrix")
        .def("transpose",&Matrix44<T>::transpose,return_internal_reference<>(),"transpose() transpose this matrix")
        .def("transposed",&Matrix44<T>::transposed,"transposed() return a transposed copy of this matrix")
        .def("minorOf",&Matrix44<T>::minorOf,"minorOf() return matrix minor of the (row,col) element of this matrix")
        .def("fastMinor",&Matrix44<T>::fastMinor,"fastMinor() return matrix minor using the specified rows and columns of this matrix")
        .def("determinant",&Matrix44<T>::determinant,"determinant() return the determinant of this matrix")
        .def("invert",&invert44<T>,invert44_overloads("invert() invert this matrix")[return_internal_reference<>()])
        .def("inverse",&inverse44<T>,inverse44_overloads("inverse() return a inverted copy of this matrix"))
        .def("gjInvert",&gjInvert44<T>,gjInvert44_overloads("gjInvert() invert this matrix")[return_internal_reference<>()])
        .def("gjInverse",&gjInverse44<T>,gjInverse44_overloads("gjInverse() return a inverted copy of this matrix"))
        .def(self == self)
        .def(self != self)
        .def("__iadd__", &iadd44<T, float>,return_internal_reference<>())
        .def("__iadd__", &iadd44<T, double>,return_internal_reference<>())
        .def("__iadd__", &iadd44T<T>,return_internal_reference<>())
        .def("__add__", &add44<T>)
        .def("__isub__", &isub44<T, float>,return_internal_reference<>())
        .def("__isub__", &isub44<T, double>,return_internal_reference<>())
        .def("__isub__", &isub44T<T>,return_internal_reference<>())
        .def("__sub__", &sub44<T>)
        .def("negate",&negate44<T>,return_internal_reference<>(),"negate() negate all entries in this matrix")
        .def("__neg__", &neg44<T>)
        .def("__imul__", &imul44T<T>,return_internal_reference<>())
        .def("__mul__", &mul44T<T>)
        .def("__rmul__", &rmul44T<T>)
        .def("__idiv__", &idiv44T<T>,return_internal_reference<>())
        .def("__itruediv__", &idiv44T<T>,return_internal_reference<>())
        .def("__div__", &div44T<T>)
        .def("__truediv__", &div44T<T>)
        .def("__add__", &add44T<T>)
        .def("__radd__", &add44T<T>)
        .def("__sub__", &subtractTL44<T>)
        .def("__rsub__", &subtractTR44<T>)
        .def("__mul__", &mul44<float, T>)
        .def("__mul__", &mul44<double, T>)
        .def("__rmul__", &rmul44<float, T>)
        .def("__rmul__", &rmul44<double, T>)
        .def("__imul__", &imul44<float, T>,return_internal_reference<>())
        .def("__imul__", &imul44<double, T>,return_internal_reference<>())
        .def("__lt__", &lessThan44<T>)
        .def("__gt__", &greaterThan44<T>)
        .def("__le__", &lessThanEqual44<T>)
        .def("__ge__", &greaterThanEqual44<T>)
	//.def(self_ns::str(self))
	    .def("__repr__",&Matrix44_repr<T>)

        .def("extractAndRemoveScalingAndShear", &extractAndRemoveScalingAndShear44<T>,
             extractAndRemoveScalingAndShear44_overloads(
             "M.extractAndRemoveScalingAndShear(scl, shr, "
             "[exc]) -- extracts the scaling component of "
             "M into scl and the shearing component of M "
             "into shr.  Also removes the scaling and "
             "shearing components from M.  "
             "Returns 1 unless the scaling component is "
             "nearly 0, in which case 0 is returned. "
             "If optional arg. exc == 1, then if the "
             "scaling component is nearly 0, then MathExc "
             "is thrown."))

        .def("extractEulerXYZ", &extractEulerXYZ<T>, "extract Euler")
        .def("extractEulerZYX", &extractEulerZYX<T>, "extract Euler")

        .def("extractSHRT", &extractSHRT44<T>, extractSHRT44_overloads(
             "M.extractSHRT(Vs, Vh, Vr, Vt, [exc]) -- "
	         "extracts the scaling component of M into Vs, "
			 "the shearing component of M in Vh (as XY, "
	         "XZ, YZ shear factors), the rotation of M "
	         "into Vr (as Euler angles in the order XYZ), "
	         "and the translaation of M into Vt. "
			 "If optional arg. exc == 1, then if the "
             "scaling component is nearly 0, then MathExc "
             "is thrown. "))

        .def("extractScaling", &extractScaling44<T>, extractScaling44_overloads("extract scaling"))
        .def("extractScalingAndShear", &extractScalingAndShear44<T>, extractScalingAndShear44_overloads("extract scaling"))
        .def("singularValueDecomposition", &singularValueDecomposition44<T>,
             "Decomposes the matrix using the singular value decomposition (SVD) into three\n"
             "matrices U, S, and V which have the following properties: \n"
             "  1. U and V are both orthonormal matrices, \n"
             "  2. S is the diagonal matrix of singular values, \n"
             "  3. U * S * V.transposed() gives back the original matrix.\n"
             "The result is returned as a tuple [U, S, V].  Note that since S is diagonal we\n"
             "don't need to return the entire matrix, so we return it as a three-vector.  \n"
             "\n"
             "The 'forcePositiveDeterminant' argument can be used to force the U and V^T to\n"
             "have positive determinant (that is, to be proper rotation matrices); if\n"
             "forcePositiveDeterminant is False, then the singular values are guaranteed to\n"
             "be nonnegative but the U and V matrices might contain negative scale along one\n"
             "of the axes; if forcePositiveDeterminant is True, then U and V cannot contain\n"
             "negative scale but S[3] might be negative.  \n"
             "\n"
             "Our SVD implementation uses two-sided Jacobi rotations to iteratively\n"
             "diagonalize the matrix, which should be quite robust and significantly faster\n"
             "than the more general SVD solver in LAPACK.  \n",
             args("matrix", "forcePositiveDeterminant"))
        .def("symmetricEigensolve", &PyImath::jacobiEigensolve<IMATH_NAMESPACE::Matrix44<T> >,
             "Decomposes the matrix A using a symmetric eigensolver into matrices Q and S \n"
             "which have the following properties: \n"
             "  1. Q is the orthonormal matrix of eigenvectors, \n"
             "  2. S is the diagonal matrix of eigenvalues, \n"
             "  3. Q.transposed() * S * Q gives back the original matrix.\n"
             "\n"
             "IMPORTANT: It is vital that the passed-in matrix be symmetric, or the result \n"
             "won't make any sense.  This function will return an error if passed an \n"
             "unsymmetric matrix.\n"
             "\n"
             "The result is returned as a tuple [Q, S].  Note that since S is diagonal \n"
             "we don't need to return the entire matrix, so we return it as a three-vector. \n"
             "\n"
             "Our eigensolver implementation uses one-sided Jacobi rotations to iteratively \n"
             "diagonalize the matrix, which should be quite robust and significantly faster \n"
             "than the more general symmetric eigenvalue solver in LAPACK.  \n")
        .def("multDirMatrix", &multDirMatrix44<double,T>, "mult matrix")
        .def("multDirMatrix", &multDirMatrix44_return_value<double,T>, "mult matrix")
        .def("multDirMatrix", &multDirMatrix44_array<double,T>, "mult matrix")
        .def("multDirMatrix", &multDirMatrix44<float,T>, "mult matrix")
        .def("multDirMatrix", &multDirMatrix44_return_value<float,T>, "mult matrix")
        .def("multDirMatrix", &multDirMatrix44_array<float,T>, "mult matrix")
        .def("multVecMatrix", &multVecMatrix44<double,T>, "mult matrix")
        .def("multVecMatrix", &multVecMatrix44_return_value<double,T>, "mult matrix")
        .def("multVecMatrix", &multVecMatrix44_array<double,T>, "mult matrix")
        .def("multVecMatrix", &multVecMatrix44<float,T>, "mult matrix")
        .def("multVecMatrix", &multVecMatrix44_return_value<float,T>, "mult matrix")
        .def("multVecMatrix", &multVecMatrix44_array<float,T>, "mult matrix")
        .def("removeScaling", &removeScaling44<T>, removeScaling44_overloads("remove scaling"))
        .def("removeScalingAndShear", &removeScalingAndShear44<T>, removeScalingAndShear44_overloads("remove scaling"))
        .def("sansScaling", &sansScaling44<T>, sansScaling44_overloads("sans scaling"))
        .def("sansScalingAndShear", &sansScalingAndShear44<T>, sansScalingAndShear44_overloads("sans scaling and shear"))
        .def("scale", &scaleSc44<T>, return_internal_reference<>(), "scale matrix")
        .def("scale", &scaleV44<T>, return_internal_reference<>(), "scale matrix")
        .def("scale", &scale44Tuple<T>, return_internal_reference<>(), "scale matrix")
        .def("rotate", &rotateV44<T>, return_internal_reference<>(), "rotate matrix")
        .def("rotationMatrix", &rotationMatrix44<T>, return_internal_reference<>(), "rotationMatrix()")
        .def("rotationMatrixWithUpDir", &rotationMatrixWithUp44<T>, return_internal_reference<>(), "roationMatrixWithUp()")
        .def("setScale", &setScaleSc44<T>, return_internal_reference<>(),"setScale()")
        .def("setScale", &setScaleV44<T>, return_internal_reference<>(),"setScale()")
        .def("setScale", &setScale44Tuple<T>, return_internal_reference<>(),"setScale()")

        .def("setShear", &setShearV44<T>, return_internal_reference<>(),"setShear()")
        .def("setShear", &setShearS44<T>, return_internal_reference<>(),"setShear()")
        .def("setShear", &setShear44Tuple<T>, return_internal_reference<>(),"setShear()")
        .def("setTranslation", &setTranslation44<T>, return_internal_reference<>(),"setTranslation()")
        .def("setTranslation", &setTranslation44Tuple<T>, return_internal_reference<>(),"setTranslation()")
        .def("setTranslation", &setTranslation44Obj<T>, return_internal_reference<>(),"setTranslation()")
        .def("setValue", &setValue44<T>, "setValue()")
        .def("shear", &shearV44<T>, return_internal_reference<>(),"shear()")
        .def("shear", &shearS44<T>, return_internal_reference<>(),"shear()")
        .def("shear", &shear44Tuple<T>, return_internal_reference<>(),"shear()")
        .def("translate", &translate44<T>, return_internal_reference<>(),"translate()")
        .def("translate", &translate44Tuple<T>, return_internal_reference<>(),"translate()")
        .def("translation", &Matrix44<T>::translation, "translation()")

        ;

    decoratecopy(matrix44_class);

    return matrix44_class;
/*
    const Matrix44 &	operator = (const Matrix44 &v);
    const Matrix44 &	operator = (T a);
    T *			getValue ();
    const T *		getValue () const;
    template <class S> void getValue (Matrix44<S> &v) const;
    template <class S> Matrix44 & setValue (const Matrix44<S> &v);
    template <class S> Matrix44 & setTheMatrix (const Matrix44<S> &v);
    template <class S> void multVecMatrix(const Vec2<S> &src, Vec2<S> &dst) const;
    template <class S> void multDirMatrix(const Vec2<S> &src, Vec2<S> &dst) const;
    template <class S> const Matrix44 &	setRotation (S r);
    template <class S> const Matrix44 &	rotate (S r);
    const Matrix44 &	setScale (T s);
    template <class S> const Matrix44 &	setScale (const Vec2<S> &s);
    template <class S> const Matrix44 &	scale (const Vec2<S> &s);
    template <class S> const Matrix44 &	setTranslation (const Vec2<S> &t);
    Vec2<T>		translation () const;
    template <class S> const Matrix44 &	translate (const Vec2<S> &t);
    template <class S> const Matrix44 &	setShear (const S &h);
    template <class S> const Matrix44 &	setShear (const Vec2<S> &h);
    template <class S> const Matrix44 &	shear (const S &xy);
    template <class S> const Matrix44 &	shear (const Vec2<S> &h);
*/
}

template <class T>
static void
setM44ArrayItem(FixedArray<IMATH_NAMESPACE::Matrix44<T> > &ma,
                Py_ssize_t index,
                const IMATH_NAMESPACE::Matrix44<T> &m)
{
    ma[ma.canonical_index(index)] = m;
}

template <class T>
class_<FixedArray<IMATH_NAMESPACE::Matrix44<T> > >
register_M44Array()
{
    class_<FixedArray<IMATH_NAMESPACE::Matrix44<T> > > matrixArray_class = FixedArray<IMATH_NAMESPACE::Matrix44<T> >::register_("Fixed length array of IMATH_NAMESPACE::Matrix44");
    matrixArray_class
         .def("__setitem__", &setM44ArrayItem<T>)
        ;
    return matrixArray_class;
}

template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Matrix44<float> > register_Matrix44<float>();
template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Matrix44<double> > register_Matrix44<double>();

template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Matrix44<float> > > register_M44Array<float>();
template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Matrix44<double> > > register_M44Array<double>();

template<> PYIMATH_EXPORT IMATH_NAMESPACE::Matrix44<float> FixedArrayDefaultValue<IMATH_NAMESPACE::Matrix44<float> >::value() { return IMATH_NAMESPACE::Matrix44<float>(); }
template<> PYIMATH_EXPORT IMATH_NAMESPACE::Matrix44<double> FixedArrayDefaultValue<IMATH_NAMESPACE::Matrix44<double> >::value() { return IMATH_NAMESPACE::Matrix44<double>(); }
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (c) 2021, Andrew Kennings
// 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.

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Includes.
////////////////////////////////////////////////////////////////////////////////
#include "detailed_hpwl.h"
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <boost/tokenizer.hpp>
#include <cmath>
#include <iostream>
#include <stack>
#include <utility>
#include "detailed_manager.h"
#include "detailed_orient.h"
#include "rectangle.h"

namespace dpo {

////////////////////////////////////////////////////////////////////////////////
// Defines.
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Classes.
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
DetailedHPWL::DetailedHPWL(Architecture* arch, Network* network,
                           RoutingParams* rt)
    : DetailedObjective(),
      m_arch(arch),
      m_network(network),
      m_rt(rt),
      m_orientPtr(0),
      m_skipNetsLargerThanThis(100) {
  m_traversal = 0;
  m_edgeMask.resize(m_network->getNumEdges());
  std::fill(m_edgeMask.begin(), m_edgeMask.end(), m_traversal);

  m_name = "hpwl";
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
DetailedHPWL::~DetailedHPWL() {}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void DetailedHPWL::init() {
  m_traversal = 0;
  m_edgeMask.resize(m_network->getNumEdges());
  std::fill(m_edgeMask.begin(), m_edgeMask.end(), m_traversal);
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void DetailedHPWL::init(DetailedMgr* mgrPtr, DetailedOrient* orientPtr) {
  m_orientPtr = orientPtr;
  m_mgrPtr = mgrPtr;
  init();
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
double DetailedHPWL::curr() {
  double x, y;
  double hpwl = 0.;
  Rectangle box;
  for (int i = 0; i < m_network->getNumEdges(); i++) {
    Edge* edi = m_network->getEdge(i);

    int npins = edi->getNumPins();
    if (npins <= 1 || npins >= m_skipNetsLargerThanThis) {
      continue;
    }

    box.reset();
    for (int pj = 0; pj < edi->getPins().size(); pj++) {
      Pin* pinj = edi->getPins()[pj];

      Node* ndj = pinj->getNode();

      x = ndj->getLeft() + 0.5*ndj->getWidth() + pinj->getOffsetX();
      y = ndj->getBottom() + 0.5*ndj->getHeight() + pinj->getOffsetY();

      box.addPt(x,y);
    }

    hpwl += (box.getWidth()+box.getHeight());
  }
  return hpwl;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
double DetailedHPWL::delta(int n, std::vector<Node*>& nodes,
                           std::vector<int>& curLeft, std::vector<int>& curBottom,
                           std::vector<unsigned>& curOri,
                           std::vector<int>& newLeft, std::vector<int>& newBottom,
                           std::vector<unsigned>& newOri) {
  // Given a list of nodes with their old positions and new positions, compute
  // the change in WL. Note that we need to know the orientation information and
  // might need to adjust pin information...

  double x, y;
  double old_wl = 0.;
  double new_wl = 0.;
  Rectangle old_box, new_box;

  // Put cells into their "old positions and orientations".
  for (int i = 0; i < n; i++) {
    nodes[i]->setLeft(curLeft[i]);
    nodes[i]->setBottom(curBottom[i]);
    if (m_orientPtr != 0) {
      m_orientPtr->orientAdjust(nodes[i], curOri[i]);
    }
  }

  ++m_traversal;
  for (int i = 0; i < n; i++) {
    Node* ndi = nodes[i];
    for (int pi = 0; pi < ndi->getPins().size(); pi++) {
      Pin* pini = ndi->getPins()[pi];

      Edge* edi = pini->getEdge();

      int npins = edi->getNumPins();
      if (npins <= 1 || npins >= m_skipNetsLargerThanThis) {
        continue;
      }
      if (m_edgeMask[edi->getId()] == m_traversal) {
        continue;
      }
      m_edgeMask[edi->getId()] = m_traversal;

      old_box.reset();
      for (int pj = 0; pj < edi->getNumPins(); pj++) {
        Pin* pinj = edi->getPins()[pj];

        Node* curr = pinj->getNode();

        x = curr->getLeft() + 0.5*curr->getWidth() + pinj->getOffsetX();
        y = curr->getBottom() + 0.5*curr->getHeight() + pinj->getOffsetY();

        old_box.addPt(x,y);
      }

      old_wl += (old_box.getWidth()+old_box.getHeight());
    }
  }

  // Put cells into their "new positions and orientations".
  for (int i = 0; i < n; i++) {
    nodes[i]->setLeft(newLeft[i]);
    nodes[i]->setBottom(newBottom[i]);
    if (m_orientPtr != 0) {
      m_orientPtr->orientAdjust(nodes[i], newOri[i]);
    }
  }

  ++m_traversal;
  for (int i = 0; i < n; i++) {
    Node* ndi = nodes[i];
    for (int pi = 0; pi < ndi->getNumPins(); pi++) {
      Pin* pini = ndi->getPins()[pi];

      Edge* edi = pini->getEdge();

      int npins = edi->getNumPins();
      if (npins <= 1 || npins >= m_skipNetsLargerThanThis) {
        continue;
      }
      if (m_edgeMask[edi->getId()] == m_traversal) {
        continue;
      }
      m_edgeMask[edi->getId()] = m_traversal;

      new_box.reset();
      for (int pj = 0; pj < edi->getNumPins(); pj++) {
        Pin* pinj = edi->getPins()[pj];

        Node* curr = pinj->getNode();

        x = curr->getLeft() + 0.5*curr->getWidth() + pinj->getOffsetX();
        y = curr->getBottom() + 0.5*curr->getHeight() + pinj->getOffsetY();

        new_box.addPt(x,y);
      }

      new_wl += (new_box.getWidth()+new_box.getHeight());
    }
  }

  // Put cells into their "old positions and orientations" before returning
  // (leave things as they were provided to us...).
  for (int i = 0; i < n; i++) {
    nodes[i]->setLeft(curLeft[i]);
    nodes[i]->setBottom(curBottom[i]);
    if (m_orientPtr != 0) {
      m_orientPtr->orientAdjust(nodes[i], curOri[i]);
    }
  }

  // +ve means improvement.
  return old_wl - new_wl;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
double DetailedHPWL::delta(Node* ndi, double new_x, double new_y) {
  // Compute change in wire length for moving node to new position.

  double old_wl = 0.;
  double new_wl = 0.;
  double x, y;
  Rectangle old_box, new_box;

  ++m_traversal;
  for (int pi = 0; pi < ndi->getNumPins(); pi++) {
    Pin* pini = ndi->getPins()[pi];

    Edge* edi = pini->getEdge();

    int npins = edi->getNumPins();
    if (npins <= 1 || npins >= m_skipNetsLargerThanThis) {
      continue;
    }
    if (m_edgeMask[edi->getId()] == m_traversal) {
      continue;
    }
    m_edgeMask[edi->getId()] = m_traversal;

    old_box.reset();
    new_box.reset();
    for (int pj = 0; pj < edi->getNumPins(); pj++) {
      Pin* pinj = edi->getPins()[pj];

      Node* ndj = pinj->getNode();

      x = ndj->getLeft() + 0.5*ndj->getWidth() + pinj->getOffsetX();
      y = ndj->getBottom() + 0.5*ndj->getHeight() + pinj->getOffsetY();

      old_box.addPt(x,y);

      if (ndj == ndi) {
        x = new_x + pinj->getOffsetX();
        y = new_y + pinj->getOffsetY();
      }

      new_box.addPt(x,y);
    }
    old_wl += (old_box.getWidth()+old_box.getHeight());
    new_wl += (new_box.getWidth()+new_box.getHeight());
  }
  return old_wl - new_wl;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void DetailedHPWL::getCandidates(std::vector<Node*>& candidates) {
  candidates.erase(candidates.begin(), candidates.end());
  candidates = m_mgrPtr->m_singleHeightCells;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
double DetailedHPWL::delta(Node* ndi, Node* ndj) {
  // Compute change in wire length for swapping the two nodes.

  double old_wl = 0.;
  double new_wl = 0.;
  double x, y;
  Rectangle old_box, new_box;
  Node* nodes[2];
  nodes[0] = ndi;
  nodes[1] = ndj;

  ++m_traversal;
  for (int c = 0; c <= 1; c++) {
    Node* ndi = nodes[c];
    for (int pi = 0; pi < ndi->getNumPins(); pi++) {
      Pin* pini = ndi->getPins()[pi];

      Edge* edi = pini->getEdge();

      //int npins = edi->getNumPins();
      int npins = edi->getNumPins();
      if (npins <= 1 || npins >= m_skipNetsLargerThanThis) {
        continue;
      }
      if (m_edgeMask[edi->getId()] == m_traversal) {
        continue;
      }
      m_edgeMask[edi->getId()] = m_traversal;

      old_box.reset();
      new_box.reset();
      for (int pj = 0; pj < edi->getNumPins(); pj++) {
        Pin* pinj = edi->getPins()[pj];

        Node* ndj = pinj->getNode();

        x = ndj->getLeft() + 0.5*ndj->getWidth() + pinj->getOffsetX();
        y = ndj->getBottom() + 0.5*ndj->getHeight() + pinj->getOffsetY();

        old_box.addPt(x,y);

        if (ndj == nodes[0]) {
          ndj = nodes[1];
        } else if (ndj == nodes[1]) {
          ndj = nodes[0];
        }

        x = ndj->getLeft() + 0.5*ndj->getWidth() + pinj->getOffsetX();
        y = ndj->getBottom() + 0.5*ndj->getHeight() + pinj->getOffsetY();

        new_box.addPt(x,y);
      }

      old_wl += (old_box.getWidth()+old_box.getHeight());
      new_wl += (new_box.getWidth()+new_box.getHeight());
    }
  }
  return old_wl - new_wl;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
double DetailedHPWL::delta(Node* ndi, double target_xi, double target_yi,
                           Node* ndj, double target_xj, double target_yj) {
  // Compute change in wire length for swapping the two nodes.

  double old_wl = 0.;
  double new_wl = 0.;
  double x, y;
  Rectangle old_box, new_box;
  Node* nodes[2];
  nodes[0] = ndi;
  nodes[1] = ndj;

  ++m_traversal;
  for (int c = 0; c <= 1; c++) {
    Node* ndi = nodes[c];
    for (int pi = 0; pi < ndi->getNumPins(); pi++) {
      Pin* pini = ndi->getPins()[pi];

      Edge* edi = pini->getEdge();

      int npins = edi->getNumPins();
      if (npins <= 1 || npins >= m_skipNetsLargerThanThis) {
        continue;
      }
      if (m_edgeMask[edi->getId()] == m_traversal) {
        continue;
      }
      m_edgeMask[edi->getId()] = m_traversal;

      old_box.reset();
      new_box.reset();
      for (int pj = 0; pj < edi->getPins().size(); pj++) {
        Pin* pinj = edi->getPins()[pj];

        Node* curr = pinj->getNode();

        x = curr->getLeft() + 0.5*curr->getWidth() + pinj->getOffsetX();
        y = curr->getBottom() + 0.5*curr->getHeight() + pinj->getOffsetY();

        old_box.addPt(x,y);

        if (curr == nodes[0]) {
          x = target_xi + pinj->getOffsetX();
          y = target_yi + pinj->getOffsetY();
        } else if (curr == nodes[1]) {
          x = target_xj + pinj->getOffsetX();
          y = target_yj + pinj->getOffsetY();
        }

        new_box.addPt(x,y);
      }

      old_wl += (old_box.getWidth()+old_box.getHeight());
      new_wl += (new_box.getWidth()+new_box.getHeight());
    }
  }
  return old_wl - new_wl;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
}  // namespace dpo


///////////////////////////////////////////////////////////////////////////////
//
// This file is a part of the PadallelFDTD Finite-Difference Time-Domain
// simulation library. It is released under the MIT License. You should have
// received a copy of the MIT License along with ParallelFDTD.  If not, see
// http://www.opensource.org/licenses/mit-license.php
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// For details, see the LICENSE file
//
// (C) 2013-2014 Jukka Saarelma
// Aalto University School of Science
//
///////////////////////////////////////////////////////////////////////////////

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "App.h"

using namespace boost::python;

void FDTD::App::initializeGeometryPy(boost::python::list indices,
                                     boost::python::list vertices) {
  int v_len = (int)boost::python::len(vertices);
  int i_len = (int)boost::python::len(indices);
  std::vector<float> std_vertices(v_len, 0.f);
  std::vector<unsigned int> std_indices(i_len, 0);

  for(int i = 0; i < v_len; i++)
    std_vertices.at(i) = boost::python::extract<float>(vertices[i]);

  for(int i = 0; i < i_len; i++)
    std_indices.at(i) = boost::python::extract<unsigned int>(indices[i]);

  this->m_geometry.initialize(std_indices, std_vertices);
}

void FDTD::App::setLayerIndicesPy(boost::python::list indices,
                                  std::string name) {
  int i_len = (int)boost::python::len(indices);
  std::vector<int> std_indices(i_len, 0.f);
  for(int i = 0; i < i_len; i++)
    std_indices.at(i) = boost::python::extract<int>(indices[i]);

  this->m_geometry.setLayerIndices(std_indices, name);
}

void FDTD::App::addSurfaceMaterials(boost::python::list material_coefficients,
                                    unsigned int number_of_surfaces,
                                    unsigned int number_of_coefficients) {
  int m_len = (int)boost::python::len(material_coefficients);
  std::vector<float> std_mat(m_len, 0.f);
  for(int i = 0; i < m_len; i++)
    std_mat.at(i) = boost::python::extract<float>(material_coefficients[i]);

  this->m_materials.addMaterials(&std_mat[0],
                                 number_of_surfaces,
                                 number_of_coefficients);
}

void FDTD::App::addSourceDataFloat(boost::python::list src_data,
                                   int num_steps,
                                   int num_sources) {
  std::vector<float> std_src_data(num_steps*num_sources, 0.0);
  for(int j = 0; j < num_sources; j++) {
    for(int i = 0; i < num_steps; i++) {
      int idx =j*num_steps+i;
      std_src_data.at(idx) = boost::python::extract<float>(src_data[idx]);
    }
  }
  this->m_parameters.addInputData(std_src_data);
}

void FDTD::App::addSourceDataDouble(boost::python::list src_data,
                              int num_steps,
                              int num_sources) {
  std::vector<double> std_src_data(num_steps*num_sources, 0.0);
  for(int j = 0; j < num_sources; j++) {
    for(int i = 0; i < num_steps; i++) {
      int idx =j*num_steps+i;
      std_src_data.at(idx) = boost::python::extract<double>(src_data[idx]);
    }
  }
  this->m_parameters.addInputDataDouble(std_src_data);
}

BOOST_PYTHON_MODULE(libPyFDTD) {

  class_< std::vector<float> >("std_vec_float")
    .def(vector_indexing_suite< std::vector<float> >() )
    ;

  class_< std::vector<double> >("std_vec_double")
    .def(vector_indexing_suite< std::vector<double> >() )
    ;

  class_<FDTD::App>("App")
    .def("initializeDevices", &FDTD::App::initializeDevices)
    .def("initializeGeometryFromFile", &FDTD::App::initializeGeometryFromFile)
    .def("initializeGeometryPy", &FDTD::App::initializeGeometryPy)
    .def("setLayerIndices", &FDTD::App::setLayerIndicesPy)
    .def("addSource", &FDTD::App::addSource)
    .def("addSourceDataFloat", &FDTD::App::addSourceDataFloat)
    .def("addSourceDataDouble", &FDTD::App::addSourceDataDouble)
    .def("addReceiver", &FDTD::App::addReceiver)
    .def("addSurfaceMaterials", &FDTD::App::addSurfaceMaterials)
    .def("setSpatialFs", &FDTD::App::setSpatialFs)
    .def("setNumSteps", &FDTD::App::setNumSteps )
    .def("setUpdateType", &FDTD::App::setUpdateType)
    .def("setUniform", &FDTD::App::setUniformMaterial)
    .def("runVisualization", &FDTD::App::runVisualization)
    .def("runSimulation", &FDTD::App::runSimulation)
    .def("runCapture", &FDTD::App::runCapture)
    .def("setUniformMaterial", &FDTD::App::setUniformMaterial)
    .def("getResponse", &FDTD::App::getResponse)
    .def("getResponseDouble", &FDTD::App::getResponseDouble)
    .def("forcePartitionTo", &FDTD::App::setForcePartitionTo)
    .def("addSliceToCapture", &FDTD::App::addSliceToCapture)
    .def("setDouble", &FDTD::App::setDouble)
    .def("setCapturedB", &FDTD::App::setCapturedB)
    .def("close", &FDTD::App::close)
    .def("getMvox", &FDTD::App::getMvoxPerSec)
    .def("getNumElems", &FDTD::App::getNumElements)
    ;
}


/**
 * Copyright (c) 2018, University Osnabrück
 * 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 University Osnabrück 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 University Osnabrück 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 "lvr2/reconstruction/PanoramaNormals.hpp"

#include "lvr2/geometry/BaseVector.hpp"
#include "lvr2/geometry/Normal.hpp"
#include "lvr2/io/Progress.hpp"
#include "lvr2/io/Timestamp.hpp"

#include <Eigen/Dense>

#include <iostream>
#include <iterator>
#include <algorithm>

#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

using std::cout;
using std::endl;

namespace lvr2
{

using Vec = BaseVector<float>;

PanoramaNormals::PanoramaNormals(ModelToImage* mti)
    : m_mti(mti)
{
    m_buffer = mti->pointBuffer();
}

PointBufferPtr PanoramaNormals::computeNormals(int width, int height, bool interpolate)
{
    // Create new point buffer and tmp storages
    PointBufferPtr out_buffer(new PointBuffer);
    vector<float> pts;
    vector<float> normals;

    // Get input buffer's points
    PointBufferPtr in_buffer = m_mti->pointBuffer();
    size_t w_color;
    size_t n_inPoints = in_buffer->numPoints();
    floatArr in_points = in_buffer->getPointArray();
    ucharArr in_colors = in_buffer->getColorArray(w_color);

    // Reserve memory for output buffers (we need a deep copy)
    floatArr p_arr(new float[n_inPoints * 3]);
    floatArr n_arr(new float[n_inPoints * 3]);
    ucharArr c_arr;
    if(in_buffer->hasColors())
    {
        c_arr = ucharArr(new unsigned char[n_inPoints * 3]);
    }

    // Get panorama
    ModelToImage::DepthListMatrix mat;
    m_mti->computeDepthListMatrix(mat);

    // If the desired neighborhood is larger than 2 x 2 pixels
    // compute offsets for i und j dimension of the image.
    int di = 2;
    if(width > 2)
    {
        di = width / 2;
    }

    int dj = 2;
    if(height > 2)
    {
        dj = height / 2;
    }

    // Compute normals
    // Create progress output
    string comment = timestamp.getElapsedTime() + "Computing normals ";
    ProgressBar progress(mat.pixels.size(), comment);

    for(size_t i = 0; i < mat.pixels.size(); i++)
    {
        #pragma omp parallel for
        for(size_t j = 0; j < mat.pixels[i].size(); j++)
        {
            // Check if image entry is empty
            if(mat.pixels[i][j].size() == 0)
            {
                continue;
            }

            // Collect 'neighboring' points
            vector<ModelToImage::PanoramaPoint> nb;

            // The points at the current position are part of the neighborhood
            std::copy(mat.pixels[i][j].begin(), mat.pixels[i][j].end(), std::back_inserter(nb));

            for(int off_i = -di; off_i <= di; off_i++)
            {
                for(int off_j = -dj; off_j <= dj; off_j++)
                {
                    int p_i = i + off_i;
                    int p_j = j + off_j;

                    if(p_i >= 0 && p_i < mat.pixels.size() &&
                       p_j >= 0 && p_j < mat.pixels[i].size())
                    {
                        // We only save the first point as representative
                        // because using all points from list will likely
                        // result in undesirable configurations for local
                        // normal estimation
                        if(mat.pixels[p_i][p_j].size() > 0)
                        {
                            nb.push_back(mat.pixels[p_i][p_j][0]);
                        }
                    }
                }
            }

            // Compute normal if more than three neighbors where found
            if(nb.size() > 3)
            {

                // Compute mean
                Vec mean;
                for(int i = 0; i < nb.size(); i++)
                {
                    // Determine position of geometry in point array
                    size_t index = nb[i].index * 3;

                    // Get point coordinates
                    Vec neighbor(in_points[index],
                                           in_points[index + 1],
                                           in_points[index + 2]);

                    // Add to mean
                    mean.x += neighbor.x;
                    mean.y += neighbor.y;
                    mean.z += neighbor.z;
                }
                mean.x /= nb.size();
                mean.y /= nb.size();
                mean.z /= nb.size();

                // Calculate covariance
                double covariance[9] = {0};

                for(int i = 0; i < nb.size(); i++)
                {
                    size_t index = nb[i].index * 3;

                    Vec pt(in_points[index    ] - mean.x,
                                     in_points[index + 1] - mean.y,
                                     in_points[index + 3] - mean.z);

                    covariance[4] += pt.y * pt.y;
                    covariance[7] += pt.y * pt.z;
                    covariance[8] += pt.z * pt.z;

                    pt.x *= pt.x;
                    pt.y *= pt.x;
                    pt.z *= pt.x;

                    covariance[0] += pt.x;
                    covariance[1] += pt.y;
                    covariance[6] += pt.z;

                }

                covariance[3] = covariance[1];
                covariance[2] = covariance[6];
                covariance[5] = covariance[7];

                for(int i = 0; i < 9; i++)
                {
                    covariance[i] /= nb.size();
                }

                // Compute eigenvalues and eigenvectors using GSL
                gsl_matrix_view m = gsl_matrix_view_array(covariance, 3, 3);
                gsl_matrix* evec = gsl_matrix_alloc(3, 3);
                gsl_vector* eval = gsl_vector_alloc(3);

                gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (3);
                gsl_eigen_symmv (&m.matrix, eval, evec, w);

                gsl_eigen_symmv_free (w);
                gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);

                gsl_vector_view evec_0 = gsl_matrix_column(evec, 0);
                float nx = gsl_vector_get(&evec_0.vector, 0);
                float ny = gsl_vector_get(&evec_0.vector, 1);
                float nz = gsl_vector_get(&evec_0.vector, 2);

                // Flip normals towards reference point
                Normal<float> nn(nx, ny, nz);
                Vec center(0, 0, 0);

                size_t index = mat.pixels[i][j][0].index * 3;
                Vec p1 = center - Vec(in_points[index], in_points[index + 1], in_points[index + 2]);

                if(Normal<float>(p1) * nn < 0)
                {
                    nx *= -1;
                    ny *= -1;
                    nz *= -1;
                }

                for(size_t k = 0; k < mat.pixels[i][j].size(); k++)
                {
                    // Assign the same normal to all points
                    // behind this pixel to preserve the complete
                    // point cloud
                    size_t index = mat.pixels[i][j][k].index * 3;
                    size_t color_index = mat.pixels[i][j][k].index * w_color;

                    // Copy point and normal to target buffer
                    p_arr[index    ] = in_points[index];
                    p_arr[index + 1] = in_points[index + 1];
                    p_arr[index + 2] = in_points[index + 2];

                    if(in_buffer->hasColors())
                    {
                        c_arr[index    ] = in_colors[color_index];
                        c_arr[index + 1] = in_colors[color_index + 1];
                        c_arr[index + 2] = in_colors[color_index + 2];
                    }

                    if(!interpolate)
                    {
                        n_arr[index    ] = nx;
                        n_arr[index + 1] = ny;
                        n_arr[index + 2] = nz;
                    }
                }
            }
        }
        ++progress;
    }
    cout << endl;

//    if(interpolate)
//    {
//        cout << timestamp << " Interpolating normals" << endl;
//        for(size_t i = 0; i < mat.pixels.size(); i++)
//        {
//            for(size_t j = 0; j < mat.pixels[i].size(); j++)
//            {
//                float x = 0.0f;
//                float y = 0.0f;
//                float z = 0.0f;
//                int np = 0;
//                for(int off_i = -di; off_i <= di; off_i++)
//                {
//                    for(int off_j = -dj; off_j <= dj; off_j++)
//                    {
//                        int p_i = i + off_i;
//                        int p_j = j + off_j;

//                        if(p_i >= 0 && p_i < mat.pixels.size() &&
//                           p_j >= 0 && p_j < mat.pixels[i].size())
//                        {
//                            for(size_t k = 0; k < mat.pixels[p_i][p_j].size(); k++)
//                            {
//                                size_t index = mat.pixels[p_i][p_j][k].index;
//                                x += in_points[index];
//                                y += in_points[index + 1];
//                                z += in_points[index + 2];
//                                np++;
//                            }
//                        }
//                    }
//                }
//                if(np > 3) // Same condition as above
//                {
//                    x /= np;
//                    y /= np;
//                    z /= np;
//                    normals.push_back(x);
//                    normals.push_back(y);
//                    normals.push_back(z);
//                }

//            }
//        }

//        cout << normals.size() << " " << pts.size() << endl;
//    }

    cout << timestamp << "Finished normal estimation" << endl;

    if(in_buffer->hasColors())
    {
        out_buffer->setColorArray(c_arr, n_inPoints);
    }
    out_buffer->setPointArray(p_arr, n_inPoints);
    out_buffer->setNormalArray(n_arr, n_inPoints);

    return out_buffer;
}

} // namespace lvr2


// [[Rcpp::depends(BH)]]
// [[Rcpp::depends(RcppEigen)]]

#include "distribution.h"
#include <boost/math/distributions/logistic.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/cauchy.hpp>
#include <boost/math/distributions/extreme_value.hpp>
#include <boost/math/distributions/students_t.hpp>

using namespace boost::math;
using namespace std;
using namespace Rcpp ;

distribution::distribution(void) {
  // Rcout << "Distribution is being created" << endl;
}

LogicalVector is_character(DataFrame A) {
  LogicalVector res(A.cols());
  for (int column = 0 ; column < A.cols() ; column++){
    // bool a89 = (TYPEOF(A[column]) == STRSXP);
    // bool a90 = ( TYPEOF(A[column]) == STRSXP || TYPEOF(A[column]) ==  INTSXP );
    bool a90 = Rf_isFactor(A[column]) || (TYPEOF(A[column]) == STRSXP);
    res[column] = a90;
  }
  return res;
}

template <int RTYPE>
IntegerVector fast_factor_template( const Vector<RTYPE>& x ) {
  Vector<RTYPE> levs = sort_unique(x);
  IntegerVector out = match(x, levs);
  out.attr("levels") = as<CharacterVector>(levs);
  out.attr("class") = "integer";
  return out;
}

IntegerVector fast_factor( SEXP x ) {
  switch( TYPEOF(x) ) {
  case INTSXP: return fast_factor_template<INTSXP>(x);
  case REALSXP: return fast_factor_template<REALSXP>(x);
  case STRSXP: return fast_factor_template<STRSXP>(x);
  }
  return R_NilValue;
}

Eigen::VectorXd sort_vector_getindex(Eigen::VectorXd x1) {
  NumericVector V(x1.data(), x1.data() + x1.size());
  int x=0;
  std::iota(V.begin(),V.end(),x++);
  sort( V.begin(),V.end(), [&](int i,int j){return x1[i]<x1[j];} );
  Eigen::Map<Eigen::VectorXd> XS(Rcpp::as<Eigen::Map<Eigen::VectorXd> >(V));
  return XS;
}

Eigen::MatrixXd sorted_rows(Eigen::MatrixXd A)
{
  Eigen::VectorXd vec1 = sort_vector_getindex(A.col(0));
  Eigen::MatrixXd B = A.row(vec1(0));
  for (int i = 1; i < A.rows(); ++i) {
    B.conservativeResize(B.rows()+1, B.cols());
    B.row(B.rows()-1) = A.row(vec1(i));
  }
  return B;
}

NumericMatrix to_dummy(SEXP A)
{
  IntegerVector cha_to_fact = fast_factor(A);
  CharacterVector levs1 = cha_to_fact.attr("levels");
  int var_lev = levs1.length();
  NumericMatrix B_Ma(cha_to_fact.length(), var_lev);
  for (int i_1 = 0; i_1 < cha_to_fact.length(); ++i_1){
    int col_ind = cha_to_fact[i_1] - 1;
    B_Ma(i_1, col_ind) = 1;
  }
  if (var_lev != 2){
    B_Ma = B_Ma( _ , Range(0,var_lev-2) );
  } else {
    B_Ma = B_Ma( _ , Range(1,var_lev-1) );
  }
  return B_Ma;
}

DataFrame sort_by_user(DataFrame A, SEXP order2)
{
  IntegerVector y_1 = (A[0]);
  StringVector y_n(A.rows());
  IntegerVector order = (fast_factor(order2));

  CharacterVector levs1 = order.attr("levels");
  CharacterVector levs2 = y_1.attr("levels");
  if (is_true(all(levs1 == levs2))) {
    for (int element_order = 0 ; element_order <= order.length(); element_order++){
      LogicalVector v0 = (y_1 == order[element_order]);
      y_n[v0] = element_order;
    }
    DataFrame B = A;
    CharacterVector y_2 = as<CharacterVector>(y_n);
    B[0] = y_2;
    return B;
  }stop("The response categories do not match the proposed order of entry");
}

std::string distribution::concatenate(std::string x, std::string level)
{
  return (x + " " +level);
}

List distribution::select_data(DataFrame x1, std::string response,
                               StringVector explanatory_complete,
                               StringVector explanatory_proportional,
                               SEXP order) {

  int P_c = explanatory_complete.size();
  if(explanatory_complete[0] == "NA"){P_c = 0; }
  // Rcout << P_c << std::endl;
  int P_p = explanatory_proportional.size();
  if(explanatory_proportional[0] == "NA"){P_p = 0; }
  // Rcout << P_p << std::endl;
  const int N = x1.nrows() ; // Number of observations

  // ADD INTERCEPT
  Eigen::VectorXd Ones1 = Eigen::VectorXd::Ones(x1.rows());
  x1["intercept"] = Ones1;

  // Zero initialization
  NumericVector a1(1);
  a1[0] = x1.findName(response);
  int n_com_cat = 0;
  for (int element = 0 ; element < explanatory_complete.size() ; element++ ){
    if(explanatory_complete[0] != "NA"){
      String element_1 = explanatory_complete[element];
      a1.push_back(x1.findName(element_1));
    }else {}
  }

  // SOLO CONTEO
  DataFrame x21 = x1[a1];
  LogicalVector n_com_cat1 = is_character(x21);
  n_com_cat = sum(n_com_cat1)-1;

  // CONTINUA PARA ANADIR PROPORTIONAL
  for (int element_p = 0 ; element_p < explanatory_proportional.size() ; element_p++ ){
    if(explanatory_proportional[0] != "NA"){
      String element_2 = explanatory_proportional[element_p];
      a1.push_back(x1.findName(element_2));
    }else {}
  }

  // SE CREA LA MATRIZ COMPLETA DONDE Y ES LA PRIMERA COLUMNA
  DataFrame x23 = x1[a1];
  // Just assign factor vector to categorical order proposed
  DataFrame x2 = sort_by_user(x23, order); // solo reemplaza por nuevo orden

  LogicalVector character_var = is_character(x2);
  NumericMatrix X_com_cat_int( x2.nrows() , 1 ) ;

  // Solo para conteo de categoricas completas
  if(explanatory_complete[0] != "NA"){
    for (int column_char = 1 ; column_char < P_c+1; column_char++){
      if (character_var(column_char)){
        NumericMatrix a2 = to_dummy(x2[column_char]);
        X_com_cat_int = cbind(X_com_cat_int, a2);
      }else{
        NumericVector a3 = x2[column_char];
        X_com_cat_int = cbind(X_com_cat_int, a3);
      }
    }
  }
  int col_com = X_com_cat_int.cols() - 1;
  // Rcout << col_com << std::endl;

  NumericMatrix result_m( x2.nrows() , 1 ) ;
  for (int column_char1 = 0 ; column_char1 < x2.cols(); column_char1++){
    if(character_var[column_char1] == TRUE){
      NumericMatrix a8 = to_dummy(x2[column_char1]);
      result_m = cbind(result_m, a8);
    }
    else{
      NumericVector a9 = x2[column_char1];
      result_m = cbind(result_m, a9);
    }
  }

  IntegerVector a4 = fast_factor(x2[0]);
  CharacterVector levs1 = a4.attr("levels");
  int K = levs1.length();
  int Q = K-1;
  NumericVector a5 = as<NumericVector>(a4);
  result_m = result_m(_, Range(1,result_m.cols()-1));
  result_m = cbind(a5,result_m);
  Eigen::Map<Eigen::MatrixXd> P = as<Eigen::Map<Eigen::MatrixXd> >(result_m);
  P = sorted_rows(P);
  Eigen::MatrixXd P1 = P.rightCols(P.cols()-1);
  Eigen::MatrixXd Y_ext = P1.leftCols(K-1);
  Eigen::MatrixXd Com_ext = P1.block(0 , K-1 , P1.rows() , col_com );
  Eigen::MatrixXd Pro_ext = P1.rightCols(P1.cols() - Com_ext.cols() - Y_ext.cols()) ;

  Eigen::MatrixXd X_EXT(2, 2);
  Eigen::MatrixXd X_M_Complete_Ext;
  if(P_c > 0){
    X_M_Complete_Ext = Eigen::kroneckerProduct(Com_ext,Eigen::MatrixXd::Identity(Q,Q)).eval();
  }
  Eigen::MatrixXd X_M_Poportional_Ext(N*Q, Pro_ext.cols()) ;
  if(P_p > 0){
    for (int x = -1; x < N-1; ++x) {
      for (int j = (x+1)*Q ; j < (x+2)*Q; ++j){
        X_M_Poportional_Ext.row(j) = (Pro_ext).row(x+1);
      }
    }
  }
  X_EXT.conservativeResize( N*Q , X_M_Complete_Ext.cols()+X_M_Poportional_Ext.cols() );
  X_EXT << X_M_Complete_Ext, X_M_Poportional_Ext;

  return List::create(_["Y_ext"] = Y_ext,
                      _["X_EXT"] = X_EXT,
                      _["levs1"] = levs1);
}

List distribution::select_data_nested(DataFrame x1, std::string response, std::string actual_response,
                                      std::string individuals,
                                      StringVector explanatory_complete,
                                      StringVector depend_y,
                                      SEXP order) {

  const int N = x1.nrows() ;

  // ADD INTERCEPT
  Eigen::VectorXd Ones1 = Eigen::VectorXd::Ones(N);
  x1["intercept"] = Ones1;

  NumericVector a1(1);
  a1[0] = x1.findName(response);
  a1.push_back(x1.findName(individuals));
  a1.push_back(x1.findName(actual_response));

  for (int element = 0 ; element < explanatory_complete.size() ; element++ ){
    if(explanatory_complete[0] != "NA"){
      String element_1 = explanatory_complete[element];
      a1.push_back(x1.findName(element_1));
    }else {}
  }
  for (int element = 0 ; element < depend_y.size() ; element++ ){
    if(depend_y[0] != "NA"){
      String element_1 = depend_y[element];
      a1.push_back(x1.findName(element_1));
    }else {}
  }

  DataFrame x2 = sort_by_user(x1[a1], order); // solo reemplaza por nuevo orden
  // SOLO PARA CONTEO DE NIVELES RESPUESTA
  IntegerVector a4 = fast_factor(x2[0]);
  CharacterVector levs1 = a4.attr("levels");
  int K = levs1.length();
  int Q = K-1;
  NumericVector ref_cat(N) ;

  for (int i = 0 ; i < N; i++){
    if(a4[i] == K){
      ref_cat[i] = 1;
    }else {}
  }

  CharacterVector response_to_reemplace = x2[0];
  x2[0] = x2[1];
  x2[1] = response_to_reemplace; //ahora tienen los nombres opuestos

  LogicalVector choice_ind  = x2[2];

  NumericVector ind_response(N/K);
  int iter = 0;
  for (int i = 0; i < choice_ind.length(); i++) {
    if (choice_ind[i]) {
      ind_response[iter] = a4[i];
      iter++;
    } else {
    }
  }

  NumericMatrix Y_ext = to_dummy(ind_response);

  NumericVector a88(1);
  if(explanatory_complete[0] != "NA"){
    String element_1 = explanatory_complete[0];
    a88[0] = x2.findName(element_1);
    for (int element_p = 1 ; element_p < explanatory_complete.size() ; element_p++ ){
      String element_2 = explanatory_complete[element_p];
      a88.push_back(x2.findName(element_2));
    }
  }else {}
  NumericMatrix complete_var = internal::convert_using_rfunction(x2[a88], "as.matrix");

  NumericVector a89(1);
  if(depend_y[0] != "NA"){
    String element_1 = depend_y[0];
    a89[0] = x2.findName(element_1);
    for (int element_p = 1 ; element_p < depend_y.size() ; element_p++ ){
      String element_2 = depend_y[element_p];
      a89.push_back(x2.findName(element_2));
    }
  }else {}

  NumericMatrix to_change44 = internal::convert_using_rfunction(x2[a89], "as.matrix");
  to_change44 = cbind(to_change44, ref_cat);

  NumericVector to_change_sub(N);
  NumericMatrix vec;
  NumericVector vec1, vec_ref , ref5;

  // AHORA COMENZAMOS EL BUQLE POR INDIVIDUOS
  for(int vector = 0 ; vector < depend_y.length(); vector++){
    if(depend_y[0] != "NA"){
      for(int indi = 1 ; indi <= (N/K) ; indi++)
      {
        vec = to_change44( Range(K*(indi-1),(indi*K)-1) , _);
        vec1 = vec( _ , vector);
        vec_ref = vec( _ , depend_y.size());
        ref5 = vec1[vec_ref == 1];
        for (int y = 1; y <= K; ++y)
        {
          to_change_sub[K*(indi-1)+y-1] = vec1[y-1] - ref5[0];
        }
      }
      to_change44 = cbind(to_change44, to_change_sub);
    }else {}
  }
  to_change44 = to_change44(_, Range(to_change44.cols()-depend_y.length(), to_change44.cols() - 1));
  to_change44 = cbind(complete_var, to_change44);

  // ELIMINO LA FILA DE REFERENCIA
  NumericMatrix x52(Dimension(N- N/K, to_change44.ncol()));
  NumericMatrix x_to_ext(Dimension(N/K, to_change44.ncol()-depend_y.length()));
  int iter2 = 0;
  int def = 0;
  for (int i = 0; i < to_change44.nrow(); i++) {
    if (ref_cat[i] != 1) {
      x52.row(iter2) = to_change44.row(i);
      iter2++;
    } else {
      x_to_ext.row(def) = to_change44.row(i);
      def++;
    }
  }
  Eigen::Map<Eigen::MatrixXd> P = as<Eigen::Map<Eigen::MatrixXd> >(x_to_ext);
  Eigen::Map<Eigen::MatrixXd> ext_dep_y = as<Eigen::Map<Eigen::MatrixXd> >(x52);

  Eigen::MatrixXd X_M_Complete_Ext = Eigen::kroneckerProduct(P, Eigen::MatrixXd::Identity(Q,Q)).eval();

  // Eigen::MatrixXd X1 = X_M_Complete_Ext.block(0,0,X_M_Complete_Ext.rows(),Q+1) ;
  //
  // Eigen::MatrixXd X2 = X_M_Complete_Ext.block(0,(2*Q),X_M_Complete_Ext.rows(),(1)) ;

  Eigen::MatrixXd X1 = X_M_Complete_Ext.block(0,0,X_M_Complete_Ext.rows(),Q) ;
  Eigen::MatrixXd X2(X_M_Complete_Ext.rows(),1);
  // Eigen::MatrixXd X_mod = X1;
  //
  //
  if(explanatory_complete.size() >= 2){

    for (int element_p1 = 0 ; element_p1 < explanatory_complete.size() - 1; element_p1++ ){

      X2 = X_M_Complete_Ext.block(0,((element_p1+1)*Q),X_M_Complete_Ext.rows(),1) ;

      X1.conservativeResize(X1.rows(), X1.cols() + 1);

      X1.col(X1.cols()-1) = X2;

      // X_mod << X_mod, X2;
    }

  }else{}

  Eigen::MatrixXd X_M_dep_y(X_M_Complete_Ext.rows(),X_M_Complete_Ext.cols()+ depend_y.length());

  X_M_dep_y << X_M_Complete_Ext, ext_dep_y.rightCols(depend_y.length());

  Eigen::MatrixXd X_M_dep_y_alt(X1.rows(),X1.cols()+ depend_y.length());

  X_M_dep_y_alt << X1, ext_dep_y.rightCols(depend_y.length());

  return List::create( _["X1"] = X1,
                       _["X_M_dep_y"] = X_M_dep_y,
                       _["X_M_dep_y_alt"] = X_M_dep_y_alt,
                       _["X_M_Complete_Ext"] = X_M_Complete_Ext,
                       _["Y_ext"] = Y_ext,
                       _["levs1"] = levs1
  );
}

Eigen::VectorXd Logistic::in_open_corner(const Eigen::VectorXd& p) const
{
  Eigen::VectorXd pi = p;
  int J = pi.size() + 1;
  for(int j=0; j<J-1; ++j)
  { pi[j] = std::max(_epsilon_0, std::min(pi[j], 1-_epsilon_1)); }
  double sum = pi.sum();
  if(sum > 1-_epsilon_1)
  {
    for(int j=0; j<J-1; ++j)
    { pi[j] *= (1.-_epsilon_1)/sum;  }
  }
  return pi;
}

Logistic::Logistic(void) {
  // Rcout << "Logistic is being created" << endl;
}
double Logistic::cdf_logit(const double& value) const
{
  boost::math::logistic dist(0., 1.);
  return boost::math::cdf(dist, value);
}
double Logistic::pdf_logit(const double& value) const
{
  boost::math::logistic dist(0., 1.);
  return boost::math::pdf(dist, value);
}
Eigen::VectorXd Logistic::InverseLinkCumulativeFunction(Eigen::VectorXd vector){
  boost::math::logistic dist(0., 1.);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = boost::math::cdf(dist, vector(i));
  return vector;
}
Eigen::VectorXd Logistic::InverseLinkDensityFunction(Eigen::VectorXd vector){
  boost::math::logistic dist(0., 1.);
  for (int i = 0; i<=vector.size()-1; i++)
    vector(i) = boost::math::pdf(dist, vector(i));
  return vector;
}
Eigen::VectorXd Logistic::InverseLinkQuantileFunction(Eigen::VectorXd vector ){
  boost::math::logistic dist(0., 1.);
  for (int i = 0; i<=vector.size()-1; i++)
    vector(i) = quantile(dist, vector(i));
  return vector;
}

Normal::Normal(void) {
  // Rcout << "normal is being created" << endl;
}
double Normal::cdf_normal(const double& value) const
{
  boost::math::normal norm;
  return boost::math::cdf(norm, value);
}
double Normal::pdf_normal(const double& value) const
{
  boost::math::normal norm;
  return boost::math::pdf(norm, value);
}

Eigen::VectorXd Normal::InverseLinkCumulativeFunction(Eigen::VectorXd vector ){
  boost::math::normal norm;
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = cdf(norm, vector(i));
  return vector;
}
Eigen::VectorXd Normal::InverseLinkDensityFunction(Eigen::VectorXd vector ){
  boost::math::normal norm;
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = pdf(norm, vector(i));
  return vector;
}
Eigen::VectorXd Normal::InverseLinkQuantileFunction(Eigen::VectorXd vector ){
  boost::math::normal norm;
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = quantile(norm, vector(i));
  return vector;
}

Cauchit::Cauchit(void) {
  // Rcout << "Cauchit is being created" << endl;
}

double Cauchit::cdf_cauchit(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  return cdf(extreme_value, value);
}
double Cauchit::pdf_cauchit(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  return pdf(extreme_value, value);
}

Eigen::VectorXd Cauchit::InverseLinkCumulativeFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = cdf(extreme_value, vector(i));
  return vector;
}
Eigen::VectorXd Cauchit::InverseLinkDensityFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = pdf(extreme_value, vector(i));
  return vector;
}
Eigen::VectorXd Cauchit::InverseLinkQuantileFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::cauchy_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = quantile(extreme_value, vector(i));
  return vector;
}

Student::Student(void) {
  // Rcout << "Student is being created" << endl;
}
double Student::cdf_student(const double& value, double df_student) const
{
  // double _degrees = 1.35;
  boost::math::students_t_distribution<> student(df_student);
  return cdf(student, value);
}
double Student::pdf_student(const double& value, double df_student) const
{
  // double _degrees = 1.35;
  boost::math::students_t_distribution<> student(df_student);
  return pdf(student, value);
}
Eigen::VectorXd Student::InverseLinkCumulativeFunction(Eigen::VectorXd vector ){
  double _degrees = 2.0;
  boost::math::students_t_distribution<> student(_degrees);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = cdf(student, vector(i));
  return vector;
}
Eigen::VectorXd Student::InverseLinkDensityFunction(Eigen::VectorXd vector ){
  double _degrees = 2.0;
  boost::math::students_t_distribution<> student(_degrees);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = pdf(student, vector(i));
  return vector;
}

Gumbel::Gumbel(void) {
  // Rcout << "Gumbel is being created" << endl;
}
double Gumbel::cdf_gumbel(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  return cdf(extreme_value, value);
}
double Gumbel::pdf_gumbel(const double& value) const
{
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  return pdf(extreme_value, value);
}
Eigen::VectorXd Gumbel::InverseLinkCumulativeFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = cdf(extreme_value, vector(i));
  return vector;
}
Eigen::VectorXd Gumbel::InverseLinkDensityFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = pdf(extreme_value, vector(i));
  return vector;
}

Gompertz::Gompertz(void) {
  // Rcout << "Gompertz is being created" << endl;
}

double Gompertz::pdf_gompertz(const double& value) const
{ double _mu = 0.0;
  double _sigma = 1.0;

  return (exp((value - _mu)/ _sigma) *  exp( - exp ((value - _mu)/ _sigma) ) ) / _sigma ; }

double Gompertz::cdf_gompertz(const double& value) const
{ double _mu = 0.0;
  double _sigma = 1.0;
  return  1 - exp( - exp((value - _mu) / _sigma) ); }

Eigen::VectorXd Gompertz::InverseLinkCumulativeFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = 1-cdf(extreme_value, -vector(i));
  return vector;
}
Eigen::VectorXd Gompertz::InverseLinkDensityFunction(Eigen::VectorXd vector ){
  double _location = 0.0;
  double _scale =1.0;
  boost::math::extreme_value_distribution<> extreme_value(_location, _scale);
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = pdf(extreme_value, -vector(i));
  return vector;
}

Eigen::VectorXd Gompertz::InverseLinkQuantileFunction(Eigen::VectorXd vector ){
  double _mu = 0.0;
  double _sigma = 1.0;
  for (int i = 0; i<=vector.rows()-1; i++)
    vector(i) = _mu + _sigma * log( -log(1-vector(i)) );
  return vector;
}

RCPP_MODULE(exportmod){
  using namespace Rcpp ;
  class_<distribution>("distribution")
    .constructor()
  ;
}

// RCPP_MODULE(exportmoddev){
//   using namespace Rcpp ;
//   class_<distribution>("distribution")
//     .constructor()
//   ;
//   class_<Logistic>("Logistic")
//     .derives<distribution>("distribution")
//     .constructor()
//     .method( "InverseLinkCumulativeFunction", &Logistic::InverseLinkCumulativeFunction )
//   ;
// }