/*
 * Copyright (c) 1990 Michael E. Hohmeyer,
 *       hohmeyer@icemcfd.com
 * Permission is granted to modify and re-distribute this code in any manner
 * as long as this notice is preserved.  All standard disclaimers apply.
 *
 * R. Seidel's algorithm for solving LPs (linear programs.)
 */

/*
 * Copyright (c) 2021 Zhepei Wang,
 *       wangzhepei@live.com
 * 1. Bug fix in "move_to_front" function that "prev[m]" is illegally accessed
 *    while "prev" originally has only m ints. It is fixed by allocating a
 *    "prev" with m + 1 ints.
 * 2. Add Eigen interface.
 * 3. Resursive template.
 * Permission is granted to modify and re-distribute this code in any manner
 * as long as this notice is preserved.  All standard disclaimers apply.
 *
 * Ref: Seidel, R. (1991), "Small-dimensional linear programming and convex
 *      hulls made easy", Discrete & Computational Geometry 6 (1): 423–434,
 *      doi:10.1007/BF02574699
 */

#ifndef SDLP_HPP
#define SDLP_HPP

#include <Eigen/Eigen>
#include <cmath>
#include <random>

namespace sdlp
{
    constexpr double eps = 1.0e-12;

    enum
    {
        /* minimum attained */
        MINIMUM = 0,
        /* no feasible region */
        INFEASIBLE,
        /* unbounded solution */
        UNBOUNDED,
        /* only a vertex in the solution set */
        AMBIGUOUS,
    };

    inline double dot2(const double a[2],
                       const double b[2])
    {
        return a[0] * b[0] + a[1] * b[1];
    }

    inline double cross2(const double a[2],
                         const double b[2])
    {
        return a[0] * b[1] - a[1] * b[0];
    }

    inline bool unit2(const double a[2],
                      double b[2])
    {
        const double mag = std::sqrt(a[0] * a[0] +
                                     a[1] * a[1]);
        if (mag < 2.0 * eps)
        {
            return true;
        }
        b[0] = a[0] / mag;
        b[1] = a[1] / mag;
        return false;
    }

    /* unitize a d + 1 dimensional point */
    template <int d>
    inline bool unit(double *a)
    {
        double mag = 0.0;
        for (int i = 0; i <= d; i++)
        {
            mag += a[i] * a[i];
        }
        if (mag < (d + 1) * eps * eps)
        {
            return true;
        }
        mag = 1.0 / std::sqrt(mag);
        for (int i = 0; i <= d; i++)
        {
            a[i] *= mag;
        }
        return false;
    }

    /* optimize the unconstrained objective */
    template <int d>
    inline int lp_no_con(const double *n_vec,
                         const double *d_vec,
                         double *opt)
    {
        double n_dot_d = 0.0;
        double d_dot_d = 0.0;
        for (int i = 0; i <= d; i++)
        {
            n_dot_d += n_vec[i] * d_vec[i];
            d_dot_d += d_vec[i] * d_vec[i];
        }
        if (d_dot_d < eps * eps)
        {
            n_dot_d = 0.0;
            d_dot_d = 1.0;
        }
        for (int i = 0; i <= d; i++)
        {
            opt[i] = -n_vec[i] +
                     d_vec[i] * n_dot_d / d_dot_d;
        }
        /* normalize the optimal point */
        if (unit<d>(opt))
        {
            opt[d] = 1.0;
            return AMBIGUOUS;
        }
        else
        {
            return MINIMUM;
        }
    }

    /* returns the plane index that is in i's place */
    inline int move_to_front(const int i,
                             int *next,
                             int *prev)
    {
        if (i == 0 || i == next[0])
        {
            return i;
        }
        const int previ = prev[i];
        /* remove i from it's current position */
        next[prev[i]] = next[i];
        prev[next[i]] = prev[i];
        /* put i at the front */
        next[i] = next[0];
        prev[i] = 0;
        prev[next[i]] = i;
        next[0] = i;
        return previ;
    }

    inline void lp_min_lin_rat(const bool degen,
                               const double cw_vec[2],
                               const double ccw_vec[2],
                               const double n_vec[2],
                               const double d_vec[2],
                               double opt[2])
    {
        /* linear rational function case */
        const double d_cw = dot2(cw_vec, d_vec);
        const double d_ccw = dot2(ccw_vec, d_vec);
        const double n_cw = dot2(cw_vec, n_vec);
        const double n_ccw = dot2(ccw_vec, n_vec);
        if (degen)
        {
            /* if degenerate simply compare values */
            if (n_cw / d_cw < n_ccw / d_ccw)
            {
                opt[0] = cw_vec[0];
                opt[1] = cw_vec[1];
            }
            else
            {
                opt[0] = ccw_vec[0];
                opt[1] = ccw_vec[1];
            }
            /* check CW/CCW bounds are not near a poles */
        }
        else if (std::fabs(d_cw) > 2.0 * eps &&
                 std::fabs(d_ccw) > 2.0 * eps)
        {
            /* the valid region does not contain a poles */
            if (d_cw * d_ccw > 0.0)
            {
                /* find which end has the minimum value */
                if (n_cw / d_cw < n_ccw / d_ccw)
                {
                    opt[0] = cw_vec[0];
                    opt[1] = cw_vec[1];
                }
                else
                {
                    opt[0] = ccw_vec[0];
                    opt[1] = ccw_vec[1];
                }
            }
            else
            {
                /* the valid region does contain a poles */
                if (d_cw > 0.0)
                {
                    opt[0] = -d_vec[1];
                    opt[1] = d_vec[0];
                }
                else
                {
                    opt[0] = d_vec[1];
                    opt[1] = -d_vec[0];
                }
            }
        }
        else if (std::fabs(d_cw) > 2.0 * eps)
        {
            /* CCW bound is near a pole */
            if (n_ccw * d_cw > 0.0)
            {
                /* CCW bound is a positive pole */
                opt[0] = cw_vec[0];
                opt[1] = cw_vec[1];
            }
            else
            {
                /* CCW bound is a negative pole */
                opt[0] = ccw_vec[0];
                opt[1] = ccw_vec[1];
            }
        }
        else if (std::fabs(d_ccw) > 2.0 * eps)
        {
            /* CW bound is near a pole */
            if (n_cw * d_ccw > 2.0 * eps)
            {
                /* CW bound is at a positive pole */
                opt[0] = ccw_vec[0];
                opt[1] = ccw_vec[1];
            }
            else
            {
                /* CW bound is at a negative pole */
                opt[0] = cw_vec[0];
                opt[1] = cw_vec[1];
            }
        }
        else
        {
            /* both bounds are near poles */
            if (cross2(d_vec, n_vec) > 0.0)
            {
                opt[0] = cw_vec[0];
                opt[1] = cw_vec[1];
            }
            else
            {
                opt[0] = ccw_vec[0];
                opt[1] = ccw_vec[1];
            }
        }
    }

    inline int wedge(const double (*halves)[2],
                     const int m,
                     int *next,
                     int *prev,
                     double cw_vec[2],
                     double ccw_vec[2],
                     bool *degen)
    {
        int i;
        double d_cw, d_ccw;
        bool offensive;

        *degen = false;
        for (i = 0; i != m; i = next[i])
        {
            if (!unit2(halves[i], ccw_vec))
            {
                /* CW */
                cw_vec[0] = ccw_vec[1];
                cw_vec[1] = -ccw_vec[0];
                /* CCW */
                ccw_vec[0] = -cw_vec[0];
                ccw_vec[1] = -cw_vec[1];
                break;
            }
        }
        if (i == m)
        {
            return UNBOUNDED;
        }
        i = 0;
        while (i != m)
        {
            offensive = false;
            d_cw = dot2(cw_vec, halves[i]);
            d_ccw = dot2(ccw_vec, halves[i]);
            if (d_ccw >= 2.0 * eps)
            {
                if (d_cw <= -2.0 * eps)
                {
                    cw_vec[0] = halves[i][1];
                    cw_vec[1] = -halves[i][0];
                    unit2(cw_vec, cw_vec);
                    offensive = true;
                }
            }
            else if (d_cw >= 2.0 * eps)
            {
                if (d_ccw <= -2.0 * eps)
                {
                    ccw_vec[0] = -halves[i][1];
                    ccw_vec[1] = halves[i][0];
                    unit2(ccw_vec, ccw_vec);
                    offensive = true;
                }
            }
            else if (d_ccw <= -2.0 * eps &&
                     d_cw <= -2.0 * eps)
            {
                return INFEASIBLE;
            }
            else if (d_cw <= -2.0 * eps ||
                     d_ccw <= -2.0 * eps ||
                     cross2(cw_vec, halves[i]) < 0.0)
            {
                /* degenerate */
                if (d_cw <= -2.0 * eps)
                {
                    unit2(ccw_vec, cw_vec);
                }
                else if (d_ccw <= -2.0 * eps)
                {
                    unit2(cw_vec, ccw_vec);
                }
                *degen = true;
                offensive = true;
            }
            /* place this offensive plane in second place */
            if (offensive)
            {
                i = move_to_front(i, next, prev);
            }
            i = next[i];
            if (*degen)
            {
                break;
            }
        }
        if (*degen)
        {
            while (i != m)
            {
                d_cw = dot2(cw_vec, halves[i]);
                d_ccw = dot2(ccw_vec, halves[i]);
                if (d_cw < -2.0 * eps)
                {
                    if (d_ccw < -2.0 * eps)
                    {
                        return INFEASIBLE;
                    }
                    else
                    {
                        cw_vec[0] = ccw_vec[0];
                        cw_vec[1] = ccw_vec[1];
                    }
                }
                else if (d_ccw < -2.0 * eps)
                {
                    ccw_vec[0] = cw_vec[0];
                    ccw_vec[1] = cw_vec[1];
                }
                i = next[i];
            }
        }
        return MINIMUM;
    }

