#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

        }

    }

}


#include <cmath>
#include <iostream>
#include <vector>
#include <catch/catch.hpp>
#include <Eigen/Dense>
#include "numeric_utils.h"

TEST_CASE("Test correlation to covariance functionality", "[Helpers]") {
  SECTION("Correlation is diagonal matrix with values of 1.0 along diagonal") {
    Eigen::MatrixXd test_corr = Eigen::MatrixXd::Zero(3, 3);
    test_corr(0, 0) = 1.0;
    test_corr(1, 1) = 1.0;
    test_corr(2, 2) = 1.0;
    Eigen::MatrixXd std_dev = Eigen::VectorXd::Ones(3);

    std_dev << 2.0, 3.0, 4.0;

    auto cov = numeric_utils::corr_to_cov(test_corr, std_dev);

    Eigen::MatrixXd expected_matrix = Eigen::MatrixXd::Zero(3, 3);
    expected_matrix(0, 0) = 4.0;
    expected_matrix(1, 1) = 9.0;
    expected_matrix(2, 2) = 16.0;

    REQUIRE(cov(0, 0) == expected_matrix(0, 0));
    REQUIRE(cov(1, 1) == expected_matrix(1, 1));
    REQUIRE(cov(2, 2) == expected_matrix(2, 2));
    REQUIRE(expected_matrix.lpNorm<2>() == Approx(cov.lpNorm<2>()).epsilon(0.01));
  }
}

TEST_CASE("Test one dimensional convolution", "[Helpers][Convolution]") {
  SECTION("One dimensional convolution of vectors with length 1") {
    std::vector<double> input_x{1.0};
    std::vector<double> input_y{2.0};
    std::vector<double> response;

    bool status;
    try {
      status = numeric_utils::convolve_1d(input_x, input_y, response);
    } catch (std::exception &exception) {
      std::cout << "Convolution error: " << exception.what() << std::endl;
      FAIL("One dimensional convolution function throws exception, check where "
           "exception is being generated interally");
    }

    REQUIRE(status);
    REQUIRE(response.size() == 1);
    REQUIRE(response[0] == Approx(2.0).epsilon(0.01));
  }

  SECTION("One dimensional convolution of vectors with length 2 and 3") {
    std::vector<double> input_x{3.0, 4.0, 5.0};
    std::vector<double> input_y{2.0, 1.0};
    std::vector<double> response(1);

    bool status;
    try {
      status = numeric_utils::convolve_1d(input_x, input_y, response);
    } catch (std::exception &exception) {
      std::cout << "Convolution error: " << exception.what() << std::endl;
      FAIL("One dimensional convolution function throws exception, check where "
           "exception is being generated interally");
    }

    REQUIRE(status);
    REQUIRE(response.size() == input_x.size() + input_y.size() - 1);
    REQUIRE(response[0] == Approx(6.0).epsilon(0.01));
    REQUIRE(response[1] == Approx(11.0).epsilon(0.01));
    REQUIRE(response[2] == Approx(14.0).epsilon(0.01));
    REQUIRE(response[3] == Approx(5.0).epsilon(0.01));
  }
}

TEST_CASE("Test trapazoid rule", "[Helpers][Trapazoid]") {

  SECTION("STL vector with unit spacing") {
    std::vector<double> input_vector{1, 4, 9, 16, 25};

    auto integral = numeric_utils::trapazoid_rule(input_vector, 1.0);
    REQUIRE(integral == 42);
  }

  SECTION("STL vector with non-unit spacing") {
    std::vector<double> input_vector(101, 0.0);

    double accumulator = 0.0;
    for (unsigned int i = 1; i < input_vector.size(); ++i) {
      accumulator += M_PI / 100.0;
      input_vector[i] = std::sin(accumulator);
    }

    auto integral = numeric_utils::trapazoid_rule(input_vector, M_PI / 100.0);
    REQUIRE(integral == Approx(1.9998).epsilon(0.01));
  }

  SECTION("Eigen vector with unit spacing") {
    Eigen::VectorXd input_vector(5);
    input_vector << 1, 4, 9, 16, 25;

    auto integral = numeric_utils::trapazoid_rule(input_vector, 1.0);
    REQUIRE(integral == 42);
  }

  SECTION("Eigen vector with non-unit spacing") {
    Eigen::VectorXd input_vector = Eigen::VectorXd::Zero(101);

    double accumulator = 0.0;
    for (unsigned int i = 1; i < input_vector.size(); ++i) {
      accumulator += M_PI / 100.0;
      input_vector[i] = std::sin(accumulator);
    }

    auto integral = numeric_utils::trapazoid_rule(input_vector, M_PI / 100.0);
    REQUIRE(integral == Approx(1.9998).epsilon(0.01));
  }
}

