#include <Eigen/Geometry>
#include <cmath>
#include <limits>
#include <sm/assert_macros.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/rotations.hpp>

namespace sm {
namespace kinematics {
template <typename Scalar_ = double>
inline bool isLessThenEpsilons4thRoot(Scalar_ x) {
    static const Scalar_ epsilon4thRoot = pow(std::numeric_limits<Scalar_>::epsilon(), 1.0 / 4.0);
    return x < epsilon4thRoot;
}

// quaternion rotation.
Eigen::Vector4d r2quat(Eigen::Matrix3d const& R) {
    const double& c1 = R(0, 0);
    const double& c2 = R(1, 0);
    const double& c3 = R(2, 0);
    const double& c4 = R(0, 1);
    const double& c5 = R(1, 1);
    const double& c6 = R(2, 1);
    const double& c7 = R(0, 2);
    const double& c8 = R(1, 2);
    const double& c9 = R(2, 2);

    Eigen::Vector4d dc(fabs(1.0 + c1 - c5 - c9), fabs(1.0 - c1 + c5 - c9), fabs(1.0 - c1 - c5 + c9),
                       fabs(1.0 + c1 + c5 + c9));

    unsigned maxq = 0;
    double maxqval = dc(0);

    for (unsigned i = 1; i < 4; i++) {
        if (dc(i) > maxqval) {
            maxq = i;
            maxqval = dc(i);
        }
    }

    double c;
    Eigen::Vector4d q;
    if (maxq == 0) {
        q(0) = 0.5 * sqrt(dc(0));
        c = 0.25 / q(0);
        q(1) = c * (c4 + c2);
        q(2) = c * (c7 + c3);
        q(3) = c * (c8 - c6);
    } else if (maxq == 1) {
        q(1) = 0.5 * sqrt(dc(1));
        c = 0.25 / q(1);
        q(0) = c * (c4 + c2);
        q(2) = c * (c6 + c8);
        q(3) = c * (c3 - c7);
    } else if (maxq == 2) {
        q(2) = 0.5 * sqrt(dc(2));
        c = 0.25 / q(2);
        q(0) = c * (c3 + c7);
        q(1) = c * (c6 + c8);
        q(3) = c * (c4 - c2);
    } else {
        q(3) = 0.5 * sqrt(dc(3));
        c = 0.25 / q(3);
        q(0) = c * (c8 - c6);
        q(1) = c * (c3 - c7);
        q(2) = c * (c4 - c2);
    }

    if (q(3) < 0) q = -q;

    return q;
}

Eigen::Matrix3d quat2r(Eigen::Vector4d const& q) {
    SM_ASSERT_NEAR_DBG(std::runtime_error, q.norm(), 1.f, 1e-4,
                       "The quaternion must be a unit vector to represent a rotation");
    // double n = q(3);
    // Eigen::Vector3d e = makeV3(q(0),q(1),q(2));
    // R = (n^2 - e'*e) * eye(3) + 2 * e * e' + 2 * n * crossMx(e);
    // Eigen::Matrix3d R = (n*n - e.dot(e))*Eigen::Matrix3d::Identity() + 2 * e * e.transpose() + 2 * n * crossMx(e);

    Eigen::Matrix3d R;

    // [ q0^2 - q1^2 - q2^2 + q3^2,           2*q0*q1 + 2*q2*q3,           2*q0*q2 - 2*q1*q3]
    // [         2*q0*q1 - 2*q2*q3, - q0^2 + q1^2 - q2^2 + q3^2,           2*q0*q3 + 2*q1*q2]
    // [         2*q0*q2 + 2*q1*q3,           2*q1*q2 - 2*q0*q3, - q0^2 - q1^2 + q2^2 + q3^2]
    R(0, 0) = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    R(0, 1) = q[0] * q[1] * 2.0 + q[2] * q[3] * 2.0;
    R(0, 2) = q[0] * q[2] * 2.0 - q[1] * q[3] * 2.0;
    R(1, 0) = q[0] * q[1] * 2.0 - q[2] * q[3] * 2.0;
    R(1, 1) = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    R(1, 2) = q[0] * q[3] * 2.0 + q[1] * q[2] * 2.0;
    R(2, 0) = q[0] * q[2] * 2.0 + q[1] * q[3] * 2.0;
    R(2, 1) = q[0] * q[3] * (-2.0) + q[1] * q[2] * 2.0;
    R(2, 2) = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];

    return R;
}

Eigen::Matrix4d quatPlus(Eigen::Vector4d const& q) {
    // [  q3,  q2, -q1, q0]
    // [ -q2,  q3,  q0, q1]
    // [  q1, -q0,  q3, q2]
    // [ -q0, -q1, -q2, q3]
    Eigen::Matrix4d Q;
    Q(0, 0) = q[3];
    Q(0, 1) = q[2];
    Q(0, 2) = -q[1];
    Q(0, 3) = q[0];
    Q(1, 0) = -q[2];
    Q(1, 1) = q[3];
    Q(1, 2) = q[0];
    Q(1, 3) = q[1];
    Q(2, 0) = q[1];
    Q(2, 1) = -q[0];
    Q(2, 2) = q[3];
    Q(2, 3) = q[2];
    Q(3, 0) = -q[0];
    Q(3, 1) = -q[1];
    Q(3, 2) = -q[2];
    Q(3, 3) = q[3];

    return Q;
}

Eigen::Matrix4d quatOPlus(Eigen::Vector4d const& q) {
    // [  q3, -q2,  q1, q0]
    // [  q2,  q3, -q0, q1]
    // [ -q1,  q0,  q3, q2]
    // [ -q0, -q1, -q2, q3]

    Eigen::Matrix4d Q;
    Q(0, 0) = q[3];
    Q(0, 1) = -q[2];
    Q(0, 2) = q[1];
    Q(0, 3) = q[0];
    Q(1, 0) = q[2];
    Q(1, 1) = q[3];
    Q(1, 2) = -q[0];
    Q(1, 3) = q[1];
    Q(2, 0) = -q[1];
    Q(2, 1) = q[0];
    Q(2, 2) = q[3];
    Q(2, 3) = q[2];
    Q(3, 0) = -q[0];
    Q(3, 1) = -q[1];
    Q(3, 2) = -q[2];
    Q(3, 3) = q[3];

    return Q;
}

Eigen::Vector4d qplus(Eigen::Vector4d const& q, Eigen::Vector4d const& p) {
    Eigen::Vector4d qplus_p;
    // p0*q3 + p1*q2 - p2*q1 + p3*q0
    qplus_p[0] = p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0];
    // p2*q0 - p0*q2 + p1*q3 + p3*q1
    qplus_p[1] = p[2] * q[0] - p[0] * q[2] + p[1] * q[3] + p[3] * q[1];
    // p0*q1 - p1*q0 + p2*q3 + p3*q2
    qplus_p[2] = p[0] * q[1] - p[1] * q[0] + p[2] * q[3] + p[3] * q[2];
    // p3*q3 - p1*q1 - p2*q2 - p0*q0
    qplus_p[3] = p[3] * q[3] - p[1] * q[1] - p[2] * q[2] - p[0] * q[0];

    return qplus_p;
}

Eigen::Vector4d qoplus(Eigen::Vector4d const& q, Eigen::Vector4d const& p) {
    Eigen::Vector4d qoplus_p;
    // p0*q3 - p1*q2 + p2*q1 + p3*q0
    qoplus_p[0] = p[0] * q[3] - p[1] * q[2] + p[2] * q[1] + p[3] * q[0];
    // p0*q2 - p2*q0 + p1*q3 + p3*q1
    qoplus_p[1] = p[0] * q[2] - p[2] * q[0] + p[1] * q[3] + p[3] * q[1];
    // p1*q0 - p0*q1 + p2*q3 + p3*q2
    qoplus_p[2] = p[1] * q[0] - p[0] * q[1] + p[2] * q[3] + p[3] * q[2];
    // p3*q3 - p1*q1 - p2*q2 - p0*q0
    qoplus_p[3] = p[3] * q[3] - p[1] * q[1] - p[2] * q[2] - p[0] * q[0];

    return qoplus_p;
}

Eigen::Vector4d quatInv(Eigen::Vector4d const& q) {
    Eigen::Vector4d qret = q;
    invertQuat(qret);
    return qret;
}

void invertQuat(Eigen::Vector4d& q) { q.head<3>() = -q.head<3>(); }

Eigen::Vector3d qeps(Eigen::Vector4d const& q) { return q.head<3>(); }

Eigen::Vector3f qeps(Eigen::Vector4f const& q) { return q.head<3>(); }

double qeta(Eigen::Vector4d const& q) { return q[3]; }

float qeta(Eigen::Vector4f const& q) { return q[3]; }

Eigen::Vector4d axisAngle2quat(Eigen::Vector3d const& a) {
    // Method of implementing this function that is accurate to numerical precision from
    // Grassia, F. S. (1998). Practical parameterization of rotations using the exponential map. journal of graphics,
    // gpu, and game tools, 3(3):29–48.

    double theta = a.norm();

    // na is 1/theta sin(theta/2)
    double na;
    if (isLessThenEpsilons4thRoot(theta)) {
        static const double one_over_48 = 1.0 / 48.0;
        na = 0.5 + (theta * theta) * one_over_48;
    } else {
        na = sin(theta * 0.5) / theta;
    }
    Eigen::Vector3d axis = a * na;
    double ct = cos(theta * 0.5);
    return Eigen::Vector4d(axis[0], axis[1], axis[2], ct);
}

/**
 * calculate arcsin(x)/x
 * @param x
 * @return
 */
template <typename Scalar_>
inline Scalar_ arcSinXOverX(Scalar_ x) {
    if (isLessThenEpsilons4thRoot(fabs(x))) {
        return Scalar_(1.0) + x * x * Scalar_(1 / 6);
    }
    return asin(x) / x;
}

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 1> quat2AxisAngle(Eigen::Matrix<Scalar_, 4, 1> const& q) {
    SM_ASSERT_LT_DBG(std::runtime_error, fabs(q.norm() - 1), 8 * std::numeric_limits<Scalar_>::epsilon(),
                     "This function is inteded for unit quternions only.");
    const Eigen::Matrix<Scalar_, 3, 1> a = qeps(q);
    const Scalar_ na = a.norm(), eta = qeta(q);
    Scalar_ scale;
    if (fabs(eta) < na) {  // use eta because it is more precise than na to calculate the scale. No singularities here.
        scale = acos(eta) / na;
    } else {
        /*
         * In this case more precision is in na than in eta so lets use na only to calculate the scale:
         *
         * assume first eta > 0 and 1 > na > 0.
         *               u = asin (na) / na  (this implies u in [1, pi/2], because na i in [0, 1]
         *    sin (u * na) = na
         *  sin^2 (u * na) = na^2
         *  cos^2 (u * na) = 1 - na^2
         *                              (1 = ||q|| = eta^2 + na^2)
         *    cos^2 (u * na) = eta^2
         *                              (eta > 0,  u * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 )
         *      cos (u * na) = eta
         *                              (u * na in [ 0, pi/2] )
         *                 u = acos (eta) / na
         *
         * So the for eta > 0 it is acos(eta) / na == asin(na) / na.
         * From some geometric considerations (mirror the setting at the hyper plane q==0) it follows for eta < 0 that
         * (pi - asin(na)) / na = acos(eta) / na.
         */
        if (eta > 0) {
            // For asin(na)/ na the singularity na == 0 can be removed. We can ask (e.g. Wolfram alpha) for its series
            // expansion at na = 0. And that is done in the following function.
            scale = arcSinXOverX(na);
        } else {
            // (pi - asin(na))/ na has a pole at na == 0. So we cannot remove this singularity.
            // It is just the cut locus of the unit quaternion manifold at identity and thus the axis angle description
            // becomes necessarily unstable there.
            scale = (M_PI - asin(na)) / na;
        }
    }
    return a * (Scalar_(2) * scale);
}
template Eigen::Matrix<double, 3, 1> quat2AxisAngle(Eigen::Matrix<double, 4, 1> const& q);
template Eigen::Matrix<float, 3, 1> quat2AxisAngle(Eigen::Matrix<float, 4, 1> const& q);

