#ifndef _MATH_HEADER__
#define _MATH_HEADER__

#include <cmath>

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

#include "exceptions.hpp"
#include "log.hpp"

namespace cpp_utils {

    /**
     * @brief pi value
     *
     * @return constexpr double
     */
    constexpr double pi() {
        return std::atan(1)*4;
    }

     /**
     * @brief compute log 2
     *
     * @note
     * this function allows for computation compile time
     *
     * @tparam T type of both the input and the output
     * @param n input
     * @return constexpr T \f$log_{2}(n)\f$
     */
    template <typename T>
    constexpr T log2(T n) {
        return (n<2) ? 0 : 1 + log2(n/2);
    }

    template <typename NUM>
    constexpr NUM abs(const NUM& n) {
        return n >= 0 ? n : -n;
    }

    namespace internal {

        template <typename NUM, typename ...OTHER>
        constexpr NUM _min2(const NUM& min, const NUM& f, const OTHER&... others) {
            return _min2(f < min ? f : min, others...);
        }

        template <typename NUM>
        constexpr NUM _min2(const NUM& min, const NUM& f) {
            return f < min ? f : min;
        }

        template <typename NUM, typename ...OTHER>
        constexpr NUM _max2(const NUM& max, const NUM& f, const OTHER&... others) {
            return _max2(f > max ? f : max, others...);
        }

        template <typename NUM>
        constexpr NUM _max2(const NUM& max, const NUM& f) {
            return f > max ? f : max;
        }

    }

    template <typename NUM, typename ...OTHER>
    constexpr NUM min(const NUM& f, const OTHER&... n) {
        return internal::_min2(f, n...);
    }

    template <typename NUM, typename ...OTHER>
    constexpr NUM max(const NUM& f, const OTHER&... n) {
        return internal::_max2(f, n...);
    }

    /**
     * @brief compute the number of digits a number has
     *
     * @note
     *  examples if `countZero` is false
     * @code
     *  getIntegerDigits(0); //0
     *  getIntegerDigits(1); //1
     *  getIntegerDigits(11); //2
     *  getIntegerDigits(0.3); //0
     *  getIntegerDigits(1.3); //1
     *  getIntegerDigits(11.3); //2
     * @endcode
     *
     * @tparam NUM type of the number involved
     * @param n nuymber involved
     * @param countZero if true, values like 0.3 will return 1, since "0" is treated as a normal digit. If false we will return 0 since "0" is not trated as a digit.
     * @return constexpr int number of digit before the decimal part
     */
    template <typename NUM>
    constexpr int getIntegerDigits(const NUM& n, bool countZero = true) {
        debug("n=", n);
        if (n < 0) {
            return getIntegerDigits(-n, countZero);
        }

        if (n < 1) {
            return countZero ? 1 : 0;
        }
        return static_cast<int>(floor(log10(n))) + 1;
    }

    // template <typename NUM>
    // NUM pow10(int a) {
    //     NUM n{1};
    //     return internal::_pow10(n, a);
    // }

    template <typename NUM>
    NUM pow10(int a) {
        static double pow[] = {
            1e-20, 1e-19, 1e-18, 1e-17, 1e-16,
            1e-15, 1e-14, 1e-13, 1e-12, 1e-11,
            1e-10, 1e-9, 1e-8, 1e-7, 1e-6,
            1e-5, 1e-4, 1e-3, 1e-2, 1e-1,
            1,
            1e+1, 1e+2, 1e+3, 1e+4, 1e+5,
            1e+6, 1e+7, 1e+8, 1e+9, 1e+10,
            1e+11, 1e+12, 1e+13, 1e+14, 1e+15,
            1e+16, 1e+17, 1e+18, 1e+19, 1e+20,
        };
        debug("a=", a);
        assert(a >= -20 && a <= +20);
        return static_cast<NUM>(pow[20 + a]);
    }

    /**
     * @brief get angular coefficient of a line going through 2 points
     *
     * The linear equation is:
     * ```
     * y = mx + q
     * ```
     *
     * @pre
     *  @li \f$ xa -xb \not = 0 \f$;
     *
     * @tparam NUM type fo the points
     * @param xa x of the first point
     * @param ya y of the first point
     * @param xb x of the second point
     * @param yb y of the second point
     * @return m
     */
    template <typename NUM1, typename NUM2, typename NUM3, typename NUM4>
    constexpr double getM(const NUM1& xa, const NUM2& ya, const NUM3& xb, const NUM4& yb) {
        return (static_cast<double>(ya) - static_cast<double>(yb)) / (static_cast<double>(xa) - static_cast<double>(xb));
    }

    /**
     * @brief get q of a line going through 2 points
     *
     * The linear equation is:
     * ```
     * y = mx + q
     * ```
     *
     * @pre
     *  @li \f$ xa -xb \not = 0 \f$;
     *
     * @tparam NUM type fo the points
     * @param xa x of the first point
     * @param ya y of the first point
     * @param xb x of the second point
     * @param yb y of the second point
     * @return q
     */
    template <typename NUM1, typename NUM2, typename NUM3, typename NUM4>
    constexpr double getQ(const NUM1& xa, const NUM2& ya, const NUM3& xb, const NUM4& yb) {
        return static_cast<double>(ya) - getM(xa, ya, xb, yb) * static_cast<double>(xa);
    }

    /**
     * @brief get q of a line going through 2 points
     *
     * The linear equation is:
     * ```
     * y = mx + q
     * ```
     *
     * @tparam NUM type fo the points
     * @param xa x of the first point
     * @param ya y of the first point
     * @param m angular coefficient of the line
     * @return q
     */
    template <typename NUM1, typename NUM2, typename NUM3>
    constexpr double getQ(const NUM1& xa, const NUM2& ya, const NUM3& m) {
        return static_cast<double>(ya) - static_cast<double>(m)*static_cast<double>(xa);
    }

    /**
     * @brief transform a value linearly
     *
     * The transformation follows the following equation:
     * ```
     * y = mx + q
     * ```
     *
     * @param x the value to transform
     * @param m angular coefficient of the line
     * @param q y of the point in x=0
     * @return y transformed value
     */
    template <typename NUM1, typename NUM2, typename NUM3>
    constexpr double linearTransform(const NUM1& x, const NUM2& m, const NUM3& q) {
        return static_cast<double>(m)*static_cast<double>(x) + static_cast<double>(q);
    }

    /**
     * @brief transform a value linearly
     *
     * The transformation follows the following equation:
     * ```
     * y = mx + q
     * ```
     *
     * @param x the value to transform
     * @param xa x of the first point the line go through
     * @param ya y of the first point the line go through
     * @param xb x of the second point the line go through
     * @param yb y of the second point the line go through
     * @return y
     */
    template <typename NUM1, typename NUM2, typename NUM3, typename NUM4, typename NUM5>
    constexpr double linearTransform(const NUM1& x, const NUM2& xa, const NUM3& ya, const NUM4& xb, const NUM5& yb) {
        auto m = getM(xa, ya, xb, yb);
        auto q = getQ(xa, ya, m);
        return linearTransform(x, m, q);
    }

    /**
     * @brief Generate a bounded sigmoid
     *
     * Use this function when you need to generate a monotonically crescent number
     *
     * @image html images/sigmoid.png ""
     *
     * @tparam NUM0 type of x castable to double
     * @tparam NUM1 type of xMin castable to double
     * @tparam NUM2 type of xMax castable to double
     * @tparam NUM3 type of yMin castable to double
     * @tparam NUM4 type of yMax castable to double
     * @tparam NUM5 type of steepness castable to double
     * @tparam NUM6 type of steepnessLocation castable to double
     * @param xMin the minimum value x can have
     * @param xMax the maximum value x can have
     * @param yMin the minimum value the output of the function can have (obtainable when `x = xMin`)
     * @param yMax the maximum value the output of the function can have (obtainable when `x = xMax`)
     * @param steepness how quickly the function increases in value. strictly greater than 0. number greater than 1 means that the sigmoid change is steep. number in (0,1) means that the sigmoid change si smooth. parameter set t 1 means no alterations to the sigmoid.
     * @param steepnessLocation a number between 0 and 1. 0 means the sigmoid y-value change happens near `xMin', 1 means the sigmoi y-value change happens near `xMax`; 0.5 it happens in the middle
     * @return constexpr double output of the sigmoid
     */
    template <typename NUM0, typename NUM1, typename NUM2, typename NUM3, typename NUM4, typename NUM5, typename NUM6>
    constexpr double getSigmoid(const NUM0& x, const NUM1& xMin, const NUM2& xMax, const NUM3& yMin, const NUM4& yMax, const NUM5& steepness, const NUM6& steepnessLocation) {
        return getSigmoid<double, double, double, double, double, double, double>(x, xMin, xMax, yMin, yMax, steepness, steepnessLocation);
    }

    template <>
    constexpr double getSigmoid(const double& x, const double& xMin, const double& xMax, const double& yMin, const double& yMax, const double& steepness, const double& steepnessLocation) {
        double deltax = xMin + steepnessLocation * (xMax - xMin);
        double actualx = (x - deltax)/steepness;
        return yMin + (yMax - yMin) * ((exp(actualx))/(exp(actualx) + 1));
    }

    template <typename NUM0, typename NUM1, typename NUM2, typename NUM3, typename NUM4, typename NUM5, typename NUM6>
    constexpr double getInverseSigmoid(const NUM0& x, const NUM1& xMin, const NUM2& xMax, const NUM3& yMin, const NUM4& yMax, const NUM5& steepness, const NUM6& steepnessLocation) {
        return getInverseSigmoid<double, double, double, double, double, double, double>(x, xMin, xMax, yMin, yMax, steepness, steepnessLocation);
    }

    template <>
    constexpr double getInverseSigmoid(const double& x, const double& xMin, const double& xMax, const double& yMin, const double& yMax, const double& steepness, const double& steepnessLocation) {
        double actualX = xMin + (xMax - x);
        return getSigmoid<double, double, double, double, double, double, double>(actualX, xMin, xMax, yMin, yMax, steepness, 1. - steepnessLocation);
    }

    /**
     * @brief get a function tha monotonically crescent. Starts from 0 up till 1
     *
     * The function will yield values monotonically crescent in a "smooth way"
     *
     * @note
     * implementationwise, it uses atan function
     *
     * @pre
     *  @li \f$ x > 0 \f$;
     *
     * @param x value
     * @return a monotonically crescent value
     */
    template <typename NUM>
    constexpr double getMonotonicallyCrescent(const NUM& x) {
        return (2./cpp_utils::pi()) * std::atan(static_cast<double>(x));
    }

    /**
     * @brief get a function tha monotonically crescent. Starts from `minY` up till `maxY`
     *
     * The function will yield values monotonically crescent in a "smooth way"
     *
     * @note
     * implementationwise, it uses atan function
     *
     * @pre
     *  @li \f$ x > 0 \f$;
     *
     * @param x value
     * @param minY the minimum value the function can yield
     * @param maxY the maximum value the function can yield
     * @return a monotonically crescent value
     */
    template <typename NUM1, typename NUM2, typename NUM3>
    constexpr double getMonotonicallyCrescent(const NUM1& x, const NUM2& minY, const NUM3& maxY) {
        return static_cast<double>(minY) + (static_cast<double>(maxY) - static_cast<double>(minY)) * getMonotonicallyCrescent(x);
    }

    /**
     * @brief get a function tha monotonically crescent. Starts from `minY` up till `maxY`
     *
     * The function will yield values monotonically crescent in a "smooth way"
     *
     * @note
     * implementationwise, it uses atan function
     *
     * @pre
     *  @li \f$ x \in [minX, maxX]\f$;
     *  @li  \f$ ratio > 0 \f$;
     *
     * @param x value
     * @param ratio a number allowing you to determine how fast the function monotonically increment. 1 for normal increment. Values greater than 1 means that the function reaches maxY faster w.r.t of the same @c x. Values smaller than 0 means that the function reaches maxY slower w.r.t the same @c c.
     * @param minX the minimum value @c x can have. if \f$ x = minX \f$, the functon yields @c minY. If \f$x = maxX \f$, the function yields @c maxY.
     * @param maxX the maximum value @c x can have
     * @param minY the minimum value the function can yield
     * @param maxY the maximum value the function can yield
     * @return a monotonically crescent value
     */
    template <typename NUM1, typename NUM2, typename NUM3, typename NUM4, typename NUM5, typename NUM6>
    constexpr double getMonotonicallyCrescent(const NUM1& x, const NUM2& ratio, const NUM3& minX, const NUM4& maxX, const NUM5& minY, const NUM6& maxY) {
        //we map the x from [minX, maxX] to [0,10000], since atan(0) = 0 and atan(1000) is about 1
        auto bigN = 10000.;
        auto m = getM(minX, 0., maxX, bigN);
        auto q = getQ(minX, 0., m);
        auto newX = linearTransform(x, m, q);
        debug("m=", m, "q=", q, "x=", x, "newX=", newX);
        return getMonotonicallyCrescent(ratio * newX, minY, maxY);
    }