    /* return the minimum on the projective line */
    inline int lp_base_case(const double (*halves)[2], /* halves --- half lines */
                            const int m,               /* m      --- terminal marker */
                            const double n_vec[2],     /* n_vec  --- numerator funciton */
                            const double d_vec[2],     /* d_vec  --- denominator function */
                            double opt[2],             /* opt    --- optimum  */
                            int *next,                 /* next, prev  --- double linked list of indices */
                            int *prev)
    {
        double cw_vec[2], ccw_vec[2];
        bool degen;
        int status;

        /* find the feasible region of the line */
        status = wedge(halves, m, next, prev, cw_vec, ccw_vec, &degen);

        if (status == INFEASIBLE)
        {
            return status;
        }
        /* no non-trivial constraints one the plane: return the unconstrained optimum */
        if (status == UNBOUNDED)
        {
            return lp_no_con<1>(n_vec, d_vec, opt);
        }

        if (std::fabs(cross2(n_vec, d_vec)) < 2.0 * eps * eps)
        {
            if (dot2(n_vec, n_vec) < 2.0 * eps * eps ||
                dot2(d_vec, d_vec) > 2.0 * eps * eps)
            {
                /* numerator is zero or numerator and denominator are linearly dependent */
                opt[0] = cw_vec[0];
                opt[1] = cw_vec[1];
                status = AMBIGUOUS;
            }
            else
            {
                /* numerator is non-zero and denominator is zero minimize linear functional on circle */
                if (!degen &&
                    cross2(cw_vec, n_vec) <= 0.0 &&
                    cross2(n_vec, ccw_vec) <= 0.0)
                {
                    /* optimum is in interior of feasible region */
                    opt[0] = -n_vec[0];
                    opt[1] = -n_vec[1];
                }
                else if (dot2(n_vec, cw_vec) > dot2(n_vec, ccw_vec))
                {
                    /* optimum is at CCW boundary */
                    opt[0] = ccw_vec[0];
                    opt[1] = ccw_vec[1];
                }
                else
                {
                    /* optimum is at CW boundary */
                    opt[0] = cw_vec[0];
                    opt[1] = cw_vec[1];
                }
                status = MINIMUM;
            }
        }
        else
        {
            /* niether numerator nor denominator is zero */
            lp_min_lin_rat(degen, cw_vec, ccw_vec, n_vec, d_vec, opt);
            status = MINIMUM;
        }
        return status;
    }

    /* find the largest coefficient in a plane */
    template <int d>
    inline void findimax(const double *pln,
                         int *imax)
    {
        *imax = 0;
        double rmax = std::fabs(pln[0]);
        for (int i = 1; i <= d; i++)
        {
            const double ab = std::fabs(pln[i]);
            if (ab > rmax)
            {
                *imax = i;
                rmax = ab;
            }
        }
    }

    template <int d>
    inline void vector_up(const double *equation,
                          const int ivar,
                          const double *low_vector,
                          double *vector)
    {
        vector[ivar] = 0.0;
        for (int i = 0; i <= d; i++)
        {
            if (i != ivar)
            {
                const int j = i < ivar ? i : i - 1;
                vector[i] = low_vector[j];
                vector[ivar] -= equation[i] * low_vector[j];
            }
        }
        vector[ivar] /= equation[ivar];
    }

    template <int d>
    inline void vector_down(const double *elim_eqn,
                            const int ivar,
                            const double *old_vec,
                            double *new_vec)
    {
        double ve = 0.0;
        double ee = 0.0;
        for (int i = 0; i <= d; i++)
        {
            ve += old_vec[i] * elim_eqn[i];
            ee += elim_eqn[i] * elim_eqn[i];
        }
        const double fac = ve / ee;
        for (int i = 0; i <= d; i++)
        {
            if (i != ivar)
            {
                new_vec[i < ivar ? i : i - 1] =
                    old_vec[i] - elim_eqn[i] * fac;
            }
        }
    }

    template <int d>
    inline void plane_down(const double *elim_eqn,
                           const int ivar,
                           const double *old_plane,
                           double *new_plane)
    {
        const double crit = old_plane[ivar] / elim_eqn[ivar];
        for (int i = 0; i <= d; i++)
        {
            if (i != ivar)
            {
                new_plane[i < ivar ? i : i - 1] =
                    old_plane[i] - elim_eqn[i] * crit;
            }
        }
    }

    template <int d>
    inline int linfracprog(const double *halves, /* halves  --- half spaces */
                           const int max_size,   /* max_size --- size of halves array */
                           const int m,          /* m       --- terminal marker */
                           const double *n_vec,  /* n_vec   --- numerator vector */
                           const double *d_vec,  /* d_vec   --- denominator vector */
                           double *opt,          /* opt     --- optimum */
                           double *work,         /* work    --- work space (see below) */
                           int *next,            /* next    --- array of indices into halves */
                           int *prev)            /* prev    --- array of indices into halves */
    /*
    **
    ** half-spaces are in the form
    ** halves[i][0]*x[0] + halves[i][1]*x[1] +
    ** ... + halves[i][d-1]*x[d-1] + halves[i][d]*x[d] >= 0
    **
    ** coefficients should be normalized
    ** half-spaces should be in random order
    ** the order of the half spaces is 0, next[0] next[next[0]] ...
    ** and prev[next[i]] = i
    **
    ** halves: (max_size)x(d+1)
    **
    ** the optimum has been computed for the half spaces
    ** 0 , next[0], next[next[0]] , ... , prev[0]
    ** the next plane that needs to be tested is 0
    **
    ** m is the index of the first plane that is NOT on the list
    ** i.e. m is the terminal marker for the linked list.
    **
    ** the objective function is dot(x,nvec)/dot(x,dvec)
    ** if you want the program to solve standard d dimensional linear programming
    ** problems then n_vec = ( x0, x1, x2, ..., xd-1, 0)
    ** and           d_vec = (  0,  0,  0, ...,    0, 1)
    ** and halves[0] = (0, 0, ... , 1)
    **
    ** work points to (max_size+3)*(d+2)*(d-1)/2 double space
    */
    {
        int status, imax;
        double *new_opt, *new_n_vec, *new_d_vec, *new_halves, *new_work;
        const double *plane_i;

        double val = 0.0;
        for (int j = 0; j <= d; j++)
        {
            val += d_vec[j] * d_vec[j];
        }
        const bool d_vec_zero = (val < (d + 1) * eps * eps);

        /* find the unconstrained minimum */
        status = lp_no_con<d>(n_vec, d_vec, opt);
        if (m <= 0)
        {
            return status;
        }

        /* allocate memory for next level of recursion */
        new_opt = work;
        new_n_vec = new_opt + d;
        new_d_vec = new_n_vec + d;
        new_halves = new_d_vec + d;
        new_work = new_halves + max_size * d;
        for (int i = 0; i != m; i = next[i])
        {
            /* if the optimum is not in half space i then project the problem onto that plane */
            plane_i = halves + i * (d + 1);
            /* determine if the optimum is on the correct side of plane_i */
            val = 0.0;
            for (int j = 0; j <= d; j++)
            {
                val += opt[j] * plane_i[j];
            }
            if (val < -(d + 1) * eps)
            {
                /* find the largest of the coefficients to eliminate */
                findimax<d>(plane_i, &imax);
                /* eliminate that variable */
                if (i != 0)
                {
                    const double fac = 1.0 / plane_i[imax];
                    for (int j = 0; j != i; j = next[j])
                    {
                        const double *old_plane = halves + j * (d + 1);
                        const double crit = old_plane[imax] * fac;
                        double *new_plane = new_halves + j * d;
                        for (int k = 0; k <= d; k++)
                        {
                            const int l = k < imax ? k : k - 1;
                            new_plane[l] = k != imax ? old_plane[k] - plane_i[k] * crit : new_plane[l];
                        }
                    }
                }
                /* project the objective function to lower dimension */
                if (d_vec_zero)
                {
                    vector_down<d>(plane_i, imax, n_vec, new_n_vec);
                    for (int j = 0; j < d; j++)
                    {
                        new_d_vec[j] = 0.0;
                    }
                }
                else
                {
                    plane_down<d>(plane_i, imax, n_vec, new_n_vec);
                    plane_down<d>(plane_i, imax, d_vec, new_d_vec);
                }
                /* solve sub problem */
                status = linfracprog<d - 1>(new_halves, max_size, i, new_n_vec,
                                            new_d_vec, new_opt, new_work, next, prev);
                /* back substitution */
                if (status != INFEASIBLE)
                {
                    vector_up<d>(plane_i, imax, new_opt, opt);

                    /* inline code for unit */
                    double mag = 0.0;
                    for (int j = 0; j <= d; j++)
                    {
                        mag += opt[j] * opt[j];
                    }
                    mag = 1.0 / sqrt(mag);
                    for (int j = 0; j <= d; j++)
                    {
                        opt[j] *= mag;
                    }
                }
                else
                {
                    return status;
                }
                /* place this offensive plane in second place */
                i = move_to_front(i, next, prev);
            }
        }
        return status;
    }

    template <>
    inline int linfracprog<1>(const double *halves,
                              const int max_size,
                              const int m,
                              const double *n_vec,
                              const double *d_vec,
                              double *opt,
                              double *work,
                              int *next,
                              int *prev)
    {
        if (m > 0)
        {
            return lp_base_case((const double(*)[2])halves, m,
                                n_vec, d_vec, opt, next, prev);
        }
        else
        {
            return lp_no_con<1>(n_vec, d_vec, opt);
        }
    }

    inline void rand_permutation(const int n,
                                 int *p)
    {
        typedef std::uniform_int_distribution<int> rand_int;
        typedef rand_int::param_type rand_range;
        static std::mt19937_64 gen;
        static rand_int rdi(0, 1);
        int j, k;
        for (int i = 0; i < n; i++)
        {
            p[i] = i;
        }
        for (int i = 0; i < n; i++)
        {
            rdi.param(rand_range(0, n - i - 1));
            j = rdi(gen) + i;
            k = p[j];
            p[j] = p[i];
            p[i] = k;
        }
    }