Eigen::Matrix<double, 4, 3> quatJacobian(Eigen::Vector4d const& p) {
    Eigen::Matrix<double, 4, 3> J;
    // [  p3, -p2,  p1]
    // [  p2,  p3, -p0]
    // [ -p1,  p0,  p3]
    // [ -p0, -p1, -p2]

    J(0, 0) = p[3];
    J(0, 1) = -p[2];
    J(0, 2) = p[1];
    J(1, 0) = p[2];
    J(1, 1) = p[3];
    J(1, 2) = -p[0];
    J(2, 0) = -p[1];
    J(2, 1) = p[0];
    J(2, 2) = p[3];
    J(3, 0) = -p[0];
    J(3, 1) = -p[1];
    J(3, 2) = -p[2];

    return J * 0.5;
}

Eigen::Vector4d updateQuat(Eigen::Vector4d const& q, Eigen::Vector3d const& dq) {
    // the following code is an optimized version of:
    // Eigen::Vector4d dq4 = axisAngle2quat(dq);
    // Eigen::Vector4d retq = quatPlus(dq4)*q;
    // return retq;

    Eigen::Vector4d dq3 = axisAngle2quat(dq);
    double ca = dq3[3];
    Eigen::Vector4d retq;
    retq[0] = q[0] * ca + dq3[0] * q[3] - dq3[1] * q[2] + dq3[2] * q[1];
    retq[1] = q[1] * ca + dq3[0] * q[2] + dq3[1] * q[3] - dq3[2] * q[0];
    retq[2] = q[2] * ca - dq3[0] * q[1] + dq3[1] * q[0] + dq3[2] * q[3];
    retq[3] = q[3] * ca - dq3[0] * q[0] - dq3[1] * q[1] - dq3[2] * q[2];

    return retq;
}

Eigen::Vector3d quatRotate(Eigen::Vector4d const& q_a_b, Eigen::Vector3d const& v_b) {
    return v_b + 2.0 * q_a_b.head<3>().cross(q_a_b.head<3>().cross(v_b) - q_a_b[3] * v_b);
}

Eigen::Vector4d quatRandom() {
    Eigen::Vector4d q_a_b;
    q_a_b.setRandom();
    q_a_b.array() -= 0.5;
    q_a_b /= q_a_b.norm();
    return q_a_b;
}

Eigen::Vector4d quatIdentity() { return Eigen::Vector4d(0, 0, 0, 1); }

Eigen::Matrix<double, 3, 4> quatS(Eigen::Vector4d q) {
    //   [  q3,  q2, -q1, -q0]
    // 2 [ -q2,  q3,  q0, -q1]
    //   [  q1, -q0,  q3, -q2]
    q *= 2.0;

    Eigen::Matrix<double, 3, 4> S;
    S << q[3], q[2], -q[1], -q[0], -q[2], q[3], q[0], -q[1], q[1], -q[0], q[3], -q[2];

    return S;
}

Eigen::Matrix<double, 4, 3> quatInvS(Eigen::Vector4d q) {
    q *= 0.5;

    // 1 [  q3, -q2,  q1]
    // - [  q2,  q3, -q0]
    // 2 [ -q1,  q0,  q3]
    //   [ -q0, -q1, -q2]

    Eigen::Matrix<double, 4, 3> invS;
    invS << q[3], -q[2], q[1], q[2], q[3], -q[0], -q[1], q[0], q[3], -q[0], -q[1], -q[2];
    return invS;
}

Eigen::Vector4d qslerp(const Eigen::Vector4d& q0, const Eigen::Vector4d& q1, double t) {
    if (t <= 0.0) {
        return q0;
    } else if (t >= 1.0) {
        return q1;
    } else {
        if ((q0 - q1).squaredNorm() > (q0 + q1).squaredNorm()) {
            // The quaternions are far away from eachother on the sphere.
            // Flip one around so that this works out.
            return qplus(q0, qexp(t * qlog(qplus(quatInv(q0), -q1))));
        } else {
            return qplus(q0, qexp(t * qlog(qplus(quatInv(q0), q1))));
        }
    }
}

/// \brief do linear interpolation between p0 and p1 for times t = [0.0,1.0]
Eigen::VectorXd lerp(const Eigen::VectorXd& p0, const Eigen::VectorXd& p1, double t) {
    SM_ASSERT_EQ(std::runtime_error, p0.size(), p1.size(), "The vectors must be the same size");
    if (t <= 0.0) {
        return p0;
    } else if (t >= 1.0) {
        return p1;
    } else {
        return (1 - t) * p0 + t * p1;
    }
}

Eigen::Matrix<double, 3, 4> quatLogJacobian(const Eigen::Vector4d& p) {
    //      [qx]
    //      [qy]                -2*x
    // p =  [qz],    g(x) = ----------------
    //      [qw]               sqrt(1-qw²)
    //
    //
    //      [2*acos(qw)      0            0            g(qx)]
    // J =  [0            2*acos(qw)      0            g(qy)]
    //      [0            0            2*acos(qw)      g(qz)]

    Eigen::Matrix<double, 3, 4> J;
    J.setZero();

    double n = qeps(p).norm();

    double de = n * n * n;                   // pow(n, 3);
    double u12 = p(1) * p(1) + p(2) * p(2);  // pow(p(1), 2) + pow(p(2), 2);
    double u02 = p(0) * p(0) + p(2) * p(2);  // pow(p(0), 2) + pow(p(2), 2);
    double u01 = p(0) * p(0) + p(1) * p(1);  // pow(p(0), 2) + pow(p(1), 2);
    double a = acos(p(3));
    double uw = sqrt(-(p(3) * p(3) - 1) * n * n);

    J(0, 0) = 2 * a * u12 / de;
    J(0, 1) = -2 * a * p(1) * p(0) / de;
    J(0, 2) = -2 * a * p(2) * p(0) / de;
    J(0, 3) = -2 * p(0) / uw;
    J(1, 0) = -2 * a * p(0) * p(1) / de;
    J(1, 1) = 2 * a * u02 / de;
    J(1, 2) = -2 * a * p(1) * p(2) / de;
    J(1, 3) = -2 * p(1) / uw;
    J(2, 0) = -2 * a * p(0) * p(2) / de;
    J(2, 1) = -2 * a * p(1) * p(2) / de;
    J(2, 2) = 2 * a * u01 / de;
    J(2, 3) = -2 * p(2) / uw;

    return J;
}

template <typename Scalar_>
inline const typename Eigen::Matrix<Scalar_, 4, 3>& quatV() {
    static const Eigen::Matrix<Scalar_, 4, 3> V = 0.5 * Eigen::Matrix<Scalar_, 4, 3>::Identity();
    return V;
}
template const Eigen::Matrix<double, 4, 3>& quatV();
template const Eigen::Matrix<float, 4, 3>& quatV();

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 3> expDiffMat(const Eigen::Matrix<Scalar_, 3, 1>& vec) {
    Scalar_ phi = vec.norm();

    if (phi == 0) {
        return Eigen::Matrix<Scalar_, 3, 3>::Identity();
    }

    Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

    Scalar_ phiAbs = fabs(phi);
    Scalar_ phiSquare = phi * phi;

    Scalar_ a;
    Scalar_ b;
    if (!isLessThenEpsilons4thRoot(phiAbs)) {
        Scalar_ siPhiHalf = sin(phi / 2);
        a = (2 * siPhiHalf * siPhiHalf / phiSquare);
        b = ((1 - sin(phi) / phi) / phiSquare);
    } else {
        a = (1.0 / 2) * (1 - (1.0 / (24 / 2)) * phiSquare);
        b = (1.0 / 6) * (1 - (1.0 / (120 / 6)) * phiSquare);
    }

    return Eigen::Matrix<Scalar_, 3, 3>::Identity() - a * vecCross + b * vecCross * vecCross;
}
template Eigen::Matrix<double, 3, 3> expDiffMat<double>(const Eigen::Matrix<double, 3, 1>&);
template Eigen::Matrix<float, 3, 3> expDiffMat<float>(const Eigen::Matrix<float, 3, 1>&);

template <typename Scalar_>
Eigen::Matrix<Scalar_, 4, 3> quatExpJacobian(const Eigen::Matrix<Scalar_, 3, 1>& vec) {
    return quatOPlus(axisAngle2quat(vec.template cast<double>())).template cast<Scalar_>() * quatV<Scalar_>() *
           expDiffMat(vec);
}
template Eigen::Matrix<double, 4, 3> quatExpJacobian(const Eigen::Matrix<double, 3, 1>& vec);
template Eigen::Matrix<float, 4, 3> quatExpJacobian(const Eigen::Matrix<float, 3, 1>& vec);

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 3> logDiffMat(const Eigen::Matrix<Scalar_, 3, 1>& vec) {
    Scalar_ phi = vec.norm();
    if (phi == 0) {
        return Eigen::Matrix<Scalar_, 3, 3>::Identity();
    }

    Scalar_ phiAbs = fabs(phi);
    Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

    Scalar_ a;
    if (!isLessThenEpsilons4thRoot(phiAbs)) {
        Scalar_ phiHalf = 0.5 * phi;
        a = ((1 - phiHalf / tan(phiHalf)) / phi / phi);
    } else {
        a = 1.0 / 12 * (1 + 1.0 / 60 * phi * phi);
    }
    return Eigen::Matrix<Scalar_, 3, 3>::Identity() + 0.5 * vecCross + a * vecCross * vecCross;
}
template Eigen::Matrix<double, 3, 3> logDiffMat<double>(const Eigen::Matrix<double, 3, 1>&);
template Eigen::Matrix<float, 3, 3> logDiffMat<float>(const Eigen::Matrix<float, 3, 1>&);

template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 4> quatLogJacobian2(const Eigen::Matrix<Scalar_, 4, 1>& p) {
    return logDiffMat(quat2AxisAngle<>(p)) * (quatV<Scalar_>().transpose() * Scalar_(4.0)) *
           quatOPlus(quatInv(p.template cast<double>())).template cast<Scalar_>();
}

template Eigen::Matrix<double, 3, 4> quatLogJacobian2(const Eigen::Matrix<double, 4, 1>&);
template Eigen::Matrix<float, 3, 4> quatLogJacobian2(const Eigen::Matrix<float, 4, 1>&);
}  // namespace kinematics
}  // namespace sm


/**
 * Created by sixdi on 1/11/2022.
 */
#include <gtest/gtest.h>

/**
 * DateTime:
 *      time points
 *      periods
 *      time of day
 *      calendar dates
 *

 *
 * Chrono and Timer:
 *      clocks to measure time
 *
 */

/**
 * DateTime:
 *      used to process time data
 *      provides extensions to account for time zones
 *      supports formatted input and output of calendar dates and times
 *
 *      - Calender Dates
 *      - Location-independent Times
 *      - Location-dependent Times
 *      - Formatted Input and Output
*/

/**
 * Calendar Dates
 * https://theboostcpplibraries.com/boost.datetime-calendar
 *
 * https://www.boost.org/doc/libs/1_62_0/doc/html/date_time.html
 *
 * based on Gregorian Calendar
 * boost/date_time/gregorian/gregorian.hpp
 */

#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>

/**
 * creating a date
 */