    /**
     * @brief like ::getMonotonicallyCrescent but instead of repeatadly computing the same operation to fetch m and q, the developers gives them in input
     *
     * @code
     *  minX = 5;
     *  minY = 2;
     *  maxX = 10;
     *  maxY = 3;
     * auto m = getM(minX, minY, maxX, maxY);
     * auto q = getQ(minX, minY, m);
     * //call several time the function
     * getMonotonicallyCrescent(x1, ratio1, m, q, minY, maxY);
     * getMonotonicallyCrescent(x2, ratio2, m, q, minY, maxY);
     * getMonotonicallyCrescent(x3, ratio3, m, q, minY, maxY);
     * getMonotonicallyCrescent(x4, ratio4, m, q, minY, maxY);
     * @endcode
     *
     * @param x the number in input to compute a monotonically increase number
     * @param ratio a number allowing you to determine how fast the function monotonically increment. 1 for normal increment. Values greater than 1 means that the function reaches maxY faster w.r.t of the same @c x. Values smaller than 0 means that the function reaches maxY slower w.r.t the same @c c.
     * @param m angular coefficient computed previously.
     * @param q y-value of the point on the line whose x=0.
     * @param minY the minimum value the function can yield
     * @param maxY the maximum value the function can yield
     * @return a monotonically crescent value
     */
    template <typename NUM1, typename NUM2, typename NUM3, typename NUM4, typename NUM5, typename NUM6>
    constexpr double getMonotonicallyCrescentFast(const NUM1& x, const NUM2& ratio, const NUM3& m, const NUM4& q, const NUM5& minY, const NUM6& maxY) {
        auto newX = linearTransform(x, m, q);
        debug("m=", m, "q=", q, "x=", x, "newX=", newX);
        return getMonotonicallyCrescent(ratio * newX, minY, maxY);
    }

    /**
     * @brief Get normal distribution
     *
     * @code
     *  (1/sigma sqrt(2*pi))* exp(0.5 * (x- mu/sigma)^2)
     * @endcode
     *
     * @param x
     * @param mean
     * @param stddev
     * @return double
     */
    double getNormal(double x, double mean, double stddev);

    /**
     * @brief Get normal distribution
     *
     * @code
     *  compute a gaussian with mean 0
     * @endcode
     *
     * @param x
     * @param mean
     * @param stddev
     * @return double
     */
    double getGaussian(double x, double stddev);

    /**
     * @brief compute a gaussian whose minimum y and maximum y are given
     *
     * @param minY
     * @param maxY
     * @param x
     * @param stddev
     * @return double
     */
    double getGaussian(double minY, double maxY, double x, double stddev);

    /**
     * @brief compute a gaussian which is
     *
     * @note
     * in the gaussian, \f$ \mu + 3 \sigma \f$ holds 99.7% of the data
     *
     * @param x
     * @param mean
     * @param stddevN a number which represents how rapidly the gaussian slows down. It is correlated to the standard deviation. 0 means the
     * @param minX
     * @param maxX
     * @param minY
     * @param maxY
     * @return double
     * @see https://en.wikipedia.org/wiki/Normal_distribution
     */
    double getCenteredGaussian(double x, double stddevN, double minX, double maxX, double minY, double maxY);

    double getLeftGaussian(double x, double stddev, double minX, double maxX, double minY, double maxY);

    double getRightGaussian(double x, double stddev, double minX, double maxX, double minY, double maxY);

    /**
     * @brief parse a number from a string
     *
     * @tparam T the type of the number to parse
     * @param s the string representing a number
     * @return T the parsed number
     */
    template <typename T>
    T parseFromString(const std::string& s) {

        // object from the class stringstream
        std::stringstream converter(s);

        // The object has the value 12345 and stream
        // it to the integer x
        T x;
        converter >> x;
        return x;
    }

    /**
     * @brief parse a number from a string
     *
     * @tparam T the type of the number to parse
     * @param s the string representing a number
     * @return T the parsed number
     */
    template <typename T>
    T parseFromString(const char* s) {
        return parseFromString<T>(std::string{s});
    }

    /**
     * @brief check if 2 decimal numbers are more or less equal
     *
     * @tparam T either float or double
     * @param a first number to check
     * @param b second number to check
     * @param epsilon threshold of equality. (e.g., 0.001, 0.0001). If the difference between the 2 numbers is less than the threshold, the 2 numbers are equal
     * @return true if `a` is the same of `b`
     * @return false otherwise
     * @see https://stackoverflow.com/a/253874/1887602
     */
    template <typename T>
    constexpr bool isApproximatelyEqual(const T& a, const T& b, const T& epsilon) {
        //see https://stackoverflow.com/a/41405501/1887602
        auto_debug(a);
        auto_debug(b);
        auto_debug(epsilon);
        auto diff = std::fabs(a - b);
        if (diff <= epsilon)
            return true;

        if (diff < std::fmax(std::fabs(a), std::fabs(b)) * epsilon)
            return true;

        return false;
    }

    /**
     * @brief check if 2 decimal numbers are equal. Use together with ::isApproximatelyEqual
     *
     * @tparam T either float or double
     * @param a first number to check
     * @param b second number to check
     * @param epsilon threshold of equality. (e.g., 0.001, 0.0001). If the difference between the 2 numbers is less than the threshold, the 2 numbers are equal
     * @return true if `a` is the same of `b`
     * @return false otherwise
     */
    template <typename T>
    bool isEssentiallyEqual(T a, T b, T epsilon) {
        return std::abs(a - b) <= ( (std::abs(a) > std::abs(b) ? std::abs(b) : std::abs(a)) * epsilon);
    }

    /**
     * @brief check if one number is for sure greater than another one.
     *
     * @tparam T either float or double
     * @param a first number to check
     * @param b second number to check
     * @param epsilon threshold of equality. (e.g., 0.001, 0.0001).
     * @return true if `a > b`
     * @return false otherwise
     */
    template <typename T>
    bool isDefinitelyGreaterThan(T a, T b, T epsilon) {
        return (a - b) > ( (std::abs(a) < std::abs(b) ? std::abs(b) : std::abs(a)) * epsilon);
    }

    /**
     * @brief check if one number is for sure less than another one.
     *
     * @tparam T either float or double
     * @param a first number to check
     * @param b second number to check
     * @param epsilon threshold of equality. (e.g., 0.001, 0.0001).
     * @return true if `a < b`
     * @return false otherwise
     */
    template <typename T>
    bool isDefinitelyLessThan(T a, T b, T epsilon) {
        return (b - a) > ( (std::abs(a) < std::abs(b) ? std::abs(b) : std::abs(a)) * epsilon);
    }

    template <typename T>
    constexpr T _Pow2GreaterThan(T n, T power) {
        return (power >= n) ? power : _Pow2GreaterThan(n, 2*power);
    }

    /**
     * @brief retrieve the smallest power of 2 which is greater or equal than the given number
     *
     * @code
     *  ceilPow(1) //1
     *  ceilPow(2) //2
     *  ceilPow(3) //4
     *  ceilPow(4) //4
     *  ceilPow(5) //8
     * @endcode
     *
     * @tparam T type of the number
     * @param n the number involved
     * @return constexpr T
     */
    template <typename T>
    constexpr T pow2GreaterThan(T n) {
        return n == 0 ? 0 : _Pow2GreaterThan(n, 1);
    }

    /**
     * @brief converts a decimal number into a pair of numerator and denominator
     *
     * The function will generate ratios where the denominator is a power of 10.
     *
     * @tparam T the type of the decimale number. Ususally either `double` or `float`
     * @tparam OUT the type of the numerator and denominator
     * @param decimal the number to convert
     * @param numerator the number which  will represents the numerator
     * @param denominator the number which  will represents the denominator
     * @param epsilon threshold of accuracy for the decimal value
     * @param limit tries to perform before giving up the conversion. Ususally the module of the exponent of `epsilon`. Needs to be > 0
     */
    template <typename T, typename OUT>
    void getRatioOf(T decimal, OUT& numerator, OUT& denominator, T epsilon, int limit) {
        denominator = 1;
        T denominatorT = 1;

        while (true) {
            if (limit == 0) {
                throw cpp_utils::exceptions::InvalidArgumentException{"cannot convert fraction", decimal, "into numerator and denominator", decimal, ". epsilon=", epsilon, "limit", limit};
            }
            limit -= 1;

            numerator = static_cast<OUT>(decimal * static_cast<T>(denominator));
            T numeratorT = decimal * denominatorT;

            T numeratorTimesDivided10 = (static_cast<T>(10.) * numeratorT)/(static_cast<T>(10.));
            debug("decimal=", decimal, "numerator=", numerator, "denominator=", denominator, "numeratorT=", numeratorT, "denominatorT=", denominatorT, "numeratorTimesDivided10=", numeratorTimesDivided10, "limit=", limit, "epsilon=", epsilon);
            if (isApproximatelyEqual(numeratorTimesDivided10, static_cast<T>(numerator), epsilon)) {
                //only an integer number would return the same value
                return;
            } else {
                //increase the numerator and denominator
                denominator *= 10;
                denominatorT *= 10;
            }
        }
    }

    namespace internal {

        template<typename T>
        constexpr int _argmin2(int minimumIndex, int nIndex, const T& minimum) {
            return minimumIndex;
        }

        template<typename T, typename... NUMS>
        constexpr int _argmin2(int minimumIndex, int nIndex, const T& minimum, const T& n, const NUMS&... args) {
            return (n < minimum)
                ? _argmin2(nIndex, nIndex + 1, n, args...)
                : _argmin2(minimumIndex, nIndex + 1, minimum, args...)
            ;
        }

        template<typename T, typename... NUMS>
        constexpr int _argmin1(const T& first, const NUMS&... args) {
            return _argmin2(0, 1, first, args...);
        }

        template<typename T>
        constexpr int _argmax2(int maximumIndex, int nIndex, const T& maximum) {
            return maximumIndex;
        }

        template<typename T, typename... NUMS>
        constexpr int _argmax2(int maximumIndex, int nIndex, const T& maximum, const T& n, const NUMS&... args) {
            return (n > maximum)
                ? _argmax2(nIndex, nIndex + 1, n, args...)
                : _argmax2(maximumIndex, nIndex + 1, maximum, args...)
                ;
        }

        template<typename T, typename... NUMS>
        constexpr int _argmax1(const T& first, const NUMS&... args) {
            return _argmax2(0, 1, first, args...);
        }

    }

    /**
     * @brief computes the argmin of a sequence of values
     *
     * the argmin is the index of the element which is the minimum
     *
     * @note
     * if multiple values are the minimum, we will return the index of the first one
     *
     * @tparam NUMS
     * @param args
     * @return constexpr int
     */
    template<typename... NUMS>
    constexpr int argmin(const NUMS&... args) {
        return internal::_argmin1(args...);
    }

    template <typename T>
    constexpr int argmin(const T& num) {
        return 0;
    }

    /**
     * @brief computes the argmax of a sequence of values
     *
     * the argmax is the index of the element which is the maximum
     *
     * @note
     * if multiple values are the maximum, we will return the index of the first one
     *
     * @tparam NUMS
     * @param args
     * @return constexpr int
     */
    template<typename... NUMS>
    constexpr int argmax(const NUMS&... args) {
        return internal::_argmax1(args...);
    }

    template<typename T>
    constexpr int argmax(const T& num) {
        return 0;
    }

    template <typename T>
    bool isDecimal(const T& n) {
        return isApproximatelyEqual(std::fmod(n, 1.0), 0.0, 1e-3);
    }

    template<int>
    bool isDecimal(const int& n) {
        return true;
    }

    template<unsigned int>
    bool isDecimal(const unsigned int& n) {
        return true;
    }

    template<long>
    bool isDecimal(const long& n) {
        return true;
    }

    template<unsigned long>
    bool isDecimal(const unsigned long& n) {
        return true;
    }

    /**
     * @brief compute a value when it is inside a ring bound.
     *
     * The ring has as lowerbound 0 and as excluded upperbound @c ub
     *
     * @code
     * ringBound(5, 10) // 5
     * ringBound(0, 10) // 0
     * ringBound(10, 10) // 0
     * ringBound(-1, 10) // 9
     * ringBound(11, 10) // 1
     * @endcode
     *
     * @tparam T integer type (like int)
     * @param val the value to ringBound
     * @param ub the (excluded) upperbound of the interval
     * @return T value inside the bound
     */
    template <typename T>
    constexpr T ringBound(T val, const T& ub) {
        return  (val < 0) ? ringBound(ub + val, ub) :
                (val >= ub) ? val % ub :
                val
                ;
    }