    template <int d>
    inline double linprog(const Eigen::Matrix<double, d, 1> &c,
                          const Eigen::Matrix<double, -1, d> &A,
                          const Eigen::Matrix<double, -1, 1> &b,
                          Eigen::Matrix<double, d, 1> &x)
    /*
    **  min cTx, s.t. Ax<=b
    **  dim(x) << dim(b)
    */
    {
        int m = b.size() + 1;
        x.setZero();
        if (m <= 1)
        {
            return c.cwiseAbs().maxCoeff() > 0.0 ? -INFINITY : 0.0;
        }

        Eigen::VectorXi perm(m - 1);
        Eigen::VectorXi next(m);
        /* original allocated size is m, here changed to m + 1 for legal tail accessing */
        Eigen::VectorXi prev(m + 1);
        Eigen::Matrix<double, d + 1, 1> n_vec;
        Eigen::Matrix<double, d + 1, 1> d_vec;
        Eigen::Matrix<double, d + 1, 1> opt;
        Eigen::Matrix<double, d + 1, -1, Eigen::ColMajor> halves(d + 1, m);
        Eigen::VectorXd work((m + 3) * (d + 2) * (d - 1) / 2);

        halves.col(0).setZero();
        halves(d, 0) = 1.0;
        halves.topRightCorner(d, m - 1) = -A.transpose();
        halves.bottomRightCorner(1, m - 1) = b.transpose();
        /* normalize all halves as required in linfracprog */
        halves.colwise().normalize();
        n_vec.head(d) = c;
        n_vec(d) = 0.0;
        d_vec.setZero();
        d_vec(d) = 1.0;

        /* randomize the input planes */
        rand_permutation(m - 1, perm.data());
        /* previous to 0 is actually never used */
        prev(0) = 0;
        /* link the zero position in at the beginning */
        next(0) = perm(0) + 1;
        prev(perm(0) + 1) = 0;
        /* link the other planes */
        for (int i = 0; i < m - 2; i++)
        {
            next(perm(i) + 1) = perm(i + 1) + 1;
            prev(perm(i + 1) + 1) = perm(i) + 1;
        }
        /* flag the last plane */
        next(perm(m - 2) + 1) = m;

        int status = sdlp::linfracprog<d>(halves.data(), m, m,
                                          n_vec.data(), d_vec.data(),
                                          opt.data(), work.data(),
                                          next.data(), prev.data());

        /* handle states for linprog whose definitions differ from linfracprog */
        double minimum = INFINITY;
        if (status != sdlp::INFEASIBLE)
        {
            if (opt(d) != 0.0 && status != sdlp::UNBOUNDED)
            {
                x = opt.head(d) / opt(d);
                minimum = c.dot(x);
            }

            if (opt(d) == 0.0 || status == sdlp::UNBOUNDED)
            {
                x = opt.head(d);
                minimum = -INFINITY;
            }
        }

        return minimum;
    }

} // namespace sdlp

#endif


#include <Eigen/Dense>
#include "simple_layer.h"
#include "simple_activation.h"
#include "simple_loss.h"

namespace MyDL{

    using namespace Eigen;

    // -------------------------------------------------
    //          MulLayer
    // -------------------------------------------------
    MatrixXd MulLayer::forward(MatrixXd& x, MatrixXd& y){
        _x = x;
        _y = y;

        return x.array() * y.array();
    }

    void MulLayer::backward(MatrixXd& dout, MatrixXd& dx, MatrixXd& dy){
        dx = dout.array() * _y.array();
        dy = dout.array() * _x.array();
    }

    // -------------------------------------------------
    //          AddLayer
    // -------------------------------------------------
    MatrixXd AddLayer::forward(MatrixXd& x, MatrixXd& y){
        return x + y;
    }

    void AddLayer::backward(MatrixXd& dout, MatrixXd& dx, MatrixXd& dy){
        dx = dout;
        dy = dout;
    }

    // -------------------------------------------------
    //          ReLU
    // -------------------------------------------------
    MatrixXd ReLU::forward(MatrixXd& x){
        mask = x.unaryExpr([](double p){return p >= 0;}).cast<double>();
        return x.array() * mask.array();
    }

    MatrixXd ReLU::backward(MatrixXd& dout){
        return dout.array() * mask.array();
    }

    // -------------------------------------------------
    //          Sigmoid
    // -------------------------------------------------
    MatrixXd Sigmoid::forward(MatrixXd& x){
        _y = x.unaryExpr([](double p){return 1/(1 + exp(-p));});
        return _y; // 内部変数に格納するの忘れがち
    }

    MatrixXd Sigmoid::backward(MatrixXd& dout){
        return dout.array() * (MatrixXd::Ones(dout.rows(), dout.cols()).array() - _y.array()) * _y.array();
    }

    // -------------------------------------------------
    //          Affine
    // -------------------------------------------------
    Affine::Affine(MatrixXd& W, VectorXd& b){
        _W = W;
        _b = b;
    }

    MatrixXd Affine::forward(MatrixXd& X){
        MatrixXd Y;
        _X = X; // 内部変数に格納するの忘れがち
        Y = (X * _W).rowwise() + _b.transpose();
        return Y;
    }

    MatrixXd Affine::backward(MatrixXd& dout){
        MatrixXd dX;
        dX = dout * _W.transpose();
        dW = _X.transpose() * dout;
        db = dout.colwise().sum();
        return dX;
    }

    // -------------------------------------------------
    //          SoftmaxWithLoss
    // -------------------------------------------------
    double SoftmaxWithLoss::forward(MatrixXd& X, MatrixXd& t){
        _t = t;
        _Y = softmax(X);
        _loss = cross_entropy_error(_Y, t);
        return _loss;
    }

    MatrixXd SoftmaxWithLoss::backward(double dout){
        double batch_size = _t.rows();
        MatrixXd dx;
        dx = (_Y - _t) / batch_size;

        return dx;
    }

}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_EAA_HPP
#define RW_MATH_EAA_HPP

/**
 * @file EAA.hpp
 */
#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/math/Constants.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Rotation3DVector.hpp>
#include <rw/math/Vector3D.hpp>