TEST_CASE("Test 1-D inverse Fast Fourier Transform", "[Helpers][FFT]") {
  SECTION("Calculate real portion of one-dimesional inverse FFT") {
    std::vector<std::complex<double>> input_vector = {
        {15.0, 0.0},
        {-2.5, 3.440954801177933},
        {-2.5, 0.812299240582266},
        {-2.5, -0.812299240582266},
        {-2.5, -3.440954801177933}};

    std::vector<double> output_vector(4);
    auto status = numeric_utils::inverse_fft(input_vector, output_vector);

    REQUIRE(status);
    REQUIRE(output_vector[0] == Approx(1.0).epsilon(0.01));
    REQUIRE(output_vector[1] == Approx(2.0).epsilon(0.01));
    REQUIRE(output_vector[2] == Approx(3.0).epsilon(0.01));
    REQUIRE(output_vector[3] == Approx(4.0).epsilon(0.01));
    REQUIRE(output_vector[4] == Approx(5.0).epsilon(0.01));
  }

  SECTION("Calculate real portion of one-dimesional inverse FFT") {
    Eigen::VectorXcd input_vector(5);
    input_vector << std::complex<double>(15.0, 0.0),
      std::complex<double>(-2.5, 3.440954801177933),
      std::complex<double>(-2.5, 0.812299240582266),
      std::complex<double>(-2.5, -0.812299240582266),
      std::complex<double>(-2.5, -3.440954801177933);

    Eigen::VectorXd output_vector;
    auto status = numeric_utils::inverse_fft(input_vector, output_vector);

    REQUIRE(status);
    REQUIRE(output_vector[0] == Approx(1.0).epsilon(0.01));
    REQUIRE(output_vector[1] == Approx(2.0).epsilon(0.01));
    REQUIRE(output_vector[2] == Approx(3.0).epsilon(0.01));
    REQUIRE(output_vector[3] == Approx(4.0).epsilon(0.01));
    REQUIRE(output_vector[4] == Approx(5.0).epsilon(0.01));
  }
}


#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 <catch2/catch.hpp>
#include <replay/math.hpp>
#include <replay/matrix2.hpp>
#include <replay/minimal_sphere.hpp>
#include <replay/vector_math.hpp>
#include <boost/math/constants/constants.hpp>
#include <random>

namespace
{

// FIXME: this is somewhat generically useful - lift it to a visible namespace?
replay::vector3f polar_to_model(float latitude, float longitude)
{
    latitude = replay::math::convert_to_radians(latitude);
    longitude = replay::math::convert_to_radians(longitude);

    float cw = std::cos(latitude);
    float sw = std::sin(latitude);
    float ch = std::cos(longitude);
    float sh = std::sin(longitude);

    return {cw * ch, sw * ch, sh};
}

template <class IteratorType>
float distance_to_sphere(const IteratorType point_begin,
                         const IteratorType point_end,
                         const replay::vector3f& center,
                         const float square_radius)
{
    float max_sqr_distance = 0.f;

    for (IteratorType i = point_begin; i != point_end; ++i)
    {
        const float sqr_distance = (center - (*i)).squared();

        max_sqr_distance = std::max(max_sqr_distance, sqr_distance);
    }

    float radius = std::sqrt(square_radius);
    return std::max(0.f, std::sqrt(max_sqr_distance) - radius);
}
} // namespace

TEST_CASE("matrix2_operations")
{
    using namespace replay;
    matrix2 Rotation = matrix2::make_rotation(boost::math::constants::pi<float>() * 0.25f); // 45deg rotation
    matrix2 Inv = Rotation;
    REQUIRE(Inv.invert());

    using math::fuzzy_equals;
    using math::fuzzy_zero;
    matrix2 I = Rotation * Inv;
    // This should be identity
    REQUIRE(fuzzy_equals(I[0], 1.f));
    REQUIRE(fuzzy_zero(I[1]));
    REQUIRE(fuzzy_zero(I[2]));
    REQUIRE(fuzzy_equals(I[3], 1.f));

    I = Inv * Rotation;
    // This should be identity
    REQUIRE(fuzzy_equals(I[0], 1.f));
    REQUIRE(fuzzy_zero(I[1]));
    REQUIRE(fuzzy_zero(I[2]));
    REQUIRE(fuzzy_equals(I[3], 1.f));
}

// This test verifies integer arithmetic with a vector3.
// Hopefully, floating-point math will behave correct if this does.
TEST_CASE("vector3_integer_operations")
{
    using namespace replay;
    typedef vector3<int> vec3;

    const vec3 a(-1, -67, 32);
    const vec3 b(7777, 0, -111);
    const vec3 c(a - b);

    REQUIRE(c - a == -b);

    const vec3 all_one(1, 1, 1);

    REQUIRE(all_one.sum() == 3);
    REQUIRE(all_one.squared() == 3);

    int checksum = dot(a, all_one);
    REQUIRE(checksum == a.sum());

    checksum = dot(b, all_one);
    REQUIRE(checksum == b.sum());

    REQUIRE(a.sum() - b.sum() == c.sum());

    REQUIRE((a * 42).sum()== a.sum() * 42);

    const vec3 all_two(2, 2, 2);
    REQUIRE(all_one * 2== all_two);
    REQUIRE(all_one + all_one== all_two);
}