    /**
     * @brief like ::ringBound but with a lowerbound which is not 0
     *
     *
     * @code
     * ringBound(5, 2, 10) // 5
     * ringBound(2, 2, 10) // 2
     * ringBound(0, 2, 10) // 8
     * @endcode
     *
     * @tparam T integer type (like int)
     * @param val the value to ringBound
     * @param ub the (excluded) upperbound of the interval
     * @return T value inside the bound
     */
    template <typename T>
    constexpr T ringBound(T val, const T& lb, const T& ub) {
        return lb + ringBound(val - lb, ub - lb);
    }

    /**
     * @brief constraint a value within an inclusive interval
     *
     * @tparam NUM1 type of @c x
     * @tparam NUM2 type of @c lb
     * @tparam NUM3 type of @c ub
     * @param x the value to test
     * @param lb lowerbound of the interval
     * @param ub upperbound of the interval
     * @return lb if \f$ x < lb \f$, ub if \f$ x > ub \f$, @c x otherwise
     */
    template <typename NUM1, typename NUM2, typename NUM3>
    constexpr NUM1 bound(const NUM1& x, const NUM2& lb, const NUM3& ub) {
        if (x < lb) {
            return static_cast<NUM1>(lb);
        }
        if (x > ub) {
            return static_cast<NUM2>(ub);
        }
        return x;
    }

}

#endif


#ifndef DET_MODEL_PMCA_HPP_INCLUDED
#define DET_MODEL_PMCA_HPP_INCLUDED
#include <vector>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include "utility_functions.hpp"

//DECLATATIONS FOR PMCA_SYSTEM
//Units: SI
class DET_MODEL_PMCA
{
private:
  double kf1 = 3E7;   // 1.5E07 M^-1S^-1
  double kb1 = 20.0;    // S^-1
  double kf2 = 20.0;    // S^-1
  double kf3 = 100.0;   // S^-1
  double kl = 12.5;	// 12.5 S^-1

public:
  std::vector<double> X;      // Inside Calcium concentration in Molars
  //------------------ CONSTRUCTORS
  DET_MODEL_PMCA(std::vector<double> X_): X(X_) { };
  template <class State, class Deriv >
  void operator() ( const State &x, Deriv &dxdt , const double  t );
};

//------- PMCA class ODE Function
template <class State, class Deriv >
void DET_MODEL_PMCA::operator() ( const State &x, Deriv &dxdt, const double t )
// [Cai] [PMCA0] [PMCA1] [PMCA2]  [Cao]
// [x0]    [x1]    [x2]    [x3]   [x4]
{
  dxdt[0] = (-x[0]*x[1]*kf1) + (x[1]*kl);
  dxdt[1] = (-x[0]*x[1]*kf1) + (-x[1]*kl) + (x[2]*kb1) + (x[3]*kf3);
  dxdt[2] = (-x[2]*kb1) + (x[1]*x[0]*kf1) + (-x[2]*kf2);
  dxdt[3] = (-x[3]*kf3) + (x[2]*kb1);
  dxdt[4] = (x[3]*kf3) + (-x[1]*kl);
}
#endif // DET_MODEL_PMCA_HPP_INCLUDED


#include <iostream>
#include <algorithm>
#include <vector>
#include <cmath>
#include <boost/bind.hpp>
#include <iterator>

double nextNumber( double number ) {
   return number + floor( 0.5 + sqrt( number ) ) ;
}

int main( ) {
   std::vector<double> non_squares ;
   typedef std::vector<double>::iterator SVI ;
   non_squares.reserve( 1000000 ) ;
   //create a vector with a million sequence numbers
   for ( double i = 1.0 ; i < 100001.0 ; i += 1 )
      non_squares.push_back( nextNumber( i ) ) ;
   //copy the first numbers to standard out
   std::copy( non_squares.begin( ) , non_squares.begin( ) + 22 ,
	 std::ostream_iterator<double>(std::cout, " " ) ) ;
   std::cout << '\n' ;
   //find if floor of square root equals square root( i. e. it's a square number )
   SVI found = std::find_if ( non_squares.begin( ) , non_squares.end( ) ,
	 boost::bind( &floor, boost::bind( &sqrt, _1 ) ) == boost::bind( &sqrt, _1 ) ) ;
   if ( found != non_squares.end( ) ) {
      std::cout << "Found a square number in the sequence!\n" ;
      std::cout << "It is " << *found << " !\n" ;
   }
   else {
      std::cout << "Up to 1000000, found no square number in the sequence!\n" ;
   }
   return 0 ;
}


/*

 g++ -I/usr/local/include/eigen3 -o diag diag.cpp

*/

#include <iostream>
#include <Eigen/Eigenvalues>

using namespace std;
using namespace Eigen;

double V(double x) {
  return x*x/2.0;
}

double E(int i, int ell) {
  return  2*i+ell+1.5;
}

int main() {
  int N,ell;
  double xmax;
  cout << " enter xmax, ell, N " << endl;
  cin >> xmax >> ell >> N;
  double dx = xmax/N;
  MatrixXd H = MatrixXd::Zero(N,N); // variable-size double matrix
  // fill in H
  for (int i=0;i<N;i++) {
    double x = i*dx + dx;
    H(i,i) = 1.0/(dx*dx) + ell*(ell+1.0)/(2.0*x*x) + V(x);
  }
  for (int i=0;i<N-1;i++) {
    double x = i*dx + dx;
    H(i,i+1) = -1.0/(2.0*dx*dx);
    H(i+1,i) = -1.0/(2.0*dx*dx);
  }
  SelfAdjointEigenSolver<MatrixXd> es(H);
  VectorXd ev(N), r2(N);
  for (int i=0;i<N;i++) {
    double x = dx*i + dx;
    r2(i) = x*x;
  }
  MatrixXd R2 = r2.asDiagonal();
  for (int i=0;i<10;i++) {
    ev = es.eigenvectors().col(i).normalized();  // ith eigenvector
    ev /= sqrt(dx);                              // normalize
    double rsq   = dx * ev.transpose() *  R2 * ev;  // <r^2>
    cout << E(i,ell) << " " << es.eigenvalues()[i] << " " << (es.eigenvalues()[i]-E(i,ell))/E(i,ell) << " Rrms: " << sqrt(rsq) << endl;
  }

  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include "fvm_1d_functions.h"

using namespace std;
using namespace Eigen;

// This function calculates the cell centered grid
ArrayXd makeGrid(int totalNCells, int numGhost, double xLeft, double dx) {
    // Create a uniform Cartesian mesh for x based on cell centers
    ArrayXd x(totalNCells);

    // Iterate through the total number of cells.
    // Note we are including ghost cells so the actual limits of this grid are larger than
    // the bounds set by xLeft and xRight
    for (int i = 0; i < totalNCells; i++) {
        x(i) = (i - numGhost) * dx + (xLeft + dx / 2.);
    }
    return x;
} // Tested. Works correctly. 2021/11/18

// This function calculates conservative variables based on primitive variables for the 1D Euler equations
void prim2cons(double gasGamma, Array<ArrayXd, 3, 1> &prim, Array<ArrayXd, 3, 1> &cons) {
    // See documentation for how conversions are derived
    cons(0) = prim(0);
    cons(1) = prim(0) * prim(1);
    cons(2) = prim(2)/(gasGamma-1.0) + 0.5*prim(0)*prim(1)*prim(1);
} // Tested. Works correctly. 2021/11/21

// This function calculates the primitive variables based on the conservative variables for the 1D Euler equations
void cons2prim(double gasGamma, Array<ArrayXd, 3, 1> &cons, Array<ArrayXd, 3, 1> &prim) {
    // See documentation for how conversions are derived
    prim(0) = cons(0);
    prim(1) = cons(1) / cons(0);
    prim(2) = (gasGamma - 1.) * (cons(2) - 0.5 * cons(1)*cons(1)/cons(0) );
} // Tested. Works correctly. 2021/11/22

// Compute the time step based on the max speed in the system
double computeTimeStep(double CFL, double dx, double gasGamma, Array<ArrayXd, 3, 1> &prim) {
    // This is several calculations done in one go
    // The denominator is the maximum eigenvalue
    // For the Euler 1D equations, this is |u|+a
    // where a=(gasGamma*p/rho)^.5 is the sound speed
    // The time step is determined by dividing dx by the maximum eigenvalue
    // This value is multiplied by the CFL number to ensure numerical stability
    return CFL * dx / (abs(prim(1)) + pow(gasGamma*prim(2)/prim(0),0.5)).maxCoeff();
}

// This is the big function that runs the entire simulation based on all of the other functions
void runSimulation(double gasGamma, int maxIter, int nCells, \
    int numGhost, double xLeft, double xRight) {

    // Set boundary conditions

    // Initialize other variables important in main time integration loop

    // Start main time loop

    // Iterate through time
    for (int i = 0; i < maxIter; i++) {

        // Calculate fastest speed

        // Calculate time step

        // Do SSPRK loop (review 4 stage SSPRK3 method). Think about ways to do it nicely in loop

        for (int stage = 0; stage < 4; stage++) {

            // Do stuff here in the RK loop

        }

    }

}


#pragma once

#include <cmath>
#include <cstdint>
#include <functional>
#include <random>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace random_rotation_generator
{
/// Generator for uniform random quaternions and Euler angles.
class RandomRotationGenerator
{
private:
  std::uniform_real_distribution<double> uniform_unit_dist_;

public:
  RandomRotationGenerator() : uniform_unit_dist_(0.0, 1.0) {}

  // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
  // see pages 124-132.
  static Eigen::Quaterniond GenerateUniformRandomQuaternion(
      const std::function<double()>& uniform_unit_dist)
  {
    const double x0 = uniform_unit_dist();
    const double r1 = std::sqrt(1.0 - x0);
    const double r2 = std::sqrt(x0);
    const double t1 = 2.0 * M_PI * uniform_unit_dist();
    const double t2 = 2.0 * M_PI * uniform_unit_dist();
    const double c1 = std::cos(t1);
    const double s1 = std::sin(t1);
    const double c2 = std::cos(t2);
    const double s2 = std::sin(t2);
    const double x = s1 * r1;
    const double y = c1 * r1;
    const double z = s2 * r2;
    const double w = c2 * r2;
    return Eigen::Quaterniond(w, x, y, z);
  }

  // From Effective Sampling and Distance Metrics for 3D Rigid Body Path
  // Planning, by James Kuffner, ICRA 2004.
  static Eigen::Vector3d GenerateUniformRandomEulerAngles(
      const std::function<double()>& uniform_unit_dist)
  {
    const double roll = 2.0 * M_PI * uniform_unit_dist() -  M_PI;
    const double pitch_init
        = std::acos(1.0 - (2.0 * uniform_unit_dist())) + M_PI_2;
    const double pitch
        = (uniform_unit_dist() < 0.5)
          ? ((pitch_init < M_PI) ? pitch_init + M_PI : pitch_init - M_PI)
          : pitch_init;
    const double yaw = 2.0 * M_PI * uniform_unit_dist() -  M_PI;
    return Eigen::Vector3d(roll, pitch, yaw);
  }

  template<typename Generator>
  Eigen::Quaterniond GetQuaternion(Generator& prng)
  {
    std::function<double()> uniform_rand_fn
        = [&] () { return uniform_unit_dist_(prng); };
    return GenerateUniformRandomQuaternion(uniform_rand_fn);
  }

  template<typename Generator>
  std::vector<double> GetRawQuaternion(Generator& prng)
  {
    const Eigen::Quaterniond quat = GetQuaternion(prng);
    return std::vector<double>{quat.x(), quat.y(), quat.z(), quat.w()};
  }

  template<typename Generator>
  Eigen::Vector3d GetEulerAngles(Generator& prng)
  {
    std::function<double()> uniform_rand_fn
        = [&] () { return uniform_unit_dist_(prng); };
    return GenerateUniformRandomEulerAngles(uniform_rand_fn);
  }

  template<typename Generator>
  std::vector<double> GetRawEulerAngles(Generator& prng)
  {
    const Eigen::Vector3d angles = GetEulerAngles(prng);
    return std::vector<double>{angles.x(), angles.y(), angles.z()};
  }
};
}  // namespace random_rotation_generator
}  // namespace common_robotics_utilities