#include <Eigen/Core>
#endif
namespace rw { namespace math {
    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A class for representing an equivalent angle-axis rotation
     *
     * This class defines an equivalent-axis-angle orientation vector also known
     * as an @f$ \thetak @f$ vector or "axis+angle" vector
     *
     * The equivalent-axis-angle vector is the product of a unit vector @f$
     * \hat{\mathbf{k}} @f$ and an angle of rotation around that axis @f$ \theta
     * @f$
     *
     * @note given two EAA vectors @f$ \theta_1\mathbf{\hat{k}}_1 @f$ and @f$
     * \theta_2\mathbf{\hat{k}}_2 @f$ it is generally not possible to subtract
     * or add these vectors, except for the special case when @f$
     * \mathbf{\hat{k}}_1 == \mathbf{\hat{k}}_2 @f$ this is why this class does
     * not have any subtraction or addition operators
     */
    template< class T = double > class EAA : public rw::math::Rotation3DVector< T >
    {
      public:
        /**
         * @brief Extracts Equivalent axis-angle vector from Rotation matrix
         *
         * @param R [in] A 3x3 rotation matrix @f$ \mathbf{R} @f$
         *
         * @f$
         * \theta = arccos(\frac{1}{2}(Trace(\mathbf{R})-1)=arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2})
         * @f$
         *
         * @f$
         * \thetak=log(\mathbf{R})=\frac{\theta}{2 sin \theta}(\mathbf{R}-\mathbf{R}^T) =
         * \frac{\theta}{2 sin \theta}
         * \left[
         * \begin{array}{c}
         * r_{32}-r_{23}\\
         * r_{13}-r_{31}\\
         * r_{21}-r_{12}
         * \end{array}
         * \right]
         * @f$
         *
         * @f$
         * \thetak=
         * \left[
         * \begin{array}{c}
         * 0\\
         * 0\\
         * 0
         * \end{array}
         * \right]
         * @f$ if @f$ \theta = 0 @f$
         *
         * @f$
         * \thetak=\pi
         * \left[
         * \begin{array}{c}
         * \sqrt{(R(0,0)+1.0)/2.0}\\
         * \sqrt{(R(1,1)+1.0)/2.0}\\
         * \sqrt{(R(2,2)+1.0)/2.0}
         * \end{array}
         * \right]
         * @f$ if @f$ \theta = \pi @f$
         *
         */
        explicit EAA (const rw::math::Rotation3D< T >& R);

        /**
         * @brief Constructs an EAA vector initialized to \f$\{0,0,0\}\f$
         */
        EAA () : _eaa (0, 0, 0) {}

        /**
         * @brief Constructs an initialized EAA vector
         * @param axis [in] \f$ \mathbf{\hat{k}} \f$
         * @param angle [in] \f$ \theta \f$
         * @pre norm_2(axis) = 1
         */
        EAA (const rw::math::Vector3D< T >& axis, T angle) : _eaa (axis * angle) {}

        /**
         * @brief Constructs an initialized EAA vector
         * @f$ \thetak =
         * \left[\begin{array}{c}
         *    \theta k_x\\
         *    \theta k_y\\
         *    \theta k_z
         * \end{array}\right]
         * @f$
         * @param thetakx [in] @f$ \theta k_x @f$
         * @param thetaky [in] @f$ \theta k_y @f$
         * @param thetakz [in] @f$ \theta k_z @f$
         */
        EAA (T thetakx, T thetaky, T thetakz) :
            _eaa (rw::math::Vector3D< T > (thetakx, thetaky, thetakz))
        {}

        /**
         * @brief Constructs an EAA vector that will rotate v1 into
         * v2. Where v1 and v2 are normalized and described in the same reference frame.
         * @param v1 [in] normalized vector
         * @param v2 [in] normalized vector
         */
        EAA (const rw::math::Vector3D< T >& v1, const rw::math::Vector3D< T >& v2);

        /**
         * @brief Constructs an initialized EAA vector
         *
         * The angle of the EAA are \f$\|eaa\|\f$ and the axis is \f$\frac{eaa}{\|eaa\|}\f$
         * @param eaa [in] Values to initialize the EAA
         */
        explicit EAA (rw::math::Vector3D< T > eaa) : _eaa (eaa) {}

        /**
         * @brief Copy Constructor
         * @param eaa [in] Values to initialize the EAA
         */
        EAA (const rw::math::EAA< T >& eaa) : _eaa (eaa._eaa) {}

        /**
         * @brief Constructs an initialized EAA vector
         *
         * The angle of the EAA are \f$\|eaa\|\f$ and the axis is \f$\frac{eaa}{\|eaa\|}\f$
         * @param eaa [in] Values to initialize the EAA
         */
        template< class R > explicit EAA (const Eigen::MatrixBase< R >& r) : _eaa (r) {}

        //! @brief destructor
        virtual ~EAA () {}

        /**
         * @brief Get the size of the EAA.
         * @return the size (always 3).
         */
        size_t size () const { return 3; }

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################
#if !defined(SWIGJAVA)
        /**
         * @copydoc Rotation3DVector::toRotation3D()
         *
         * @f$
         * \mathbf{R} = e^{[\mathbf{\hat{k}}],\theta}=\mathbf{I}^{3x3}+[\mathbf{\hat{k}}]
         * sin\theta+[{\mathbf{\hat{k}}}]^2(1-cos\theta) = \left[ \begin{array}{ccc}
         *      k_xk_xv\theta + c\theta & k_xk_yv\theta - k_zs\theta & k_xk_zv\theta + k_ys\theta \\
         *      k_xk_yv\theta + k_zs\theta & k_yk_yv\theta + c\theta & k_yk_zv\theta - k_xs\theta\\
         *      k_xk_zv\theta - k_ys\theta & k_yk_zv\theta + k_xs\theta & k_zk_zv\theta + c\theta
         *    \end{array}
         *  \right]
         * @f$
         *
         * where:
         * - @f$ c\theta = cos \theta @f$
         * - @f$ s\theta = sin \theta @f$
         * - @f$ v\theta = 1-cos \theta @f$
         */

#endif
        virtual const rw::math::Rotation3D< T > toRotation3D () const;

        /**
         * @brief Extracts the angle of rotation @f$ \theta @f$
         * @return @f$ \theta @f$
         */
        T angle () const { return _eaa.norm2 (); }

        /**
         * @brief change the angle of the EAA
         * @param angle [in] the new angle
         * @return this object
         */
        EAA< T >& setAngle (const T& angle)
        {
            (*this) = EAA< T > (this->axis (), angle);
            return (*this);
        }

        /**
         * @brief Extracts the axis of rotation vector @f$ \mathbf{\hat{\mathbf{k}}} @f$
         * @return @f$ \mathbf{\hat{\mathbf{k}}} @f$
         */
        const rw::math::Vector3D< T > axis () const
        {
            T theta = angle ();
            if (theta < 1e-6)
                return rw::math::Vector3D< T > (0, 0, 0);
            else
                return _eaa / theta;
        }

        /**
         * @brief get the underling Vector
         * @return the vector
         */
        rw::math::Vector3D< T >& toVector3D () { return this->_eaa; }

        /**
         * @brief get the underling Vector
         * @return the vector
         */
        rw::math::Vector3D< T > toVector3D () const { return this->_eaa; }

        /**
         * @brief get as eigen vector
         * @return Eigenvector
         */
        Eigen::Matrix< T, 3, 1 >& e () { return this->_eaa.e (); }

        /**
         * @brief get as eigen vector
         * @return Eigenvector
         */
        Eigen::Matrix< T, 3, 1 > e () const { return this->_eaa.e (); }

#if !defined(SWIG)
        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        const T& operator[] (size_t i) const
        {
            assert (i < 3);
            return _eaa[i];
        }

        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        T& operator[] (size_t i)
        {
            assert (i < 3);
            return _eaa[i];
        }

        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        const T& operator() (size_t i) const
        {
            assert (i < 3);
            return _eaa[i];
        }

        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        T& operator() (size_t i)
        {
            assert (i < 3);
            return _eaa[i];
        }
#else
        ARRAYOPERATOR (T);
#endif
        // ###################################################
        // #                 Math Operators                  #
        // ###################################################

        // ########## Eigen Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        template< class R > EAA< T > elemDivide (const Eigen::MatrixBase< R >& rhs) const
        {
            EAA< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._eaa[i] /= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        template< class R > EAA< T > elemMultiply (const Eigen::MatrixBase< R >& rhs) const
        {
            EAA< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._eaa[i] *= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > EAA< T > operator- (const Eigen::MatrixBase< R >& rhs) const
        {
            return EAA< T > (_eaa - rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend EAA< T > operator- (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs - rhs.e ());
        }

        /**
         * @brief Vector addition.
         */
        template< class R > EAA< T > operator+ (const Eigen::MatrixBase< R >& rhs) const
        {
            return EAA< T > (_eaa + rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend EAA< T > operator+ (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs + rhs.e ());
        }

        // ########### EAA Operators

        /**
         * @brief Unary minus.
         * @brief negative version
         */
        EAA< T > operator- () const { return EAA< T > (-_eaa); }

        /**
         * @brief element wise addition
         * @param rhs [in] the EAA to be added
         * @return the sum of the two EAA's
         */
        EAA< T > elemAdd (const EAA< T >& rhs) const { return EAA< T > (this->_eaa + rhs._eaa); }

        /**
         * @brief element wise subtraction
         * @param rhs [in] the EAA to be subtracted
         * @return the difference between the two EAA's
         */
        EAA< T > elemSubtract (const EAA< T >& rhs) const
        {
            return EAA< T > (this->_eaa - rhs._eaa);
        }

        /**
         * @brief element wise devision ( \b this / \b rhs )
         * @param rhs [in] the EAA to be devided with
         * @return the result of division
         */
        EAA< T > elemDivide (const EAA< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemDivide (rhs._eaa));
        }

        /**
         * @brief element wise multiplication
         * @param rhs [in] the EAA to be multiplyed with
         * @return the result of division
         */
        EAA< T > elemMultiply (const EAA< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemMultiply (rhs._eaa));
        }

        /**
         * @brief This is rotation multiplcation, and it is multiplication of two EAA's first
         * converted to a Rotation3D
         * @param rhs [in] the eaa to multiply with
         * @return the new rotation
         */
        EAA< T > operator* (const EAA< T >& rhs) const
        {
            return EAA< T > (this->toRotation3D () * rhs.toRotation3D ());
        }

        // ########### Rotation3D Operators
#if !defined(SWIG)
        /**
         * @brief Calculates \f$ \robabx{a}{c}{\thetak} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{\thetak}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bTKc [in] \f$ \robabx{b}{c}{\thetak} \f$
         * @return \f$ \robabx{a}{c}{\thetak} \f$
         */
        friend EAA< T > operator* (const rw::math::Rotation3D< T >& aRb, const EAA< T >& bTKc)
        {
            return EAA (aRb * bTKc._eaa);
        }
#endif

        /**
         * @brief matrix multiplication converting EAA to rotation
         * @param rhs [in] the roation matrix to multiply with
         * @return EAA ( this->toRotation3D() * rhs)
         */
        template< class R > EAA< T > operator* (const rw::math::Rotation3D< R >& rhs)
        {
            return EAA (this->toRotation3D () * rhs);
        }

        // ########### Scalar operators

        /**
         * @brief scalar multiplication
         * @param rhs [in] the scalar to multiply with
         * @return the product
         */
        EAA< T > elemMultiply (const T& rhs) const { return EAA< T > (this->_eaa * rhs); }

        /**
         * @brief scalar devision
         * @param rhs [in] the scalar to devide with
         * @return the resulting EAA
         */
        EAA< T > elemDivide (const T& rhs) const { return EAA< T > (this->_eaa / rhs); }

        /**
         * @brief Scalar subtraction.
         */
        EAA< T > elemSubtract (const T rhs) const { return EAA< T > (_eaa.elemSubtract (rhs)); }

        /**
         * @brief Scalar addition.
         */
        EAA< T > elemAdd (const T rhs) const { return EAA< T > (_eaa.elemAdd (rhs)); }

        /**
         * @brief scale the angle, keeping the axis the same
         * @param scale [in] how much the angle should change
         * @return a new EAA with the scaled angle
         */
        EAA< T > scaleAngle (const T& scale)
        {
            return EAA< T > (this->axis (), this->angle () * scale);
        }

        // ############ Vector3D Operators

        /**
         * @brief element wise multiplication.
         * @param rhs [in] the vector being devided with
         * @return the resulting EAA
         */
        EAA< T > operator+ (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa + rhs);
        }

#if !defined(SWIG)
        /**
         * @brief Vector addition
         * @param lhs [in] left side value
         * @param rhs [in] right side value
         * @return the resulting EAA
         */
        friend EAA< T > operator+ (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs + rhs._eaa);
        }
#endif

        /**
         * @brief Vector addition
         * @param rhs [in] the vector being added
         * @return the resulting EAA
         */
        EAA< T > operator- (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa - rhs);
        }

#if !defined(SWIG)
        /**
         * @brief Vector addition
         * @param lhs [in] left side value
         * @param rhs [in] right side value
         * @return the resulting EAA
         */
        friend EAA< T > operator- (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs - rhs._eaa);
        }
#endif
        /**
         * @brief element wise devision ( \b this / \b rhs )
         * @param rhs [in] the Vector to be devided with
         * @return the result of division
         */
        EAA< T > elemDivide (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemDivide (rhs));
        }

        /**
         * @brief element wise multiplication
         * @param rhs [in] the Vector to be multiplyed with
         * @return the result of division
         */
        EAA< T > elemMultiply (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemMultiply (rhs));
        }

        // ############ Ostream operators

#if !defined(SWIG)
        /**
         * @brief Ouputs EAA to stream
         * @param os [in/out] stream to use
         * @param eaa [in] equivalent axis-angle
         * @return the resulting stream
         */
        friend std::ostream& operator<< (std::ostream& os, const EAA< T >& eaa)
        {
            return os << " EAA( " << eaa (0) << ", " << eaa (1) << ", " << eaa (2) << ")";
        }
#else
        TOSTRING (rw::math::EAA< T >);