TEST_CASE("quadratic_equation_solver")
{
    using namespace replay;
    using range_type = std::uniform_real_distribution<float>;
    // Attempt to solve a few equations of the form (x-b)(x-a)=0 <=> x^2+(-a-b)*x+b*a=0

    std::mt19937 rng;
    range_type range(-100.f, 300.f);
    auto die = [&] { return range(rng); };

    for (std::size_t i = 0; i < 32; ++i)
    {
        float a = die();
        float b = die();

        if (replay::math::fuzzy_equals(a, b))
            continue;

        interval<> r;
        // FIXME: use a relative epsilon
        math::solve_quadratic_eq(1.f, -a - b, a * b, r, 0.001f);

        if (a > b)
            std::swap(a, b);

        REQUIRE(r[0] == Approx(a).margin(0.01f));
        REQUIRE(r[1] == Approx(b).margin(0.01f));
    }
}

TEST_CASE("matrix4_determinant_simple")
{
    using namespace replay;
    matrix4 M(0.f, 0.f, 3.f, 0.f, 4.f, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f, 0.f, 0.f, 0.f, 0.f, 1.f);

    float d = M.determinant();

    REQUIRE(d == Approx(24.f).margin(0.0001f));

    matrix4 N(2.f, 1.f, 0.f, 0.f, 1.f, 2.f, 1.f, 0.f, 0.f, 1.f, 2.f, 1.f, 0.f, 0.f, 1.f, 2.f);

    float e = N.determinant();

    REQUIRE(e == Approx(5.f).margin(0.0001f));
}

TEST_CASE("circumcircle")
{
    using namespace replay;

    // Construct a rotational matrix
    vector3f x = polar_to_model(177.f, -34.f);
    vector3f y = normalized(math::construct_perpendicular(x));
    matrix3 M(x, y, cross(x, y));

    // Construct three points on a circle and rotate them
    const float radius = 14.f;
    float angle = math::convert_to_radians(34.f);
    vector3f a = M * (vector3f(std::cos(angle), std::sin(angle), 0.f) * radius);
    angle = math::convert_to_radians(134.f);
    vector3f b = M * (vector3f(std::cos(angle), std::sin(angle), 0.f) * radius);
    angle = math::convert_to_radians(270.f);
    vector3f c = M * (vector3f(std::cos(angle), std::sin(angle), 0.f) * radius);

    // Move the circle
    const vector3f center(45.f, 32.f, -37.f);
    a += center;
    b += center;
    c += center;

    // Reconstruct it
    equisphere<float, 3> s(1e-16f);
    REQUIRE(s.push(a.ptr()));
    REQUIRE(s.push(b.ptr()));
    REQUIRE(s.push(c.ptr()));

    vector3f equisphere_center(vector3f::cast(s.get_center()));
    vector3f center_delta = center - equisphere_center;
    REQUIRE(center_delta.squared() < 0.001f);
    REQUIRE(std::sqrt(s.get_squared_radius()) == Approx(radius).margin(0.001f));
}

// Simple test case directly testing the minimal ball solver in 3D
TEST_CASE("minimal_ball")
{
    using namespace replay;
    typedef vector3f vec3;
    using range_type = std::uniform_real_distribution<float>;

    // setup random number generators
    std::mt19937 rng;
    auto random_latitude = [&] { return range_type(-180.f, 180.f)(rng); };
    auto random_longitude = [&] { return range_type(-90.f, 90.f)(rng); };
    auto random_scale = [&] { return range_type(0.f, 1.0f)(rng); };

    // setup a simple point set
    std::list<vec3> points{ vec3(1.f, 0.f, 0.f), vec3(0.f, 1.f, 0.f), vec3(0.f, 0.f, 1.f), vec3(0.f, -1.f, 0.f) };

    for (std::size_t i = 0; i < 32; ++i)
    {
        vector3f t = polar_to_model(random_latitude(), random_longitude());
        float s = random_scale();
        points.push_back(t * s);
    }

    // run the solver
    replay::minimal_ball<float, replay::vector3f, 3> ball(points, 1e-15f);

    // check correctness
    REQUIRE(ball.square_radius() == Approx(1.f).margin(0.001f));
    REQUIRE(ball.center().squared() < 0.001f);
    REQUIRE(distance_to_sphere(points.begin(), points.end(), ball.center(), ball.square_radius()) < 0.001f);
}