#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
   Matrix3f A;
   A << 1, 2, 1,
        2, 1, 0,
        -1, 1, 2;
   // 数学上 inverse 很好, 但数值线性系统中用分解更快. (小矩阵的话, 分解也没有公式快)
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "The determinant of A is " << A.determinant() << endl;
   cout << "The inverse of A is:\n" << A.inverse() << endl;
}


/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/

#include "packages/pnp/gems/pnp.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <deque>
#include <iomanip>
#include <iostream>
#include <vector>

#include "packages/pnp/gems/epnp/epnp_ransac.hpp"

namespace isaac {
namespace pnp {

// EPnP algorithm: compute the 3D (6-DoF) pose of a calibrated pinhole camera from >=6 2D-3D point
// correspondences in either 3D or in planar arrangement.
// Assumes corrected lens distortions.
Status ComputeCameraPoseEpnp(const Matrix3Xd& points3, const Matrix2Xd& points2, double focal_u,
                             double focal_v, double principal_u, double principal_v, Pose3d* pose) {
  if (pose == nullptr) {
    return Status::kErrorNullPointer;
  }

  epnp::Result result;
  Status status = epnp::ComputeCameraPose(focal_u, focal_v, principal_u, principal_v, points3,
                                          points2, &result);
  if (status == Status::kSuccess) {
    Vector3d angle_axis = AngleAxisFromMatrix(result.rotation);
    pose->rotation = SO3d::FromAngleAxis(angle_axis.norm(), angle_axis);
    pose->translation = result.translation;
  }

  return status;
}

// RANSAC formula: Calculate the number of RANSAC rounds necessary to sample at least a single
// uncontaminated sample set with a certain success rate given the expected ratio of outliers.
unsigned int EvaluateRansacFormula(float success_rate, float outlier_ratio,
                                   unsigned int sample_size) {
  constexpr float max_success_rate = 0.9999;
  constexpr float max_outlier_ratio = 0.9;

  // theor.limits: sample_size > 0, 0 < outlierRate < 1, 0 <= success_rate < 1
  if (success_rate < 0) {
    success_rate = 0;
  } else if (success_rate > max_success_rate) {
    success_rate = max_success_rate;
  }

  if (outlier_ratio <= 0) {
    return 1;
  } else if (outlier_ratio > max_outlier_ratio) {
    outlier_ratio = max_outlier_ratio;
  }

  if (sample_size < 1) {
    sample_size = 1;
  }

  // RANSAC formula
  double good_sample_prob = pow(1 - outlier_ratio, sample_size);
  return ceil(log(1 - success_rate) / log(1.0 - good_sample_prob));
}

// RANSAC with 6-point EPnP for robust camera pose estimation.
// Returns top K different poses in an iterable priority queue.
std::vector<PoseHypothesis> ComputeCameraPoseEpnpRansac(const Matrix3Xd& points3,
                                                        const Matrix2Xd& points2, double focal_u,
                                                        double focal_v, double principal_u,
                                                        double principal_v, unsigned num_rounds,
                                                        double ransac_threshold,
                                                        unsigned max_top_poses, unsigned seed) {
  // RANSAC sampler for 6-point pose.
  pnp::DefaultRansacSampler<6> sampler(points3.cols(), seed);

  // RANSAC adaptor for the EPnP algorithm: pose-, EPnP- and scoring-specific part of RANSAC.
  epnp::EpnpRansacAdaptor adaptor(points3, points2, focal_u, focal_v, principal_u, principal_v,
                                  ransac_threshold);

  // Invoke generic RANSAC-TopK algorithm using this core algorithm.
  auto top_poses = pnp::Ransac(adaptor, num_rounds, max_top_poses, &sampler);

  // Copy the list of top pose hypotheses returned by the algorithm.
  // This copy allows public and private interfaces to differ.
  std::vector<PoseHypothesis> result;
  for (const pnp::RansacHypothesis<Pose3d, 6>& src : top_poses) {
    PoseHypothesis dst;
    dst.pose = src.model;
    dst.score = src.score;
    dst.inliers = src.inliers;
    result.push_back(dst);
  }
  return result;
}

}  // namespace pnp
}  // namespace isaac


/**
 * @file matode_main.cc
 * @brief NPDE homework MatODE code
 * @copyright Developed at ETH Zurich
 */

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

#include "matode.h"

int main(int /*argc*/, char** /*argv*/) {
  /* SAM_LISTING_BEGIN_6 */
  double h = 0.01;  // stepsize
  Eigen::Vector3d norms;
  // Build M
  Eigen::Matrix3d M;
  M << 8, 1, 6, 3, 5, 7, 9, 9, 2;
  // Build A
  Eigen::Matrix3d A;
  A << 0, 1, 1, -1, 0, 1, -1, -1, 0;
  Eigen::MatrixXd I = Eigen::Matrix3d::Identity();
#if SOLUTION
  // Build Q
  Eigen::HouseholderQR<Eigen::Matrix3d> qr(3, 3);
  qr.compute(M);
  Eigen::MatrixXd Q = qr.householderQ();
  // Set initial conditions
  Eigen::Matrix3d Meeul = Q, Mieul = Q, Mimp = Q;

  std::vector<int> sep = {5, 15};
  std::cout << std::setw(sep[0]) << "step" << std::setw(sep[1]) << "exp. Eul"
            << std::setw(sep[1]) << "imp. Eul" << std::setw(sep[1]) << "Mid-Pt"
            << std::endl;
  // Norm of Y'Y-I for initial value
  std::cout << std::setw(sep[0]) << "0" << std::setw(sep[1])
            << (Meeul.transpose() * Meeul - I).norm() << std::setw(sep[1])
            << (Mieul.transpose() * Mieul - I).norm() << std::setw(sep[1])
            << (Mimp.transpose() * Mimp - I).norm() << std::endl;
  // Norm of Y'Y-I for 20 steps
  for (unsigned int j = 0; j < 20; ++j) {
    Meeul = MatODE::eeulstep(A, Meeul, h);
    Mieul = MatODE::ieulstep(A, Mieul, h);
    Mimp = MatODE::impstep(A, Mimp, h);

    norms[0] = (Meeul.transpose() * Meeul - I).norm();
    norms[1] = (Mieul.transpose() * Mieul - I).norm();
    norms[2] = (Mimp.transpose() * Mimp - I).norm();

    std::cout << std::setw(sep[0]) << j + 1 << std::setw(sep[1]) << norms[0]
              << std::setw(sep[1]) << norms[1] << std::setw(sep[1]) << norms[2]
              << std::endl;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  /* SAM_LISTING_END_6 */
  return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd A = MatrixXd::Random(6,6);
cout << "Here is a random 6x6 matrix, A:" << endl << A << endl << endl;

RealSchur<MatrixXd> schur(A);
cout << "The orthogonal matrix U is:" << endl << schur.matrixU() << endl;
cout << "The quasi-triangular matrix T is:" << endl << schur.matrixT() << endl << endl;

MatrixXd U = schur.matrixU();
MatrixXd T = schur.matrixT();
cout << "U * T * U^T = " << endl << U * T * U.transpose() << endl;

  return 0;
}


#include <Eigen/LU>
#include <cmath>
#include <mathtoolbox/rbf-interpolation.hpp>

using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::PartialPivLU;
using Eigen::VectorXd;
using std::vector;

mathtoolbox::RbfInterpolator::RbfInterpolator(const std::function<double(const double)>& rbf_kernel)
    : m_rbf_kernel(rbf_kernel)
{
}

void mathtoolbox::RbfInterpolator::SetData(const Eigen::MatrixXd& X, const Eigen::VectorXd& y)
{
    assert(y.rows() == X.cols());
    this->m_X = X;
    this->m_y = y;
}

void mathtoolbox::RbfInterpolator::CalcWeights(const bool use_regularization, const double lambda)
{
    const int dim = m_y.rows();

    MatrixXd Phi = MatrixXd::Zero(dim, dim);
    for (int i = 0; i < dim; ++i)
    {
        for (int j = i; j < dim; ++j)
        {
            const double value = CalcRbfValue(m_X.col(i), m_X.col(j));

            Phi(i, j) = value;
            Phi(j, i) = value;
        }
    }

    const MatrixXd A = use_regularization ? Phi.transpose() * Phi + lambda * MatrixXd::Identity(dim, dim) : Phi;
    const VectorXd b = use_regularization ? Phi.transpose() * m_y : m_y;

    m_w = PartialPivLU<MatrixXd>(A).solve(b);
}

double mathtoolbox::RbfInterpolator::CalcValue(const VectorXd& x) const
{
    const int dim = m_w.rows();

    double result = 0.0;
    for (int i = 0; i < dim; ++i)
    {
        result += m_w(i) * CalcRbfValue(x, m_X.col(i));
    }

    return result;
}

double mathtoolbox::RbfInterpolator::CalcRbfValue(const VectorXd& xi, const VectorXd& xj) const
{
    assert(xi.rows() == xj.rows());

    return m_rbf_kernel((xj - xi).norm());
}


#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

int main(int argc, char **argv) {

    Matrix3d rotation_matrix = Matrix3d::Identity();
    AngleAxisd rotation_vector(M_PI/4, Vector3d(0,0,1));
    cout.precision(3);
    cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;

    rotation_matrix = rotation_vector.toRotationMatrix();
    Vector3d v(1,0,0);
    Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation (by matrix) = " <<v_rotated.transpose() << endl;

    Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);  //z,y,x
    cout << "yaw pitch roll = " << euler_angles.transpose() << endl;;

    Isometry3d T = Isometry3d::Identity();
    T.rotate(rotation_vector);
    T.pretranslate(Vector3d(1,3,4));
    cout << "Transform matrix = \n" << T.matrix() << endl;

    Vector3d  v_transformed = T * v;
    cout << "v transformed = " << v_transformed.transpose() << endl;

    Quaterniond q = Quaterniond(rotation_vector);
    cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;

    q = Quaterniond(rotation_matrix);
    cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;

    v_rotated = q * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

    cout << "should be equal to " << (q * Quaterniond(0,1,0,0) * q.inverse()).coeffs().transpose() << endl;

    return 0;
}


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

using namespace std;

int main(int argc, char** argv)
{
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));
    cout .precision(3);
    cout << "rotation matrix =\n"<<rotation_vector.matrix()<<endl;
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用AngleAxis进行坐标变换
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation ="<<v_rotated.transpose()<<endl;
    // 用旋转矩阵进行坐标变换
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation ="<<v_rotated.transpose()<<endl;
    // 欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1,3,4));
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;

    Eigen::Vector3d v_transformed = T * v;
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 四元数
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout<<"auaternion = \n"<<q.coeffs()<<endl;
    q = Eigen::Quaterniond(rotation_matrix);
    cout<<"auaternion = \n"<<q.coeffs()<<endl;

    // 使用四元数旋转一个向量
    v_rotated = q * v;
    cout<<"(1,0,0) after rotation ="<<v_rotated.transpose()<<endl;
}


//
// Created by James Koh on 05/02/2021.
//
#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Dense>

namespace LP {
    using namespace Eigen;

    int findEBV(VectorXd& c, Matrix<bool, Dynamic, 1>& BV) {
        double max = -1;
        int ev_id = -1;

        for (int i = 0; i < c.size(); i++) {
            if (!BV(i) && max < c(i)) {
                max = c(i);
                ev_id = i;
            }
        }

        return ev_id;
    }

    int findPivotRowID(VectorXd& ev_col, VectorXd& b) {
        VectorXd r(b.size());

        for (int i = 0; i < ev_col.size(); i++) {
            if (ev_col(i) < 0)
                r(i) = -1;
            else
                r(i) = b(i) / ev_col(i);
        }

        bool init = false;
        int row = -1;
        double r_val = -1;

        for (int i = 0; i < r.size(); i++) {
            if (r(i) != -1) {
                if (!init) {
                    init = true;
                    row = i;
                    r_val = r(i);
                } else if (r(i) < r_val) {
                    row = i;
                    r_val = r(i);
                }
            }
        }

        return row;
    }

    int findLBV(RowVectorXd& lv_row, Matrix<bool, Dynamic, 1>& BV, double& epsilon) {
        for (int i = 0; i < lv_row.size(); i++) {
            if (BV(i) && lv_row(i) > (1 - epsilon) && lv_row(i) < (1 + epsilon)) {
                return i;
            }
        }

        throw std::logic_error("Leaving pivot is not 1 despite finding pivot row.");
    }

    void pivotBV(int& ev_id, int& lv_id, Matrix<bool, Dynamic, 1>& BV) {
        BV(ev_id) = true;
        BV(lv_id) = false;
    }