TEST(datetime, dates){
    boost::gregorian::date d{2014, 1, 31};
    std::cout << d.year() << std::endl;
    std::cout << d.month() << std::endl;
    std::cout << d.day() << std::endl;
    std::cout << d.day_of_year() << std::endl;
    std::cout << d.end_of_month() << std::endl;
    std::cout << d.day_of_week() << std::endl;
}
/**
 * Getting a date from a clock or a string
 */
 using namespace boost::gregorian;
 using std::cout ;
 using std::cin;
 using std::endl;
TEST(datetime, date_from_clock_or_string){
    date d{day_clock::universal_day()};
    cout << d.year() << ", " << d.month() << endl;
    d = date_from_iso_string("20140321");
    cout << d.year() << endl;
}

/**
 * date duration
 */
TEST(datetime, date_duration){
    date d1{2015, 1, 31};
    date d2{2016, 3, 3};
    date_duration dd = d2 - d1;
    cout << dd.days() << endl;
}

/**
 * specialized duration
 */
TEST(datetime, specialize_duration){
    date_duration dd{4};
    cout << dd.days() << endl;
    weeks ws{4};
    cout << ws.days() << endl;
    months ms{4};
    cout << ms.number_of_months() << endl;
    years yrs{4};
    cout << yrs.number_of_years() << endl;
}

/**
 * processing specialized durations
 */
TEST(datetime, processing_specialized_duration){
    date d{2014, 2, 4};
    months ms{1};
    date d2 = d + ms;
    cout << d2 << endl;
}

/**
 * using date_period
 */
TEST(datetime, using_date_period){
    date d1{2014, 1, 1};
    date d2{2014, 2,28};
    date_period dp{d1, d2};
    date_duration dd = dp.length();
    cout << dd.days() << endl;
}

/**
 * whether a period contains dates
 */
TEST(datetime, whether_a_period_contains_dates){
    date d1{2014, 1, 1};
    date d2{2014, 2, 28};
    date_period dp{d1, d2};
    cout.setf(std::ios::boolalpha);
    cout << dp.contains(d1) << endl
            << dp.contains(d2) << endl;

}

/**
 * iterating over dates
 */
TEST(datetime, iterating_over_dates){
    date d{2014, 4, 1};
    day_iterator itr{d};
    cout << itr->day_of_week() << endl;
    cout << *++itr << endl;
    cout << itr->day_of_week() << endl;
    cout << boost::date_time::next_weekday(*itr, greg_weekday(boost::date_time::Friday)) << endl;
}

/**
 * outputs teh weekdays for next December 24 and following two public holidays.
 */

#include <vector>
#include <boost/smart_ptr.hpp>
#include <boost/bind.hpp>

void print_date(boost::shared_ptr<year_based_generator> d, int year){
    std::cout << d.get()->get_date(year) << "[" << d.get()->get_date(year).day_of_week() << "]" << endl;
}

TEST(datetime, output_holiday){
    std::vector<boost::shared_ptr<year_based_generator>>  holidays;
    // Christmas' Eve
    boost::shared_ptr<year_based_generator > h{new partial_date (24, boost::date_time::Dec)};
    holidays.push_back(h);
    //Christmas
    h.reset(new partial_date(25, boost::date_time::Dec));
    holidays.push_back(h);
    // New Year's Day
    h.reset(new partial_date(1, boost::date_time::Jan));
    holidays.push_back(h);
    std::for_each(holidays.begin(), holidays.end(), boost::bind(print_date, _1, 2022));
}

/**
 * calculate your age in days
 */

#include <chrono>

TEST(datetime, calculate_your_age){
    date dob{1998, 3, 4};
    date today{day_clock::local_day()};
    date_duration d = today - dob;
    std::cout << d << endl;
}

/**
 * uisng boost::posix_time::ptime
 * defines a location-independent time
 */
#include <boost/date_time/posix_time/posix_time.hpp>
using namespace boost::posix_time;
TEST(datetime, times){
    ptime pt{date{2022, 1, 5}, time_duration{12, 0, 0}};
    date d{pt.date()};
    cout << d << endl;
    time_duration td = pt.time_of_day();
    cout << td << endl;
}

/**
 * create a time point with a clock or a string
 */
TEST(datetime, time_point_with_clock_string){
    ptime pt{second_clock::universal_time()};
    cout << pt.date() << endl;
    cout<< pt.time_of_day() << endl;

    pt = from_iso_string("20220103T213440");
    cout << pt.date() << endl;
    cout << pt.time_of_day() << endl;

    pt = second_clock::local_time();
    cout << pt.date() << endl;
    cout << pt.time_of_day() << endl;
}

/**
 * time duration
 */
TEST(datetime, timeduration){
    time_duration td{16, 30, 9};
    cout << td.hours() << endl;
    cout << td.minutes() << endl;
    cout << td.seconds() << endl;
    cout << td.total_seconds() << endl;
}

/**
 * processing time_point
 */

TEST(datetime, time_point){
    ptime pt{date{2022, 2, 5}, time_duration{12, 3, 4}};
    ptime pt2{date{2022, 3, 4}, time_duration{2, 4, 4}};
    time_duration dt = pt2 - pt;
    cout << dt << endl;
    cout << dt.hours() << endl;
    cout << dt.minutes() << endl;
    cout << dt.seconds() << endl;
    cout << dt.total_seconds() << endl;
}

/**
 * processing time durations
 */
TEST(datetime, process_time_duration){
    ptime pt{date{2022, 4, 5}, time_duration{23, 4, 5}};
    time_duration td{2, 4, 6};
    ptime pt2 = pt + td;
    cout << pt2.date() << endl;
    cout << pt2.time_of_day() << endl;
}

/**
 * using boost::posix_time::time_period
 */
TEST(datetime, time_period){
    ptime pt1{date{2022, 2, 3}, time_duration{3, 4, 5}};
    ptime pt2{date{2022, 3, 4}, time_duration{13, 29, 3 }};
    time_period tp{pt1, pt2};
    cout << tp << endl;
    cout.setf(std::ios::boolalpha);
    cout << tp.contains(pt1) << endl;
    cout << tp.contains(pt2) << endl;
}

/**
 * iterator over points in time
 */
TEST(datetime, iterate_over_point_in_time){
    ptime pt{date{2022, 5, 6}, time_duration{14, 34, 6}};
    time_iterator itr{pt, time_duration{4, 30, 0}};
    cout << *itr << endl;
    cout << *++itr << endl;
    cout << *++itr << endl;
}

/**
 * Location-dependent times
 * account for time zones
 */
#include <boost/date_time/local_time/local_time.hpp>
using namespace boost::local_time;
TEST(datetime, local_dependent_time){
    time_zone_ptr tz{new posix_time_zone{"CET+1"}};
    ptime pt{date{2022, 3, 4}, time_duration{3, 5, 6}};
    local_date_time dt{pt, tz};
    cout << dt.utc_time() << endl;
    cout << dt.local_time() << endl;
    cout << dt.zone_name() << endl;
}

/**
 * location-dependent points in time and different time zones
 *
 */
TEST(datetime, diff_tz){
    time_zone_ptr tz{new posix_time_zone {"CET+1"}};
    ptime pt{date{2022, 3, 2}, time_duration{12, 0, 0}};
    local_date_time dt{pt, tz};
    cout << dt.local_time() << endl;

    time_zone_ptr tz2{new posix_time_zone {"EET+2"}};
    cout << dt.local_time_in(tz2).local_time() << endl;
}
/**
 *
 */
TEST(datetime, local_time_period){
   time_zone_ptr tz{new posix_time_zone {"CET+0"}};
   ptime pt1{date{2022, 7, 9}, time_duration{13, 8, 0}};
   local_date_time dt1{pt1, tz};

   ptime pt2{date{2024, 7, 9}, time_duration{3, 0, 7}};
   local_date_time dt2{pt2, tz};

   local_time_period tp{dt2, dt1};
   cout.setf(std::ios::boolalpha);
   cout << tp.contains(dt1) << endl;
   cout << tp.contains(dt2) << endl;
}

/**
 * user Defined format for a date
 */
#include<memory>
#include<locale>
TEST(datetime, formatted_IO){
    date d{2022, 2, 1};
    std::unique_ptr<date_facet> df{new date_facet {"%A, %d %B %Y"}};
    cout.imbue(std::locale{cout.getloc(), df.get()});
    cout << d ;

}
/**
 * change names of weekdays and months
 */