// Slightly more sophisticated test for the minimal ball routines using
// the wrapper from vector_math.hpp and an std::vector
TEST_CASE("minimal_sphere")
{
    using namespace replay;
    using range_type = std::uniform_real_distribution<float>;

    std::mt19937 rng;

    auto random_coord = [&] { return range_type(-100.f, 100.f)(rng); };
    auto random_radius = [&] { return range_type(1.f, 3.f)(rng); };

    auto random_latitude = [&] { return range_type(-180.f, 180.f)(rng); };
    auto random_longitude = [&] { return range_type(-90.f, 90.f)(rng); };
    auto random_scale = [&] { return range_type(0.0f, 1.0f)(rng); };

    std::vector<vector3f> p(64);

    for (std::size_t i = 0; i < 16; ++i)
    {
        const vector3f center(random_coord(), random_coord(), random_coord());
        const float radius = random_radius();

        std::size_t boundary_n = 2 + rng() % 3;

        for (std::size_t j = 0; j < boundary_n; ++j)
            p[j] = center + polar_to_model(random_latitude(), random_longitude()) * radius;

        for (std::size_t j = boundary_n; j < 64; ++j)
            p[j] = center + polar_to_model(random_latitude(), random_longitude()) * (random_scale() * radius);

        std::shuffle(p.begin(), p.end(), rng);

        auto const [result_center, result_square_radius] = math::minimal_sphere(p);

        float square_radius = radius * radius;

        // The generated boundary doesn't necessarily define the minimal ball, but it's an upper bound
        REQUIRE(result_square_radius <= Approx(square_radius).margin(0.0001));
        REQUIRE(distance_to_sphere(p.begin(), p.end(), result_center, result_square_radius) < 0.001f);
    }
}


#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 Evan Miller 2020
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#include <pch_light.hpp>
#include <boost/math/concepts/real_concept.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // for test_main
#include <boost/test/tools/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE
#include <boost/math/distributions/kolmogorov_smirnov.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>

template <typename RealType> // Any floating-point type RealType.
void test_spots(RealType)
{
    using namespace boost::math;
    // Test quantiles, CDFs, and complements
    RealType eps = tools::epsilon<RealType>();
    RealType tol = tools::epsilon<RealType>() * 25;
    for (int n=10; n<100; n += 10) {
        kolmogorov_smirnov_distribution<RealType> dist(n);
        for (int i=0; i<1000; i++) {
            RealType p = 1.0 * (i+1) / 1001;
            RealType crit1 = quantile(dist, 1 - p);
            RealType crit2 = quantile(complement(dist, p));
            RealType p1 = cdf(dist, crit1);
            BOOST_CHECK_CLOSE_FRACTION(crit1, crit2, tol);
            BOOST_CHECK_CLOSE_FRACTION(1 - p, p1, tol);
        }

        for (int i=0; i<1000; i++) {
            RealType x = 1.0 * (i+1) / 1001;
            RealType p = cdf(dist, x);
            RealType p1 = cdf(complement(dist, x));
            RealType x1;
            if (p < 0.5)
                x1 = quantile(dist, p);
            else
                x1 = quantile(complement(dist, p1));
            if (p > tol && p1 > tol) // skip the extreme tails
                BOOST_CHECK_CLOSE_FRACTION(x, x1, tol);
        }
    }

    kolmogorov_smirnov_distribution<RealType> dist(100);

    // Basics
    BOOST_CHECK_THROW(pdf(dist, RealType(-1.0)), std::domain_error);
    BOOST_CHECK_THROW(cdf(dist, RealType(-1.0)), std::domain_error);
    BOOST_CHECK_THROW(quantile(dist, RealType(-1.0)), std::domain_error);
    BOOST_CHECK_THROW(quantile(dist, RealType(2.0)), std::domain_error);

    // Confirm mode is at least a local minimum
    RealType mode = boost::math::mode(dist);

    using std::sqrt;
    BOOST_TEST_CHECK(pdf(dist, mode) >= pdf(dist, RealType(mode - sqrt(eps))));
    BOOST_TEST_CHECK(pdf(dist, mode) >= pdf(dist, RealType(mode + sqrt(eps))));

    // Test the moments - each one integrates the entire distribution
    quadrature::exp_sinh<RealType> integrator;

    auto f_one = [&, dist](RealType t) { return pdf(dist, t); };
    BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f_one, eps), RealType(1), tol);

    RealType mean = boost::math::mean(dist);
    auto f_mean = [&, dist](RealType t) { return pdf(dist, t) * t; };
    BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f_mean, eps), mean, tol);

    RealType var = variance(dist);
    auto f_var = [&, dist, mean](RealType t) { return pdf(dist, t) * (t - mean) * (t - mean); };
    BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f_var, eps), var, tol);

    RealType skew = skewness(dist);
    auto f_skew = [&, dist, mean, var](RealType t) { return pdf(dist, t)
        * (t - mean) * (t - mean) * (t - mean) / var / sqrt(var); };
    BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f_skew, eps), skew, 10*tol);

    RealType kurt = kurtosis(dist);
    auto f_kurt= [&, dist, mean, var](RealType t) { return pdf(dist, t)
        * (t - mean) * (t - mean) * (t - mean) * (t - mean) / var / var; };
    BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f_kurt, eps), kurt, 5*tol);

    BOOST_CHECK_CLOSE_FRACTION(kurt, kurtosis_excess(dist) + 3, eps);
}