#endif
        // ############ Math Operations

        /**
         * @brief Calculates the cross product and returns the result
         * @param v [in] a Vector3D
         * @return the resulting 3D vector
         */
        rw::math::Vector3D< T > cross (const rw::math::Vector3D< T >& v) const
        {
            return rw::math::cross (this->_eaa, v);
        }

        /**
         * @brief Calculates the cross product and returns the result
         * @param eaa [in] a EAA
         * @return the resulting 3D vector
         */
        EAA< T > cross (const EAA< T >& eaa) const
        {
            return EAA< T > (rw::math::cross (this->_eaa, eaa._eaa));
        }

        /**
         * @brief Calculates the dot product and returns the result
         * @param v [in] a Vector3D
         * @return the resulting scalar
         */
        T dot (const rw::math::Vector3D< T >& v) { return rw::math::dot (this->_eaa, v); }

        /**
         * @brief Calculates the cross product and returns the result
         * @param eaa [in] a EAA
         * @return the resulting 3D vector
         */
        T dot (const EAA< T >& eaa) { return rw::math::dot (this->_eaa, eaa._eaa); }

        /**
         * @brief Returns the Euclidean norm (2-norm) of the vector
         * @return the norm
         */
        T norm2 () const { return _eaa.norm2 (); }

        /**
         * @brief Returns the Manhatten norm (1-norm) of the vector
         * @return the norm
         */
        T norm1 () const { return _eaa.norm1 (); }

        /**
         * @brief Returns the infinte norm (\f$\inf\f$-norm) of the vector
         * @return the norm
         */
        T normInf () const { return _eaa.normInf (); }

        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief copy operator
         * @param rhs [in] the EAA to be copied
         * @return reference to this EAA
         */
        EAA< T >& operator= (const EAA< T >& rhs)
        {
            this->_eaa = rhs._eaa;
            return *this;
        }

        /**
         * @brief assign vector to EAA
         * @param rhs [in] the vector to asign
         * @return reference to this EAA
         */
        EAA< T >& operator= (const rw::math::Vector3D< T >& rhs)
        {
            this->_eaa = rhs;
            return *this;
        }

        /**
         * @brief addition operator
         * @param rhs [in] the right hand side of the operation
         * @return reference to this EAA
         */
        EAA< T >& operator+= (const rw::math::Vector3D< T >& rhs)
        {
            this->_eaa += rhs;
            return *this;
        }

        /**
         * @brief subtraction operator
         * @param rhs [in] the right hand side of the operation
         * @return reference to this EAA
         */
        EAA< T >& operator-= (const rw::math::Vector3D< T >& rhs)
        {
            this->_eaa -= rhs;
            return *this;
        }
#if !defined(SWIG)
        /**
         * @brief Implicit converter to Vector3D
         */
        operator rw::math::Vector3D< T > () const { return _eaa; }

        /**
         * @brief Implicit converter to Vector3D
         */
        operator rw::math::Vector3D< T > & () { return _eaa; }