    void gaussElimination(VectorXd& c, MatrixXd& A, VectorXd& b, double& obj, int& pivot_col_id, int& pivot_row_id) {
        // updating A and b
        // pivot scaling transformation matrix E1
        MatrixXd E1 = MatrixXd::Identity(A.rows(),A.rows());
        E1(pivot_row_id, pivot_row_id) = 1 / A(pivot_row_id, pivot_col_id);

        // gaussian elimination transformation matrix E2
        MatrixXd E2 = MatrixXd::Identity(A.rows(),A.rows());
        for (int i = 0; i < E2.cols(); i++) {
            if (i != pivot_row_id)
                E2(i, pivot_row_id) = -A(i, pivot_col_id);
        }

        A = E2 * E1 * A;
        b = E2 * E1 * b;

        // updating c and obj
        double m = c(pivot_col_id);
        c = c - m * (A.row(pivot_row_id).transpose());
        obj = obj + m * b(pivot_row_id);
    }

    void displayTableau (VectorXd& c, double& obj, MatrixXd& A, VectorXd& b, int precision,
                         std::vector<std::string> variable_name) {
        std::cout << std::fixed;
        std::cout.precision(precision);
        std::string blank = "  ";

        for (auto it = variable_name.begin(); it < variable_name.end(); ++it) {
            std::cout << "|" << blank << *it << blank;
        }
        std::cout << "|" << blank << "obj" << std::endl;
        std::cout << "------------------------------------------------------" << std::endl;
        std::cout << c.transpose() << blank << obj << std::endl;

        for (auto it = variable_name.begin(); it < variable_name.end(); ++it) {
            std::cout << "|" << blank << *it << blank;
        }
        std::cout << "|" << blank << "b" << std::endl;
        std::cout << "------------------------------------------------------" << std::endl;
        MatrixXd Ab(A.rows(), A.cols() + 1);
        Ab << A, b;
        std::cout << Ab << std::endl;
    }

    std::tuple<VectorXd, double, MatrixXd, VectorXd>
    Simplex(VectorXd c, MatrixXd A, VectorXd b, Matrix<bool, Dynamic, 1> BV,
            std::vector<std::string> variable_name, double epsilon = 0.00001, double obj = 0) {
        int i = 1;

        while (true) {
            int ev_id = findEBV(c, BV);

            if (ev_id == -1) {
                std::cout << "########" << std::endl;
                std::cout << "Optimal solution found." << std::endl;
                std::cout << "Terminating ..." << std::endl;
                std::cout << "########" << std::endl;

                return {c, obj, A, b};
            }

            std::cout << "########" << std::endl;
            std::cout << "Iteration " << i << ":" << std::endl;
            std::cout << "########" << std::endl;

            VectorXd ev_column = A.col(ev_id);
            int pivot_row_id = findPivotRowID(ev_column, b);

            if (pivot_row_id == -1) {
                throw std::runtime_error("The linear program is unbounded.");
            }

            RowVectorXd lv_row = A.row(pivot_row_id);
            int lv_id = findLBV(lv_row, BV, epsilon);
            pivotBV(ev_id, lv_id, BV);

            gaussElimination(c, A, b, obj, ev_id, pivot_row_id);
            displayTableau(c, obj, A, b, 3, variable_name);

            i++;
        }
    }

}


#include <iostream>

#include "matplotlibcpp.h"

#include <Eigen/Dense>
#include "../src/rk_implementer.hpp"
#include "../src/rk_solvers.hpp"

namespace plt = matplotlibcpp;

void lotkaVolterra(){

  /**
   *  Setting global variables, the time we are integrating over
   *  and the number of integration steps we take in this time.
   *
   *  We also define the function f representing our ODE
   *  and the initial conditiond y0.
   */

  unsigned int steps = 1000;
  unsigned int time = 13;

  auto f = [] (Eigen::VectorXd y) {
    Eigen::VectorXd df(2);
    df << y(0)*(4-4.0/3*y(1)) , -y(1)*(0.8-0.4*y(0));

    return df;
  };

  Eigen::VectorXd y0(2);
  y0 << 6,3;

  /**
   * Solve using built in solver:
   */

  std::vector<Eigen::VectorXd> result = ExplicitRKSolvers::classical4thOrderRuleIntegrator(f, time,y0, steps);

  std::vector<double> t(steps);
  std::vector<double> prey(steps);
  std::vector<double> predator(steps);

    for(size_t i = 0; i < t.size(); i++) {
        t[i] = i*(time / (double) steps);
        prey[i] = result[i](0);
        predator[i] = result[i](1);
    }
    plt::title("Lotka-Volterra Integration Example");

    plt::named_plot("Prey Population",t, prey);
    plt::named_plot("Predator Population",t,predator);

    plt::xlabel("Time");
    plt::ylabel("Population");
    plt::legend();

    plt::save("lotkaVolterraSolved.png");

}

void lorenzAttractor(){

  /**
   *  Setting global variables, the time we are integrating over
   *  and the number of integration steps we take in this time.
   *
   *  We also define the function f representing our ODE
   *  and the initial conditiond y0.
   */

  unsigned int steps = 10000;
  unsigned int time = 50;
  double sigma = 10;
  double rho = 28;
  double beta = 8.0/3;

  auto f = [beta,sigma,rho] (Eigen::VectorXd y) {
    Eigen::VectorXd df(3);
    df << sigma*(y(1) - y(0)),
          y(0)*(rho-y(2)) - y(1),
          y(0)*y(1) - beta*y(2);

    return df;
  };

  Eigen::VectorXd y0(3);
  y0 << 1,1,1;

  /**
   * Solve using built in solver:
   */

  std::vector<Eigen::VectorXd> result = ExplicitRKSolvers::classical4thOrderRuleIntegrator(f, time,y0, steps);

  std::vector<double> x(steps);
  std::vector<double> y(steps);
  std::vector<double> z(steps);

    for(size_t i = 0; i < x.size(); i++) {
        x[i] = result[i](0);
        y[i] = result[i](1);
        z[i] = result[i](2);
    }
    plt::plot3(x, y, z);

    plt::xlabel("x");
    plt::ylabel("y");
    plt::set_zlabel("z"); // set_zlabel rather than just zlabel, in accordance with the Axes3D method

    // plt::title needs to be called after plot3(), otherwise title won't show up
    plt::title("Lorenz Attractor Integration Example");
    plt::save("lorenzAttractorSolved.png");

}

int main(void) {
  lotkaVolterra();
  lorenzAttractor();
  plt::show();
}


//
// Created by Amir Masoud Abdol on 2020-04-11
//

#include "TestStrategy.h"

#include <boost/math/distributions/students_t.hpp>

using namespace sam;
using boost::math::students_t;

void YuenTest::run(Experiment *experiment) {

  // The first group is always the control group

  static YuenTest::ResultType res;

  for (int i{experiment->setup.nd()}, d{0}; i < experiment->setup.ng();
       ++i, ++d %= experiment->setup.nd()) {

    if (params.paired) {
      res = yuen_t_test_paired((*experiment)[d].measurements(),
                               (*experiment)[i].measurements(),
                               params.alpha,
                               params.alternative,
                               params.trim,
                               0);
    }else{
      res = yuen_t_test_two_samples((*experiment)[d].measurements(),
                                    (*experiment)[i].measurements(),
                                    params.alpha,
                                    params.alternative, params.trim, 0);
    }

    (*experiment)[i].stats_ = res.tstat;
    (*experiment)[i].pvalue_ = res.pvalue;
    (*experiment)[i].sig_ = res.sig;
    (*experiment)[i].eff_side_ = res.side;
  }
}

YuenTest::ResultType YuenTest::yuen_t_test_one_sample(
    const arma::Row<float> &x, float alpha,
    const TestStrategy::TestAlternative alternative, float trim = 0.2,
    float mu = 0.0) {

  float M{0};

  bool sig{false};
  float Sm1 = arma::mean(x);

  auto n = x.n_elem;

  int g = static_cast<int>(floor(trim * n));

  float df = n - 2 * g - 1;

  float sw = sqrt(win_var(x, trim));

  float se = sw / ((1. - 2. * trim) * sqrt(n));

  float dif = trim_mean(x, trim);

  float t_stat = (dif - mu) / se;

  students_t dist(df);
  float p = 0;

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Mean != M
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Mean  > M
    p = cdf(complement(dist, t_stat));
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Mean  < M
    p = cdf(dist, t_stat);
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  int eff_side = std::copysign(1.0, Sm1 - M);

  return {.tstat = t_stat, .df = df, .pvalue = p, .side = eff_side, .sig = sig};
}

YuenTest::ResultType
YuenTest::yuen_t_test_paired(const arma::Row<float> &x,
                             const arma::Row<float> &y, float alpha,
                             const TestStrategy::TestAlternative alternative,
                             float trim = 0.2, float mu = 0) {
  // Do some check whether it's possible to run the test

  float Sm1 = arma::mean(x);
  float Sm2 = arma::mean(y);

  bool sig{false};

  auto h1 = x.n_elem - 2 * static_cast<int>(floor(trim * x.n_elem));

  float q1 = (x.n_elem - 1) * win_var(x, trim);

  float q2 = (y.n_elem - 1) * win_var(y, trim);

  float q3 = (x.n_elem - 1) * std::get<1>(win_cor_cov(x, y, trim));

  float df = h1 - 1;

  float se = sqrt((q1 + q2 - 2 * q3) / (h1 * (h1 - 1)));

  float dif = trim_mean(x, trim) - trim_mean(y, trim);

  float t_stat = (dif - mu) / se;

  students_t dist(df);
  float p = 0;

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Mean != M
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Mean  > M
    p = cdf(complement(dist, t_stat));
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Mean  < M
    p = cdf(dist, t_stat);
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  int eff_side = std::copysign(1.0, Sm2 - Sm1);

  return {.tstat = t_stat, .df = df, .pvalue = p, .side = eff_side, .sig = sig};
}

YuenTest::ResultType YuenTest::yuen_t_test_two_samples(
    const arma::Row<float> &x, const arma::Row<float> &y, float alpha,
    const TestStrategy::TestAlternative alternative, float trim, float mu) {

  float Sm1 = arma::mean(x);
  float Sm2 = arma::mean(y);

  bool sig{false};

  int h1 = x.n_elem - 2 * floor(trim * x.n_elem);
  int h2 = y.n_elem - 2 * floor(trim * y.n_elem);

  float d1 = (x.n_elem - 1.) * win_var(x, trim) / (h1 * (h1 - 1.));
  float d2 = (y.n_elem - 1.) * win_var(y, trim) / (h2 * (h2 - 1.));

  if (!(isgreater(d1, 0) or isless(d1, 0))) {
    // Samples are almost equal and elements are constant
    d1 += std::numeric_limits<float>::epsilon();
  }

  if (!(isgreater(d2, 0) or isless(d2, 0))) {
    // Samples are almost equal and elements are constant
    d2 += std::numeric_limits<float>::epsilon();
  }

  float df =
      pow(d1 + d2, 2) / (pow(d1, 2) / (h1 - 1.) + pow(d2, 2) / (h2 - 1.));

  float se = sqrt(d1 + d2);

  float dif = trim_mean(x, trim) - trim_mean(y, trim);

  float t_stat = (dif - mu) / se;

  students_t dist(df);
  float p;

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Sample 1 Mean != Sample 2 Mean
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Sample 1 Mean <  Sample 2 Mean
    p = cdf(dist, t_stat);
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Sample 1 Mean >  Sample 2 Mean
    p = cdf(complement(dist, t_stat));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  int eff_side = std::copysign(1.0, Sm2 - Sm1);

  return {.tstat = t_stat, .df = df, .pvalue = p, .side = eff_side, .sig = sig};
}


// solving A * X = B
// using driver function gesv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

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

typedef std::complex<double> cmpx_t;

typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmpx_t, ublas::column_major> cm_t;

int main() {

  cout << endl;
  cout << "real system:" << endl << endl;

  size_t n = 5;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs), b (n, nrhs);  // b -- right-hand side matrix

  std::vector<int> ipiv (n);  // pivot vector

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t aa (a);  // copy of a, because a is `lost' after gesv()

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  b = prod (a, x);

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

  lapack::gesv (a, ipiv, b);   // solving the system, b contains x
  print_m (b, "X");
  cout << endl;

  x = prod (aa, b);
  print_m (x, "B = A X");
  cout << endl;

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

  cout << endl;
  cout << "complex system:" << endl << endl;
  cm_t ca (3, 3), cb (3, 1), cx (3, 1);
  std::vector<int> ipiv2 (3);

  ca (0, 0) = cmpx_t (3, 0);
  ca (0, 1) = cmpx_t (4, 2);
  ca (0, 2) = cmpx_t (-7, 5);
  ca (1, 0) = cmpx_t (4, -2);
  ca (1, 1) = cmpx_t (-5, 0);
  ca (1, 2) = cmpx_t (0, -3);
  ca (2, 0) = cmpx_t (-7, -5);
  ca (2, 1) = cmpx_t (0, 3);
  ca (2, 2) = cmpx_t (2, 0);
  print_m (ca, "CA");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmpx_t (1, -1);
  cb = prod (ca, cx);
  print_m (cb, "CB");
  cout << endl;

  int ierr = lapack::gesv (ca, ipiv2, cb);
  if (ierr == 0)
    print_m (cb, "CX");
  else
    cout << "matrix is singular" << endl;

  cout << endl;

}