BOOST_AUTO_TEST_CASE( test_main )
{
  BOOST_MATH_CONTROL_FP;

  // (Parameter value, arbitrarily zero, only communicates the floating point type).
     test_spots(0.0F); // Test float.
 test_spots(0.0); // Test double.
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !defined(BOOST_MATH_NO_REAL_CONCEPT_TESTS)
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#endif
}


#pragma once
#include "load_vector.hpp"
#include <Eigen/Core>

//----------------AssembleVectorBegin----------------
//! Assemble the load vector into the full right hand side
//! for the linear system
//!
//! @param[out] F will at the end contain the RHS values for each vertex.
//! @param[in] vertices a list of triangle vertices
//! @param[in] dofs a list of the dofs' indices in each triangle
//! @param[in] f the RHS function f.
void assembleLoadVector(Eigen::VectorXd &      F,
                        const Eigen::MatrixXd &vertices,
                        const Eigen::MatrixXi &dofs,
                        const int &            N,
                        const std::function<double(double, double)> &f) {
	const int numberOfElements = dofs.rows();

	F.resize(N);
	F.setZero();
	// (write your solution here)
	for (int i = 0; i < numberOfElements; ++i) {
		const auto &indexSet = dofs.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));
		const auto &c = vertices.row(indexSet(2));

		Eigen::VectorXd elementVector;
		computeLoadVector(elementVector, a, b, c, f);

		for (int j = 0; j < 6; ++j) {
			F(indexSet(j)) += elementVector(j);
		}
	}
}
//----------------AssembleVectorEnd----------------