#endif
        /**
         * @brief copy a vector from eigen type
         * @param r [in] an Eigen Vector
         */
        template< class R > EAA< T >& operator= (const Eigen::MatrixBase< R >& r)
        {
            _eaa = r;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        template< class R > EAA< T >& operator+= (const Eigen::MatrixBase< R >& r)
        {
            _eaa += r;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > EAA< T >& operator-= (const Eigen::MatrixBase< R >& r)
        {
            _eaa -= r;
            return *this;
        }
#if !defined(SWIG)
        /**
         * @brief implicit conversion to EigenVector
         */
        operator Eigen::Matrix< T, 3, 1 > () const { return this->e (); }

        /**
         * @brief implicit conversion to EigenVector
         */
        operator Eigen::Matrix< T, 3, 1 > & () { return this->e (); }
#endif

        /**
         * @brief copy operator
         * @param rhs [in] the Rotation3D to be copied
         * @return reference to this EAA
         */
        EAA< T >& operator= (const rw::math::Rotation3D< T >& rhs)
        {
            return (*this) = EAA< T > (rhs);
        }

        // ###################################################
        // #                    Comparetors                  #
        // ###################################################

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const rw::math::Vector3D< T >& rhs) const { return _eaa == rhs; }

#if !defined(SWIG)
        /**
         * @brief Compare with \b rhs for equality.
         * @param lhs [in] first Vector
         * @param rhs [in] second vector.
         * @return True if a equals b, false otherwise.
         */
        friend bool operator== (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return lhs == rhs._eaa;
        }
#endif

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param rhs [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        bool operator!= (const rw::math::Vector3D< T >& rhs) const { return _eaa != rhs; }

#if !defined(SWIG)
        /**
         *  @brief Compare with \b rhs for inequality.
         * @param lhs [in] first Vector
         * @param rhs [in] second vector.
         *  @return True if a and b are different, false otherwise.
         */
        friend bool operator!= (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return lhs != rhs._eaa;
        }
#endif

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const EAA< T >& rhs) const { return _eaa == rhs._eaa; }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param rhs [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        bool operator!= (const EAA< T >& rhs) const { return _eaa != rhs._eaa; }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R > bool operator== (const Eigen::MatrixBase< R >& rhs) const
        {
            return this->_eaa == rhs;
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R >
        friend bool operator== (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return lhs == rhs._eaa;
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R > bool operator!= (const Eigen::MatrixBase< R >& rhs) const
        {
            return !(*this == rhs);
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R >
        friend bool operator!= (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return !(lhs == rhs);
        }

      private:
        rw::math::Vector3D< T > _eaa;
    };

    template< class T >
    rw::math::Vector3D< T > cross (const rw::math::Vector3D< T >& v1, const EAA< T >& v2)
    {
        return rw::math::cross (v1, v2.axis () * v2.angle ());
    }

    /**
     * @brief Casts EAA<T> to EAA<Q>
     * @param eaa [in] EAA with type T
     * @return EAA with type Q
     */
    template< class Q, class T > const EAA< Q > cast (const EAA< T >& eaa)
    {
        return EAA< Q > (
            static_cast< Q > (eaa (0)), static_cast< Q > (eaa (1)), static_cast< Q > (eaa (2)));
    }

    using EAAd = EAA< double >;
    using EAAf = EAA< float >;

    /*@}*/

}}    // namespace rw::math

#if !defined(SWIG)
extern template class rw::math::EAA< double >;
extern template class rw::math::EAA< float >;
#else
SWIG_DECLARE_TEMPLATE (EAAd, rw::math::EAA< double >);
SWIG_DECLARE_TEMPLATE (EAAf, rw::math::EAA< float >);
#endif

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::EAA
         */
        template<>
        void write (const rw::math::EAA< double >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::EAA
         */
        template<>
        void write (const rw::math::EAA< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::EAA
         */
        template<>
        void read (rw::math::EAA< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::EAA
         */
        template<>
        void read (rw::math::EAA< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


/************************************************************************/
/*                   QR-PCA-FaceRec  by John Hany                       */
/*																		*/
/*	A face recognition algorithm using QR based PCA.              		*/
/*																		*/
/*	Released under MIT license.											*/
/*																		*/
/*	Contact me at johnhany@163.com										*/
/*																		*/
/*	Welcome to my blog http://johnhany.net/, if you can read Chinese:)	*/
/*																		*/
/************************************************************************/

#include <opencv2/opencv.hpp>
#include <armadillo>
#include <iostream>

using namespace std;

int component_num = 7;

string orl_path = "G:\\Datasets\\orl_faces";

enum distance_type {ECULIDEAN = 0, MANHATTAN, MAHALANOBIS};
//double distance_criterion[3] = { 10.0, 30.0, 3.0};
double distance_criterion[3] = { 1000.0, 1000.0, 1000.0};

bool compDistance(pair<int, double> a, pair<int, double> b);
double calcuDistance(const arma::vec vec1, const arma::vec vec2, distance_type dis_type);
double calcuDistance(const arma::vec vec1, const arma::vec vec2, const arma::mat cov2, distance_type dis_type);

int main(int argc, const char *argv[]) {

	int class_num = 40;
	int sample_num = 10;

	int img_cols = 92;
	int img_rows = 112;
	cv::Size sample_size(img_cols, img_rows);

	arma::mat mat_sample(img_rows*img_cols, sample_num*class_num);

	//Load samples in one matrix `mat_sample`.

	for(int class_idx = 0; class_idx < class_num; class_idx++) {
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {

			string filename = orl_path + "\\s" + to_string(class_idx+1) + "\\" + to_string(sample_idx+1) + ".pgm";
			cv::Mat img_frame = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
			cv::Mat img_sample;
			cv::resize(img_frame, img_sample, sample_size);

			for(int i = 0; i < img_rows; i++) {
				uchar* pframe = img_sample.ptr<uchar>(i);
				for(int j = 0; j < img_cols; j++) {
					mat_sample(i*img_cols+j, class_idx*sample_num+sample_idx) = (double)pframe[j]/255.0;
				}
			}
		}
	}
//	cout <<	mat_sample.n_rows << endl << mat_sample.n_cols << endl << mat_sample(img_rows*img_cols/2, 0) << endl;

	//Calculate PCA transform matrix `mat_pca`.

	arma::mat H = mat_sample;
	arma::mat mean_x = arma::mean(mat_sample, 1);

	for(int j = 0; j < class_num * sample_num; j++) {
		H.col(j) -= mean_x.col(0);
	}
	H *= 1.0/sqrt(sample_num-1);

	arma::mat Q, R;
	arma::qr_econ(Q, R, H);

	arma::mat U, V;
	arma::vec d;
	arma::svd_econ(U, d, V, R.t());

//	cout << "d" << endl << d << endl;

//	arma::rowvec vec_eigen = d.head(component_num).t();
//	cout << "vec_eigen" << endl << vec_eigen << endl;

	arma::mat V_h(V.n_rows, component_num);
	if(component_num == 1) {
		V_h = V.col(0);
	}else {
		V_h = V.cols(0, component_num-1);
	}

	arma::mat mat_pca = Q * V_h;

	//Calculate eigenfaces `mat_eigen_vec`.

	arma::mat mat_eigen = mat_pca.t() * mat_sample;
//	cout << "mat_eigen" << endl << mat_eigen << endl;
	arma::mat mat_eigen_vec(component_num, class_num, arma::fill::zeros);
	vector<arma::mat> mat_cov_list;

	for(int class_idx = 0; class_idx < class_num; class_idx++) {

		arma::vec eigen_sum(component_num, arma::fill::zeros);
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {
			eigen_sum += mat_eigen.col(class_idx*sample_num+sample_idx);
		}
		eigen_sum /= (double)sample_num;
		mat_eigen_vec.col(class_idx) = eigen_sum;

		mat_cov_list.push_back(arma::cov((mat_eigen.cols(class_idx*sample_num, class_idx*sample_num+sample_num-1)).t()));

//		cout << mat_cov_list[class_idx] << endl;

	}

//	cout << "mat_eigen_vec" << endl << mat_eigen_vec << endl;

/*
	cout << "dis within class" << endl;
	for(int class_idx = 0; class_idx < class_num; class_idx++) {
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {
			double dis = calcuDistance(mat_eigen.col(class_idx*sample_num+sample_idx), mat_eigen_vec.col(class_idx), mat_cov_list[class_idx], distance_type::MAHALANOBIS);
			cout << dis << " ";
		}
		cout << endl;
	}

	cout << "dis between classes" << endl;
	for(int class_idx = 0; class_idx < class_num; class_idx++) {
		for(int sample_idx = 0; sample_idx < class_num; sample_idx++) {
			double dis = calcuDistance(mat_eigen.col(sample_idx*sample_num), mat_eigen_vec.col(class_idx), mat_cov_list[class_idx], distance_type::MAHALANOBIS);
			cout << dis << " ";
		}
		cout << endl;
	}
*/

	//Classify new sample.

	int correct_count = 0;

	double max_dis = 0.0;

	for(int class_idx = 0; class_idx < class_num; class_idx++){
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {
			arma::mat mat_new_sample(img_rows*img_cols, 1);

			string filename = orl_path + "\\s" + to_string(class_idx+1) + "\\" + to_string(sample_idx+1) + ".pgm";
			cv::Mat img_new_frame = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
			cv::Mat img_new_sample;
			cv::resize(img_new_frame, img_new_sample, sample_size);

			for(int i = 0; i < img_rows; i++) {
				uchar* pframe = img_new_sample.ptr<uchar>(i);
				for(int j = 0; j < img_cols; j++) {
					mat_new_sample(i*img_cols+j, 0) = (double)pframe[j]/255.0;
				}
			}

			arma::mat mat_new_eigen = mat_pca.t() * mat_new_sample;

			vector<pair<int, double>> dis_list;
			for(int new_class_idx = 0; new_class_idx < class_num; new_class_idx++) {
				double dis = calcuDistance(mat_new_eigen.col(0), mat_eigen_vec.col(new_class_idx), mat_cov_list[new_class_idx], distance_type::MAHALANOBIS);
				dis_list.push_back(make_pair(new_class_idx, dis));
			}
			sort(dis_list.begin(), dis_list.end(), compDistance);

			if(dis_list[0].first == class_idx && dis_list[0].second <= distance_criterion[distance_type::MAHALANOBIS]) {
				correct_count++;
			}

			if(dis_list.back().second > max_dis) {
				max_dis = dis_list.back().second;
			}
		}
	}

	cout << "Maximum distance: " << max_dis << endl;

	double correct_ratio = (double)correct_count / (class_num * sample_num);
	cout << "Correctness ratio: " << correct_ratio * 100.0 << "%" << endl;

	cin.get();

	return 0;
}

bool compDistance(pair<int, double> a, pair<int, double> b) {
	return (a.second < b.second);
}

double calcuDistance(const arma::vec vec1, const arma::vec vec2, distance_type dis_type) {

	if(dis_type == ECULIDEAN) {
		return arma::norm(vec1-vec2, 2);
	}else if(dis_type == MANHATTAN) {
		return arma::norm(vec1-vec2, 1);
	}else if(dis_type == MAHALANOBIS) {
		arma::mat tmp = (vec1-vec2).t() * (vec1 - vec2);
		return sqrt(tmp(0,0));
	}

	return -1.0;
}

double calcuDistance(const arma::vec vec1, const arma::vec vec2, const arma::mat cov2, distance_type dis_type) {

	if(dis_type == ECULIDEAN) {
		return arma::norm(vec1-vec2, 2);
	}else if(dis_type == MANHATTAN) {
		return arma::norm(vec1-vec2, 1);
	}else if(dis_type == MAHALANOBIS) {
		arma::mat tmp = (vec1-vec2).t() * cov2.i() * (vec1 - vec2);
		return sqrt(tmp(0,0));
	}

	return -1.0;
}


//
// 	Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
// 	Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

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

int main()
{
  namespace ublas = boost::numeric::ublas;
  using value   = float;
  using layout  = ublas::layout::first_order; // storage format
  using tensor  = ublas::tensor_dynamic<value,layout>;
  using shape   = typename tensor::extents_type;
  using matrix  = ublas::matrix<value,layout>;

  constexpr auto ones = ublas::ones<value,layout>{};

  // NOLINTNEXTLINE(google-build-using-namespace)
  using namespace boost::numeric::ublas::index;

  using namespace boost::numeric::ublas::index;
  using tensor  = boost::numeric::ublas::tensor_dynamic<float>;
  auto fones    = boost::numeric::ublas::ones<float>{};

  tensor X = fones(3,4,5);
  tensor Y = fones(4,6,3,2);

  tensor Z = 2*ones(5,6,2) + X(_i,_j,_k)*Y(_j,_l,_i,_m) + 5;

  // Matlab Compatible Formatted Output
  std::cout << "C=" << Z << ";" << std::endl;

  // Tensor-Vector-Multiplications - Including Transposition
  try {

    auto n  = shape{3,4,2};

    tensor A  = ones(n);
    matrix B1 = 2*matrix(n[1],n[2]);
    tensor v1 = 2*ones(n[0],1);
    tensor v2 = 2*ones(n[1],1);
    //      auto v3 = tensor(shape{n[2],1},2);

    // C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);
    // tensor C1 = B1 + prod(A,vector_t(n[0],1),1);
    tensor C1 = B1 + A(_i,_,_) * v1(_i,_);

    // C2(i,k) = A(i,j,k)*v2(j) + 4;
    //tensor C2 = prod(A,vector_t(n[1],1),2) + 4;
    tensor C2 = A(_,_i,_) * v2(_i,_) + 4;

    // not yet implemented!
    // C3() = A(i,j,k)*T1(i)*T2(j)*T2(k);
        // tensor C3 = prod(prod(prod(A,v1,1),v2,1),v3,1);
    // tensor C3 = A(_i,_j,_k) * v1(_i,_) * v2(_j,_) * v3(_k,_);

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);" << std::endl << std::endl;
    std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C2(i,k) = A(i,j,k)*v2(j) + 4;" << std::endl << std::endl;
    std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of multiply-tensor-einstein-notation when doing tensor-vector multiplication." << std::endl;
  }

  // Tensor-Matrix-Multiplications - Including Transposition
  try {
    auto n  = shape{3,4,2};
    auto m  = 5u;
    tensor A  = 2*ones(n);
    tensor B  = 2*ones(n[1],n[2],m);
    tensor B1 =   ones(m,n[0]);
    tensor B2 =   ones(m,n[1]);

    // C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);
    // tensor C1 = B + prod(A,B1,1);
    tensor C1 = B + A(_i,_,_) * B1(_,_i);

    // C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;
    // tensor C2 = prod(A,B2) + 4;
    tensor C2 =  A(_,_j,_) * B2(_,_j) + 4;

    // C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);
    // not yet implemented.

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);" << std::endl << std::endl;
    std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;" << std::endl << std::endl;
    std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

    // formatted output
    //        std::cout << "% --------------------------- " << std::endl;
    //        std::cout << "% --------------------------- " << std::endl << std::endl;
    //        std::cout << "% C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);" << std::endl << std::endl;
    //        std::cout << "C3=" << C3 << ";" << std::endl << std::endl;
  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of multiply-tensor-einstein-notation when doing tensor-matrix multiplication." << std::endl;
  }

  // Tensor-Tensor-Multiplications Including Transposition
  try {
    auto na = shape{3,4,5};
    auto nb = shape{4,6,3,2};
    tensor A  = 2*ones(na);
    tensor B  = 3*ones(nb);
    tensor T1 = 2*ones(na[2],na[2]);
    tensor T2 = 2*ones(na[2],nb[1],nb[3]);

    // C1(j,l) = T1(j,l) + A(i,j,k)*A(i,j,l) + 5;
    // tensor C1 = T1 + prod(A,A,perm_t{1,2}) + 5;
    tensor C1 = T1 + A(_i,_j,_m)*A(_i,_j,_l) + 5;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C1(k,l) = T1(k,l) + A(i,j,k)*A(i,j,l) + 5;" << std::endl << std::endl;
    std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

    // C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;
    //tensor C2 = T2 + prod(A,B,perm_t{1,2},perm_t{3,1}) + 5;
    tensor C2 = T2 + A(_i,_j,_k)*B(_j,_l,_i,_m) + 5;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "%  C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;" << std::endl << std::endl;
    std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of multiply-tensor-einstein-notation when doing transpose." << std::endl;
  }
}


#include <state_estimation/utilities/plotting_utilities.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace state_estimation {

std::vector<Eigen::Vector2d> getEllipsePoints(double a, double b, const Eigen::Vector2d& offset,
                                              double angle, uint32_t num_pts) {
    std::vector<Eigen::Vector2d> pts(num_pts);

    // Sample the points evenly with respect to angle around the ellipse
    const Eigen::Rotation2D<double> R(angle);
    for (int i = 0; i < num_pts; ++i) {
        const double theta = i * 2.0 * M_PI / num_pts;

        const Eigen::Vector2d pos(a * cos(theta), b * sin(theta));
        pts[i] = R * pos + offset;
    }

    return pts;
}

std::vector<Eigen::Vector2d> get2DCovarianceEllipsePoints(const Eigen::Matrix2d& cov,
                                                          const Eigen::Vector2d& offset,
                                                          uint32_t num_pts) {
    // Get the eigen vectors
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 2, 2>> solver(cov);
    Eigen::Matrix<double, 2, 2> vectors = solver.eigenvectors();
    Eigen::Matrix<double, 2, 1> values = solver.eigenvalues();

    // The major axis corresponds to the first eigen vector, and the orientation of the ellipse is
    // the directory of the first eigen vector
    const double a = values(0);
    const double b = values(1);
    const double angle = atan2(vectors(1, 0), vectors(0, 0));
    return getEllipsePoints(a, b, offset, angle, num_pts);
}

}  // namespace state_estimation


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(4,4);
MatrixXd A = X * X.transpose();
cout << "Here is a random positive-definite matrix, A:" << endl << A << endl << endl;

SelfAdjointEigenSolver<MatrixXd> es(A);
cout << "The inverse square root of A is: " << endl;
cout << es.operatorInverseSqrt() << endl;
cout << "We can also compute it with operatorSqrt() and inverse(). That yields: " << endl;
cout << es.operatorSqrt().inverse() << endl;

  return 0;
}


/**
 * @file burgersequation.cc
 * @brief NPDE homework BurgersEquation code
 * @author Oliver Rietmann
 * @date 15.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "burgersequation.h"

#include <Eigen/Core>
#include <cmath>

namespace BurgersEquation {
/* SAM_LISTING_BEGIN_1 */
constexpr double PI = 3.14159265358979323846;

double Square(double x) { return x * x; }

double w0(double x) {
  return 0.0 <= x && x <= 1.0 ? Square(std::sin(PI * x)) : 0.0;
}

double f(double x) { return 2.0 / 3.0 * std::sqrt(x * x * x); }

Eigen::VectorXd solveBurgersGodunov(double T, unsigned int N) {
  double h = 5.0 / N;           // meshwidth
  double tau = h;               // timestep = meshwidth by CFL condition
  int m = std::round(T / tau);  // no. of timesteps

  // initialize vector with initial nodal values
  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N + 1, -1.0, 4.0);
  Eigen::VectorXd mu = x.unaryExpr(&w0);

#if SOLUTION
  for (int i = 0; i < m; ++i) {
    for (int j = N; 0 < j; --j) {
      // Standard fully discrete evolution based on explicit Euler timestepping
      mu(j) = mu(j) - tau / h * (f(mu(j)) - f(mu(j - 1)));
    }
    // truncation to a finite vector. Only required on one side, because all
    // information flows from left to right.
    mu(0) = 0.0;  // Value of u0 to the left of x=0
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return mu;
}
/* SAM_LISTING_END_1 */

/**
 * @brief Converts a large vector on  a grid to a smaller vector correponding to
 * a sub-grid.
 *
 * @param mu vector of function values on a spacial grid of size N_large
 * @param N divides the size N_large of mu
 * @return a vector mu_sub of size N, that represents mu on a sub-grid of size N
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd reduce(const Eigen::VectorXd &mu, unsigned int N) {
  Eigen::VectorXd mu_sub(N + 1);
  int fraction = mu.size() / N;
  for (int j = 0; j < N + 1; ++j) {
    mu_sub(j) = mu(j * fraction);
  }
  return mu_sub;
}

Eigen::Matrix<double, 3, 4> numexpBurgersGodunov() {
  const unsigned int N_large = 3200;
  Eigen::Vector2d T{0.3, 3.0};
  Eigen::Vector4i N{5 * 10, 5 * 20, 5 * 40, 5 * 80};
  Eigen::Vector4d h;
  for (int i = 0; i < 4; ++i) h(i) = 5.0 / N(i);

  Eigen::Matrix<double, 3, 4> result;
  result.row(0) = h.transpose();

#if SOLUTION
  for (int k = 0; k < 2; ++k) {
    Eigen::VectorXd mu_ref = solveBurgersGodunov(T(k), N_large);
    Eigen::Vector4d error;
    for (int i = 0; i < 4; ++i) {
      Eigen::VectorXd mu = solveBurgersGodunov(T(k), N(i));
      Eigen::VectorXd mu_ref_sub = reduce(mu_ref, N(i));
      error(i) = h(i) * (mu - mu_ref_sub).lpNorm<1>();
    }
    result.row(k + 1) = error.transpose();
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return result;
}
/* SAM_LISTING_END_2 */

}  // namespace BurgersEquation


//
// Created by philipp on 22.12.19.
//

#ifndef FUNNELS_CPP_LYAPUNOV_HH
#define FUNNELS_CPP_LYAPUNOV_HH

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <cmath>
#include <memory>

namespace lyapunov{

  template <class DERIVED>
  double projected_max_radius(const Eigen::MatrixBase<DERIVED> & C0,
                                     const Eigen::MatrixBase<DERIVED> & C1){
    // todo optimize for usage of triangular shape
    auto Cinv = C0.inverse();
    auto P1_prime = (Cinv.transpose()*(C1.transpose()*C1)*Cinv);
    double max_rad = 1./P1_prime.eigenvalues().real().array().minCoeff();
//    Eigen::MatrixXd Cinv = C0.inverse();
//    Eigen::MatrixXd P1_prime = (Cinv.transpose()*(C1.transpose()*C1)*Cinv);
//    double max_rad = 1./P1_prime.eigenvalues().real().array().minCoeff();
    return std::sqrt(max_rad);
  }

  struct lyap_zone_t{};

  class lyapunov_t{
  public:
    virtual bool intersect(const lyap_zone_t & other) const;
    virtual bool covers(const lyap_zone_t & other) const;
    virtual bool is_covered(const lyap_zone_t & other) const;
    virtual double get_alpha() const;
  };

  struct fixed_ellipsoidal_zone_t{
    virtual const Eigen::VectorXd & x0() const {
      std::cerr << __FILE__ << ": " << __LINE__ << std::endl;
      throw std::runtime_error("Virtual");
      return Eigen::Vector2d::Zero();
    };
    virtual const Eigen::MatrixXd & C() const {
      std::cerr << __FILE__ << ": " << __LINE__ << std::endl;
      throw std::runtime_error("Virtual");
      return Eigen::Matrix2d::Zero();
    };
  };

  struct fixed_ellipsoidal_zone_copied_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_copied_t( const Eigen::VectorXd & x0,
        const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    Eigen::VectorXd _x0;
    Eigen::MatrixXd _C;
  };

  struct fixed_ellipsoidal_zone_ref_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_ref_t( const Eigen::VectorXd & x0,
                                     const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    const Eigen::VectorXd &_x0;
    const Eigen::MatrixXd &_C;
  };

  // todo
//  // Does zone0 cover zone1
//  template<class DIST>
//  bool covers_helper(const fixed_ellipsoidal_zone_t &zone0,
//                     const fixed_ellipsoidal_zone_t &zone1,
//                     DIST &dist){
//
//
//    // First compute the projected distance
//    auto dy = zone0.C()*(dist.cp_vv(zone1.x0(), zone0.x0()));
//    double dy_norm = dy.norm(); //l2 norm
//    // center out of bounds
//    if (dy_norm>=1.){
//      return false;
//    }
//
//    // Compute radius
//    return dy_norm+projected_max_radius(zone0.C(), zone1.C())<=1.;
//  }

  template<class TRAJ, class DIST>
  class fixed_ellipsoidal_lyap_t: public TRAJ{
  public:
    using dist_t = DIST;
    using traj_t = TRAJ;
    using dyn_t = typename traj_t::dyn_t;

    using matrix_t = typename traj_t::matrix_t;
    using vector_x_t = typename traj_t::vector_x_t;
    using vector_u_t = typename traj_t::vector_u_t;
    using vector_t_t = typename traj_t::vector_t_t;
    using matrix_ptr_t = typename traj_t::matrix_ptr_t;
    using vector_x_ptr_t = typename traj_t::vector_x_ptr_t;
    using vector_u_ptr_t = typename traj_t::vector_u_ptr_t;
    using vector_t_ptr_t = typename traj_t::vector_t_ptr_t;

    using traj_t::dimx;
    using traj_t::dimp;
    using traj_t::dimv;
    using traj_t::dimu;

    using s_mat_t = Eigen::Matrix<double,dimx,dimx>;
    using s_vec_t = Eigen::Matrix<double,dimx,1>;

    using args = std::tuple<std::shared_ptr<s_mat_t>, double, DIST*>;

    template <class DERIVED>
    fixed_ellipsoidal_lyap_t(const Eigen::MatrixBase<DERIVED> &P,
        double gamma, DIST &dist):
        _gamma(gamma), _dist(dist){
      set_P(P);
    }

    template <class ...CARGS>
    void compute(CARGS &&...cargs){
      // Just forwarding
      traj_t::compute(cargs...);
    }

    template <class DERIVED>
    void set_P(const Eigen::MatrixBase<DERIVED>& P) {
      assert(P.rows() == dimx);
      assert(P.rows() == P.cols());

      // Compute cholesky
      Eigen::LLT<Eigen::Matrix<double, dimx, dimx>> llt_pre_comp;
      llt_pre_comp.compute(P);
      _C = llt_pre_comp.matrixU();

      // Compute the bounding box corner
      std::cout << P << std::endl;
      std::cout << _C << std::endl;
      for(size_t i=0; i<dimx; i++){
        _box_corner(i) = 1./_C.row(i).norm();
      }
      std::cout << _box_corner << std::endl;
    }
    s_mat_t get_P(){
      return _C.transpose()*_C;
    }

    const s_mat_t &C() const {
      return _C;
    }
    double gamma() const {
      return _gamma;
    }
    s_vec_t min_corner(){
      return -_box_corner;
    }
    const s_vec_t &max_corner(){
      return _box_corner;
    }
// todo
//    // Conservative intersect
//    // If true, the zone may intersect with this
//    // If false, the zone does definitively not intersect
//    template<class DERIVED>
//    bool intersect(const Eigen::MatrixBase<DERIVED> &x0,
//        const fixed_ellipsoidal_zone_t &zone) const {
//      // First compute the projected distance
//      auto dy = _C*(_dist.cp_vv(zone.x0(), x0));
//      double dy_norm = dy.norm(); //l2 norm
//
//      if (dy_norm <= 1.){
//        return true;
//      }
//
//      return dy_norm>=1.+projected_max_radius(_C, zone.C());
//    }
//
//    // Conservative cover
//    // If true, zone is definitively covered by this
//    // If false, it may not be covered
//    template<class DERIVED>
//    bool covers(const Eigen::MatrixBase<DERIVED> &x0,
//        const fixed_ellipsoidal_zone_t &zone) const {
//      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
//      return covers_helper(this_zone, zone, _dist);
//    }
//
//    // Conservative cover
//    // If true, this is definitively covered by zone
//    // If false, it may not be covered
//    template<class DERIVED>
//    bool is_covered(const Eigen::MatrixBase<DERIVED> &x0,
//        const fixed_ellipsoidal_zone_t &zone) const {
//      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
//      return covers_helper(zone, this_zone, _dist);
//    }

    double get_alpha(){
      return (double) 1./((_C.transpose()*_C)(0,0));
    }

    double get_gamma()const{
      return _gamma;
    }
    void set_gamma(double gamma){
      _gamma = gamma;
    }

    dist_t & dist() const {
      return _dist;
    }

    args get_args()const{
//      Eigen::MatrixXd P_scaled = _C.transpose()*_C;
      std::shared_ptr<s_mat_t> P = std::make_shared<s_mat_t>();
      *P = _C.transpose()*_C;
      return std::make_tuple(P, _gamma, &_dist);
    }

  protected:
    s_mat_t _C; // todo: Optimize to use triangular shape
    s_vec_t _box_corner;
    double _gamma;
    DIST &_dist;
  };

}

#endif //FUNNELS_CPP_LYAPUNOV_HH


#ifndef QUATERNION_AVERAGING_H
#define QUATERNION_AVERAGING_H

//
#include <vector>
#include <stdexcept>      // std::length_error
//
//#include <Eigen/Core>
//#include <Eigen/Eigenvalues>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Eigenvalues>
//
#include <tf/transform_datatypes.h> // tf quaternion
#include <tf2/LinearMath/Quaternion.h> // tf2 quaternion

namespace math_quat {

    static tf2::Quaternion getAverageQuaternion(
            const std::vector<tf2::Quaternion> &quaternions,
            const std::vector<double> &weights)
    {

        if (quaternions.size() != weights.size())
            throw std::length_error("Weights and quaternions needs to be same"
                                    " length");

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < quaternions.size(); ++i) {
            // Weigh the quaternions according to their associated weight
            tf2::Quaternion quat = quaternions[i] * weights[i];
            // Append the weighted Quaternion to a matrix Q.
            Q(0, i) = quat.x();
            Q(1, i) = quat.y();
            Q(2, i) = quat.z();
            Q(3, i) = quat.w();
        }

        // Creat a solver for finding the eigenvectors and eigenvalues
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());

        // Find index of maximum (real) Eigenvalue.
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i) {
            double real = eigenvalues[i].real();
            if (real > max_value) {
                max_value = real;
                max_idx = i;
            }
        }

        // Get corresponding Eigenvector, normalize it and return it as the average quat
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();

        tf2::Quaternion mean_orientation(
                eigenvector[0].real(),
                eigenvector[1].real(),
                eigenvector[2].real(),
                eigenvector[3].real()
        );

        return mean_orientation.normalized();
    }

    static tf::Quaternion getAverageQuaternion(
            const std::vector<tf::Quaternion> &quaternions,
            const std::vector<double> &weights)
    {
        if (quaternions.size() != weights.size())
            throw std::length_error("Weights and quaternions needs to be same"
                                    " length");

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < quaternions.size(); ++i) {
            // Weigh the quaternions according to their associated weight
            tf::Quaternion quat = quaternions[i] * weights[i];
            // Append the weighted Quaternion to a matrix Q.
            Q(0, i) = quat.x();
            Q(1, i) = quat.y();
            Q(2, i) = quat.z();
            Q(3, i) = quat.w();
        }

        // Creat a solver for finding the eigenvectors and eigenvalues
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());

        // Find index of maximum (real) Eigenvalue.
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i) {
            double real = eigenvalues[i].real();
            if (real > max_value) {
                max_value = real;
                max_idx = i;
            }
        }

        // Get corresponding Eigenvector, normalize it and return it as the average quat
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();

        tf::Quaternion mean_orientation(
                eigenvector[0].real(),
                eigenvector[1].real(),
                eigenvector[2].real(),
                eigenvector[3].real()
        );

        return mean_orientation.normalized();
    }

} //ns
#endif //QUATERNION_AVERAGING_H