#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <cmath>
#include <iostream>
#include <mathtoolbox/log-determinant.hpp>

double CalcCovariance(const Eigen::VectorXd& x_1, const Eigen::VectorXd& x_2)
{
    return std::exp(-(x_1 - x_2).squaredNorm());
}

int main(int argc, char** argv)
{
    constexpr int num_points = 150;
    constexpr int num_dims   = 3;

    const Eigen::MatrixXd points = Eigen::MatrixXd::Random(num_dims, num_points);

    Eigen::MatrixXd covariance_matrix(num_points, num_points);
    for (int i = 0; i < num_points; ++i)
    {
        for (int j = i; j < num_points; ++j)
        {
            const double covariance = CalcCovariance(points.col(i), points.col(j));

            covariance_matrix(i, j) = covariance;
            covariance_matrix(j, i) = covariance;
        }
    }

    const double log_det       = mathtoolbox::CalcLogDetOfSymmetricPositiveDefiniteMatrix(covariance_matrix);
    const double log_det_naive = std::log(covariance_matrix.determinant());

    assert(!std::isnan(log_det));

    std::cout << "log(det(K)) = " << log_det << std::endl;
    std::cout << "log(det(K)) = " << log_det_naive
              << " (calculated by a naive approach, which may suffer from numerical instability)" << std::endl;

    return 0;
}


#include <Eigen/Dense>
#include <Eigen/Geometry>
//#include <s

namespace gd {
using namespace Eigen;
//using namespace std;
typedef Matrix<double, 3, 1> Vector3;
typedef Matrix<double, 3, 3> Matrix3;

/*

 System3(System2(System1(..)))

*/
class CoordinateSystem {
public:
	CoordinateSystem() {
	}/*
	virtual Matrix3 matrix(double x, double y, double z) {
		return this->matrix_local(x, y, z) * parent->matrix(x, y, z);
	}*/
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	//virtual Vector3 get_origin_cartesian() { return Vector3(0,0,0); }
	virtual Vector3 to_cartesian(Vector3d v) = 0;
	virtual Vector3 from_cartesian(Vector3d v) = 0;

	Matrix3 basis_matrix(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	/*Matrix3 matrix_local(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	Matrix3 matrix(Vector3 v) {
		return this->matrix(v(0),v(1),v(2));
	}

	Matrix3 transformation_matrix(double x, double y, double z, CoordinateSystem* prime_system) {
		Matrix3 m1 = this->matrix(x, y, z);
		Matrix3 m2 = prime_system->matrix(x, y, z);
		return m1*m2.transpose();
	}*/
};

class Cartesian : public CoordinateSystem {
public:
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian(Vector3d v) {
		return v;
	}
	Vector3 from_cartesian(Vector3d v) {
		return v;
	}
};

class Cylindrical : public CoordinateSystem {
public:
	Vector3 ex(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(x/rho, y/rho, 0);
	}
	Vector3 ey(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(y/rho, -x/rho, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian(Vector3d v) {
		double rho = v(0);
		double theta = v(1);
		double z = v(2);
		double x = rho * cos(theta);
		double y = rho * sin(theta);
		Vector3 cartesian(x,y,z);
		return cartesian;
	}
	Vector3 from_cartesian(Vector3d v) {
		double x = v(0);
		double y = v(1);
		double z = v(2);
		double rho = sqrt(x*x+y*y);
		double theta = atan2(y,x);
		Vector3 cylindrical(rho,theta,z);
		return cylindrical;
	}
};

class SphericalGalactic : public CoordinateSystem {
public:
	SphericalGalactic() {
		//rotation = AngleAxisd(M_PI, Vector3::UnitZ());
		//std::cout << "rot[" << rotation << "]" << std::endl;
	}
	Vector3 ex(double x, double y, double z) {
		double r = sqrt(x*x+y*y+z*z);
		return Vector3(x/r, y/r, z/r);
	}
	Vector3 ey(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(-y/rho, x/rho, 0);
	}
	Vector3 ez(double x, double y, double z) {
		double r = sqrt(x*x+y*y+z*z);
		double rho = sqrt(x*x+y*y);
		Vector3 v = Vector3(z*x/rho/r, z*y/rho/r, -rho/r);
		return v;
	}
	Vector3 to_cartesian(Vector3d v) {
		double r = v(0);
		double phi = v(1);
		double theta = v(2);
		double x = r * sin(theta) * cos(phi);
		double y = r * sin(theta) * sin(phi);
		double z = r * cos(theta);
		Vector3 cartesian(x,y,z);
		return cartesian;
	}
	Vector3 from_cartesian(Vector3d cartesian) {
		double x = cartesian(0);
		double y = cartesian(1);
		double z = cartesian(2);
		double r = sqrt(x*x+y*y+z*z);
		double phi = atan2(y,x);
		double theta = acos(z/r);
		Vector3 spherical(r,phi,theta);
		return spherical;
	}
};

class Coordinate {
public:
	CoordinateSystem* coordinate_system;
	Vector3 x;
	Coordinate(double x1, double x2, double x3, CoordinateSystem* coordinate_system) : coordinate_system(coordinate_system), x(x1, x2, x3) {
	}
	/*virtual Matrix3 matrix(double x, double y, double z) {
		return this->matrix_local(x, y, z) * parent->matrix(x, y, z);
	}
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	//virtual Vector3 get_origin_cartesian() { return Vector3(0,0,0); }
	virtual Vector3 cartesian_transformation(Vector3 v) { return v;}*/
	virtual Vector3 to_cartesian() {
		return coordinate_system->to_cartesian(x);
	}
	//virtual Vector3 to_cartesian_local(Vector3d v) = 0;
	//virtual Vector3 from_cartesian_local(Vector3d v) = 0;
	/*Matrix3 matrix_local(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	Matrix3 matrix(Vector3 v) {
		return this->matrix(v(0),v(1),v(2));
	}

	Matrix3 transformation_matrix(double x, double y, double z, CoordinateSystem* prime_system) {
		Matrix3 m1 = this->matrix(x, y, z);
		Matrix3 m2 = prime_system->matrix(x, y, z);
		return m1*m2.transpose();
	}*/
};

class VelocityCoordinate {
public:
	CoordinateSystem* coordinate_system;
	Vector3 v;
	VelocityCoordinate(double v1, double v2, double v3, CoordinateSystem* coordinate_system) : coordinate_system(coordinate_system), v(v1, v2, v3) {
	}
	/*virtual Matrix3 matrix(double x, double y, double z) {
		return this->matrix_local(x, y, z) * parent->matrix(x, y, z);
	}
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	//virtual Vector3 get_origin_cartesian() { return Vector3(0,0,0); }
	virtual Vector3 cartesian_transformation(Vector3 v) { return v;}*/
	virtual Vector3 to_cartesian(double x, double y, double z) {
		return \
			v(0) * coordinate_system->ex(x, y, z) +\
			v(1) * coordinate_system->ey(x, y, z) +\
			v(2) * coordinate_system->ez(x, y, z);
	}
	virtual Vector3 to_cartesian_vec(Vector3 p) {
		return to_cartesian(p(0), p(1), p(2));
	}

};

class ReferenceFrameBase {
public:
	Matrix3 basis_matrix(double x, double y, double z) {
		Matrix3 m;
		m.block<3,1>(0,0) = ex(x, y, z);
		m.block<3,1>(0,1) = ey(x, y, z);
		m.block<3,1>(0,2) = ez(x, y, z);
		return m;
	}
	virtual Vector3 ex(double x, double y, double z) = 0;
	virtual Vector3 ey(double x, double y, double z) = 0;
	virtual Vector3 ez(double x, double y, double z) = 0;
	virtual Vector3 to_global(Vector3 local) = 0;
	virtual Vector3 to_local(Vector3 global) = 0;
	virtual Vector3 to_global_velocity(Vector3 v_local) { return v_local; }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return v_global; }
};

class Position {
public:
	Coordinate* c;
	ReferenceFrameBase* f;
	Position(Coordinate* c, ReferenceFrameBase* f) : c(c), f(f) {
	}
	Vector3 to(ReferenceFrameBase* target_frame) {
		return target_frame->to_local( f->to_global(c->to_cartesian()) );;
	}
	Vector3 to_global() {
		return f->to_global(c->to_cartesian());
	}
	Vector3 to_coordinate_system(ReferenceFrameBase* target_frame, CoordinateSystem* target_coordinate_system) {
		Vector3 v = to(target_frame);
		return target_coordinate_system->from_cartesian(v);
	}
};

class Velocity {
public:
	VelocityCoordinate* vc;
	ReferenceFrameBase* f;
	Position* p;
	ReferenceFrameBase* pos_frame;
	Velocity(VelocityCoordinate* vc, ReferenceFrameBase* f, Position* p, ReferenceFrameBase* pos_frame) : vc(vc), f(f), p(p), pos_frame(pos_frame) {
	}
	Vector3 to_coordinate_system(ReferenceFrameBase* target_frame, CoordinateSystem* target_coordinate_system) {
		Vector3 v = to(target_frame);
		Vector3 local_coordinate = p->to(target_frame); //f->to_global(Vector3(0, 0, 0));
		double x = local_coordinate(0);
		double y = local_coordinate(1);
		double z = local_coordinate(2);
		//std::cout << "[x,y,z = ( " << x << ", " << y << ", " << z <<")]" << std::endl;
		Matrix3 m = target_coordinate_system->basis_matrix(x, y, z);
		return m.inverse() * v;
	}
	Vector3 to(ReferenceFrameBase* target_frame) {
		Vector3 global_velocity = to_global();
		return target_frame->to_local_velocity(global_velocity);
	}
	Vector3 to_global() {
		Vector3 local_coordinate = p->to(pos_frame); //f->to_global(Vector3(0, 0, 0));
		Vector3 local_velocity = vc->to_cartesian_vec(local_coordinate);
		Vector3 global_velocity = f->to_global_velocity(local_velocity);
		return global_velocity;
	}
};

class ZeroReferenceFrame : public ReferenceFrameBase {
public:
	ZeroReferenceFrame()  {
	}
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	virtual Vector3 to_global(Vector3 local) { return local; }
	virtual Vector3 to_local(Vector3 global) { return global; }
};

class ReferenceFrame : public ReferenceFrameBase {
public:
	Vector3 x0;
	ReferenceFrameBase* parent;
	ReferenceFrame(Position* origin, ReferenceFrameBase* parent) : x0(origin->to(parent)), parent(parent) {
	}
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(local + x0); }
	virtual Vector3 to_local(Vector3 global) { return parent->to_local(global) - x0; }
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
};

class RotatedReferenceFrame : public ReferenceFrameBase {
public:
	Matrix3 rotation;
	Matrix3 rotation_inverse;
	ReferenceFrameBase *parent;
	RotatedReferenceFrame(double angle, ReferenceFrameBase* parent) : parent(parent) {
		rotation = AngleAxisd(angle, Vector3::UnitZ());
		rotation_inverse = rotation.inverse();
	}
	virtual Vector3 to_global_velocity(Vector3 v_local) { return parent->to_global_velocity(rotation*v_local); }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return rotation_inverse*parent->to_local_velocity(v_global); }
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(rotation*local); }
	virtual Vector3 to_local(Vector3 global) { return rotation_inverse*parent->to_local(global); }
	//Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		return rotation * Vector3::UnitX();
	}
	Vector3 ey(double x, double y, double z) {
		return rotation * Vector3::UnitY();
	}
	Vector3 ez(double x, double y, double z) {
		return rotation * Vector3::UnitZ();
	}
};