/*
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


#include <armadillo>
#include <iostream>
#include <catch.hpp>
#include "util.hpp"

TEST_CASE( "Random unitary matrix logarithms", "[Util]"){
  for (arma::uword N = 2; N < 100; N*=1.5){
    for (int i = 0; i < 10; i++){
      arma::mat A(N,N, arma::fill::randu);
      util::unitarize(A);

      CHECK(arma::norm(util::logmat_unitary(A) -
                       arma::real(arma::logmat(A)))
            == Approx(0.0).margin(1e-9));
    }
  }
}


/**
 * @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;
}


// main.cpp

//   Abbreviations:
// 1) SLE - system of linear equations
// 2) SOR - successive over-relaxation

//   A main file with code. Here I tesing SLE solvers, count
// determinants of some matrices, find inverse matrices, determine whether
// Gaussian elimination is stable and count the speed of convergence rate of
// iterations

#include <iostream>              // cin, cout
#include <iomanip>               // setprecision
#include <fstream>               // ofstream
#include <boost/filesystem.hpp>  // path, create_directory

#include "matrix.h"
#include "gaussian_method.h"
#include "tester.h"
#include "tests.h"
#include "matrix_functions.h"
#include "SLE_solvers.h"

using namespace std;

using namespace GaussianJordanElimination;
using namespace MatrixFunctions;
using namespace SLESolvers;
using SLESolvers::TesterT;
using SLESolvers::TesterA;

using element_type = double;
using matrix_type = Matrix<element_type>;
using Me = Matrix<element_type>;

//   Some constants
enum
{
    //   For random generator
    SEED = 0,
};

template <class T>
ostream &operator << (ostream &out, const pair<T, T> &a)
{
    out << a.first << endl;
    out << a.second;
    return out;
}

//   Create folder (delete it if it exists)
void new_folder(string path)
{
    boost::filesystem::path dir(path);
    boost::filesystem::remove_all(dir);
    boost::filesystem::create_directory(dir);
}

string get_fout_for_test(const string &pref, const string &name,
        size_t number, size_t max_number, string type = ".txt")
{
    int test_number_digits = to_string(max_number).size();
    int make_zeros = pow(10, test_number_digits);

    return pref + name + to_string(make_zeros + number).substr(1) + type;
}

//   Write tests from Tester object to given folder
template <class T, class A>
void write_tests_to_folder(const Tester<T, A> &tester,
        string path_to_tests = "tests/")
{
    //   Create folder for tests
    new_folder(path_to_tests);

    //   I use these variables to calculate number of digits for
    // each test number
    int test_number_digits = to_string(tester.get_num_tests()).size();
    int make_zeros = pow(10, test_number_digits);

    //   Writing tests
    for (int i = 0; i < tester.get_num_tests(); ++i) {
        ofstream fout(path_to_tests + "/test" +
                to_string(make_zeros + i + 1).substr(1) + ".txt");
        fout << tester.next_test() << endl;
        fout.close();
    }
}

int main()
{
    write_tests_to_folder<TesterT<element_type>, TesterA<element_type>>(
            Tests::create_tester_with_tests<element_type>());

    //   Testing SLEGU
    cout << "Testing SLEGU\n";
    test_SLE_solver<element_type>(SLEGU, "answer_SLEGU/");
    cout << endl;

    //   Testing SLEGM
    cout << "Testing SLEGM\n";
    test_SLE_solver<element_type>(SLEGM, "answer_SLEGM/");
    cout << endl;

    //   Testing SLE_SOR
    cout << "Testing SLE_SOR\n";
    test_SLE_solver<element_type>(SLE_SOR_standard, "answer_SLE_SOR/");
    cout << endl;

    //   Finding determinants
    cout << "Finding determinants" << endl;

    const string determinants = "determinants/";
    new_folder(determinants);

    // Determinants are stored here
    vector<element_type> dets;

    auto tester = Tests::create_tester_with_tests<element_type>();
    for (int i = 0; i < tester.get_num_tests(); ++i) {
        auto det = determinant(tester.next_test().first);
        dets.push_back(det);

        // Print found determinant
        ofstream fout(get_fout_for_test(determinants, "det", i + 1,
                tester.get_num_tests()));
        fout << det << endl;
        fout.close();
    }

    //   Finding inverse matrices
    cout << "Finding inverse matrices" << endl;

    const string inverse_matrices = "inverse_matrices/";
    new_folder(inverse_matrices);

    tester = Tests::create_tester_with_tests<element_type>();
    for (int i = 0; i < tester.get_num_tests(); ++i) {
        if (check_is_zero(dets[i])) {
            tester.next_test();
            continue;
        }

        // Print found inverse matrix
        ofstream fout(get_fout_for_test(inverse_matrices, "inv", i + 1,
                tester.get_num_tests()));
        fout << inverse_matrix(tester.next_test().first) << endl;
        fout.close();
    }

    //   Determine whether Gaussian elimination is stable
    cout << "Determining Gaussian elimination stability" << endl;

    const string stability = "gauss_stability/";
    new_folder(stability);

    element_type max_deviation = 0;

    tester = Tests::create_tester_with_tests<element_type>();
    for (int i = 0; i < tester.get_num_tests(); ++i) {
        //   Creating generator of random numbers
        std::mt19937 gen(SEED);
        const element_type eps1 = 1e-3;
        std::uniform_real_distribution<> urd(-eps1, eps1);

        auto test = tester.next_test();
        auto A = test.first;
        auto f = test.second;

        //   Solving SLE Ax = f
        Me sol1;
        try {
            sol1 = SLEGM(A, f);
        } catch (domain_error &e) {
            cerr << e.what() << endl;
            continue;
        }

        //   Solving SLE Bx = g, where B and g are slightly modified matrices
        // A and f
        auto B = test.first;
        for (int row = 0; row < A.get_rows(); ++row) {
            for (int col = 0; col < A.get_cols(); ++col) {
                B[row][col] += urd(gen);
            }
        }

        auto g = test.second;
        for (int row = 0; row < g.get_rows(); ++row) {
            g[row][0] += urd(gen);
        }

        Me sol2;
        try {
            sol2 = SLEGM(B, g);
        } catch (domain_error &e) {
            cerr << e.what() << endl;
            continue;
        }

        //   Counting standard deviation
        element_type dif = 0;
        for (int row = 0; row < sol1.get_rows(); ++row) {
            dif += pow(sol1[row][0] - sol2[row][0], 2);
        }
        dif = sqrt(dif / sol1.get_rows());

        max_deviation = max(max_deviation, dif);

        //   Print it out
        ofstream fout(get_fout_for_test(stability, "stab", i + 1,
                tester.get_num_tests()));
        fout << dif << endl;
        fout.close();
    }
    cout << "\tMaximum standard deviation: " << max_deviation << endl;

    //   Counting speed of convergence rate of iterations
    cout << "Counting speed of convergence rate of iterations" << endl;

    const string iter_cov = "iter_convergance/";
    new_folder(iter_cov);

    tester = Tests::create_tester_with_tests<element_type>();
    for (int i = 0; i < tester.get_num_tests(); ++i) {
        auto test = tester.next_test();
        auto A = test.first;
        auto f = test.second;

        const double eps2 = 1e-3;

        int min_iters = -1;
        double wmin = -1;

        //   Looking for minimum number of iterations for every w with
        // step eps2
        for (double w = eps2; w <= 2 - eps2; w += eps2) {
            //   Run SOR solver with w and count number of iterations
            int iters = -1;
            try {
                SLE_SOR(A, f, w, &iters);
            } catch (domain_error &e) {
                cerr << e.what() << endl;
                continue;
            }

            if (iters == -1) {
                continue;
            }

            //   Find minimum number of iterations
            if (min_iters == -1 || min_iters > iters) {
                min_iters = iters;
                wmin = w;
            }
        }

        // Print minimum number of iterations and coresponding w
        ofstream fout(get_fout_for_test(iter_cov, "cov", i + 1,
                tester.get_num_tests()));
        fout << min_iters << " (" << wmin << ")" << endl;
        fout.close();
    }

    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 <Eigen/Dense>

using Eigen::MatrixXd;
using namespace Eigen;
using namespace std;

int test()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  cout << m << endl;
}

int main()
{
  MatrixXd m = MatrixXd::Random(3,3);
  m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
  cout << "m =" << endl << m << endl;
  VectorXd v(3);
  v << 1, 2, 3;
  cout << "m * v =" << endl << m * v << endl;
}


#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();
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2005 StatPro Italia srl
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/quantlib.hpp>
#include <boost/timer.hpp>
#include "utilities.hpp"
#include "adjointcurvefittingtest.hpp"
#include "adjointtestutilities.hpp"

using namespace QuantLib;
using namespace boost::unit_test_framework;

namespace
{
	class BraceVolatilityFunction
	{
		Real a_, b_, c_, d_;
	public:
		inline BraceVolatilityFunction(Real a, Real b, Real c, Real d)
			: a_(a), b_(b), c_(c), d_(d)
		{
		}
		inline ~BraceVolatilityFunction() {}

		inline Real operator() (Real t)
		{
			return (a_ + b_*t)*std::exp(-c_*t) + d_;
		}
	};

	struct LSCurvesValue
	{
		static std::deque<std::string > get_columns()
		{
			static std::deque<std::string > columns =
			{
				"x", "Observed", "Estimated", "True"
			};

			return columns;
		}

		template <typename stream_type>
		friend inline stream_type&
			operator << (stream_type& stm, LSCurvesValue& v)
		{
				stm << v.xvalue_
					<< ";" << v.observed_
					<< ";" << v.estimated_
					<< ";" << v.true_ << std::endl;

				return stm;
			}

		Real observed_;
		Real estimated_;
		Real true_;
		Real xvalue_;
	};

	/*!
	Curve Fitting Problem
	*/
	class CurveFittingProblem : public LeastSquareProblem
	{
		const Array &ttm_;
		const Array &target_;

	public:
		/*!
		Default constructor : set time to maturity vector
		and target value
		*/

		static Size counter;

		CurveFittingProblem(const Array &ttm, const Array &target) : ttm_(ttm), target_(target)
		{
		}

		//! Destructor
		virtual ~CurveFittingProblem() {}

		//! Size of the least square problem
		virtual size_t size()
		{
			return ttm_.size();
		}

		//! return function and target values
		virtual void targetAndValue(const Array& coefficients, Array& target, Array& fct2fit)
		{
			BraceVolatilityFunction bvf(coefficients[0], coefficients[1], coefficients[2], coefficients[3]);

			target = target_;// target values
			for (Size i = 0; i < ttm_.size(); ++i)
				fct2fit[i] = bvf(ttm_[i]);
		}

		//! return function, target and first derivatives values
		virtual void targetValueAndGradient(const Array& coefficients, Matrix& grad_fct2fit, Array& target, Array& fct2fit)
		{
			Size n = coefficients.size();
			std::vector<cl::TapeDouble> coef(n);
			std::vector<double> coefD(n);
			Size m = ttm_.size();
			for (size_t i = 0; i < n; i++)
			{
				coef[i] = coefficients[i].value();
				coefD[i] = (double)coefficients[i];
			}

			cl::Independent(coef);

			BraceVolatilityFunction bvf(coef[0], coef[1], coef[2], coef[3]);

			std::vector<cl::TapeDouble> y(m);
			for (Size i = 0; i < m; ++i)
			{
				y[i] = bvf(ttm_[i]).value();
			}
			cl::TapeFunction<double> f(coef, y);
			//Compute derivatives using Jacobian
			std::vector<double> Jacobian(m*n);
			Jacobian = f.Jacobian(coefD);

			target = target_;// target values
			for (Size i = 0; i < ttm_.size(); ++i)
			{
				// function value at current point coefficients
				fct2fit[i] = bvf(ttm_[i]);
			}

			/*
			matrix of first derivatives :
			the derivatives with respect to the parameter a,b,c,d
			are stored by row.
			*/
			counter++;
			for (size_t i = 0; i < m; i++)
			for (size_t j = 0; j < n; j++)
				grad_fct2fit[i][j] = Jacobian[i*n + j];

		}
	};

	Size CurveFittingProblem::counter = 0;
}