#include <cassert>
#include <cmath>

#include <type_traits>
#include <limits>
#include <climits>

#include <boost/integer/integer_mask.hpp>

namespace hlf {
    namespace math {

        constexpr long double pi = 3.141592653589793238462643383279502884;
        constexpr long double pi_2 = pi / 2.0;
        constexpr long double pi_4 = pi / 4.0;
        constexpr long double twicePi = 2.0 * pi;

        constexpr long double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
        constexpr long double RAD_TO_DEGREE = 1.0 / DEGREE_TO_RAD;

        template<typename TFloat>
        inline TFloat degToRad(const TFloat degree) {
            return static_cast<TFloat>(degree * DEGREE_TO_RAD);
        }

        template<typename TFloat>
        inline TFloat radToDeg(const TFloat radian) {
            return static_cast<TFloat>(radian * RAD_TO_DEGREE);
        }

/// Convert [0, 360] degrees bearing into [-pi, pi] azimuth.
        template<typename TFloat>
        inline TFloat Bearing2Azimuth(const TFloat degree) {
            TFloat radians = degToRad(degree);
            assert(radians >= 0.0);
            return (radians > pi ? radians - twicePi : radians);
        }

/// Positive angle between 2 azimuths.
        template<typename TFloat>
        inline TFloat AngleBetween(TFloat rad1, TFloat rad2) {
            TFloat res = rad1 - rad2;
            if (res < 0.0)
                res += twicePi;
            return (res > pi ? twicePi - res : res);
        }