class EqReferenceFrame : public ReferenceFrameBase {
public:
	Matrix3 rotation;
	Matrix3 rotation_inverse;
	ReferenceFrameBase *parent;
	EqReferenceFrame(double theta0, double a_NGP, double d_NGP, ReferenceFrameBase* parent) : parent(parent) {
		Matrix3 a,b,c;
		c << 	cos(a_NGP),  sin(a_NGP), 0,
				sin(a_NGP), -cos(a_NGP), 0,
				0, 0, 1;
		b << 	-sin(d_NGP), 0, cos(d_NGP),
				0, -1, 0,
				cos(d_NGP), 0, sin(d_NGP);
		a << 	cos(theta0),  sin(theta0), 0,
				sin(theta0), -cos(theta0), 0,
				0, 0, 1;
		rotation = a*b*c;
		rotation_inverse = rotation.inverse();
		//std::cout << "[" << rotation << "]" << std::endl;
		//std::cout << rotation_inverse << std::endl;
	}
	virtual Vector3 to_global_velocity(Vector3 v_local) { return parent->to_global_velocity(rotation*v_local); }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return rotation_inverse*parent->to_local_velocity(v_global); }
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(rotation*local); }
	virtual Vector3 to_local(Vector3 global) { return rotation_inverse*parent->to_local(global); }
	//Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		return rotation * Vector3::UnitX();
	}
	Vector3 ey(double x, double y, double z) {
		return rotation * Vector3::UnitY();
	}
	Vector3 ez(double x, double y, double z) {
		return rotation * Vector3::UnitZ();
	}
};

class MovingReferenceFrame : public ReferenceFrameBase {
public:
	ReferenceFrameBase* parent;
	Vector3 velocity_frame;
	MovingReferenceFrame(Velocity* velocity , ReferenceFrameBase* parent) : parent(parent) {
		velocity_frame = velocity->to(parent);
	}
	virtual Vector3 to_global_velocity(Vector3 v_local) { return parent->to_global_velocity(v_local+velocity_frame); }
	virtual Vector3 to_local_velocity(Vector3 v_global) { return parent->to_local_velocity(v_global)-velocity_frame; }
	virtual Vector3 to_global(Vector3 local) { return parent->to_global(local); }
	virtual Vector3 to_local(Vector3 global) { return parent->to_local(global); }
	Vector3 ex(double x, double y, double z) {
		return Vector3::UnitX();
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3::UnitY();
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3::UnitZ();
	}
};

/*
class TranslatedReferenceFrame : public CoordinateSystem {
public:
	Vector3 translation;
	TranslatedReferenceFrame(double x, double y, double z, CoordinateSystem* parent) : CoordinateSystem(parent), translation(x, y, z) {
	}
	Vector3 ex(double x, double y, double z) {
		return Vector3(1, 0, 0);
	}
	Vector3 ey(double x, double y, double z) {
		return Vector3(0, 1, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian_local(Vector3d v) {
		return v+translation;
	}
	Vector3 from_cartesian_local(Vector3d cartesian) {
		return cartesian-translation;
	}
};*/
/*
class Cylindrical : public CoordinateSystem {
public:
	Position* origin;
	Cylindrical(Position* origin) : origin(origin) {
	}
	Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(x/rho, y/rho, 0);
	}
	Vector3 ey(double x, double y, double z) {
		double rho = sqrt(x*x+y*y);
		return Vector3(y/rho, -x/rho, 0);
	}
	Vector3 ez(double x, double y, double z) {
		return Vector3(0, 0, 1);
	}
	Vector3 to_cartesian_local(Vector3d v) {
		double rho = v(0);
		double theta = v(1);
		double z = v(2);
		double x = rho * cos(theta);
		double y = rho * sin(theta);
		Vector3 cartesian(x,y,z);
		return cartesian;
	}
	Vector3 from_cartesian_local(Vector3d v) {
		double x = v(0);
		double y = v(1);
		double z = v(2);
		double rho = sqrt(x*x+y*y);
		double theta = atan2(y,x);
		Vector3 cylindrical(rho,theta,z);
		return cylindrical;
	}
};
*/
/*
class RotatedCoordinateSystem : public CoordinateSystem {
public:
	Matrix3 rotation;
	Matrix3 rotation_inverse;
	RotatedCoordinateSystem(CoordinateSystem* parent) : CoordinateSystem(parent) {
		rotation = AngleAxisd(M_PI, Vector3::UnitZ());
		rotation_inverse = rotation.inverse();
	}
	//Vector3 get_origin_cartesian() { return origin->to_cartesian(); }
	Vector3 ex(double x, double y, double z) {
		return rotation * parent->ex(x, y, z);
	}
	Vector3 ey(double x, double y, double z) {
		return rotation * parent->ey(x, y, z);
	}
	Vector3 ez(double x, double y, double z) {
		return rotation * parent->ez(x, y, z);
	}
	Vector3 to_cartesian_local(Vector3d v) {
		//return rotation*parent->to_cartesian(v);
		return rotation*v;
	}
	Vector3 from_cartesian_local(Vector3d cartesian) {
		//return parent->from_cartesian(rotation_inverse * cartesian);
		return rotation_inverse*cartesian;
	}
};

*/

/*
class Spherical {

	Vector3 ex(double x, double y, double z) {

	}
	Vector3 ey(double x, double y, double z) {
	}
	Vector3 ez(double x, double y, double z) {
	}
};*/

}


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

int main() {
    Eigen::MatrixXd A(2, 2);
    A << 3.0, -1.5,
         2.5,   0.5;
    std::cout << "A =" << std::endl << A << std::endl;
    Eigen::VectorXd b(2);
    b << 3.5, -2.5;
    std::cout << "b =" << std::endl << b << std::endl;
    auto decomposition = A.partialPivLu();
    Eigen::VectorXd x = decomposition.solve(b);
    std::cout << "x =" << std::endl << x << std::endl;
    std::cout << "A*x - b =" << std::endl << A*x - b << std::endl;
    return 0;
}


#ifndef KALMAN_FILTER_HPP_

#define KALMAN_FILTER_HPP_
#include <Eigen/Dense>

namespace mineral_deposit_tracking
{

template<int StateSize>
class KalmanFilter
{
public:
	using VectorType = Eigen::Matrix<double, StateSize, 1>;
	using MatrixType = Eigen::Matrix<double, StateSize, StateSize>;

	KalmanFilter(
		const MatrixType & transition_matrix,
		const MatrixType & process_covariance,
		const MatrixType & observation_matrix)
	:	transition_matrix_(transition_matrix),
		process_covariance_(process_covariance),
		observation_matrix_(observation_matrix),
		estimate_(VectorType::Zero()),
		estimate_covariance_(MatrixType::Identity() * 500)
	{
	}

	void Reset(const VectorType & initial_state, const MatrixType & initial_covariance)
	{
		estimate_ = initial_state;
		estimate_covariance_ = initial_covariance;
	}

	void TimeUpdate()
	{
		estimate_ = transition_matrix_ * estimate_;
		estimate_covariance_ = (transition_matrix_ * estimate_covariance_ * transition_matrix_.transpose()) + process_covariance_;
	}

	void MeasurementUpdate(const VectorType & measurement, const MatrixType & measurement_covariance)
	{
		const MatrixType innovation_covariance = (observation_matrix_ * estimate_covariance_ * observation_matrix_.transpose()) + measurement_covariance;

		const MatrixType kalman_gain = estimate_covariance_ * observation_matrix_.transpose() * innovation_covariance.inverse();

		estimate_ = estimate_ + (kalman_gain * (measurement - (observation_matrix_ * estimate_)) );

		const MatrixType tmp = MatrixType::Identity() - (kalman_gain * observation_matrix_);

		estimate_covariance_ = ( tmp * estimate_covariance_ * tmp.transpose() ) + ( kalman_gain * measurement_covariance * kalman_gain.transpose() );

	}

	const VectorType & GetEstimate() const
	{
		return estimate_;
	}

	const MatrixType & GetEstimateCovariance() const
	{
		return estimate_covariance_;
	}

private:
	const MatrixType transition_matrix_;
	const MatrixType process_covariance_;
	const MatrixType observation_matrix_;
	VectorType estimate_;
	MatrixType estimate_covariance_;
};

}

#endif


/**
 *  @file    Mesh.hpp
 *  @brief   Definition of the finite difference mesh.
 *  @author  Francois Roy
 *  @date    12/04/2019
 */
#ifndef MESH_H
#define MESH_H

#include <stdexcept>
#include <vector>
#include <unordered_set>
#include <Eigen/Core>
#include "spdlog/spdlog.h"
#include "utils/Utils.hpp"

namespace numerical {

namespace fdm {

/*!
* Holds data structures for a uniform mesh on a hypercube in
* space, plus a uniform mesh in time.
*
*  \anchor fig_mesh
*  \image html bloc.png <"Figure 1: A typical mesh for a bloc. The scene shows
*  the bottom (orange), left (magenta) and front (blue) boundaries.">
*  \image latex bloc.eps "bloc" width=10cm
*
* @param lengths List of 2-lists of min and max coordinates in each spatial
* direction.
* @param t0 The initial time in time mesh.
* @param tend The final time in time mesh.
* @param nt Number of cells in time mesh.
* @param n List of number of cells in the spatial directions.
*/
template <typename T>
class Mesh {
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
private:
	const std::vector<std::vector<T>> m_lengths;
	const T m_t0, m_tend;
	T m_dt;
	const std::vector<int> m_nx;
	std::vector<T> m_dx;
	const int m_nt;
	std::vector<Vec> m_x;
	Vec m_t;
	std::size_t m_dim;
public:
	Mesh(const std::vector<std::vector<T>>& lengths, T t0, T tend,
		const std::vector<int>& nx, int nt)
	  : m_lengths(lengths),
	  m_t0(t0),
      m_tend(tend),
      m_nx(nx),
      m_nt(nt)
	{
        m_dim = m_lengths.size();
        if (m_dim < 1){
        	throw std::invalid_argument(
        		"At least one 2-lists of min and max coords must be given."
        		);
        }
        if (m_nx.size() != m_lengths.size()){
        	throw std::invalid_argument("n and lengths have different size.");
        }
        for(int i=0; i<m_dim; ++i){
        	if(m_lengths[i].size() != 2){
        		throw std::invalid_argument("length item is not a 2-list.");
        	}
        	m_dx.push_back(T (m_lengths[i][1] - m_lengths[i][0])/m_nx[i]);
        }
        m_dt = m_tend / T(nt);
        for(int i=0; i<m_dim; ++i){
        	Vec temp(m_nx[i] + 1);
        	for(int j=0; j<m_nx[i] + 1; ++j){
        		temp[j] = j * m_dx[i];
        	}
            m_x.push_back(temp);
        }
        m_t = Vec(m_nt + 1);
        for(int i=0; i<m_nt; ++i){
        	m_t[i] = i * m_nt;
        }
	}

    /*!
    * If 1D, \c ny\c and \c nz\c = 0, if 2D \c nz\c = 0, else all are > 1. See
    * \ref fig_mesh
    *
    * @return The number of divisions per axis.
    */
    std::vector<int> division_per_axis(){
        int n_x = m_nx[0];
        int n_y = 0;
        int n_z = 0;
        if (m_dim >= 2){
            n_y = m_nx[1];
        }
        if (m_dim == 3) {
            n_z = m_nx[2];
        }
        return {n_x, n_y, n_z};
    }

    /*!
    *  The indices of the nodes on the left boundary,
    *  i.e. at x = lengths[0][0].
    */
    std::vector<int> left(){
        std::vector<int> nx = division_per_axis();
        std::unordered_set<int> temp1;
        std::vector<int> temp;
        if (m_dim == 1){
            temp = {0};
        }
        else if (m_dim == 2){
            for (int j=0; j<nx[1] + 1; ++j){
                temp.push_back(j * (nx[0] + 1));
            }
        }
        else {  // m_dim = 3
            for (int j=0; j<nx[1] + 1; ++j){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(j * (nx[0] + 1) +
                        k * ((nx[0] + 1)*(nx[1] + 1)));
                }
            }
        }
        return temp;
    }

    /*!
    *  The indices of the nodes on the right boundary, i.e. at
    *  x = lengths[0][1]. It is just left + n_x.
    */
    std::vector<int> right(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> out = left();
        for(int& i : out){
            i += nx[0];
        }
    	return out;
    }

    /*!
    *  The indices of the nodes on the bottom boundary, i.e. at
    *  y = lengths[1][0]. Only for 2D and 3D models.
    */
    std::vector<int> bottom(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> temp;
        if (m_dim == 2){
            for (int i=0; i<nx[0] + 1; ++i){
                temp.push_back(i);
            }
        }
        else {  // m_dim = 3
            for (int i=0; i<nx[0] + 1; ++i){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(i + k * (nx[0] + 1) * (nx[1] + 1));
                }
            }
        }
        return temp;
    }