/*
We define here an inverse problem to show how to fit
parametric function to data.
*/
bool AdjointCurveFittingTest::testCurveFitting()
{
	BOOST_TEST_MESSAGE("Testing curve fitting with AAD...\n");
	bool result = false;
#ifdef CL_TAPE_CPPAD
	boost::mt19937 rng;
	boost::normal_distribution<> nd(0.0, 0.01);
	boost::variate_generator<boost::mt19937&, boost::normal_distribution<>> noise(rng, nd);

	/*
	Parameter values that produce the volatility hump.
	Consider it as optimal values of the curve fitting
	problem.
	*/
	std::vector<Real> coefficients_ =
	{
		0.147014,
		0.057302,
		0.249964,
		0.148556
	};

	Array coefficients(coefficients_.begin(), coefficients_.end());

	// Define the target volatility function
	BraceVolatilityFunction bvf(coefficients[0], coefficients[1], coefficients[2], coefficients[3]);

	// start date of volatilty
	const Real startDate = 0.0;
	// end date of volatility
	const Real endDate = 20.;
	// period length between values (in year fraction : quarterly)
	const Real period = 0.1;

	// number of period
	size_t periodNumber = (size_t)(endDate / period);

	Array targetValue(periodNumber);
	Array timeToMaturity(periodNumber);

	// Fill target and time to maturity arrays
	for (size_t i = 0; i < periodNumber; ++i)
	{
		const Real t = startDate + i * period;
		timeToMaturity[i] = t;
		targetValue[i] = bvf(t) + noise();
	}

	// Accuracy of the optimization method
	const Real accuracy = 1e-5;// It is the square of the accuracy
	// Maximum number of iterations
	Size maxiter = 10000;

	Array initialValue(4, 0.1);

	// Least square optimizer
	NoConstraint nc;
	NonLinearLeastSquare lsqnonlin(nc, accuracy, maxiter, boost::shared_ptr<OptimizationMethod>(new ConjugateGradient()));

	// Define the least square problem
	CurveFittingProblem cfp(timeToMaturity, targetValue);

	// Set initial values
	lsqnonlin.setInitialValue(initialValue);
	// perform fitting
	Array solution = lsqnonlin.perform(cfp);
	BraceVolatilityFunction bvf_est(solution[0], solution[1], solution[2], solution[3]);

	// Plot stream
	cl::AdjointTestOutput out("AdjointCurveFitting"
							  , { { "filename", "CurveFitting" }
	, { "ylabel", "F(x)" }
	, { "not_clear", "Not" }
	, { "title", "Least-squares curve fitting" }
	, { "cleanlog", "false" }
	, { "xlabel", "x" } });

	cl::AdjointTestOutput outPerform("AdjointCurveFitting/"
									 , { { "filename", "LSCurves" }
	, { "not_clear", "Not" }
	, { "line_box_width", "-5" }
	, { "title", "Swaption NPV differentiation performance with respect to volatility" }
	, { "ylabel", "Time (s)" }
	, { "xlabel", "Number of volatilities" } });

#if defined CL_GRAPH_GEN

	// The observed volatility function
	std::vector<LSCurvesValue> Volatility;

	LSCurvesValue temp;
	// Fill target and time to maturity arrays
	for (size_t i = 0; i < periodNumber; ++i)
	{
		const Real t = startDate + i * period;
		temp.xvalue_ = t;
		timeToMaturity[i] = t;
		temp.observed_ = targetValue[i];
		temp.estimated_ = bvf_est(t);
		temp.true_ = bvf(t);
		Volatility.push_back(temp);
	}
	std::cout << Volatility.size() << std::endl;
	outPerform << Volatility;
#endif

	// check the result with defined tolerance
	const Real tollerance = 1e-2;
	result = true;
	for (Size i = 0; i < solution.size(); i++)
	{
		if (solution[i] - coefficients[i] > tollerance)
		{
			result = false;
			BOOST_ERROR("Optimal and solution values [" << i << "] mismatch."
						<< "\nOptimal values  : " << coefficients[i]
						<< "\nSolution values : " << solution[i]
						<< "\nError  : " << (solution[i] - coefficients[i])
						<< "\nTolerance:  " << tollerance);
		}
	}

#endif
	return result;
}

test_suite* AdjointCurveFittingTest::suite()
{
	test_suite* suite = BOOST_TEST_SUITE("AAD curve fitting test");
	suite->add(QUANTLIB_TEST_CASE(&AdjointCurveFittingTest::testCurveFitting));
	return suite;
}

#ifdef CL_ENABLE_BOOST_TEST_ADAPTER

BOOST_AUTO_TEST_SUITE(ad_curve_fitting)

BOOST_AUTO_TEST_CASE(testCurveFitting)
{
	BOOST_CHECK(AdjointCurveFittingTest::testCurveFitting());
}

BOOST_AUTO_TEST_SUITE_END()

#endif


//
// 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};
}