        template<class T>
        T Log2(T x) {
            return log(x) / log(2);
        }

        template<typename T>
        inline T Abs(T x) {
            return (x < 0 ? -x : x);
        }

// Compare floats or doubles for almost equality.
// maxULPs - number of closest floating point values that are considered equal.
// Infinity is treated as almost equal to the largest possible floating point values.
// NaN produces undefined result.
// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
// for details.
        template<typename TFloat>
        bool AlmostEqualULPs(TFloat x, TFloat y, unsigned int maxULPs = 256) {
            static_assert(std::is_floating_point<TFloat>::value, "");
            static_assert(std::numeric_limits<TFloat>::is_iec559, "");

            // Make sure maxUlps is non-negative and small enough that the
            // default NaN won't compare as equal to anything.
            assert(maxULPs < 4 * 1024 * 1024);

            int const bits = CHAR_BIT * sizeof(TFloat);
            typedef typename boost::int_t<bits>::exact IntType;
            typedef typename boost::uint_t<bits>::exact UIntType;

            IntType xInt = *reinterpret_cast<IntType const *>(&x);
            IntType yInt = *reinterpret_cast<IntType const *>(&y);

            // Make xInt and yInt lexicographically ordered as a twos-complement int
            IntType const highestBit = IntType(1) << (bits - 1);
            if (xInt < 0)
                xInt = highestBit - xInt;
            if (yInt < 0)
                yInt = highestBit - yInt;

            UIntType const diff = Abs(xInt - yInt);

            return diff <= maxULPs;
        }

// Returns true if x and y are equal up to the absolute difference eps.
// Does not produce a sensible result if any of the arguments is NaN or infinity.
// The default value for eps is deliberately not provided: the intended usage
// is for the client to choose the precision according to the problem domain,
// explicitly define the precision constant and call this function.
        template<typename TFloat>
        inline bool AlmostEqualAbs(TFloat x, TFloat y, TFloat eps) {
            return fabs(x - y) < eps;
        }

// Returns true if x and y are equal up to the relative difference eps.
// Does not produce a sensible result if any of the arguments is NaN, infinity or zero.
// The same considerations as in AlmostEqualAbs apply.
        template<typename TFloat>
        inline bool AlmostEqualRel(TFloat x, TFloat y, TFloat eps) {
            return fabs(x - y) < eps * max(fabs(x), fabs(y));
        }

        template<typename T>
        inline T id(T const &x) {
            return x;
        }

        template<typename T>
        inline T sq(T const &x) {
            return x * x;
        }

        template<typename T, typename TMin, typename TMax>
        inline T clamp(T x, TMin xmin, TMax xmax) {
            if (x > xmax)
                return xmax;
            if (x < xmin)
                return xmin;
            return x;
        }

        template<typename T>
        inline T cyclicClamp(T x, T xmin, T xmax) {
            if (x > xmax)
                return xmin;
            if (x < xmin)
                return xmax;
            return x;
        }

        template<typename T>
        inline bool between_s(T a, T b, T x) {
            return (a <= x && x <= b);
        }

        template<typename T>
        inline bool between_i(T a, T b, T x) {
            return (a < x && x < b);
        }

        inline int rounds(double x) {
            return (x > 0.0 ? int(x + 0.5) : int(x - 0.5));
        }

        inline size_t SizeAligned(size_t size, size_t align) {
            // static_cast    .
            return size + (static_cast<size_t>(-static_cast<ptrdiff_t>(size)) & (align - 1));
        }

        template<typename T>
        bool IsIntersect(T const &x0, T const &x1, T const &x2, T const &x3) {
            return !((x1 < x2) || (x3 < x0));
        }

// Computes x^n.
        template<typename T>
        inline T PowUint(T x, uint64_t n) {
            T res = 1;
            for (T t = x; n > 0; n >>= 1, t *= t)
                if (n & 1)
                    res *= t;
            return res;
        }

        template<typename T>
        inline T NextModN(T x, T n) {
            return x + 1 == n ? 0 : x + 1;
        }

        template<typename T>
        inline T PrevModN(T x, T n) {
            return x == 0 ? n - 1 : x - 1;
        }

        inline uint32_t NextPowOf2(uint32_t v) {
            v = v - 1;
            v |= (v >> 1);
            v |= (v >> 2);
            v |= (v >> 4);
            v |= (v >> 8);
            v |= (v >> 16);

            return v + 1;
        }

// Greatest Common Divisor
        template<typename T>
        T GCD(T a, T b) {
            T multiplier = 1;
            T gcd = 1;
            while (true) {
                if (a == 0 || b == 0) {
                    gcd = max(a, b);
                    break;
                }

                if (a == 1 || b == 1) {
                    gcd = 1;
                    break;
                }

                if ((a & 0x1) == 0 && (b & 0x1) == 0) {
                    multiplier <<= 1;
                    a >>= 1;
                    b >>= 1;
                    continue;
                }

                if ((a & 0x1) != 0 && (b & 0x1) != 0) {
                    T const minV = min(a, b);
                    T const maxV = max(a, b);
                    a = (maxV - minV) >> 1;
                    b = minV;
                    continue;
                }

                if ((a & 0x1) != 0)
                    std::swap(a, b);
                a >>= 1;
            }

            return multiplier * gcd;
        }

    }
}