TEST(datetime, change_name){
    std::locale::global(std::locale{"German"});
    std::string months[12]{"Januar", "Februar", "M\xe4rz", "April",
                           "Mai", "Juni", "Juli", "August", "September", "Oktober",
                           "November", "Dezember"};
    std::string weekdays[7]{"Sonntag", "Montag", "Dienstag", "Mittwoch",
                            "Donnerstag", "Freitag", "Samstag"};
    date d{2022, 2, 4};
    std::unique_ptr<date_facet> df{new date_facet {"%A, %d. %B %Y"}};
    df->long_month_names(std::vector<std::string>{std::begin(months), std::end(months)});
    df->long_weekday_names(std::vector<std::string>{weekdays, weekdays+7});

    cout.imbue(std::locale{cout.getloc(), df.get()});
    cout << d;

}
int main(int argc, char **argv){
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


// Copyright Paul A. Bristow 2016
// Copyright John Z. Maddock 2016

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

/*! \brief Graph showing use of Lambert W function to compute current
through a diode-connected transistor with preset series resistance.

\details T. C. Banwell and A. Jayakumar,
Exact analytical solution of current flow through diode with series resistance,
Electron Letters, 36(4):291-2 (2000).
DOI:  doi.org/10.1049/el:20000301

The current through a diode connected NPN bipolar junction transistor (BJT)
type 2N2222 (See https://en.wikipedia.org/wiki/2N2222 and
https://www.fairchildsemi.com/datasheets/PN/PN2222.pdf Datasheet)
was measured, for a voltage between 0.3 to 1 volt, see Fig 2 for a log plot, showing a knee visible at about 0.6 V.

The transistor parameter I sat was estimated to be 25 fA and the ideality factor = 1.0.
The intrinsic emitter resistance re was estimated from the rsat = 0 data to be 0.3 ohm.

The solid curves in Figure 2 are calculated using equation 5 with rsat included with re.

http://www3.imperial.ac.uk/pls/portallive/docs/1/7292572.PDF

*/

#include <boost/math/special_functions/lambert_w.hpp>
using boost::math::lambert_w0;
#include <boost/math/special_functions.hpp>
using boost::math::isfinite;
#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;

#include <iostream>
// using std::cout;
// using std::endl;
#include <exception>
#include <stdexcept>
#include <string>
#include <array>
#include <vector>
#include <utility>
using std::pair;
#include <map>
using std::map;
#include <set>
using std::multiset;
#include <limits>
using std::numeric_limits;
#include <cmath> //

/*!
Compute thermal voltage as a function of temperature,
about 25 mV at room temperature.
https://en.wikipedia.org/wiki/Boltzmann_constant#Role_in_semiconductor_physics:_the_thermal_voltage

\param temperature Temperature (degrees Celsius).
*/
const double v_thermal(double temperature)
{
  BOOST_CONSTEXPR const double boltzmann_k = 1.38e-23; // joules/kelvin.
  BOOST_CONSTEXPR double charge_q = 1.6021766208e-19; // Charge of an electron (columb).
  double temp = +273; // Degrees C to K.
  return boltzmann_k * temp / charge_q;
} // v_thermal

  /*!
  Banwell & Jayakumar, equation 2, page 291.
  */
double i(double isat, double vd, double vt, double nu)
{
  double i = isat * (exp(vd / (nu * vt)) - 1);
  return i;
} //

  /*!
  Banwell & Jayakumar, Equation 4, page 291.
  i current flow = isat
  v voltage source.
  isat reverse saturation current in equation 4.
  (might implement equation 4 instead of simpler equation 5?).
  vd voltage drop = v - i* rs  (equation 1).
  vt  thermal voltage, 0.0257025 = 25 mV.
  nu junction ideality factor (default = unity), also known as the emission coefficient.
  re intrinsic emitter resistance, estimated to be 0.3 ohm from low current.
  rsat reverse saturation current

  \param v Voltage V to compute current I(V).
  \param vt Thermal voltage, for example 0.0257025 = 25 mV, computed from boltzmann_k * temp / charge_q;
  \param rsat Resistance in series with the diode.
  \param re Intrinsic emitter resistance (estimated to be 0.3 ohm from the Rs = 0 data)
  \param isat Reverse saturation current (See equation 2).
  \param nu Ideality factor (default = unity).

  \returns I amp as function of V volt.
  */

//[lambert_w_diode_graph_2
double iv(double v, double vt, double rsat, double re, double isat, double nu = 1.)
{
  // V thermal 0.0257025 = 25 mV
  // was double i = (nu * vt/r) * lambert_w((i0 * r) / (nu * vt)); equ 5.

  rsat = rsat + re;
  double i = nu * vt / rsat;
 // std::cout << "nu * vt / rsat = " << i << std::endl; // 0.000103223

  double x = isat * rsat / (nu * vt);
//  std::cout << "isat * rsat / (nu * vt) = " << x << std::endl;

  double eterm = (v + isat * rsat) / (nu * vt);
 // std::cout << "(v + isat * rsat) / (nu * vt) = " << eterm << std::endl;

  double e = exp(eterm);
//  std::cout << "exp(eterm) = " << e << std::endl;

  double w0 = lambert_w0(x * e);
//  std::cout << "w0 = " << w0 << std::endl;
  return i * w0 - isat;
} // double iv

//] [\lambert_w_diode_graph_2]

std::array<double, 5> rss = { 0., 2.18, 10., 51., 249 };  // series resistance (ohm).
std::array<double, 7> vds = { 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9 };  // Diode voltage.
std::array<double, 7> lni = { -19.65, -15.75, -11.86, -7.97, -4.08, -0.0195, 3.6 }; // ln(current).

int main()
{
  try
  {
    std::cout << "Lambert W diode current example." << std::endl;

//[lambert_w_diode_graph_1
    double nu = 1.0; // Assumed ideal.
    double vt = v_thermal(25); // v thermal, Shockley equation, expect about 25 mV at room temperature.
    double boltzmann_k = 1.38e-23; // joules/kelvin
    double temp = 273 + 25;
    double charge_q = 1.6e-19; // column
    vt = boltzmann_k * temp / charge_q;
    std::cout << "V thermal " << vt << std::endl; // V thermal 0.0257025 = 25 mV
    double rsat = 0.;
    double isat = 25.e-15; //  25 fA;
    std::cout << "Isat = " << isat << std::endl;
    double re = 0.3;  // Estimated from slope of straight section of graph (equation 6).
    double v = 0.9;
    double icalc = iv(v, vt, 249., re, isat);
    std::cout << "voltage = " << v << ", current = " << icalc << ", " << log(icalc) << std::endl; // voltage = 0.9, current = 0.00108485, -6.82631
//] [/lambert_w_diode_graph_1]

    // Plot a few measured data points.
    std::map<const double, double> zero_data;  // Extrapolated from slope of measurements with no external resistor.
    zero_data[0.3] = -19.65;
    zero_data[0.4] = -15.75;
    zero_data[0.5] = -11.86;
    zero_data[0.6] = -7.97;
    zero_data[0.7] = -4.08;
    zero_data[0.8] = -0.0195;
    zero_data[0.9] =  3.9;

    std::map<const double, double>  measured_zero_data; // No external series resistor.
    measured_zero_data[0.3] = -19.65;
    measured_zero_data[0.4] = -15.75;
    measured_zero_data[0.5] = -11.86;
    measured_zero_data[0.6] = -7.97;
    measured_zero_data[0.7] = -4.2;
    measured_zero_data[0.72] = -3.5;
    measured_zero_data[0.74] = -2.8;
    measured_zero_data[0.76] = -2.3;
    measured_zero_data[0.78] = -2.0;
    // Measured from Fig 2 as raw data not available.

    double step = 0.1;
    for (int i = 0; i < vds.size(); i++)
    {
      zero_data[vds[i]] = lni[i];
      std::cout << lni[i] << "  " << vds[i] << std::endl;
    }
    step = 0.01;

    std::map<const double, double> data_2;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 2., re, isat);
      data_2[v] = log(current);
      // std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }
    std::map<const double, double> data_10;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 10., re, isat);
      data_10[v] = log(current);
    //  std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }
    std::map<const double, double> data_51;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 51., re, isat);
      data_51[v] = log(current);
     // std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }
    std::map<const double, double> data_249;
    for (double v = 0.3; v < 1.; v += step)
    {
      double current = iv(v, vt, 249., re, isat);
      data_249[v] = log(current);
      // std::cout << "v " << v << ", current = " << current << " log current = " << log(current) << std::endl;
    }

    svg_2d_plot data_plot;

    data_plot.title("Diode current versus voltage")
      .x_size(400)
      .y_size(300)
      .legend_on(true)
      .legend_lines(true)
      .x_label("voltage (V)")
      .y_label("log(current) (A)")
      //.x_label_on(true)
      //.y_label_on(true)
      //.xy_values_on(false)
      .x_range(0.25, 1.)
      .y_range(-20., +4.)
      .x_major_interval(0.1)
      .y_major_interval(4)
      .x_major_grid_on(true)
      .y_major_grid_on(true)
      //.x_values_on(true)
      //.y_values_on(true)
      .y_values_rotation(horizontal)
      //.plot_window_on(true)
      .x_values_precision(3)
      .y_values_precision(3)
      .coord_precision(4) // Needed to avoid stepping on curves.
      .copyright_holder("Paul A. Bristow")
      .copyright_date("2016")
      //.background_border_color(black);
      ;

    // ₀ = subscript zero.
    data_plot.plot(zero_data, "I₀(V)").fill_color(lightgray).shape(none).size(3).line_on(true).line_width(0.5);
    data_plot.plot(measured_zero_data, "Rs=0 Ω").fill_color(lightgray).shape(square).size(3).line_on(true).line_width(0.5);
    data_plot.plot(data_2, "Rs=2 Ω").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
    data_plot.plot(data_10, "Rs=10 Ω").line_color(purple).shape(none).line_on(true).bezier_on(false).line_width(1);
    data_plot.plot(data_51, "Rs=51 Ω").line_color(green).shape(none).line_on(true).line_width(1);
    data_plot.plot(data_249, "Rs=249 Ω").line_color(red).shape(none).line_on(true).line_width(1);
    data_plot.write("./diode_iv_plot");

    // bezier_on(true);
  }
  catch (std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }

}  // int main()

   /*

   //[lambert_w_output_1
   Output:
   Lambert W diode current example.
   V thermal 0.0257025
   Isat = 2.5e-14
   voltage = 0.9, current = 0.00108485, -6.82631
   -19.65  0.3
   -15.75  0.4
   -11.86  0.5
   -7.97  0.6
   -4.08  0.7
   -0.0195  0.8
   3.6  0.9

   //] [/lambert_w_output_1]
   */


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <boost/math/special_functions/gamma.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "libs/math/test/functor.hpp"