    /*!
    *  The indices of the nodes on the top boundary i.e. at
    *  y = lengths[1][1]. Only for 2D and 3D models.
    */
    std::vector<int> top(){
    	std::vector<int> nx = division_per_axis();
        std::vector<int> out = bottom();
        if (m_dim == 2){
            for(int& i : out){
                i += nx[1] * (nx[0] + 1);
            }
        }
        else { // 3D
            for(int& i : out){
                i += (nx[0] + 1) * (nx[0] + 1);
            }
        }
        return out;
    }

    /*!
    *  The indices of the nodes on the front, i.e. at
    *  z = length[2][0]. Only for 3D models.
    */
    std::vector<int> front(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> out = back();
        for(int& i : out){
            i += nx[2] * (nx[0] + 1) * (nx[1] + 1);
        }
        return out;
    }

    /*!
    *  The indices of the nodes on the back boundary, i.e. at
    *  z = length[2][1]. Only for 3D models.
    */
    std::vector<int> back(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> temp;
        for (int i=0; i<nx[0] + 1; ++i){
                for (int j=0; j<nx[1] + 1; ++j){
                    temp.push_back(i + j * (nx[0] + 1));
                }
            }
        return temp;
    }

    /*!
    * @return The indices of the external boundaries.
    */
    std::vector<int> boundaries(){
        // points in 1D
        // lines in 2D
        // surfaces in 3D
        std::vector<int> nx = division_per_axis();
        std::vector<int> temp;
        if (m_dim == 1){
            temp = {0, nx[0]};
        }
        else if (m_dim == 2){
            for (int j=0; j<nx[1] + 1; ++j){
                temp.push_back(j * (nx[0] + 1)); // left
                temp.push_back(j * (nx[0] + 1) + nx[0]); // right
            }
            for (int i=0; i<nx[0] + 1; ++i){
                temp.push_back(i);  // bottom
                temp.push_back(i + nx[1] * (nx[0] + 1));  // top
            }
            // make unique
            std::sort(temp.begin(), temp.end());
            auto last = std::unique(temp.begin(), temp.end());
            temp.erase(last, temp.end());
        }
        else {  // m_dim = 3
            for (int j=0; j<nx[1] + 1; ++j){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(j * (nx[0] + 1) +
                        k * ((nx[0] + 1)*(nx[1] + 1)));  // left
                    temp.push_back(j * (nx[0] + 1) +
                        k * ((nx[0] + 1)*(nx[1] + 1)) + nx[0]); // right
                }
            }
            for (int i=0; i<nx[0] + 1; ++i){
                for (int k=0; k<nx[2] + 1; ++k){
                    temp.push_back(i + k * (nx[0] + 1) *
                        (nx[1] + 1));  // bottom
                    temp.push_back(i + k * (nx[0] + 1) *
                        (nx[1] + 1) + (nx[0] + 1) * (nx[0] + 1));  // top
                }
            }
            for (int i=0; i<nx[0] + 1; ++i){
                for (int j=0; j<nx[1] + 1; ++j){
                    temp.push_back(i + j * (nx[0] + 1));  // back
                    temp.push_back(i + j * (nx[0] + 1) +
                        nx[2] * (nx[0] + 1) * (nx[1] + 1));   // front
                }
            }
            // make unique
            std::sort(temp.begin(), temp.end());
            auto last = std::unique(temp.begin(), temp.end());
            temp.erase(last, temp.end());
        }
        return temp;
    }

    /*!
    *
    */
    std::vector<int> domain(){
        std::vector<int> nx = division_per_axis();
        std::vector<int> all;
        if (m_dim == 1){
            all == utils::linear_spaced<int>(0, nx[0], nx[0] + 1);
        }
        else if (m_dim ==2){
            all == utils::linear_spaced<int>(0, (nx[0] + 1) * (nx[1] + 1) - 1,
                (nx[0] + 1) * (nx[1] + 1));
        } else {
            all == utils::linear_spaced<int>(0,
                (nx[0] + 1) * (nx[1] + 1) * (nx[2] + 1) - 1,
                (nx[0] + 1) * (nx[1] + 1) * (nx[2] + 1));
        }
        return all;
    }

    /*!
    *  @return The interior nodes.
    */
    std::vector<int> interior(){
        return {0};
    }

    /*!
    *  Returns the boundary nodes in 3D.
    *  @return The surfaces.
    */
    std::vector<int> surfaces(){
        std::vector<int> out;
        if (m_dim == 3){
            out = boundaries();
        }
        return out;
    }

    /*!
    *  2D, 3D
    */
    std::vector<int> edges(){
        // TODO define in 3D
        // boundaries in 2D
        return {0};
    }

    /*!
    *  1D, 2D, 3D
    */
    std::vector<int> corners(){
        // TODO define in 3D
        // 2D: 0, nx, ny * (nx+1), (ny+1) * (nx+1) -1
        // boundaries in 1D
        return {0};
    }

	/*!
	* @return the space dimension.
	*/
	std::size_t dim(){
		return m_dim;
	}
	/*!
	* @return the space increment for each dimension.
	*/
	std::vector<T> dx() {
		return m_dx;
	}

    /*!
    * Defines a mapping \f$m(i, j, k)\f$ from a mesh point with indices
    * \f$(i, j, k)\f$ to the corresponding unknown index \f$p\f$ in the
    * equation system:
    *
    * \f[
    *  p = m(i, j, k) = i+ j(n_x + 1) + k((n_x + 1)(n_y+1)))
    * \f]
    *
    * where \f$n_x\f$  and \f$n_y\f$ are the number of division along the
    * \f$x\f$- and \f$y\f$-directions.
    *
    * We number the points along the x axis, starting with y = y_0, and
    * z = z_0. We then progress one mesh line at a time
    * (from y = y_0 to y = y_end) until the slice
    * located at z = z_0 has been processed. We then continue on the next
    * slice until the entire meshed cube has been mapped.
    *
    * The coordinates are returned in a 3D vector.
    * @return The mesh node coordinates.
    */
    Eigen::Matrix<Eigen::Matrix<T, 3, 1>, Eigen::Dynamic, 1> coordinates() {
        // get the number of divisions per axis
        int n_x = m_nx[0];
        int n_y = 0;
        int n_z = 0;
        Vec y = Vec::Zero(n_x + 1);
        Vec z = Vec::Zero(n_x + 1);
        if (m_dim >= 2){
            n_y = m_nx[1];
            y = m_x[1];
            z = Vec::Zero((n_x + 1)*(n_y + 1));
        }
        if (m_dim == 3) {
            n_z = m_nx[2];
            z = m_x[2];
        }
        // generate the vector of coordinates
        Eigen::Matrix<T, 3, 1> node;
        Eigen::Matrix<Eigen::Matrix<T, 3, 1>, Eigen::Dynamic, 1> coords;
        for (int k=0; k<n_z + 1; ++k){
            for (int j=0; j<n_y + 1; ++j) {
                for (int i=0; i<n_x + 1; ++i){
                    // spdlog::debug("{}",i + j*(n_x + 1) + k*((n_x + 1) * (n_y+1)));
                    node = {m_x[0][i], y[j], z[k]};
                    // spdlog::debug("x: {} y: {} z: {}", m_x[0][i], y[j], z[k]);
                }
            }
        }
        return coords;
    }

	/*!
	* @return the time increment.
	*/
	T dt() {
		return m_dt;
	}
	/*!
	* @return the list of coordinates in each dimensions.
	*/
	std::vector<Vec> x() {
		return m_x;
	}
	/*!
	* @return the time list.
	*/
	Vec t() {
		return m_t;
	}
};

}  // namespace fdm

} // namespace numerical

#endif  // MESH_H


#include <Eigen/Dense>

/*
   A collection of metrics to use for determining the
   error between the predicted and actual Y values.
*/

double mse(Eigen::MatrixXd X, Eigen::VectorXd W, Eigen::VectorXd Y) {
	/*
	  Computes the mean squared error for a XW and Y.
	*/
	return 0.5 * (Y - X * W).transpose() * (Y - X * W);
}


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

using namespace MotionControl;

Matrix<double,3,3> Kinematics::Rodrigues(Matrix<double,3,1> a, double q)
{
	return AngleAxisd(q,a).toRotationMatrix();
}

Matrix<double,3,1> Kinematics::rot2omega(Matrix<double,3,3> R)
{
	double alpha = (R(0,0)+R(1,1)+R(2,2)-1)/2;
	double th;
	Matrix<double,3,1> vector_R(Matrix<double,3,1>::Zero());

	if(fabs(alpha-1) < eps)
		return Matrix<double,3,1>::Zero();

	th = acos(alpha);
	vector_R << R(2,1)-R(1,2), R(0,2)-R(2,0), R(1,0)-R(0,1);
	return 0.5*th/sin(th)*vector_R;
}

vector<int> Kinematics::FindRoute(int to)
{
	vector<int> idx;
	int link_num = to;

	while(link_num != 0)
	{
		idx.push_back(link_num);
		link_num = ulink[link_num].parent;
	}
	reverse(idx.begin(), idx.end());
	return idx;
}

Matrix<double,6,1> Kinematics::calcVWerr(Link Cref, Link Cnow)
{
	Matrix<double,3,1> perr = Cref.p - Cnow.p;
	Matrix<double,3,3> Rerr = Cref.R - Cnow.R;
	Matrix<double,3,1> werr = Cnow.R * rot2omega(Rerr);
	Matrix<double,6,1> err;

	err << perr,werr;
	return err;
}

void Kinematics::calcForwardKinematics(int rootlink)
{
	if(rootlink == -1)
		return ;
	if(rootlink != 0)
	{
		int parent = ulink[rootlink].parent;
		ulink[rootlink].p = ulink[parent].R * ulink[rootlink].b + ulink[parent].p;
		ulink[rootlink].R = ulink[parent].R * Rodrigues(ulink[rootlink].a, ulink[rootlink].q);
	}
	calcForwardKinematics(ulink[rootlink].sister);
	calcForwardKinematics(ulink[rootlink].child);
}

MatrixXd Kinematics::calcJacobian(vector<int> idx)
{
	size_t jsize = idx.size();
	Matrix<double,3,1> target = ulink[idx.back()].p;
	Matrix<double,6,11> J = MatrixXd::Zero(6,11);

	for(size_t i=0;i<jsize;i++)
	{
		int j = idx[i];
		Matrix<double,3,1> a = ulink[j].R * ulink[j].a;
		Matrix<double,3,1> b = a.cross(target - ulink[j].p);
		J(0,i) = b(0); J(1,i) = b(1); J(2,i) = b(2);
		J(3,i) = a(0); J(4,i) = a(1); J(5,i) = a(2);
	}

	return J;
}

template <typename t_matrix>
t_matrix Kinematics::PseudoInverse(const t_matrix& m, const double &tolerance)
{
	typedef JacobiSVD<t_matrix> TSVD;
	unsigned int svd_opt(ComputeThinU | ComputeThinV);
	if(m.RowsAtCompileTime!=Dynamic || m.ColsAtCompileTime!=Dynamic)
		svd_opt= ComputeFullU | ComputeFullV;
	TSVD svd(m, svd_opt);
	const typename TSVD::SingularValuesType &sigma(svd.singularValues());
	typename TSVD::SingularValuesType sigma_inv(sigma.size());
	for(long i=0; i<sigma.size(); ++i)
	{
		if(sigma(i) > tolerance)
			sigma_inv(i)= 1.0/sigma(i);
		else
			sigma_inv(i)= 0.0;
	}
	return svd.matrixV()*sigma_inv.asDiagonal()*svd.matrixU().transpose();
}

bool Kinematics::calcInverseKinematics(int to, Link target)
{
	MatrixXd J, dq;
	Matrix<double,6,1> err;

	ColPivHouseholderQR<MatrixXd> QR; //QR分解?
	const double dampingConstantSqr = 1.0e-12;
	const double lambda = 0.5;
	const int iteration = 100;

	calcForwardKinematics(WAIST);

	vector<int> idx = FindRoute(to);
	const int jsize = idx.size();

	J.resize(6,jsize); dq.resize(jsize,1);

	for(int n=0;n<iteration;n++){
		J = calcJacobian(idx);
		err = calcVWerr(target, ulink[to]);
		if(err.norm() < eps) return true;

		MatrixXd JJ = J*J.transpose()+dampingConstantSqr*MatrixXd::Identity(J.rows(),J.rows());
		dq = J.transpose() * QR.compute(JJ).solve(err) * lambda;

		for(size_t nn=0;nn<jsize;nn++){
			int j = idx[nn];
			ulink[j].q += dq(nn);
		}
		calcForwardKinematics(WAIST);
	}
	return false;
}