#include "libs/math/test/handle_test_result.hpp"
#include "../table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template<class Real, class T>
void do_test_gamma(const T& data, const char* type_name, const char* test_name) {
    typedef typename T::value_type row_type;
    typedef Real value_type;

    typedef value_type (*pg)(value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    pg funcp = boost::math::tgamma<value_type>;
#else
    pg funcp = boost::math::tgamma;
#endif

    boost::math::tools::test_result<value_type> result;

    std::cout << "Testing " << test_name << " with type " << type_name
              << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

    //
    // test tgamma against data:
    //
    result = boost::math::tools::test_hetero<Real>(data, bind_func<Real>(funcp, 0), extract_result<Real>(1));
    handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::tgamma", test_name);

    //
    // test lgamma against data:
    //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    funcp = boost::math::lgamma<value_type>;
#else
    funcp = boost::math::lgamma;
#endif
    result = boost::math::tools::test_hetero<Real>(data, bind_func<Real>(funcp, 0), extract_result<Real>(2));
    handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::lgamma", test_name);

    std::cout << std::endl;
}

template<class T>
void test_gamma(T, const char* name) {
#include "gamma.ipp"

    do_test_gamma<T>(gamma, name, "random values");

#include "gamma_1_2.ipp"

    do_test_gamma<T>(gamma_1_2, name, "Values near 1 and 2");

#include "gamma_0.ipp"

    do_test_gamma<T>(gamma_0, name, "Values near 0");

#include "gamma_neg.ipp"

    do_test_gamma<T>(gamma_neg, name, "Negative arguments");
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <simd_test.hpp>
#include <boost/simd/function/asec.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/two.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N], a2[N], c[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = i%2 ?N/T(i) : -(N/T(i));
    a2[i] = T(i)/10000;
    b[i] = bs::asec(a1[i]);
    c[i] = bs::asec(a2[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t aa2(&a2[0], &a2[0]+N);
  p_t bb (&b[0], &b[0]+N);
  p_t cc (&c[0], &c[0]+N);
  STF_ULP_EQUAL(bs::asec(aa1), bb, 4);
  STF_ULP_EQUAL(bs::asec(aa2), cc, 2);
}

STF_CASE_TPL("Check asec on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" asec",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::asec;
  using p_t = bs::pack<T>;
  using r_t = decltype(asec(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(asec(bs::Inf<p_t>()), bs::Pio_2<r_t>(), 0);
  STF_ULP_EQUAL(asec(bs::Minf<p_t>()), bs::Pio_2<r_t>(), 0);
  STF_ULP_EQUAL(asec(bs::Nan<p_t>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(asec(bs::Zero<p_t>()), bs::Nan<r_t>(), 0);
#endif
  STF_ULP_EQUAL(asec(-bs::Two<p_t>()), bs::Twopio_3<r_t>(), 0.5);
  STF_ULP_EQUAL(asec(bs::Mone<p_t>()), bs::Pi<r_t>(), 0.5);
  STF_ULP_EQUAL(asec(bs::One<p_t>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(asec(bs::Two<p_t>()), bs::Pio_3<r_t>(), 0.5);
}


#include <mesh_array.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(MESH_ARRAY);

BOOST_AUTO_TEST_CASE(positive_orientation_test)
{
  mesh_array::T4Mesh  M;

  mesh_array::VertexAttribute<float, mesh_array::T4Mesh >  X;
  mesh_array::VertexAttribute<float, mesh_array::T4Mesh >  Y;
  mesh_array::VertexAttribute<float, mesh_array::T4Mesh >  Z;

  M.set_capacity(4u,1u);

  mesh_array::Vertex const vi = M.push_vertex();
  mesh_array::Vertex const vj = M.push_vertex();
  mesh_array::Vertex const vk = M.push_vertex();
  mesh_array::Vertex const vm = M.push_vertex();

  mesh_array::Tetrahedron const t = M.push_tetrahedron(vi,vj,vk,vm);

  X.bind(M);
  Y.bind(M);
  Z.bind(M);

  X(vi) = 0.0f;  Y(vi) = 0.0f;  Z(vi) = 0.0f;
  X(vj) = 1.0f;  Y(vj) = 0.0f;  Z(vj) = 0.0f;
  X(vk) = 0.0f;  Y(vk) = 1.0f;  Z(vk) = 0.0f;
  X(vm) = 0.0f;  Y(vm) = 0.0f;  Z(vm) = 1.0f;

  bool const is_pos = mesh_array::is_positive_orientation( M, X, Y, Z);

  BOOST_CHECK( is_pos );

}

BOOST_AUTO_TEST_CASE(negative_orientation_test)
{
  mesh_array::T4Mesh  M;

  mesh_array::VertexAttribute<float, mesh_array::T4Mesh >  X;
  mesh_array::VertexAttribute<float, mesh_array::T4Mesh >  Y;
  mesh_array::VertexAttribute<float, mesh_array::T4Mesh >  Z;

  M.set_capacity(4u,1u);

  mesh_array::Vertex const vi = M.push_vertex();
  mesh_array::Vertex const vj = M.push_vertex();
  mesh_array::Vertex const vk = M.push_vertex();
  mesh_array::Vertex const vm = M.push_vertex();

  mesh_array::Tetrahedron const t = M.push_tetrahedron(vi,vj,vk,vm);

  X.bind(M);
  Y.bind(M);
  Z.bind(M);

  X(vi) = 0.0f;  Y(vi) = 0.0f;  Z(vi) = 0.0f;
  X(vj) = 1.0f;  Y(vj) = 0.0f;  Z(vj) = 0.0f;
  X(vk) = 0.0f;  Y(vk) = 1.0f;  Z(vk) = 0.0f;
  X(vm) = 0.0f;  Y(vm) = 0.0f;  Z(vm) = -1.0f;

  bool const is_pos = mesh_array::is_positive_orientation( M, X, Y, Z);

  BOOST_CHECK( !is_pos );

}

BOOST_AUTO_TEST_SUITE_END();


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick
// Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <fstream>
#include <iostream>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include "gtest/gtest.h"

#include "bark/commons/distribution/distributions_1d.hpp"
#include "bark/commons/distribution/multivariate_normal.hpp"
#include "bark/commons/params/setter_params.hpp"

// TODO(fortiss): fill our this test
TEST(distribution_test, normal_dist_1d) {
  auto params_ptr = std::make_shared<bark::commons::SetterParams>(true);
  params_ptr->SetReal("Mean", -3.0f);
  params_ptr->SetReal("StdDev", 2.0f);
  params_ptr->SetInt("RandomSeed", 1000.0f);

  auto dist_normal = bark::commons::NormalDistribution1D(params_ptr);

  size_t samples = 30000;
  double mean = 0.0f;
  for (size_t i = 0; i < samples; ++i) {
    mean += dist_normal.Sample()[0];
  }
  mean /= samples;
  EXPECT_NEAR(mean, -3.0f, 0.01);

  double std_dev = 0.0f;
  for (size_t i = 0; i < samples; ++i) {
    auto sample = dist_normal.Sample()[0];
    std_dev += abs(mean - sample) * abs(mean - sample);
  }
  EXPECT_NEAR(sqrt(std_dev / samples), 2.0f, 0.01);
}

// TODO(fortiss): fill our this test
TEST(distribution_test, uniform_dist_1d) {
  auto params_ptr = std::make_shared<bark::commons::SetterParams>(true);
  const double lower_bound = -3.0f;
  const double upper_bound = 10.0f;
  params_ptr->SetReal("LowerBound", -3.0f);
  params_ptr->SetReal("UpperBound", 10.0f);
  params_ptr->SetInt("RandomSeed", 1000.0f);

  auto dist_uniform = bark::commons::UniformDistribution1D(params_ptr);

  size_t samples = 10000;
  double mean = 0.0f;
  int num_buckets = 100;
  double bucket_size = (upper_bound - lower_bound) / num_buckets;
  std::vector<std::vector<double>> sample_container(num_buckets);
  for (size_t i = 0; i < samples; ++i) {
    auto sample = dist_uniform.Sample()[0];
    EXPECT_TRUE(sample <= upper_bound);
    EXPECT_TRUE(sample >= lower_bound);
    sample_container[std::floor((sample - lower_bound) / bucket_size)]
        .push_back(sample);
    mean += sample;
  }

  auto uniform_prob = 1.0f / (upper_bound - lower_bound);
  for (const auto& container : sample_container) {
    auto bucket_prob =
        static_cast<float>(container.size()) / static_cast<float>(samples);
    EXPECT_NEAR(bucket_prob, bucket_size * uniform_prob, 0.01);
  }

  EXPECT_NEAR(mean / samples, (lower_bound + upper_bound) / 2.0f, 0.05);

  EXPECT_NEAR(dist_uniform.Density({2.0f}), uniform_prob, 0.001f);
  EXPECT_NEAR(dist_uniform.Density({3.0f}), uniform_prob, 0.001f);
  EXPECT_NEAR(dist_uniform.Density({12.0f}), 0.0f, 0.001f);
  EXPECT_NEAR(dist_uniform.Density({-4.0f}), 0.0f, 0.001f);

  EXPECT_NEAR(dist_uniform.CDF({0.0f}), 3.0f * uniform_prob, 0.001f);
}

TEST(distribution_test, multivariate_distribution) {
  // First test zero covariances
  auto params_ptr = std::make_shared<bark::commons::SetterParams>(true);
  const double lower_bound = -3.0f;
  const double upper_bound = 10.0f;
  params_ptr->SetListListFloat(
      "Covariance", {{1.0, 0.2, 0.1}, {0.2, 3.0, -0.5}, {0.1, -0.5, 0.125553}});
  params_ptr->SetListFloat("Mean", {1.2f, 12.0f, 0.1234f});
  params_ptr->SetInt("RandomSeed", 1000.0f);

  auto dist_multivariate = bark::commons::MultivariateDistribution(params_ptr);

  size_t samples = 200000;
  std::vector<double> mean(3, 0.0f);
  for (size_t i = 0; i < samples; ++i) {
    auto sample = dist_multivariate.Sample();
    for (int j = 0; j < 3; ++j) {
      mean[j] += sample[j];
    }
  }
  for (int j = 0; j < 3; ++j) {
    mean[j] /= samples;
  }
  auto mean_desired = params_ptr->GetListFloat("Mean", "", {});
  for (int j = 0; j < 3; ++j) {
    EXPECT_NEAR(mean[j], mean_desired[j], 0.01);
  }

  std::vector<std::vector<double>> covar(3, std::vector<double>(3, 0.0f));
  for (size_t i = 0; i < samples; ++i) {
    auto sample = dist_multivariate.Sample();
    for (int j = 0; j < 3; ++j) {
      for (int z = 0; z < 3; ++z) {
        covar[j][z] += (sample[j] - mean[j]) * (sample[z] - mean[z]);
      }
    }
  }
  for (int j = 0; j < 3; ++j) {
    for (int z = 0; z < 3; ++z) {
      covar[j][z] /= samples;
    }
  }
  auto covar_desired = params_ptr->GetListListFloat("Covariance", "", {{}});
  for (int j = 0; j < 3; ++j) {
    for (int z = 0; z < 3; ++z) {
      EXPECT_NEAR(covar[j][z], covar_desired[j][z], 0.01);
    }
  }
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#pragma once
#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace Eigen;

namespace inv_kinematics {

/*!
 * Class containing the algorithmic part of the package.
 */
class AnalyticalIK
{
 public:
  /*!
   * Constructor.
   */
  AnalyticalIK();

  /*!
   * Destructor.
   */
  virtual ~AnalyticalIK();

  /*!
   * Calculate joint angles from the given tool tip position
   * @return the joint angles
   */
  Matrix<float, 6, 1> getJointAngles(MatrixXd H);

  MatrixXd getR03(Matrix<float, 3, 1> theta_);

  /*!
   * Calculate the tranformation for 1 link
   * @return the 4x4 Homogenous transformation matrix
   */
  MatrixXd MatrixTransformation(float theta, float d_, float a_, float alpha_);

 private:

  //! Internal variable to test if Eigen works
  Matrix2d testMatrix;

  //! DH Parameters of a 6DOF Robot
  Matrix<float, 6, 1> theta;
  Matrix<float, 6, 1> d;
  Matrix<float, 6, 1> a;
  Matrix<float, 6, 1> alpha;
};

} /* namespace */


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "lapack_common.h"
#include <Eigen/SVD>

// computes the singular values/vectors a general M-by-N matrix A using divide-and-conquer
EIGEN_LAPACK_FUNC(gesdd,(char *jobz, int *m, int* n, Scalar* a, int *lda, RealScalar *s, Scalar *u, int *ldu, Scalar *vt, int *ldvt, Scalar* /*work*/, int* lwork,
                         EIGEN_LAPACK_ARG_IF_COMPLEX(RealScalar */*rwork*/) int * /*iwork*/, int *info))
{
  // TODO exploit the work buffer
  bool query_size = *lwork==-1;
  int diag_size = (std::min)(*m,*n);

  *info = 0;
        if(*jobz!='A' && *jobz!='S' && *jobz!='O' && *jobz!='N')  *info = -1;
  else  if(*m<0)                                                  *info = -2;
  else  if(*n<0)                                                  *info = -3;
  else  if(*lda<std::max(1,*m))                                   *info = -5;
  else  if(*lda<std::max(1,*m))                                   *info = -8;
  else  if(*ldu <1 || (*jobz=='A' && *ldu <*m)
                   || (*jobz=='O' && *m<*n && *ldu<*m))           *info = -8;
  else  if(*ldvt<1 || (*jobz=='A' && *ldvt<*n)
                   || (*jobz=='S' && *ldvt<diag_size)
                   || (*jobz=='O' && *m>=*n && *ldvt<*n))         *info = -10;

  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GESDD ", &e, 6);
  }

  if(query_size)
  {
    *lwork = 0;
    return 0;
  }

  if(*n==0 || *m==0)
    return 0;

  PlainMatrixType mat(*m,*n);
  mat = matrix(a,*m,*n,*lda);

  int option = *jobz=='A' ? ComputeFullU|ComputeFullV
             : *jobz=='S' ? ComputeThinU|ComputeThinV
             : *jobz=='O' ? ComputeThinU|ComputeThinV
             : 0;

  BDCSVD<PlainMatrixType> svd(mat,option);

  make_vector(s,diag_size) = svd.singularValues().head(diag_size);

  if(*jobz=='A')
  {
    matrix(u,*m,*m,*ldu)   = svd.matrixU();
    matrix(vt,*n,*n,*ldvt) = svd.matrixV().adjoint();
  }
  else if(*jobz=='S')
  {
    matrix(u,*m,diag_size,*ldu)   = svd.matrixU();
    matrix(vt,diag_size,*n,*ldvt) = svd.matrixV().adjoint();
  }
  else if(*jobz=='O' && *m>=*n)
  {
    matrix(a,*m,*n,*lda)   = svd.matrixU();
    matrix(vt,*n,*n,*ldvt) = svd.matrixV().adjoint();
  }
  else if(*jobz=='O')
  {
    matrix(u,*m,*m,*ldu)        = svd.matrixU();
    matrix(a,diag_size,*n,*lda) = svd.matrixV().adjoint();
  }

  return 0;
}

// computes the singular values/vectors a general M-by-N matrix A using two sided jacobi algorithm
EIGEN_LAPACK_FUNC(gesvd,(char *jobu, char *jobv, int *m, int* n, Scalar* a, int *lda, RealScalar *s, Scalar *u, int *ldu, Scalar *vt, int *ldvt, Scalar* /*work*/, int* lwork,
                         EIGEN_LAPACK_ARG_IF_COMPLEX(RealScalar */*rwork*/) int *info))
{
  // TODO exploit the work buffer
  bool query_size = *lwork==-1;
  int diag_size = (std::min)(*m,*n);

  *info = 0;
        if( *jobu!='A' && *jobu!='S' && *jobu!='O' && *jobu!='N') *info = -1;
  else  if((*jobv!='A' && *jobv!='S' && *jobv!='O' && *jobv!='N')
           || (*jobu=='O' && *jobv=='O'))                         *info = -2;
  else  if(*m<0)                                                  *info = -3;
  else  if(*n<0)                                                  *info = -4;
  else  if(*lda<std::max(1,*m))                                   *info = -6;
  else  if(*ldu <1 || ((*jobu=='A' || *jobu=='S') && *ldu<*m))    *info = -9;
  else  if(*ldvt<1 || (*jobv=='A' && *ldvt<*n)
                   || (*jobv=='S' && *ldvt<diag_size))            *info = -11;

  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GESVD ", &e, 6);
  }

  if(query_size)
  {
    *lwork = 0;
    return 0;
  }

  if(*n==0 || *m==0)
    return 0;

  PlainMatrixType mat(*m,*n);
  mat = matrix(a,*m,*n,*lda);

  int option = (*jobu=='A' ? ComputeFullU : *jobu=='S' || *jobu=='O' ? ComputeThinU : 0)
             | (*jobv=='A' ? ComputeFullV : *jobv=='S' || *jobv=='O' ? ComputeThinV : 0);

  JacobiSVD<PlainMatrixType> svd(mat,option);

  make_vector(s,diag_size) = svd.singularValues().head(diag_size);
  {
        if(*jobu=='A') matrix(u,*m,*m,*ldu)           = svd.matrixU();
  else  if(*jobu=='S') matrix(u,*m,diag_size,*ldu)    = svd.matrixU();
  else  if(*jobu=='O') matrix(a,*m,diag_size,*lda)    = svd.matrixU();
  }
  {
        if(*jobv=='A') matrix(vt,*n,*n,*ldvt)         = svd.matrixV().adjoint();
  else  if(*jobv=='S') matrix(vt,diag_size,*n,*ldvt)  = svd.matrixV().adjoint();
  else  if(*jobv=='O') matrix(a,diag_size,*n,*lda)    = svd.matrixV().adjoint();
  }
  return 0;
}


/**
 * \file Chebyshev2Filter.cpp
 */

#include <boost/math/special_functions/asinh.hpp>

#include "Chebyshev2Filter.h"
#include "helpers.h"
#include "IIRFilter.h"

namespace
{
  template<typename DataType>
  void create_chebyshev2_analog_coefficients(int order, DataType ripple, std::vector<std::complex<DataType> >& z, std::vector<std::complex<DataType> >& p, DataType& k)
  {
    z.clear(); // no zeros for this filter type
    p.clear();

    DataType de = static_cast<DataType>(1.0 / std::sqrt(std::pow(10, (0.1 * ripple)) - 1));
    DataType mu = static_cast<DataType>(boost::math::asinh(1.0 / de) / order);

    for(int i = -order+1; i < order; i += 2)
    {
      std::complex<DataType> p1 = -std::complex<DataType>(std::cos(i * boost::math::constants::pi<DataType>() / (2 * order)), std::sin(i * boost::math::constants::pi<DataType>() / (2 * order)));
      std::complex<DataType> p2 = std::complex<DataType>(std::sinh(mu) * p1.real(), std::cosh(mu) * p1.imag());

      p.push_back(std::complex<DataType>(1, 0) / p2);

      if(i == 0)
      {
        continue;
      }

      z.push_back(std::complex<DataType>(0, 1 / std::sin(i * boost::math::constants::pi<DataType>() / (2 * order))));
    }

    std::complex<DataType> f = 1;
    for(int i = 0; i < p.size(); ++i)
    {
      f *= - p[i];
    }
    for(int i = 0; i < z.size(); ++i)
    {
      f /= - z[i];
    }
    k = f.real();
  }

  template<typename DataType>
  void create_default_chebyshev2_coeffs(size_t order, DataType ripple, DataType Wn, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order), ripple, z, p, k);
    DataType warped = 2 * fs * std::tan(boost::math::constants::pi<DataType>() *  Wn / fs);
    zpk_lp2lp(warped, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bp_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bp(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bs_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bs(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev2LowPassCoefficients<DataType>::Chebyshev2LowPassCoefficients(int nb_channels)
  :Parent(1, 1), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2HighPassCoefficients<DataType>::Chebyshev2HighPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev2_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(int i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev2BandPassCoefficients<DataType>::Chebyshev2BandPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev2BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bp_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2BandStopCoefficients<DataType>::Chebyshev2BandStopCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev2BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev2BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bs_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template class Chebyshev2LowPassCoefficients<float>;
  template class Chebyshev2LowPassCoefficients<double>;
  template class Chebyshev2HighPassCoefficients<float>;
  template class Chebyshev2HighPassCoefficients<double>;
  template class Chebyshev2BandPassCoefficients<float>;
  template class Chebyshev2BandPassCoefficients<double>;
  template class Chebyshev2BandStopCoefficients<float>;
  template class Chebyshev2BandStopCoefficients<double>;

  template class IIRFilter<Chebyshev2LowPassCoefficients<float> >;
  template class IIRFilter<Chebyshev2LowPassCoefficients<double> >;
  template class IIRFilter<Chebyshev2HighPassCoefficients<float> >;
  template class IIRFilter<Chebyshev2HighPassCoefficients<double> >;
  template class IIRFilter<Chebyshev2BandPassCoefficients<float> >;
  template class IIRFilter<Chebyshev2BandPassCoefficients<double> >;
  template class IIRFilter<Chebyshev2BandStopCoefficients<float> >;
  template class IIRFilter<Chebyshev2BandStopCoefficients<double> >;

}


#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <yaml-cpp/yaml.h>
#include "NumpySaver.hpp"

constexpr double alpha = 1 / 137.035399; // unitless;
constexpr double hbarc = 197.3269631;    // MeV*fm;
constexpr double c = 299792458;          // m/s

typedef std::complex<double> mattype;
constexpr std::complex<double> I(0, 1);

YAML::Node
get_config(int argc, char const *argv[])
{
    YAML::Node config;
    switch (argc)
    {
    case 2:
        config = YAML::LoadFile(argv[1]);
        break;
    default:
        config = YAML::LoadFile("config.yaml");
        break;
    }
    return config;
}

/**
 * @brief Get the potential at each point in r.
 *
 * This is not yet a good implementation, since I am still learning eigen.
 * There is a lot of optimizing potential.
 *
 * @param r in fm
 * @param R0 in fm
 * @param V0 in MeV
 * @param Z1 number of protons
 * @param Z2 number of protons
 * @return potential in MeV
 */
Eigen::ArrayXd get_potential_middle(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows() - 1; i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 * 2 / (r[i - 1] + r[i]);
    }
    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::ArrayXd get_potential_integral(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows() - 1; i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 * std::log(r[i] / r[i - 1]) / (r[i] - r[i - 1]);
    }
    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::ArrayXd get_middle_points(Eigen::ArrayXd &r)
{
    Eigen::ArrayXd m(r.size() - 1);
    for (int i = 0; i < m.size(); i++)
        m[i] = (r[i] + r[i + 1]) / 2;
    return m;
}

Eigen::ArrayXd get_potential_realistic(Eigen::ArrayXd &r, double V0, double Z1, double Z2, double R0)
{
    Eigen::ArrayXd pot(r.rows() + 1);
    auto mid = get_middle_points(r);

    double a = 0.55;

    for (unsigned int i = 0; i < pot.rows() - 2; i++)
    {
        pot[i] = V0 * (1 + std::cosh(R0 / a)) / (std::cosh(mid[i] / a) + std::cosh(R0 / a));
        if (mid[i] >= R0)
            pot[i] += alpha * hbarc * Z1 * Z2 / (mid[i]);
        else
            pot[i] += alpha * hbarc * Z1 * Z2 / (2 * R0) * (3 - std::pow(mid[i] / R0, 2));
    }
    pot[pot.rows() - 2] = 0;
    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::ArrayXd get_potential_left(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows(); i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 / r[i - 1];
    }

    return pot;
}

Eigen::ArrayXd get_potential_right(Eigen::ArrayXd &r, double V0, double Z1, double Z2)
{
    Eigen::ArrayXd pot(r.rows() + 1);

    pot[0] = alpha * hbarc * Z1 * Z2 / r[0] + V0;

    for (unsigned int i = 1; i < pot.rows() - 1; i++)
    {
        pot[i] = alpha * hbarc * Z1 * Z2 / r[i];
    }

    pot[pot.rows() - 1] = 0;

    return pot;
}

Eigen::SparseMatrix<mattype> get_matrix(Eigen::ArrayXcd &k, Eigen::ArrayXd &r)
{
    int N = r.size() - 1;
    auto matrix = Eigen::SparseMatrix<mattype>(2 * N + 3, 2 * N + 3);

    // 2N+2 equations, 4 coefficients in each row, +1 A constraint -2 last boundary
    matrix.reserve(4 * (2 * N + 2) + 1 - 2);

    matrix.insert(0, 0) = 1;

    for (int i = 0; i < N; i++)
    {
        auto j = 2 * i + 1;
        matrix.insert(j, j - 1) = std::exp(I * k[i] * r[i]);
        matrix.insert(j, j) = std::exp(-I * k[i] * r[i]);
        matrix.insert(j, j + 1) = -std::exp(I * k[i + 1] * r[i]);
        matrix.insert(j, j + 2) = -std::exp(-I * k[i + 1] * r[i]);

        matrix.insert(j + 1, j - 1) = I * k[i] * std::exp(I * k[i] * r[i]);
        matrix.insert(j + 1, j) = -I * k[i] * std::exp(-I * k[i] * r[i]);
        matrix.insert(j + 1, j + 1) = -I * k[i + 1] * std::exp(I * k[i + 1] * r[i]);
        matrix.insert(j + 1, j + 2) = I * k[i + 1] * std::exp(-I * k[i + 1] * r[i]);
    }

    matrix.insert(2 * N + 1, 2 * N) = std::exp(I * k[N] * r[N]);
    matrix.insert(2 * N + 1, 2 * N + 1) = std::exp(-I * k[N] * r[N]);
    matrix.insert(2 * N + 1, 2 * N + 2) = -std::exp(I * k[N + 1] * r[N]);

    matrix.insert(2 * N + 2, 2 * N) = I * k[N] * std::exp(I * k[N] * r[N]);
    matrix.insert(2 * N + 2, 2 * N + 1) = -I * k[N] * std::exp(-I * k[N] * r[N]);
    matrix.insert(2 * N + 2, 2 * N + 2) = -I * k[N + 1] * std::exp(I * k[N + 1] * r[N]);

    return matrix;
}

Eigen::ArrayXd get_pdf(Eigen::VectorXcd &sol, Eigen::ArrayXcd &k, Eigen::ArrayXd &r, Eigen::ArrayXd &r_plot)
{
    Eigen::ArrayXd pdf(r_plot.size());
    int i_k = 0;

    auto this_k = k[0];
    auto this_r = r[0];
    auto this_A = sol[0];
    auto this_B = sol[1];
    for (int i = 0; i < pdf.size(); i++)
    {
        while (r_plot[i] > this_r)
        {
            i_k++;
            this_k = k[i_k];
            this_r = r[i_k];
            this_A = sol[2 * i_k];
            this_B = sol[2 * i_k + 1];
        }
        pdf[i] = std::norm(this_A * std::exp(I * this_k * r_plot[i]) + this_B * std::exp(-I * this_k * r_plot[i]));
    }
    return pdf;
}

Eigen::VectorXcd get_rhs(int N)
{
    Eigen::VectorXcd vec = Eigen::VectorXcd::Zero(2 * N + 3);
    vec[0] = 1;
    return vec;
}

void run_simulation(YAML::Node &simulation_settings, YAML::Node &data)
{
    auto N = simulation_settings["N"].as<int>();
    auto R0 = simulation_settings["R0"].as<double>();
    auto R_last = simulation_settings["R_last"].as<double>();
    auto V0 = simulation_settings["V0"].as<double>();
    auto E_bind_alpha = simulation_settings["E_bind_alpha"].as<double>();
    auto pot_method = simulation_settings["pot_method"].as<std::string>();

    auto name = simulation_settings["Name"].as<std::string>();

    std::cout << "Simulation Setting: " << name << "\n"
              << std::endl;

    for (std::size_t i = 0; i < data.size(); i++)
    {
        auto A_parent = data[i]["A_parent"].as<double>();
        auto Z_parent = data[i]["Z_parent"].as<double>();
        auto E_bind_p = data[i]["E_bind_p"].as<double>();
        auto E_bind_d = data[i]["E_bind_d"].as<double>();
        auto half_life_lit = data[i]["half_life"].as<double>();

        auto symbol = data[i]["Symbol"].as<std::string>();

        auto E_alpha = E_bind_p - E_bind_d - E_bind_alpha;

        double A_daughter = A_parent - 4;
        double Z_daughter = Z_parent - 2;
        double m_alpha = 3727.379;

        auto R0_this = R0 * std::pow(A_daughter, 1. / 3.);

        // lets first construct the r array as it is constant for every nucleus
        Eigen::ArrayXd r;
        if (pot_method == "realistic")
            r = Eigen::ArrayXd::LinSpaced(N + 1, 0, R_last);
        else
            r = Eigen::ArrayXd::LinSpaced(N + 1, R0_this, R_last);

        Eigen::ArrayXcd pot;
        if (pot_method == "middle")
        {
            pot = get_potential_middle(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "left")
        {
            pot = get_potential_left(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "right")
        {
            pot = get_potential_right(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "integrate")
        {
            pot = get_potential_integral(r, V0, Z_daughter, 2); // MeV
        }
        else if (pot_method == "realistic")
        {
            pot = get_potential_realistic(r, V0, Z_daughter, 2, R0_this); // MeV
        }
        else
        {
            std::cerr << "Warning! Unkown potential method: " << pot_method << std::endl;
            pot = get_potential_middle(r, V0, Z_daughter, 2); // MeV
        }

        Eigen::ArrayXcd k = (2 * m_alpha * (E_alpha - pot)).sqrt() / hbarc;

        // Save potential to plot it!
        Eigen::ArrayXd r_pot(r.size() + 1);
        if (pot_method == "realistic")
        {
            r_pot << 0, get_middle_points(r), r(Eigen::last);
        }
        else
        {
            r_pot << 0, r;
        }
        NumpySaver(std::string("build/output/" + name + "-" + symbol + "-pot.npy")) << r_pot << pot.real();

        auto mat = get_matrix(k, r);
        auto b = get_rhs(N);

        Eigen::SparseLU<Eigen::SparseMatrix<mattype>, Eigen::COLAMDOrdering<int>> solver;
        // Compute the ordering permutation vector from the structural pattern of A
        solver.analyzePattern(mat);
        // Compute the numerical factorization
        solver.factorize(mat);
        // Use the factors to solve the linear system
        Eigen::VectorXcd x = solver.solve(b);

        // R and T factor
        double R = std::norm(x(1));
        double T = std::norm(x(Eigen::last)) * std::abs(k(Eigen::last)) / std::abs(k[0]);
        // check if R+T=1
        if (std::abs(R + T - 1) > 1e-3)
        {
            std::cerr << "Warning! R+T not equal to 1; R+T = " << R + T << "\n";
        }

        // plot pdf
        Eigen::ArrayXd r_plot = Eigen::ArrayXd::LinSpaced(1000, 0, R_last);
        auto pdf = get_pdf(x, k, r, r_plot);

        NumpySaver pdf_saver(std::string("build/output/" + name + "-" + symbol + "-pdf.npy")); // << r_plot << pdf;
        pdf_saver.setPrecision(15);
        pdf_saver << r_plot << pdf;

        double v = std::sqrt(2 * E_alpha / m_alpha) * c;              // m/s
        double half_life = std::log(2) * 2 * R0_this / T / v * 1e-15; // s

        std::cerr << "Lifetime of " << symbol << " " << half_life << "s (Difference to literature value of "
                  << (half_life / half_life_lit - 1) * 100 << "%)" << std::endl;
    }
}

int main(int argc, char const *argv[])
{
    auto config = get_config(argc, argv);
    auto data = config["data"];

    for (std::size_t i = 0; i < config["simulation_settings"].size(); i++)
    {
        std::cout << "##################################\n"
                  << "# Running Simulation " << i + 1 << "/" << config["simulation_settings"].size()
                  << "\n##################################\n";
        auto settings = config["simulation_settings"][i];
        run_simulation(settings, data);
        std::cout << "\n\n";
    }

    return 0;
}


//    boost asinh.hpp header file

//  (C) Copyright Eric Ford 2001 & Hubert Holin.
//  (C) Copyright John Maddock 2008.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_ACOSH_HPP
#define BOOST_ACOSH_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/config.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/precision.hpp>

// This is the inverse of the hyperbolic cosine function.

namespace boost
{
namespace math
{
namespace detail
{
template <typename T, typename Policy>
inline T acosh_imp(const T x, const Policy& pol)
{
    BOOST_MATH_STD_USING

    if ((x < 1) || (boost::math::isnan)(x))
    {
        return policies::raise_domain_error<T>(
            "boost::math::acosh<%1%>(%1%)",
            "acosh requires x >= 1, but got x = %1%.", x, pol);
    }
    else if ((x - 1) >= tools::root_epsilon<T>())
    {
        if (x > 1 / tools::root_epsilon<T>())
        {
            // http://functions.wolfram.com/ElementaryFunctions/ArcCosh/06/01/06/01/0001/
            // approximation by laurent series in 1/x at 0+ order from -1 to 0
            return log(x) + constants::ln_two<T>();
        }
        else if (x < 1.5f)
        {
            // This is just a rearrangement of the standard form below
            // devised to minimse loss of precision when x ~ 1:
            T y = x - 1;
            return boost::math::log1p(y + sqrt(y * y + 2 * y), pol);
        }
        else
        {
            // http://functions.wolfram.com/ElementaryFunctions/ArcCosh/02/
            return (log(x + sqrt(x * x - 1)));
        }
    }
    else
    {
        // see
        // http://functions.wolfram.com/ElementaryFunctions/ArcCosh/06/01/04/01/0001/
        T y = x - 1;

        // approximation by taylor series in y at 0 up to order 2
        T result = sqrt(2 * y) * (1 - y / 12 + 3 * y * y / 160);
        return result;
    }
}
} // namespace detail

template <typename T, typename Policy>
inline typename tools::promote_args<T>::type acosh(T x, const Policy&)
{
    typedef typename tools::promote_args<T>::type result_type;
    typedef typename policies::evaluation<result_type, Policy>::type value_type;
    typedef typename policies::normalise<
        Policy, policies::promote_float<false>, policies::promote_double<false>,
        policies::discrete_quantile<>, policies::assert_undefined<>>::type
        forwarding_policy;
    return policies::checked_narrowing_cast<result_type, forwarding_policy>(
        detail::acosh_imp(static_cast<value_type>(x), forwarding_policy()),
        "boost::math::acosh<%1%>(%1%)");
}
template <typename T>
inline typename tools::promote_args<T>::type acosh(T x)
{
    return boost::math::acosh(x, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif /* BOOST_ACOSH_HPP */


//////////////////////////////////////////////////////////////////////////////
///   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_POLYNOMIALS_FUNCTION_SCALAR_LAGUERRE_HPP_INCLUDED
#define NT2_TOOLBOX_POLYNOMIALS_FUNCTION_SCALAR_LAGUERRE_HPP_INCLUDED
#include <boost/math/special_functions.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <nt2/include/functions/oneplus.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::laguerre_, tag::cpu_,
                          (A0)(A1),
                          (integer_<A0>)(arithmetic_<A1>)
                         )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::laguerre_(tag::integer_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      return nt2::laguerre(a0, type(a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::laguerre_, tag::cpu_,
                          (A0)(A1),
                          (integer_<A0>)(real_<A1>)
                         )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::laguerre_(tag::integer_,tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> : meta::strip<A1>{};

    NT2_FUNCTOR_CALL(2)
    {
      A1 p0 = One<A1>();
      if(a0 == 0) return p0;
      A1 p1 = p0-a1;
      A0 c = 1;
      while(c < a0)
      {
        std::swap(p0, p1);
        p1 = laguerre_next(c, a1, p0, p1);
        ++c;
      }
      return p1;
    }
  private:
    template <class T, class T1, class T2>
    static inline T
    laguerre_next(const uint32_t& n, const T& x, const T1 &Ln, const T2& Lnm1)
    {
      const T np1 = oneplus(n);
      return ((n + np1 - x) * Ln - n *Lnm1) / np1;
    }
  };
} }

#endif
// modified by jt the 26/12/2010


/*

Copyright (c) 2013  Ghassen Hamrouni

Abstract:

This module provides some spectral graph algorithms

Author:

Ghassen Hamrouni <ghamrouni.iptech@gmail.com> 23-07-2013

Revision History:

*/

#ifndef R_1_PUBLIC_GRAPH_LAPLACIAN_SPECTRAL_H_
#define R_1_PUBLIC_GRAPH_LAPLACIAN_SPECTRAL_H_

#include <Eigen/Dense>
#include "SpectralGraph.hpp"

namespace R1 {
	class LaplacianSpectralGraph : public SpectralGraph {
	public:
		virtual ~LaplacianSpectralGraph() {}

		LaplacianSpectralGraph(int NVertex) : SpectralGraph(NVertex) {

		}

		//////////////////////////////////////////////////////////////////////////
		//								Laplacian
		//////////////////////////////////////////////////////////////////////////

		double laplacian(int u, int v) {

			double dv = degree(v);

			if (u == v && dv != 0) {
				return 1 - adjacency_matrix(v, v) / dv;
			}

			if (adjacency_matrix(u, v) >= 1.0) {

				double du = degree(u);

				return -adjacency_matrix(u, v) / sqrt(du * dv);
			}

			return 0.0;
		}

		Eigen::MatrixXd getLaplacianMatrix() {
			Eigen::MatrixXd laplacian_matrix(n_vertex, n_vertex);

			for (int i = 0; i < n_vertex; i++)	{
				for (int j = 0; j < n_vertex; j++)	{
					laplacian_matrix(i, j) = laplacian(i, j);
				}
			}

			return laplacian_matrix;
		}

		Eigen::VectorXd laplacianSpectrum() {

			Eigen::MatrixXd L = getLaplacianMatrix();

			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eg1(L);
			return eg1.eigenvalues();
		}

		static bool isospectralLaplacian(LaplacianSpectralGraph g1, LaplacianSpectralGraph g2, double epsilon) {

			Eigen::VectorXd normalizedEg1 = g1.laplacianSpectrum().normalized();
			Eigen::VectorXd normalizedEg2 = g2.laplacianSpectrum().normalized();

			if (normalizedEg1.cols() != normalizedEg2.cols()) {
				return false;
			}

			if (normalizedEg1.rows() != normalizedEg2.rows()) {
				return false;
			}

			for (int i = 0; i < normalizedEg1.rows(); i++)	{
				for (int j = 0; j < normalizedEg1.cols(); j++)	{
					if (abs(normalizedEg1(i, j) - normalizedEg2(i, j)) > epsilon) {
						return false;
					}
				}
			}

			return true;
		}

		static double distance(LaplacianSpectralGraph g1, LaplacianSpectralGraph g2) {
			Eigen::VectorXd normalizedEg1 = g1.laplacianSpectrum().normalized();
			Eigen::VectorXd normalizedEg2 = g2.laplacianSpectrum().normalized();

			double dist = 0.0;

			if (normalizedEg1.cols() != normalizedEg2.cols()) {
				return 0.0;
			}

			if (normalizedEg1.rows() != normalizedEg2.rows()) {
				return 0.0;
			}

			for (int i = 0; i < normalizedEg1.rows(); i++)	{
				for (int j = 0; j < normalizedEg1.cols(); j++)	{
					dist += abs(normalizedEg1(i, j) - normalizedEg2(i, j));
				}
			}

			return dist;
		}
	};
}

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   testVector.cpp
 * @brief  Unit tests for Vector class
 * @author Frank Dellaert
 **/

#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/Vector.h>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST( TestVector, Vector_variants )
{
  Vector a = Vector_(2,10.0,20.0);
  double data[] = {10,20};
  Vector b = Vector_(2,data);
  EXPECT(assert_equal(a, b));
}

/* ************************************************************************* */
TEST( TestVector, copy )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  double data[] = {10,20};
  Vector b(2);
  copy(data,data+2,b.data());
  EXPECT(assert_equal(a, b));
}

/* ************************************************************************* */
TEST( TestVector, zero1 )
{
  Vector v = Vector::Zero(2);
  EXPECT(zero(v));
}

/* ************************************************************************* */
TEST( TestVector, zero2 )
{
  Vector a = zero(2);
  Vector b = Vector::Zero(2);
  EXPECT(a==b);
  EXPECT(assert_equal(a, b));
}

/* ************************************************************************* */
TEST( TestVector, scalar_multiply )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  Vector b(2); b(0) = 1; b(1) = 2;
  EXPECT(assert_equal(a,b*10.0));
}

/* ************************************************************************* */
TEST( TestVector, scalar_divide )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  Vector b(2); b(0) = 1; b(1) = 2;
  EXPECT(assert_equal(b,a/10.0));
}

/* ************************************************************************* */
TEST( TestVector, negate )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  Vector b(2); b(0) = -10; b(1) = -20;
  EXPECT(assert_equal(b, -a));
}

/* ************************************************************************* */
TEST( TestVector, sub )
{
  Vector a(6);
  a(0) = 10; a(1) = 20; a(2) = 3;
  a(3) = 34; a(4) = 11; a(5) = 2;

  Vector result(sub(a,2,5));

  Vector b(3);
  b(0) = 3; b(1) = 34; b(2) =11;

  EXPECT(b==result);
  EXPECT(assert_equal(b, result));
}

/* ************************************************************************* */
TEST( TestVector, subInsert )
{
	Vector big = zero(6),
		   small = ones(3);

	size_t i = 2;
	subInsert(big, small, i);

	Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0);

	EXPECT(assert_equal(expected, big));
}

/* ************************************************************************* */
TEST( TestVector, householder )
{
  Vector x(4);
  x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1;

  Vector expected(4);
  expected(0) = 1.0; expected(1) = -0.333333; expected(2) = -1.66667; expected(3) = -0.333333;

  pair<double, Vector> result = house(x);

  EXPECT(result.first==0.5);
  EXPECT(equal_with_abs_tol(expected,result.second,1e-5));
}

/* ************************************************************************* */
TEST( TestVector, concatVectors)
{
  Vector A(2);
  for(int i = 0; i < 2; i++)
    A(i) = i;
  Vector B(5);
  for(int i = 0; i < 5; i++)
    B(i) = i;

  Vector C(7);
  for(int i = 0; i < 2; i++) C(i) = A(i);
  for(int i = 0; i < 5; i++) C(i+2) = B(i);

  list<Vector> vs;
  vs.push_back(A);
  vs.push_back(B);
  Vector AB1 = concatVectors(vs);
  EXPECT(AB1 == C);

  Vector AB2 = concatVectors(2, &A, &B);
  EXPECT(AB2 == C);
}

/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse )
{
	// column from a matrix
	Vector x(2);
	x(0) = 1.0; x(1) = 2.0;

	// create sigmas
	Vector sigmas(2);
	sigmas(0) = 0.1; sigmas(1) = 0.2;
	Vector weights = reciprocal(emul(sigmas,sigmas));

	// perform solve
	Vector actual; double precision;
	boost::tie(actual, precision) = weightedPseudoinverse(x, weights);

	// construct expected
	Vector expected(2);
	expected(0) = 0.5; expected(1) = 0.25;
	double expPrecision = 200.0;

	// verify
	EXPECT(assert_equal(expected,actual));
	EXPECT(fabs(expPrecision-precision) < 1e-5);
}

/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_constraint )
{
	// column from a matrix
	Vector x(2);
	x(0) = 1.0; x(1) = 2.0;

	// create sigmas
	Vector sigmas(2);
	sigmas(0) = 0.0; sigmas(1) = 0.2;
	Vector weights = reciprocal(emul(sigmas,sigmas));

	// perform solve
	Vector actual; double precision;
	boost::tie(actual, precision) = weightedPseudoinverse(x, weights);

	// construct expected
	Vector expected(2);
	expected(0) = 1.0; expected(1) = 0.0;

	// verify
	EXPECT(assert_equal(expected,actual));
	EXPECT(isinf(precision));
}

/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_nan )
{
	Vector a = Vector_(4, 1., 0., 0., 0.);
	Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.);
	Vector weights = reciprocal(emul(sigmas,sigmas));
	Vector pseudo; double precision;
	boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);

	Vector expected = Vector_(4, 1., 0., 0.,0.);
	EXPECT(assert_equal(expected, pseudo));
	DOUBLES_EQUAL(100, precision, 1e-5);
}

/* ************************************************************************* */
TEST( TestVector, ediv )
{
  Vector a = Vector_(3,10.,20.,30.);
  Vector b = Vector_(3,2.0,5.0,6.0);
  Vector actual(ediv(a,b));

  Vector c = Vector_(3,5.0,4.0,5.0);
  EXPECT(assert_equal(c,actual));
}

/* ************************************************************************* */
TEST( TestVector, dot )
{
  Vector a = Vector_(3,10.,20.,30.);
  Vector b = Vector_(3,2.0,5.0,6.0);
  DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9);
}

/* ************************************************************************* */
TEST( TestVector, axpy )
{
  Vector x = Vector_(3,10.,20.,30.);
  Vector y0 = Vector_(3,2.0,5.0,6.0);
  Vector y1 = y0, y2 = y0;
  axpy(0.1,x,y1);
  axpy(0.1,x,y2.head(3));
  Vector expected = Vector_(3,3.0,7.0,9.0);
  EXPECT(assert_equal(expected,y1));
  EXPECT(assert_equal(expected,Vector(y2)));
}

/* ************************************************************************* */
TEST( TestVector, equals )
{
	Vector v1 = Vector_(1, 0.0/std::numeric_limits<double>::quiet_NaN()); //testing nan
	Vector v2 = Vector_(1, 1.0);
	double tol = 1.;
	EXPECT(!equal_with_abs_tol(v1, v2, tol));
}

/* ************************************************************************* */
TEST( TestVector, greater_than )
{
	Vector v1 = Vector_(3, 1.0, 2.0, 3.0),
		   v2 = zero(3);
	EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
	EXPECT(greaterThanOrEqual(v1, v2)); // test equals
}

/* ************************************************************************* */
TEST( TestVector, reciprocal )
{
	Vector v = Vector_(3, 1.0, 2.0, 4.0);
  EXPECT(assert_equal(Vector_(3, 1.0, 0.5, 0.25),reciprocal(v)));
}

/* ************************************************************************* */
TEST( TestVector, linear_dependent )
{
	Vector v1 = Vector_(3, 1.0, 2.0, 3.0);
	Vector v2 = Vector_(3, -2.0, -4.0, -6.0);
	EXPECT(linear_dependent(v1, v2));
}

/* ************************************************************************* */
TEST( TestVector, linear_dependent2 )
{
	Vector v1 = Vector_(3, 0.0, 2.0, 0.0);
	Vector v2 = Vector_(3, 0.0, -4.0, 0.0);
	EXPECT(linear_dependent(v1, v2));
}

/* ************************************************************************* */
TEST( TestVector, linear_dependent3 )
{
	Vector v1 = Vector_(3, 0.0, 2.0, 0.0);
	Vector v2 = Vector_(3, 0.1, -4.1, 0.0);
	EXPECT(!linear_dependent(v1, v2));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


#ifndef LOGISTIC_H
#define LOGISTIC_H
#include <iostream>
#include <stdlib.h>
#include <cfloat>
#include <cmath>
#include <algorithm>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include "../utils/c_utils.hpp"

using namespace Eigen;
using namespace std;

class LogisticRegression
{
 public:
 	LogisticRegression();
	void init(bool _normalization = false, bool _standardization = false,bool _with_bias=false);
	void init(MatrixXd &_X,VectorXd &_Y,double lambda=1.0, bool _normalization = false, bool _standardization = true,bool _with_bias=true);
 	double logPosterior();
 	void setWeights(VectorXd &_W);
    void setData(MatrixXd &_X,VectorXd &_Y);
 	VectorXd getWeights();
 	double getBias();
 	void setBias(double bias);
 	double getGradientBias();
 	//MatrixXd computeHessian(MatrixXd &_X, VectorXd &_Y, VectorXd &_W);
 	RowVectorXd featureMean,featureStd,featureMin,featureMax;
 	bool initialized = false;
 	virtual double train(int n_iter,double alpha,double tol) = 0 ;
 	virtual VectorXd predict(MatrixXd &_X_test, bool prob=false, bool data_processing = true) = 0;

 protected:
 	VectorXd weights;
 	MatrixXd *X_train;
 	VectorXd *Y_train;
	VectorXd eta,phi;
	VectorXd momemtum;
 	int rows,dim;
 	double lambda,bias,grad_bias;
 	bool normalization, standardization, with_bias;
 	VectorXd sigmoid(VectorXd &_eta);
 	//VectorXd logSigmoid(VectorXd &_eta);
 	double logPrior();
 	double logLikelihood();
 	MatrixXd Hessian;
 	C_utils tools;
	virtual void preCompute() = 0;
 	virtual VectorXd computeGradient() = 0;
    //MVNGaussian posterior;
};

#endif


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * 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.
 *
 * ****************************************************************************
 * Simple Least-Squares Solver.
 * ****************************************************************************
 */

#ifndef VISIONCORE_MATH_LEAST_SQUARES_HPP
#define VISIONCORE_MATH_LEAST_SQUARES_HPP

#include <VisionCore/Platform.hpp>
#include <Eigen/Cholesky>

namespace vc
{

namespace math
{

// TODO eigenify

template<typename T, int dim>
class LSQ
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef T Scalar;
    static constexpr int Dimension = dim;
    typedef Eigen::Matrix<Scalar,Dimension,Dimension> MatrixType;
    typedef Eigen::Matrix<Scalar,Dimension,1> VectorType;
    typedef Eigen::AutoDiffScalar<Eigen::Matrix<Scalar,Dimension,1>> JetType;

    EIGEN_DEVICE_FUNC inline LSQ(const std::size_t nc = 0)
    {
        reset();
        NumConstraints = nc;
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        A.setZero();
        B.setZero();
        Error = Scalar(0.0);
        NumConstraints = 0;
    }

    EIGEN_DEVICE_FUNC inline void update(const VectorType& J, const Scalar& res, const Scalar& weight = Scalar(1.0))
    {
        A.noalias() += J * J.transpose() * weight;
        B.noalias() -= J * (res * weight);
        Error += res * res * weight;
        NumConstraints += 1;
    }

    EIGEN_DEVICE_FUNC inline void update(const JetType& j, const Scalar& weight = Scalar(1.0))
    {
        update(j.deriviatives(), j.value(), weight);
    }

    EIGEN_DEVICE_FUNC inline void finishAndDivide()
    {
        A /= (Scalar)NumConstraints;
        B /= (Scalar)NumConstraints;
        Error /= (Scalar)NumConstraints;
    }

    EIGEN_DEVICE_FUNC inline VectorType solve()
    {
        return A.ldlt().solve(-B);
    }

    MatrixType A;
    VectorType B;
    Scalar Error;
    std::size_t NumConstraints;
};

}

}

#endif // VISIONCORE_MATH_LEAST_SQUARES_HPP