#pragma once
#include <cmath>
#include <autoppl/math/math.hpp>
#include <autoppl/util/traits/dist_expr_traits.hpp>
#include <fastad_bits/util/type_traits.hpp>
#include <Eigen/Dense>

// MSVC does not seem to support M_PI
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace ppl {
namespace math {

using dist_value_t = util::dist_value_t;

/////////////////////////////////
// Compile-time Constants
/////////////////////////////////

inline constexpr double SQRT_TWO_PI =
    2.506628274631000502415765284811045;
inline constexpr double LOG_SQRT_TWO_PI =
    0.918938533204672741780329736405617;

/////////////////////////////////
// Normal Density
/////////////////////////////////

// sss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_pdf(const XType& x,
                               const MeanType& mean,
                               const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z = (x - mean) / sigma;
    return std::exp(-0.5 * z * z) / (sigma * SQRT_TWO_PI);
}

// vss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const MeanType& mean,
                               const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean).matrix().squaredNorm() / (sigma * sigma);
    return std::exp(-0.5 * z_sq) / std::pow(sigma * SQRT_TWO_PI, x.size());
}

// vvs
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const Eigen::MatrixBase<MeanType>& mean,
                               const SigmaType& sigma)
{
    static_assert(ad::util::is_eigen_vector_v<MeanType>);
    assert(x.size() == mean.size());
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean.array()).matrix().squaredNorm() / (sigma * sigma);
    return std::exp(-0.5 * z_sq) / std::pow(sigma * SQRT_TWO_PI, x.size());
}

// vsv, vsm
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType>
        > >
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const MeanType& mean,
                               const Eigen::MatrixBase<SigmaType>& sigma)
{
    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean)/sigma.array()).matrix().squaredNorm();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * sigma.array().prod());

    } else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * det);
    }
}

// vvv, vvm
template <class XType
        , class MeanType
        , class SigmaType>
inline dist_value_t normal_pdf(const Eigen::MatrixBase<XType>& x,
                               const Eigen::MatrixBase<MeanType>& mean,
                               const Eigen::MatrixBase<SigmaType>& sigma)
{
    static_assert(ad::util::is_eigen_vector_v<MeanType>);

    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        assert(x.size() == mean.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean.array())/sigma.array()).matrix().squaredNorm();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * sigma.array().prod());
    }

    else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        assert(x.size() == mean.size());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean.array()).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return std::exp(-0.5 * z_sq) / (std::pow(SQRT_TWO_PI, x.size()) * det);
    }
}

// sss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_log_pdf(const XType& x,
                                   const MeanType& mean,
                                   const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z = (x - mean) / sigma;
    return -0.5 * z * z - std::log(sigma) - LOG_SQRT_TWO_PI;
}

// vss
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType> &&
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                           const MeanType& mean,
                           const SigmaType& sigma)
{
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean).matrix().squaredNorm() / (sigma * sigma);
    return -0.5 * z_sq - (x.size() * (std::log(sigma) + LOG_SQRT_TWO_PI));
}

// vvs
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<SigmaType>
        > >
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                           const Eigen::MatrixBase<MeanType>& mean,
                           const SigmaType& sigma)
{
    static_assert(ad::util::is_eigen_vector_v<MeanType>);
    assert(x.size() == mean.size());
    if (sigma <= 0) return math::neg_inf<dist_value_t>;
    dist_value_t z_sq = (x.array() - mean.array()).matrix().squaredNorm() / (sigma * sigma);
    return -0.5 * z_sq - (x.size() * (std::log(sigma) + LOG_SQRT_TWO_PI));
}

// vsv, vsm
template <class XType
        , class MeanType
        , class SigmaType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MeanType>
        > >
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                                   const MeanType& mean,
                                   const Eigen::MatrixBase<SigmaType>& sigma)
{
    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean)/sigma.array()).matrix().squaredNorm();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(sigma.array().prod());
    }

    else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(det);
    }
}

// vvv, vvm
template <class XType
        , class MeanType
        , class SigmaType>
inline dist_value_t normal_log_pdf(const Eigen::MatrixBase<XType>& x,
                                   const Eigen::MatrixBase<MeanType>& mean,
                                   const Eigen::MatrixBase<SigmaType>& sigma)
{
    if constexpr (ad::util::is_eigen_vector_v<SigmaType>) {
        assert(x.size() == sigma.size());
        assert(x.size() == mean.size());
        if ((sigma.array() <= 0).any()) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = ((x.array() - mean.array())/sigma.array()).matrix().squaredNorm();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(sigma.array().prod());
    }

    else if constexpr (ad::util::is_eigen_matrix_v<SigmaType>) {
        assert(x.size() == sigma.rows());
        assert(x.size() == mean.size());
        Eigen::LLT<Eigen::MatrixXd> llt(sigma);
        if (llt.info() != Eigen::Success) return math::neg_inf<dist_value_t>;
        dist_value_t z_sq = llt.matrixL().solve((x.array() - mean.array()).matrix()).squaredNorm();
        dist_value_t det = llt.matrixL().determinant();
        return -0.5 * z_sq - (x.size() * LOG_SQRT_TWO_PI) - std::log(det);
    }
}

/////////////////////////////////
// Cauchy Density
/////////////////////////////////

// sss
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<LocType> &&
            std::is_arithmetic_v<ScaleType>
        > >
inline dist_value_t cauchy_log_pdf(const XType& x,
                                   const LocType& loc,
                                   const ScaleType& scale)
{
    auto diff = x - loc;
    return (scale > 0) ? -std::log(scale + diff * diff / scale) : neg_inf<double>;
}

// vss
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<LocType> &&
            std::is_arithmetic_v<ScaleType>
        > >
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                   const LocType& loc,
                                   const ScaleType& scale)
{
    bool cond = scale > 0.;
    auto diff = x.array() - loc;
    return cond ? -(scale + (1./scale) * diff * diff).log().sum() :
                  neg_inf<double>;
}

// vvs
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<ScaleType>
        > >
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<LocType>& loc,
                                    const ScaleType& scale)
{
    bool cond = scale > 0.;
    auto diff = x.array() - loc.array();
    return cond ? -(scale + (1./scale) * diff * diff).log().sum() : neg_inf<double>;
}

// vsv
template <class XType
        , class LocType
        , class ScaleType
        , class = std::enable_if_t<
            std::is_arithmetic_v<LocType>
        > >
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const LocType& loc,
                                    const Eigen::MatrixBase<ScaleType>& scale)
{
    bool cond = (scale.array() > 0.).all();
    auto diff = x.array() - loc;
    auto gamma = scale.array();
    return cond ? -(gamma + (1./gamma) * diff * diff).log().sum() : neg_inf<double>;
}

// vvv
template <class XType
        , class LocType
        , class ScaleType>
inline dist_value_t cauchy_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<LocType>& loc,
                                    const Eigen::MatrixBase<ScaleType>& scale)
{
    bool cond = (scale.array() > 0.).all();
    auto diff = x.array() - loc.array();
    auto gamma = scale.array();
    return cond ? -(gamma + (1./gamma) * diff * diff).log().sum() : neg_inf<double>;
}

/////////////////////////////////
// Uniform Density
/////////////////////////////////

// sss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_pdf(const XType& x,
                                const MinType& min,
                                const MaxType& max)
{
    return (min < x && x < max) ? 1. / (max - min) : 0;
}

// vss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const MinType& min,
                                const MaxType& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max).all();
    return cond ? std::pow(1./(max-min), x.size()) : 0;
}

// vvs
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const Eigen::MatrixBase<MinType>& min,
                                const MaxType& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max).all();
    return cond ? (1./(max-min.array())).prod() : 0;
}

// vsv
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType>
        > >
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const MinType& min,
                                const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max.array()).all();
    return cond ? (1./(max.array()-min)).prod() : 0;
}

// vvv
template <class XType
        , class MinType
        , class MaxType>
inline dist_value_t uniform_pdf(const Eigen::MatrixBase<XType>& x,
                                const Eigen::MatrixBase<MinType>& min,
                                const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max.array()).all();
    return cond ? (1./(max.array()-min.array())).prod() : 0;
}

// sss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_log_pdf(const XType& x,
                                    const MinType& min,
                                    const MaxType& max)
{
    return (min < x && x < max) ? -std::log(max - min) : neg_inf<double>;
}

// vss
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType> &&
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const MinType& min,
                                    const MaxType& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max).all();
    return cond ?
        static_cast<dist_value_t>(x.size())*(-std::log(max-min)) :
        neg_inf<double>;
}

// vvs
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MaxType>
        > >
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<MinType>& min,
                                    const MaxType& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max).all();
    return cond ? -(max-min.array()).log().sum() : neg_inf<double>;
}

// vsv
template <class XType
        , class MinType
        , class MaxType
        , class = std::enable_if_t<
            std::is_arithmetic_v<MinType>
        > >
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const MinType& min,
                                    const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min < x.array()).all() && (x.array() < max.array()).all();
    return cond ? -(max.array()-min).log().sum() : neg_inf<double>;
}

// vvv
template <class XType
        , class MinType
        , class MaxType>
inline dist_value_t uniform_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<MinType>& min,
                                    const Eigen::MatrixBase<MaxType>& max)
{
    bool cond = (min.array() < x.array()).all() && (x.array() < max.array()).all();
    return cond ? -(max.array()-min.array()).log().sum() : neg_inf<double>;
}

/////////////////////////////////
// Bernoulli Density
/////////////////////////////////

/**
 * Bernoulli pdf and log pdf (pmf actually).
 * It is defined to clip when p is out of the range [0,1],
 * i.e. if p < 0, then we take p = 0 and
 * if p > 1, then we take p = 1.
 */

// ss
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_pdf(const XType& x,
                                  const PType& p)
{
    if (p <= 0) return (x == 0) + 0.;
    else if (p >= 1) return (x == 1) + 0.;

    if (x == 1) return p;
    else if (x == 0) return 1. - p;
    else return 0.0;
}

// vs
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_pdf(const Eigen::MatrixBase<XType>& x,
                                  const PType& p)
{
    double pdf = 1.;
    for (int i = 0; i < x.size(); ++i) {
        pdf *= bernoulli_pdf(x(i), p);
    }
    return pdf;
}

// vv
template <class XType
        , class PType>
inline dist_value_t bernoulli_pdf(const Eigen::MatrixBase<XType>& x,
                                  const Eigen::MatrixBase<PType>& p)
{
    assert(x.size() == p.size());
    double pdf = 1.;
    for (int i = 0; i < x.size(); ++i) {
        pdf *= bernoulli_pdf(x(i), p(i));
    }
    return pdf;
}

// ss
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<XType> &&
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_log_pdf(const XType& x,
                                      const PType& p)
{
    if (p <= 0) {
        if (x == 0) return 0.;
        else return neg_inf<PType>;
    }
    else if (p >= 1) {
        if (x == 1) return 0.;
        else return neg_inf<PType>;
    }

    if (x == 1) return std::log(p);
    else if (x == 0) return std::log(1. - p);
    else return neg_inf<PType>;
}

// vs
template <class XType
        , class PType
        , class = std::enable_if_t<
            std::is_arithmetic_v<PType>
        > >
inline dist_value_t bernoulli_log_pdf(const Eigen::MatrixBase<XType>& x,
                                      const PType& p)
{
    double logpdf = 0.;
    for (int i = 0; i < x.size(); ++i) {
        logpdf += bernoulli_log_pdf(x(i), p);
    }
    return logpdf;
}

// vv
template <class XType
        , class PType>
inline dist_value_t bernoulli_log_pdf(const Eigen::MatrixBase<XType>& x,
                                      const Eigen::MatrixBase<PType>& p)
{
    assert(x.size() == p.size());
    double logpdf = 0.;
    for (int i = 0; i < x.size(); ++i) {
        logpdf += bernoulli_log_pdf(x(i), p(i));
    }
    return logpdf;
}

/////////////////////////////////
// Wishart Density
// Note: drops unnecessary constant terms
/////////////////////////////////
template <class XType
        , class VType
        , class NType>
inline dist_value_t wishart_log_pdf(const Eigen::MatrixBase<XType>& x,
                                    const Eigen::MatrixBase<VType>& v,
                                    const NType& n)
{
    Eigen::LLT<Eigen::MatrixXd> x_llt(x);
    Eigen::LLT<Eigen::MatrixXd> v_llt(v);
    auto log_det_x = std::log(x_llt.matrixL().determinant());
    auto log_det_v = std::log(v_llt.matrixL().determinant());
    auto tr = v_llt.solve(x).trace();
    dist_value_t p = v.rows();
    return (n - p - 1.) * log_det_x - 0.5 * tr - n * log_det_v;
}

} // namespace math
} // namespace ppl


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

void printResult(LCP sol, const int &testNum) {
    std::cout << "---Test #" << testNum << "---" << std::endl;
    std::cout << "M = \n" << sol.M << std::endl;
    std::cout << "q = " << sol.q.transpose() << std::endl;
    std::cout << "z = " << sol.z.transpose() << std::endl;
    std::cout << "w = " << sol.w.transpose() << std::endl;
    std::cout << "Exit condition: " << sol.exitCond << std::endl << std::endl;
}

void displayPassRate(const bool &result, int &nTestComplete, int &nTestPassed) {
    nTestComplete++;
    if (result) {
        nTestPassed++;
    }
    double passRate = (nTestPassed/nTestComplete) * 100;
    std::cout << "Pass Rate: " << passRate << " %\n" << std::endl;
}

bool test_1() {
    Eigen::Matrix<double, 2, 2> M {
        {2, -1},
        {-1, 1}
    };
    Eigen::Vector<double, 2> q {-3, 1};
    LCP sol = LCPSolve(M, q);

    Eigen::Vector<double, 2> zTruth {2.0, 1.0};
    double eps = 0.001;
    std::cout << "Running Test 1..." << std::endl;
    if ((sol.z - zTruth).norm() < eps) {
        std::cout << "Test 1 Passed" << std::endl;
        return true;
    } else {
        std::cout << "Test 1 Failed" << std::endl;
        return false;
    }
}

bool test_2() {
    Eigen::Matrix<double, 3, 3> M {
        {2, -1, 3},
        {-1, 1, 6},
        {1, 5, -2}
    };
    Eigen::Vector<double, 3> q {-2, 5, -1};
    LCP sol = LCPSolve(M, q);

    Eigen::Vector<double, 3> zTruth {1, 0, 0};
    double eps = 0.001;
    std::cout << "Running Test 2..." << std::endl;
    if ((sol.z - zTruth).norm() < eps) {
        std::cout << "Test 2 Passed" << std::endl;
        return true;
    } else {
        std::cout << "Test 2 Failed" << std::endl;
        return false;
    }
}

bool test_3() {
    Eigen::Matrix<double, 4, 4> M {
            {1, 5, -3, 4},
            {1, -4, -5, -4},
            {-3, 3, -4, 4},
            {4, 9, 0, 1}
        };
    Eigen::Vector<double, 4> q {-3, 4, 5, 1};
    LCP sol = LCPSolve(M, q);

    Eigen::Vector<double, 4> zTruth {1.5294, 0.64705, 0.58823, 0};
    double eps = 0.001;
    std::cout << "Running Test 3..." << std::endl;
    if ((sol.z - zTruth).norm() < eps) {
        std::cout << "Test 3 Passed" << std::endl;
        return true;
    } else {
        std::cout << "Test 3 Failed" << std::endl;
        return false;
    }
}

bool test_4() {
    // Secondary ray.
    Eigen::Matrix<double, 3, 3> M {
        {2, -1, 3},
        {-1, 1, 6},
        {1, 5, -2}
    };
    Eigen::Vector<double, 3> q {-2, 5, -1.5};
    LCP sol = LCPSolve(M, q);

    Eigen::Vector<double, 3> zTruth {0, 0, 0.1};
    double eps = 0.001;
    std::cout << "Running Test 4..." << std::endl;
    if ((sol.z - zTruth).norm() < eps) {
        std::cout << "Test 4 Passed" << std::endl;
        return true;
    } else {
        std::cout << "Test 4 Failed" << std::endl;
        return false;
    }
}

bool test_5() {
    Eigen::Matrix<double, 2, 2> M {
        {1, -5},
        {2, 1}
    };
    Eigen::Vector<double, 2> q {-4, 3};
    LCP sol = LCPSolve(M, q);

    Eigen::Vector<double, 2> zTruth {4, 0};
    double eps = 0.001;
    std::cout << "Running Test 5..." << std::endl;
    if ((sol.z - zTruth).norm() < eps) {
        std::cout << "Test 5 Passed" << std::endl;
        return true;
    } else {
        std::cout << "Test 5 Failed" << std::endl;
        return false;
    }
}

int main() {
    // Setup percentage display.
    int nTestComplete = 0;
    int nTestPassed = 0;
    bool result;

    // Run tests.
    std::cout << "Running LCPSolve Tests...\n" << std::endl;

    // Test 1.
    result = test_1();
    displayPassRate(result, nTestComplete, nTestPassed);

    // Test 2.
    result = test_2();
    displayPassRate(result, nTestComplete, nTestPassed);

    // Test 3.
    result = test_3();
    displayPassRate(result, nTestComplete, nTestPassed);

    // Test 4.
    result = test_4();
    displayPassRate(result, nTestComplete, nTestPassed);

    // Test 5.
    result = test_5();
    displayPassRate(result, nTestComplete, nTestPassed);

    return 0;
}


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

int main() {

   Eigen::Vector3d v1, v2;
   std::vector<int> v3, v4;
   std::array<int,3> v5 {1, 2, 3};
   std::array<int,3> v6;
   int i;

   // using eigen:
   v1 << 1.0, 2.0, 3.0;
   v2 = v1;
   v1(0) = 4.0;

   std::cout << "using eigen" << std::endl;
   std::cout << v1.transpose() << std::endl;
   std::cout << v2.transpose() << std::endl;

   // using vector:
   v3 = {1, 2, 3};
   v4 = v3;
   v3[0] = 4;

   std::cout << " " << std::endl;
   std::cout << "using vector" << std::endl;
   for (i=0; i<=2; i=i+1) {
      std::cout << v3[i] ;
   }
   std::cout << " " << std::endl;
   for (i=0; i<=2; i=i+1) {
      std::cout << v3[i] ;
   }
   std::cout << " " << std::endl;

   // using array (the library. built in errors can't use "="):
   v6 = v5;
   v5[0] = 4;

   std::cout << " " << std::endl;
   std::cout << "using array (the library):" << std::endl;
   for (i=0; i<=2; i=i+1) {
      std::cout << v5[i] ;
   }
   std::cout << " " << std::endl;
   for (i=0; i<=2; i=i+1) {
      std::cout << v6[i] ;
   }
   std::cout << " " << std::endl;

}


/*
	* Generates the basis { i  sigma_1 , i sigma_2 ......, i sigma_n }
	* Where sigma_i is a generalized pauli matrix
*/

#define _PAULI
#ifdef _PAULI

#include <cstdlib>
#include <iostream>
#include <cmath>
#include <armadillo>
#include <complex>

using namespace arma;
using std::vector;

class Pauli
{
	public:

		Pauli(int matSize);
		~Pauli();

		vector<cx_mat> pauliBasisObject;
		//cx_mat lieBracket(cx_mat A, cx_mat B);

	private:

		//size of matrix - n x n matrices
		long n;
		//dimension of space
		long dimension;
		int kD(int r, int s);

		cx_double cmplxI = cx_double(0.0, 1.0);
		cx_mat E(int r, int s);

		//generating methods
		Pauli& sigmaTypeOne();
		Pauli& sigmaTypeTwo();
		Pauli& sigmaTypeThree();

};

#include "Pauli.cpp"
#endif


#include "gtest/gtest.h"

#include <iostream>
#include <Eigen/Dense>
#include "polytope.hpp"
#include "oblog.hpp"

using Eigen::MatrixXd;

TEST(TESTEigen, basicTest) {
  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);
  std::cout << m << std::endl;

  OB::Transform t;
  std::cout << "Size of isometry: " << sizeof(t) << std::endl;;
  OB::Matrix3 rot = t.rotation();
  OB::Vector trans = t.translation();
  std::cout << " rotation: " << rot << std::endl;
  std::cout << " translation: " << trans << std::endl;
  //spdlog::info(" translation: {}", trans);
  //spdlog::info(" rotation: {}", v);

  std::cout.precision(17);

  OB::Real valor = 7;
  OB::Real otro = 7;
  std::cout << "valor: "<< valor << " isApprox " << otro<< " ->  " << Eigen::internal::isApprox(valor, otro) << std::endl;
  otro = 7.00000000001;
  std::cout << "valor: "<< valor << " isApprox " << otro<< " ->  " << Eigen::internal::isApprox(valor, otro) << std::endl;
  otro = 7.000000000001;
  std::cout << "valor: "<< valor << " isApprox " << otro<< " ->  " << Eigen::internal::isApprox(valor, otro) << std::endl;
  otro = 6.99999999999;
  std::cout << "valor: "<< valor << " isApprox " << otro<< " ->  " << Eigen::internal::isApprox(valor, otro) << std::endl;
  otro = 6.999999999999;
  std::cout << "valor: "<< valor << " isApprox " << otro<< " ->  " << Eigen::internal::isApprox(valor, otro) << std::endl;

  // for is_zero
  valor = 0.0001;
  std::cout << "valor: " << valor << " is_zero -> " << Eigen::internal::isMuchSmallerThan(valor, static_cast<OB::Real>(1.0)) << std::endl;;
  valor = 1;
  std::cout << "valor: " << valor << " is_zero -> " << Eigen::internal::isMuchSmallerThan(valor, static_cast<OB::Real>(1.0)) << std::endl;;
  valor = 0;
  std::cout << "valor: " << valor << " is_zero -> " << Eigen::internal::isMuchSmallerThan(valor, static_cast<OB::Real>(1.0)) << std::endl;;
  valor = 0.00000000001;
  std::cout << "valor: " << valor << " is_zero -> " << Eigen::internal::isMuchSmallerThan(valor, static_cast<OB::Real>(1.0)) << std::endl;;
  valor = 0.000000000001;
  std::cout << "valor: " << valor << " is_zero -> " << Eigen::internal::isMuchSmallerThan(valor, static_cast<OB::Real>(1.0)) << std::endl;;

  valor = -0.00000000001;
  std::cout << "valor: " << valor << " is_zero -> " << Eigen::internal::isMuchSmallerThan(valor, static_cast<OB::Real>(1.0)) << std::endl;;
  valor = -0.000000000001;
  std::cout << "valor: " << valor << " is_zero -> " << Eigen::internal::isMuchSmallerThan(valor, static_cast<OB::Real>(1.0)) << std::endl;;

}

TEST(TESTEigen, matrixInizialization) {
  Eigen::Vector3d v1;
  v1 << 1,2,3;
  Eigen::Vector3d v2;
  v2 << 4,5,6;
  Eigen::Vector3d v3;
  v3 << 7,8,9;

  Eigen::Matrix3d matrix;
  matrix << v1, v2, v3;
  std::cout << "Matrix " << matrix << std::endl;
}


#pragma once // header guard

#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>

#include <cmath>

//! Shortcut for container vector of nodes/weights
using vector = Eigen::VectorXd;

//! Structure containing a Quadrature rule on [-1,1], comprised of weights and nodes
struct QuadRule {
    vector weights;
    vector nodes;
};

//! \brief Golub-Welsh implementation 5.3.35
//! \param[in] n number of Gauss nodes
//! \return structure QuadRule containing nodes and wights of gauss Quadrature
QuadRule gauleg(unsigned int n);

Nummerical Methods for CSE/PS11/templates_ps11/gauleg.hpp


// Copyright Paul Bristow 2012.
// Copyright John Maddock 2010.

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

/*! \brief Examples of using the enhanced math constants.
    \details This allows for access to constants via functions like @c pi(),
    and also via namespaces, @c using @c namespace boost::math::double_constants;
    called simply @c pi.
*/

#include <boost/math/constants/constants.hpp>

#include <iostream>
using std::cout;
using std::endl;

#include <limits>
using std::numeric_limits;

/*! \brief Examples of a template function using constants.
    \details This example shows using of constants from function calls like @c pi(),
    rather than the 'cute' plain @c pi use in non-template applications.

    \tparam Real radius parameter that can be a built-in like float, double,
      or a user-defined type like multiprecision.
    \returns Area = pi * radius ^ 2
*/

//[math_constants_eg1
template<class Real>
Real area(Real r)
{
  using namespace boost::math::constants;

  return pi<Real>() * r * r;
}
//] [/math_constants_eg1]

int main()
{

  { // Boost.Math constants using function calls like pi().
    // using namespace boost::math::constants;

   using boost::math::constants::pi;
   using boost::math::constants::one_div_two_pi;

  cout.precision(numeric_limits<double>::max_digits10);
  cout << "double pi =  boost::math::double_constants::pi = " << pi<double>() << endl;
  //   double pi =  boost::math::double_constants::pi = 3.1415926535897931
  double r = 1.234567890123456789;
  double d = pi<double>() * r * r;

  cout << "d = " << d << ", r = " << r << endl;

  float rf = 0.987654321987654321f;

  float pif = boost::math::constants::pi<float>();
  cout << "pidf =  boost::math::constants::pi() = " << pif << endl;
  //   pidf =  boost::math::float_constants::pi = 3.1415927410125732

    //float df = pi * rf * rf; // conversion from 'const double' to 'float', possible loss of data
  float df = pif * rf * rf;

  cout << "df = " << df << ", rf = " << rf << endl;

  cout << "one_div_two_pi " << one_div_two_pi<double>() << endl;

  using boost::math::constants::one_div_two_pi;

  cout << "one_div_root_two_pi " << one_div_two_pi<double>() << endl;
  }

  { // Boost math new constants using namespace selected values, like pi.

    //using namespace boost::math::float_constants;
    using namespace boost::math::double_constants;

    double my2pi = two_pi; // Uses boost::math::double_constants::two_pi;

    cout << "double my2pi = " << my2pi << endl;

    using boost::math::float_constants::e;
    float my_e = e;
    cout << "float my_e  " << my_e << endl;

    double my_pi = boost::math::double_constants::pi;
    cout << "double my_pi = boost::math::double_constants::pi =  " << my_pi << endl;

    // If you try to use two namespaces, this may, of course, create ambiguity:
    // it is not too difficult to do this inadvertently.
    using namespace boost::math::float_constants;
    //cout << pi << endl; // error C2872: 'pi' : ambiguous symbol

  }
  {

//[math_constants_ambiguity
     // If you use more than one namespace, this will, of course, create ambiguity:
     using namespace boost::math::double_constants;
     using namespace boost::math::constants;

      //double my_pi = pi(); // error C2872: 'pi' : ambiguous symbol
      //double my_pi2 = pi; // Context does not allow for disambiguation of overloaded function

     // It is also possible to create ambiguity inadvertently,
     // perhaps in other peoples code,
     // by making the scope of a namespace declaration wider than necessary,
     // therefore is it prudent to avoid this risk by localising the scope of such definitions.
//] [/math_constants_ambiguity]

  }

  { // You can, of course, use both methods of access if both are fully qualified, for examples:

    //cout.precision(std::numeric_limits<double>::max_digits10);// Ideally.
    cout.precision(2 + std::numeric_limits<double>::digits * 3010/10000); // If no max_digits10.

    double my_pi1 = boost::math::constants::pi<double>();
    double my_pid = boost::math::double_constants::pi;
    cout << "boost::math::constants::pi<double>() = " << my_pi1 << endl
         << "boost::math::double_constants::pi = " << my_pid << endl;

    // cout.precision(std::numeric_limits<float>::max_digits10); // Ideally.
    cout.precision(2 + std::numeric_limits<double>::digits * 3010/10000); // If no max_digits10.
    float my_pif = boost::math::float_constants::pi;
    cout << "boost::math::float_constants::pi = " << my_pif << endl;

  }

  { // Use with templates

    // \warning it is important to be very careful with the type provided as parameter.
    // For example, naively providing an @b integer instead of a floating-point type can be disastrous.
    // cout << "Area = " << area(2) << endl; // warning : 'return' : conversion from 'double' to 'int', possible loss of data
    // Failure to heed this warning can lead to very wrong answers!
    //  Area = 12 !!  = 3 * 2 * 2
//[math_constants_template_integer_type
    //cout << "Area = " << area(2) << endl; //   Area = 12!
    cout << "Area = " << area(2.) << endl;  //   Area = 12.566371

    // You can also avoid this by being explicit about the type of @c area.
    cout << "Area = " << area<double>(2) << endl;

//] [/math_constants_template_integer_type]

  }
  {
    using  boost::math::constants::pi;
    double my_pi3 = pi<double>();
    //double my_pi4 = pi<>(); cannot find template type.
    //double my_pi4 = pi();

  }

} // int main()

/*[constants_eq1_output

Output:

  double pi =  boost::math::double_constants::pi = 3.1415926535897931
  d = 4.7882831840285398, r = 1.2345678901234567
  pidf =  boost::math::constants::pi() = 3.1415927410125732
  df = 3.0645015239715576, rf = 0.98765432834625244
  one_div_two_pi 0.15915494309189535
  one_div_root_two_pi 0.15915494309189535
  double my2pi = 6.2831853071795862
  float my_e  2.7182817459106445
  double my_pi = boost::math::double_constants::pi =  3.1415926535897931
  boost::math::constants::pi<double>() = 3.1415926535897931
  boost::math::double_constants::pi = 3.1415926535897931
  boost::math::float_constants::pi = 3.1415927410125732
  Area = 12.566370614359172
  Area = 12.566370614359172

] [/constants_eq1_output]
*/


/**
 * @file propagator.h
 * @brief NPDE homework NonLinSchroedingerEquation code
 * @author Oliver Rietmann
 * @date 04.05.2020
 * @copyright Developed at ETH Zurich
 */

#include "propagator.h"

#include <cmath>
#include <complex>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

namespace NonLinSchroedingerEquation {

// KineticPropagator
/* SAM_LISTING_BEGIN_1 */
KineticPropagator::KineticPropagator(const SparseMatrixXd &A,
                                     const SparseMatrixXcd &M, double tau) {
  // Defeats the rationale of expression templates, but acceptable here, because
  // executed only once in the constructor.
  B_plus_ = M + 0.5 * tau * A.cast<std::complex<double>>();
  SparseMatrixXcd B_minus = M - 0.5 * tau * A.cast<std::complex<double>>();
  // This is the expensive step: LU-factorization of a big sparse matrix.
  // Precomputation is essential
  solver_.compute(B_minus);
}

Eigen::VectorXcd KineticPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  // Cheap elimination steps operating on the LU-factors. Effort is almost O(N)
  // thanks to sophisticated fill-in avoiding techniques employed by the sparse
  // solvers.
  return solver_.solve(B_plus_ * mu);
}
/* SAM_LISTING_END_1 */

// InteractionPropagator
/* SAM_LISTING_BEGIN_2 */
InteractionPropagator::InteractionPropagator(double tau) {
  // We know that the discrete evolution operator for the non-linear part of the
  // method-of-lines ODE boils down to componentwise multiplication of the
  // vector with a single phase shift. Thus, this phase shift can be
  // precomputed, here by means of a lambda function
  phase_multiplier_ = [tau](std::complex<double> z) {
    const std::complex<double> i(0, 1);
    return std::exp(-i * tau * std::norm(z)) * z;
  };
}

Eigen::VectorXcd InteractionPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  // Eigen's way of applying a function to all components of a vector.
  return mu.unaryExpr(phase_multiplier_);
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
SplitStepPropagator::SplitStepPropagator(const SparseMatrixXd &A,
                                         const SparseMatrixXcd &M, double tau)
    : kineticPropagator_(A, M, 0.5 * tau), interactionPropagator_(tau) {}

Eigen::VectorXcd SplitStepPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  Eigen::VectorXcd nu(mu.size());
  nu = kineticPropagator_(mu);
  nu = interactionPropagator_(nu);
  nu = kineticPropagator_(nu);
  return nu;
}
/* SAM_LISTING_END_3 */

}  // namespace NonLinSchroedingerEquation


#pragma once

#include <ros/ros.h>
#include <math.h>
#include <iostream>
// #include <Eigen/Core>
// #include <Eigen/LU>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <cav_msgs/TrajectoryPlan.h>

namespace platoon_control
{

    class PurePursuit
    {

    public:
    	PurePursuit() = default;

    	double calculateSteer(const cav_msgs::TrajectoryPlanPoint& tp);

		// geometry pose
		geometry_msgs::Pose current_pose_;

    private:

		// calculate the lookahead distance from next trajectory point
		double getLookaheadDist(const cav_msgs::TrajectoryPlanPoint& tp) const;

		// calculate yaw angle of the vehicle
		double getYaw() const;

		// calculate alpha angle
		double getAlpha(double lookahead, std::vector<double> v1, std::vector<double> v2) const;

		// calculate steering direction
		int getSteeringDirection(std::vector<double> v1, std::vector<double> v2) const;

		// calculate command velocity from trajecoty point
		double getVelocity(const cav_msgs::TrajectoryPlanPoint& tp, double delta_pos) const;

		// vehicle wheel base
		double wheelbase_ = 2.7;

		// coefficient for smooth steering
		double Kdd_ = 4.5;

		double prev_steering = 0.0;

		// previous trajectory point
		cav_msgs::TrajectoryPlanPoint tp0;

		// helper function (if needed)
		// inline double deg2rad(double deg) const
		// {
		// 	return deg * M_PI / 180;
		// }  // convert degree to radian

    };
}


#include "problemes.h"
#include "arithmetique.h"

#include <boost/math/constants/constants.hpp>

typedef unsigned long long nombre;
typedef std::pair<long double, long double> coordonnee;

namespace {
    long double norme(const coordonnee &v) {
        return std::hypot(v.first, v.second);
    }

    std::vector<coordonnee> convertion(const std::vector<coordonnee> &v) {
        if (v.empty())
            return v;

        std::vector<coordonnee> resultat{
                std::make_pair(v.front().first - v.back().first, v.front().second - v.back().second)};
        for (size_t k = 1; k < v.size(); ++k) {
            auto p = std::make_pair(v.at(k).first - v.at(k - 1).first, v.at(k).second - v.at(k - 1).second);
            auto n = norme(p);
            p.first /= n;
            p.second /= n;
            resultat.push_back(p);
        }

        return resultat;
    }

    std::vector<coordonnee> S(size_t n) {
        std::vector<coordonnee> s;
        for (size_t k = 0; k < n; ++k) {
            const long double x = (2.0L * k - 1L) / n + M_PIl;
            s.emplace_back(std::cos(x), std::sin(x));
        }

        return s;
    }

    long double produit_scalaire(const coordonnee &v1, const coordonnee &v2) {
        return (v1.first * v2.first + v1.second * v2.second) / (norme(v1) * norme(v2));
    }
}

ENREGISTRER_PROBLEME(228, "Minkowski Sums") {
    // Let Sn be the regular n-sided polygon – or shape – whose vertices vk (k = 1,2,…,n) have coordinates:
    //
    // 			x_k = cos( 2*(k-1)/n × 180° )
    // 			y_k = sin( 2*(k-1)/n × 180° )
    //
    // Each Sn is to be interpreted as a filled shape consisting of all points on the perimeter and in the interior.
    //
    // The Minkowski sum, S+T, of two shapes S and T is the result of adding every point in S to every point in T, where
    // point addition is performed coordinate-wise: (u, v) + (x, y) = (u+x, v+y).
    //
    // For example, the sum of S3 and S4 is the six-sided shape shown in pink below:
    //
    //							picture showing S_3 + S_4
    //
    // How many sides does S1864 + S1865 + … + S1909 have?
    std::set<coordonnee, std::function<bool(const coordonnee &v1, const coordonnee &v2)>> minkowski(
            [](const coordonnee &v1, const coordonnee &v2) {
                if (std::abs(1.0L - produit_scalaire(v1, v2)) < 0.0000000000001L)
                    return false;

                return v1 < v2;
            });

    for (size_t n = 1864; n <= 1909; ++n) {
        auto Sn = convertion(S(n));
        minkowski.insert(Sn.begin(), Sn.end());
    }

    nombre resultat = minkowski.size();
    return std::to_string(resultat);
}


#ifndef NEURALNET_HPP
#define NEURALNET_HPP

#include <eigen3/Eigen/Eigen>
#include <iostream>
#include <vector>
#include <string>
#include <boost/range/combine.hpp>
#include <fstream>

class LayerDense {
    public:
        int n_inputs; // number of inputs
        int n_neurons; // number of neurons

        Eigen::MatrixXd inputs; // inputs
        Eigen::MatrixXd weights;
        Eigen::MatrixXd weight_momentums;
        Eigen::VectorXd biases;
        Eigen::VectorXd bias_momentums;
        Eigen::MatrixXd output;

        Eigen::MatrixXd dweights; // derivative wrt weights
        Eigen::VectorXd dbiases; // derivative wrt biases
        Eigen::MatrixXd dinputs; // derivative wrt inputs

        // constructor
        LayerDense(int n_inputs, int n_neurons) {
            this->weights = Eigen::MatrixXd::Random(n_inputs,n_neurons) * 0.01; // initialize weights
            this->weights = weights;
            this->biases = Eigen::VectorXd::Zero(n_neurons); // initialize biases
            this->n_inputs = n_inputs;
            this->n_neurons = n_neurons;
            this->weight_momentums = Eigen::MatrixXd::Zero(n_inputs,n_neurons); // weight momentum
            this->bias_momentums = Eigen::VectorXd::Zero(n_neurons); // bias momentum
        }

        // Member functions declaration
        void forward(const Eigen::MatrixXd &inputs);
        void backward(const Eigen::MatrixXd &dvalues);
};

class ActivationRelu {
    public:
        Eigen::MatrixXd inputs; // inputs
        Eigen::MatrixXd dinputs; // derivative wrt inputs
        Eigen::MatrixXd output;

        // Member functions declaration
        void forward(const Eigen::MatrixXd &inputs);
        void backward(const Eigen::MatrixXd &dvalues);
};

class ActivationSoftmax {
    public:
        Eigen::MatrixXd inputs; // inputs
        Eigen::MatrixXd dinputs; // derivative wrt inputs
        Eigen::MatrixXd output;

        // Member functions declaration
        void forward(const Eigen::MatrixXd &inputs);
        void backward(const Eigen::MatrixXd &dvalues);
};

class CrossEntropyLoss {
    public:
        Eigen::MatrixXd dinputs;

        // Member functions declaration
        Eigen::VectorXd forward(const Eigen::MatrixXd &y_pred, const Eigen::VectorXi &y_true);
        double calculate(const Eigen::MatrixXd &output, const Eigen::VectorXi &y);
        void backward(const Eigen::MatrixXd &dvalues, const Eigen::VectorXi &y_true);
};

class StochasticGradientDescent {
    public:
        double learning_rate; // learning rate
        double decay; // decay
        double momentum; // momentum
        double iterations; // initialize number of iterations to 0

        // constructor
        StochasticGradientDescent(double learning_rate, double decay, double momentum) {
            this->learning_rate = learning_rate;
            this->decay = decay;
            this->momentum = momentum;
            this->iterations = 0.0;
        }

        // Member functions declaration
        void pre_update_params(double start_learning_rate);
        void update_params(LayerDense &layer);
        void post_update_params();
};

Eigen::MatrixXd load_matrix_data(std::string fileToOpen);
Eigen::VectorXi load_vector_data(std::string fileToOpen);

#endif // NEURALNET_HPP


#include "drake/math/discrete_lyapunov_equation.h"

#include <limits>
#include <stdexcept>
#include <vector>

#include <Eigen/Core>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/expect_no_throw.h"

namespace drake {
namespace math {
namespace {

using Eigen::MatrixXd;

const double kTolerance = 5 * std::numeric_limits<double>::epsilon();

void SolveRealLyapunovEquationAndVerify(const Eigen::Ref<const MatrixXd>& A,
                                        const Eigen::Ref<const MatrixXd>& Q) {
  MatrixXd X{RealDiscreteLyapunovEquation(A, Q)};
  // Check that X is symmetric.
  EXPECT_TRUE(
      CompareMatrices(X, X.transpose(), 0, MatrixCompareType::absolute));
  // Check that X is the solution to the discrete time ARE.
  EXPECT_TRUE(CompareMatrices(A.transpose() * X * A - X, -Q,
                              5 * kTolerance * Q.norm(),
                              MatrixCompareType::absolute));
}

GTEST_TEST(RealDiscreteLyapunovEquation, ThrowInvalidSizedMatricesTest) {
  // This tests if the exceptions are thrown for invalidly sized matrices. A and
  // Q need to be square and of same size.
  const int n{1}, m{2};

  // non-square A
  MatrixXd A1(n, m);
  A1 << 1, 1;
  MatrixXd Q1(m, m);
  Q1 << 1, 1, 1, 1;

  // non-square Q
  MatrixXd A2(m, m);
  A2 << 1, 1, 1, 1;
  MatrixXd Q2(n, m);
  Q2 << 1, 1;

  // not-same sized
  MatrixXd A3(m, m);
  A3 << 1, 1, 1, 1;
  MatrixXd Q3(n, n);
  Q3 << 1;

  std::vector<MatrixXd> As{A1, A2, A3};
  std::vector<MatrixXd> Qs{Q1, Q2, Q3};

  for (int i = 0; i < static_cast<int>(As.size()); ++i) {
    EXPECT_ANY_THROW(RealDiscreteLyapunovEquation(As[i], Qs[i]));
  }
}

GTEST_TEST(RealDiscreteLyapunovEquation, ThrowEigenValuesATest) {
  // Given the Eigenvalues of @param A as lambda_1, ..., lambda_n, then the
  // solution is unique if and only if lambda_i * lambda_j != 1 for all  i, j.
  // (see Barraud, A.Y., "A numerical algorithm to solve AᵀXA - X = Q," IEEE®
  // Trans. Auto. Contr., AC-22, pp. 883-885, 1977.)
  // This tests if an exception is thrown if the eigenvalues violate this
  // requirement.
  const int n{3};
  // pair of eigenvalues that multiplies to 1
  MatrixXd A1(n, n);
  A1 << 1, 0, 0, 0, 1, 0, 0, 0, 1;
  // pairs of eigenvalues that multiply to ±i, i.e should not throw
  MatrixXd A2(n, n);
  A2 << 0.5, 0, 0, 0, 0, 2.0, 0, -2.0, 0;
  // pair of eigenvalues whose product is within tol of 1
  MatrixXd A3(n, n);
  A3 << 1 + 1e-6, 0, 0, 0, 0.5, 0, 0, 0, 1 - 1e-6;
  // no pair of eigenvalues whose product is within tol of 1
  MatrixXd A4(n, n);
  A4 << 1 + 1e-6, 0, 0, 0, 1 + 1e-6, 0, 0, 0, 1 + 1e-6;
  MatrixXd Q(n, n);
  Q << 1, 0, 0, 0, 1, 0, 0, 0, 1;

  EXPECT_ANY_THROW(RealDiscreteLyapunovEquation(A1, Q));
  DRAKE_EXPECT_NO_THROW(RealDiscreteLyapunovEquation(A2, Q));
  EXPECT_ANY_THROW(RealDiscreteLyapunovEquation(A3, Q));
  DRAKE_EXPECT_NO_THROW(RealDiscreteLyapunovEquation(A4, Q));
}

GTEST_TEST(RealDiscreteLyapunovEquation, Solve1by1Test) {
  // This is a simple 1-by-1 test case, it tests the internal 1-by-1 solver.
  const int n{1};
  MatrixXd A(n, n);
  A << 0.5;
  MatrixXd Q(n, n);
  Q << 1;
  MatrixXd X(n, n);
  X << 4.0 / 3.0;

  EXPECT_TRUE(
      CompareMatrices(internal::Solve1By1RealDiscreteLyapunovEquation(A, Q), X,
                      kTolerance, MatrixCompareType::absolute));

  SolveRealLyapunovEquationAndVerify(A, Q);
}

GTEST_TEST(RealDiscreteLyapunovEquation, Solve2by2Test) {
  // This is a simple 2-by-2 test case, which tests the internal 2-by-2 solver.
  // The result is compared to the one generated by Matlab's dlyap function.
  const int n{2};
  MatrixXd A(n, n);
  A << 0.5, -0.5, 0, 0.25;
  MatrixXd X(n, n);
  X << 4.0, 0, 0, 2.0 + 1.0 / 7.5;

  MatrixXd Q_internal(n, n);
  Q_internal << 3, 1, NAN, 1;
  EXPECT_TRUE(CompareMatrices(
      internal::Solve2By2RealDiscreteLyapunovEquation(A, Q_internal), X,
      kTolerance, MatrixCompareType::absolute));

  MatrixXd Q(n, n);
  Q << 3, 1, 1, 1;
  EXPECT_TRUE(CompareMatrices(RealDiscreteLyapunovEquation(A, Q), X, kTolerance,
                              MatrixCompareType::absolute));
  SolveRealLyapunovEquationAndVerify(A.transpose(), Q);
}

GTEST_TEST(RealDiscreteLyapunovEquation, Solve3by3Test1) {
  // Tests if a 3-by-3 problem is reduced.
  const int n{3};
  MatrixXd A(n, n);
  A << -0.5 * MatrixXd::Identity(n, n);
  MatrixXd Q(n, n);
  Q << MatrixXd::Identity(n, n);

  SolveRealLyapunovEquationAndVerify(A, Q);
}

GTEST_TEST(RealDiscreteLyapunovEquation, Solve3by3Test2) {
  // The system has eigenvalues: lambda_1/2  = -0 +/- 0.5i
  // and lambda_3 = -0.5. Therefore, there exists a 2-by-2 block on the
  // diagonal.
  // The compared solution is generated by matlab's dlyap function.

  int n{3};
  MatrixXd A(n, n);
  A << 0.5, 0, 0, 0, 0, 0.5, 0, -0.5, 0;
  MatrixXd Q(n, n);
  Q << MatrixXd::Identity(n, n);
  MatrixXd X(n, n);
  X << (4.0 / 3.0) * MatrixXd::Identity(n, n);

  EXPECT_TRUE(CompareMatrices(RealDiscreteLyapunovEquation(A, Q), X, kTolerance,
                              MatrixCompareType::absolute));
  SolveRealLyapunovEquationAndVerify(A, Q);
}

GTEST_TEST(RealDiscreteLyapunovEquation, Solve4by4Test1) {
  // The system has eigenvalues: lambda_1/2  = -0.4500 +/- 0.7794i
  // and lambda_3/4 = -0.9.

  const int n{4};
  MatrixXd A(n, n);
  A << -0.9, 0, 0, 0, 0, 0, 0.9, 0, 0, -0.9, -0.9, 0, 0, 0, 0, -0.9;
  MatrixXd Q(n, n);
  Q << MatrixXd::Identity(n, n);
  SolveRealLyapunovEquationAndVerify(A, Q);
}

GTEST_TEST(RealDiscreteLyapunovEquation, Solve4by4Test2) {
  // The system has eigenvalues: lambda_1/2  = 0.3 +/- 0.4i
  // and lambda_3/4 = 0.4 +/- 0.5i

  const int n{4};
  MatrixXd A(n, n);
  A << 0.3, 0, 0, 0.4, 0, 0.4, 0.5, 0, 0, -0.5, 0.4, 0, -0.4, 0, 0, 0.3;
  MatrixXd Q(n, n);
  Q << MatrixXd::Identity(n, n);
  SolveRealLyapunovEquationAndVerify(A, Q);
}

}  // namespace
}  // namespace math
}  // namespace drake


//
// Created by haohanwang on 1/24/16.
//

#include <Eigen/Dense>
#include <unordered_map>
#include <queue>

using namespace Eigen;
using namespace std;

#ifndef ALGORITHMS_MODEL_HPP
#define ALGORITHMS_MODEL_HPP

typedef unsigned int model_id_t;

struct modelResult{
    string rowStr;
    string colStr;
    MatrixXf beta;
};

class Model {
protected:
    MatrixXf X;
    MatrixXf beta;
    MatrixXf y;
    bool logisticFlag;
    static float sigmoid(float x){return 1.0 / (1.0 + exp(-x));}
    void checkLogisticRegression();
public:
    void imputation();
    virtual void setX(const MatrixXf&);
    virtual void setY(const MatrixXf&);
    virtual void setAttributeMatrix(const string&, MatrixXf*);
    void initBeta();
    void initBeta(MatrixXf);
    void updateBeta(MatrixXf);
    MatrixXf getX();
    MatrixXf getBeta();
    MatrixXf getY();
    modelResult getClusteringResult(); // Scheduler needs to run model.getClusteringResult() for the result, instead of getBeta();

    MatrixXf predict();
    MatrixXf predict(MatrixXf);

    virtual void assertReadyToRun(){};
    virtual MatrixXf derivative();
    virtual MatrixXf proximal_derivative();
    virtual MatrixXf proximal_operator(MatrixXf, float);

    virtual float cost();

    Model();
    Model(const unordered_map<string, string>&);
    Model(MatrixXf, VectorXf);

    virtual ~Model(){};
};

#endif //ALGORITHMS_MODEL_HPP


#include "kalman_filter.hpp"
#include <Eigen/QR>
namespace biggles
{

matrix4x2f kalman_gain(const matrix2f& R, const matrix4f& Pred, const matrix2x4f& B) {
    matrix4x2f LHS = Pred*B.transpose();
    matrix2f S = B*LHS + R;
    S = enforce_symmetry(S);
    if (not is_symmetric(S)) {
        OK1(S);
        OK1(R);
        OK1(B*LHS);
        BOOST_ASSERT(is_symmetric(S));
    }
    matrix2x4f gain_transposed = S.transpose().colPivHouseholderQr().solve(LHS.transpose());
    return gain_transposed.transpose();
}

//template<typename InputIterator>
void kalman_filter::reinitialise(time_stamp first_time_stamp,
                                 time_stamp last_time_stamp,
                                 track::const_iterator first, track::const_iterator last,
                                 const matrix2f &R, const matrix4f &Q,
                                 float dynamic_drag)
{
    // set the dynamic drag associated with this filter
    dynamic_drag_ = dynamic_drag;

    // remove any existing solution
    prediction_states_and_covs_.clear();
    correction_states_and_covs_.clear();

    BOOST_ASSERT(last_time_stamp >= first_time_stamp);
    BOOST_ASSERT(first != last);

    // shortcut the entire filter if we have nothing to generate
    if(first_time_stamp == last_time_stamp)
        return;

    // the state evolution matrix: X_{k+1} = A X_k + <noise>
    Eigen::Matrix4f A(Eigen::Matrix4f::Identity());
    A(0,1) = A(2,3) = dynamic_drag_; // <- off-diagonal velocity integration

    //// covariance matrix of the state noise
    //Eigen::Matrix4f Q(Eigen::Matrix4f::Zero());
    //Q(0,0) = Q(2,2) = 1e-1f * 1e-1f * detail::speed_of_light_ * detail::speed_of_light_;
    //Q(1,1) = Q(3,3) = 1e-2f * 1e-2f * detail::speed_of_light_ * detail::speed_of_light_;

    // the observation matrix: Y_k = B X_k + <noise>
    Eigen::Matrix<float, 2, 4> B( Eigen::Matrix<float, 2, 4>::Zero());
    B(0,0) = B(1,2) = 1.f;

    // initial state
    Eigen::Vector4f mu;
    mu << 64.f, 0.f, 64.f, 0.f;
    //mu << x(*first), 0.f, y(*first), 0.f;
    //std::cout << mu << std::endl;

    // initial error
    //const float image_size(128.f);
    //const float s2(4.f*image_size*image_size);
    //const float s2(1e-1f * detail::speed_of_light_ * 1e-1f * detail::speed_of_light_);
    //const float s2(powf(2.f, 16.f));
    const float s2(powf(2.f, 16.f));
    Eigen::Matrix4f Sigma(s2 * Eigen::Matrix4f::Identity());
    Sigma(1,1) = Sigma(3,3) = 1e-2f * detail::speed_of_light_ * detail::speed_of_light_;

    // prediction_states_and_covs_ is mu_{t|t-1} and Sigma_{t|t-1}
    // correction_states_and_covs_ is mu_{t|t} and Sigma_{t|t}
    // forward loop
    // BOOST_ASSERT(Q.determinant() > 0.f);
    for(time_stamp time_stamp = first_time_stamp; time_stamp < last_time_stamp; ++time_stamp)
    {
        // prediction
        Eigen::Vector4f predict_mu = A * mu;
        Eigen::Matrix4f predict_Sigma = A * Sigma * A.transpose() + Q;

        // record the predictions mu_{t|t-1} and Sigma_{t|t-1}
        /*
        if (not (predict_Sigma.determinant() > 0)) {
            OK(Sigma.determinant());
            OK(Q.determinant());
        }
        */
        prediction_states_and_covs_.push_back(state_covariance_pair(predict_mu, predict_Sigma));

        // do we have an observation for this time step?
        if((first == last) || (t(*first) != time_stamp))
        {
            // no observation for this time stamp, simply evolve state
            mu = predict_mu;
            Sigma = predict_Sigma;
        }
        else
        {
            // we have an observation, woo! Retrieve it and increment observation iterator.
            const observation& obs(*first);
            ++first;

            // extract observation location
            Eigen::Vector2f obs_vec;
            obs_vec << x(obs), y(obs);

            // update
            matrix4x2f K = kalman_gain(R, predict_Sigma, B);
                //predict_Sigma * B.transpose() * (B * predict_Sigma * B.transpose() + R).inverse();

            mu = predict_mu + K * (obs_vec - B * predict_mu);
            //Eigen::Matrix4f proto_sigma = predict_Sigma - K * B * predict_Sigma;
            Eigen::Matrix4f raw_sigma = predict_Sigma - K * B * predict_Sigma;
            Sigma = enforce_symmetry(raw_sigma);
            if (not is_symmetric(Sigma)) {
                OK1((K * B));
                OK1(K * B * predict_Sigma);
                OK(is_symmetric(K * B * predict_Sigma));
                OK(measure_asymmetry(K * B * predict_Sigma));
            }
        }

        if (not is_symmetric(Sigma) and measure_asymmetry(Sigma) > 0.f) {
            OK("1");
            OK1(Sigma);
            OK(measure_asymmetry(Sigma));
            OK1(predict_Sigma);
            OK1(R);
            BOOST_ASSERT(is_symmetric(Sigma));
        }

        // record the corrections mu_{t|t} and Sigma_{t|t}
        correction_states_and_covs_.push_back(state_covariance_pair(mu, Sigma));
    }

    // check we generated the expected number of states
    BOOST_ASSERT(prediction_states_and_covs_.size() == static_cast<size_t>(last_time_stamp - first_time_stamp));
    BOOST_ASSERT(prediction_states_and_covs_.size() > 0);
    BOOST_ASSERT(correction_states_and_covs_.size() == static_cast<size_t>(last_time_stamp - first_time_stamp));
    BOOST_ASSERT(correction_states_and_covs_.size() > 0);

    // last element of prediction_states_and_covs_ is (mu_{T|T-1}, Sigma_{T|T-1})
    // last element of correction_states_and_covs_ is (mu_{T|T}, Sigma_{T|T})
    // where T is last_time_stamp
}

} // namespace biggles


/***************************************************************************

The MIT License (MIT)

Copyright (c) 2013-2017 Joel E. Merritt

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

***************************************************************************/

//  This module defines the GNSolver (Gauss-Newton Solver) class whose
//  objects solve the camera resection (calibration) described in
//  main.cpp.

#include "g_n_solver.h"

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

//
//  Member Function: GNSolver::GetErr2
//
//  Return the length squared of input vector.
//
//  Input parameters:
//  a -- vector to calculate length squared
//

double GNSolver::GetErr2(Eigen::VectorXd const &a) const
{
  double ret = 0.0;
  for(int i = 0;i < a.size();i++)
  {
    ret += a[i] * a[i];
  }

  return ret;
}

//
//  Member Function: GNSolver::EvalRFunctionVector
//
//  Return the vector containing the outputs of the r functions.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::VectorXd GNSolver::EvalRFunctionVector(FunctionObjectList const &r,
                                              Eigen::VectorXd const &beta)
                                              const
{
  int num_conds = r.size();
  Eigen::VectorXd ret(num_conds);

  for(int i = 0;i < num_conds;i++)
  {
    ret[i] = r[i](beta);
  }

  return ret;
}

//
//  Member Function: GNSolver::GetPartialD
//
//  Return the partial derivative (partial d)r[i] / (partial d)beta[j].
//
//  Input parameters:
//  i    -- row index
//  j    -- column index
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

double GNSolver::GetPartialD(int i, int j, FunctionObjectList const &r,
                             Eigen::VectorXd const &beta) const
{
  double epsilon = 0.001;

  Eigen::VectorXd beta_epsilon = beta;
  beta_epsilon[j] += epsilon;

  double ret = r[i](beta);
  double ret_epsilon = r[i](beta_epsilon);

  double partd = (ret - ret_epsilon) / epsilon;

  return partd;
}

//
//  Member Function: GNSolver::Jacobian
//
//  Return the Jacobian matrix of vector function r.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::MatrixXd GNSolver::Jacobian(FunctionObjectList const &r,
                                   Eigen::VectorXd const &beta) const
{
  int num_conds = r.size();
  Eigen::MatrixXd ret(num_conds, beta.size());

  for(int i = 0;i < num_conds;i++)
  {
    for(int j = 0;j < beta.size();j++)
    {
      ret(i, j) = GetPartialD(i, j, r, beta);
    }
  }

  return ret;
}

//
//  Member Function: GNSolver::CalcErr2
//
//  Return the error of vector function r.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

double GNSolver::CalcErr2(FunctionObjectList const &r,
                          Eigen::VectorXd const &beta) const
{
  Eigen::VectorXd vcol = EvalRFunctionVector(r, beta);

  return GetErr2(vcol);
}

//
//  Member Function: GNSolver::CalcDiff
//
//  Return the calculated difference to adjust the beta vector.
//
//  The Gauss-Newton algorithm calculates successive approximations of
//  the beta vector by the following equation:
//
//  beta_next = beta_current - (J^T*J)^-1*J^T * r(beta_current)
//
//  where:
//  beta_next is the next iteration of the beta vector
//  beta_current is the current iteration of the beta vector
//  J is the Jacobian matrix
//  J^T is the transpose of the Jacobian matrix
//  r is the r vector of functions to be minimized
//
//  CalcDiff calculates and returns this part:
//
//  (J^T*J)^-1*J^T * r(beta_current)
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::VectorXd GNSolver::CalcDiff(FunctionObjectList const &r,
                                   Eigen::VectorXd const &beta) const
{
  Eigen::MatrixXd ejf = Jacobian(r, beta);

  Eigen::MatrixXd ejft = ejf.transpose();
  Eigen::MatrixXd ejf2 = ejft * ejf;
  Eigen::MatrixXd ejf3 = ejf2.inverse() * ejft;

  Eigen::MatrixXd vcol = EvalRFunctionVector(r, beta);

  return ejf3 * vcol;
}

//
//  Member Function: GNSolver::operator()
//
//  Return the optimized beta vector.
//
//  The main solver, iterates to improve the beta vector, including
//  "dialing back" the next iteration of the beta vector until it
//  produces less error than the current beta vector.
//
//  Input parameters:
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

Eigen::VectorXd GNSolver::operator()(FunctionObjectList const &r,
                                     Eigen::VectorXd &beta) const
{
  std::cout << "Entering Solve" << std::endl;

  double err2_min;
  // iterate loop
  for(int j = 0;j < 100;j++)
  {
    std::cout << "iteration " << j << std::endl;

    std::cout << "BETA" << std::endl;
    std::cout << beta << std::endl;
    std::cout << std::endl;

    double err2 = CalcErr2(r, beta);
    Eigen::VectorXd ediff = CalcDiff(r, beta);

    std::cout << "err2 (orig) " << err2 << std::endl;

    double atten = 1.0;
    err2_min = err2;
    Eigen::VectorXd beta_min = beta;
    for(int j1 = 0;j1 < 20;j1++)
    {
      Eigen::VectorXd beta_next = atten * ediff + beta;

      double err2_next = CalcErr2(r, beta_next);
      std::cout << "j1 " << j1 << " atten " << atten << " err2_next " <<
          err2_next << std::endl;
      if(err2_next < err2_min)
      {
        err2_min = err2_next;
        beta_min = beta_next;
        atten *= 0.7;
      }
      else
      {
        atten *= 0.7;
      }
    }

    beta = beta_min;

    // stop iterating if the error is almost zero
    if(err2_min < 0.0000001)
    {
      std::cout << "solved" << std::endl;

      break;
    }

    // stop iterating if the error has decreased minimally
    if(err2_min / err2 > 0.99999)
    {
      std::cout << "converged" << std::endl;

      break;
    }
  } // iterate

  std::cout << "**********OUTPUT************" << std::endl;
  std::cout << "BETA" << std::endl;
  std::cout << beta << std::endl;
  std::cout << std::endl;
  std::cout << "err2_min " << err2_min << std::endl;

  return beta;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)
//

//! \file
//!\brief Test for a successful bare-metal configuration of fixed_point using a 16-bit negatable type.

// For further mathematical details pertaining to this file,
// see C.M. Kormanyos, Real-Time C++: Efficient Object-Oriented and
// Template Microcontroller Programming (Springer, Heidelberg, 2013).
// in Section 12.7 and Chapter 13.
// See http://link.springer.com/chapter/10.1007/978-3-642-34688-0_13

#define BOOST_TEST_MODULE test_negatable_basic_metal_config_16bit
#define BOOST_LIB_DIAGNOSTIC

// Configure Boost.Fixed_point for a bare-metal system.

#define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION // Do not use Boost.Multiprecision.
#define BOOST_FIXED_POINT_DISABLE_IOSTREAM       // Do not use I/O streaming.

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename RealValueType,
           typename RealFunctionType>
  RealValueType first_derivative(const RealValueType& x,
                                 const RealValueType& dx,
                                 RealFunctionType real_function)
  {
    // Evaluate first derivative of real_function using
    // a three-point central-difference rule of O(dx^6).

    const RealValueType dx2(dx  + dx);
    const RealValueType dx3(dx2 + dx);

    const RealValueType m1((  real_function(x + dx)
                            - real_function(x - dx))  / 2U);
    const RealValueType m2((  real_function(x + dx2)
                            - real_function(x - dx2)) / 4U);
    const RealValueType m3((  real_function(x + dx3)
                            - real_function(x - dx3)) / 6U);

    const RealValueType fifteen_m1(m1 * 15U);
    const RealValueType six_m2    (m2 *  6U);
    const RealValueType ten_dx    (dx * 10U);

    return ((fifteen_m1 - six_m2) + m3) / ten_dx;
  }
} // namespace local

BOOST_AUTO_TEST_CASE(test_negatable_basic_metal_config_16bit)
{
  typedef boost::fixed_point::negatable<6, -9> fixed_point_type;

  const fixed_point_type a = fixed_point_type(12) / 10U;
  const fixed_point_type b = fixed_point_type(34) / 10U;
  const fixed_point_type c = fixed_point_type(56) / 10U;

  // Compute the approximate derivative of [(a * x^2) + (b * x) + c]
  // evaluated at 1/2, where the approximate values of the coefficients
  // are: a = 1.2, b = 3.4, and c = 5.6. The step size is set to a value
  // of approximately 1/8.

  const fixed_point_type d =
    local::first_derivative(fixed_point_type(1) / 2U,  // x
                            fixed_point_type(1) / 4U,  // Step size dx.
                            [&a, &b, &c](const fixed_point_type& x) -> fixed_point_type
                            {
                              return (((a * x) + b) * x) + c;
                            });

  // The expected result is ((2 * a) + b) = (1.2 + 3.4) = 4.6 (exact).
  // We obtain a fixed-point result of approximately 4.594.

  // Verify that the result lies within (4.5 < result < 4.7).
  // The expected result is 4.6, so this is a wide tolerance.

  // This is the test of the result that has been used
  // on the bare-metal system.
  const bool result_is_ok = ((d > (fixed_point_type(45) / 10U)) && (d < (fixed_point_type(47) / 10U)));

  BOOST_CHECK_EQUAL(result_is_ok, true);

  // Now we perform a second test of the result based on a
  // close-fraction test. We do not use BOOST_CHECK_CLOSE_FRACTION()
  // here becasue that would require I/O streaming, which is
  // disabled in this bare-metal configuration.
  const fixed_point_type fraction = fabs(d / (fixed_point_type(46) / 10U));
  const fixed_point_type delta    = fabs(1 - fraction);
  const fixed_point_type tol      = ldexp(fixed_point_type(1), fixed_point_type::resolution + 2);

  const bool result_is_within_tolerance = (delta <= tol);

  BOOST_CHECK_EQUAL(result_is_within_tolerance, true);
}


#include <iostream>
#include <boost/numeric/odeint.hpp>
#include "gnuplot-iostream.h"
#include "sysfunc_exercise2_2_b.h"

namespace odeint = boost::numeric::odeint;

int main(int argc, char const *argv[])
{
    // Odeint
    std::vector<double> position;
    {
        system_function::state_type x; // a vector with size 3. [x; x_dot; x_dot_dot]
        //x = {1,1,1, 1};                    // start at x=1, 1, 1, 1;
        x = {1, -1, 1, -1};
        size_t steps = odeint::integrate(system_function::harmonic_oscillator,
                                         x, 0.0, 10.0, 0.1,
                                         system_function::push_back_state_and_time);

        for (size_t i = 0; i <= steps; i++)
        {
            position.push_back(system_function::m_states[i][0]);
        }
    }

    // GNU Plot
    {
        /* output */
        Gnuplot gp;
        gp << "plot '-' using 1:2 with linespoint" << std::endl;
        gp.send1d(std::make_tuple(system_function::m_times, position));
    }
    return 0;
}


/*****************************************************************************
*
* Copyright (C) 2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <iomanip>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

template<class T>
class func {
public:
  typedef T real_t;
  func(real_t a) : a_(a) {}
  real_t operator()(real_t x) const {
    using std::abs; using std::cos; using std::log;
    auto s = abs(a_ * a_ - 2 * a_ * cos(x) + 1);
    return (s > 0) ? log(s) : real_t(0);
  }
  real_t result() const {
    using std::abs; using std::log;
    using boost::math::constants::pi;
    return (abs(a_) < 1) ? real_t(0) : (2 * pi<real_t>() * log(abs(a_)));
  }
private:
  real_t a_;
};

int main() {
  using std::abs; using std::sqrt;
  typedef boost::multiprecision::cpp_dec_float_50 real_t;
  const real_t pi = boost::math::constants::pi<real_t>();

  boost::math::quadrature::tanh_sinh<real_t> integrator;
  real_t termination = sqrt(std::numeric_limits<real_t>::epsilon());
  real_t error, L1;
  size_t levels;

  real_t values[5] = { 0.5, 1 - 1e-6, 1, 1 + 1e-6, 1.5 };
  for (auto a : values) {
    func<real_t> f(a);
    real_t q = integrator.integrate(f, 0, pi, termination, &error, &L1, &levels);

    std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
              << "value of a: " << a << std::endl
              << "result: " << q << std::endl
              << "estimated error: " << error << std::endl
              << "real error: " << abs(q - f.result()) << std::endl
              << "L1 * error: " << L1 * error << std::endl
              << "levels: " << levels << std::endl;
  }
}


/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <algorithm>
#include <random>

#include <iostream>

#define LOG_2PI 1.8378770664093453

template<typename T, int D>
class Normal
{
 public:

  Normal();
  Normal(const Eigen::Matrix<T,D,1>& mu, const Eigen::Matrix<T,D,D>& Sigma);
  Normal(const Eigen::Matrix<T,D,D>& Sigma);
  Normal(const Normal<T,D>& other);
  ~Normal();

  T logPdf(const Eigen::Matrix<T,D,D>& x) const;
  T logPdfSlower(const Eigen::Matrix<T,D,D>& x) const;
  T logPdf(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const;
  T logPdfSlower(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const;
  T logPdf(const Eigen::Matrix<T,D,D>& scatter, T count) const;

  Eigen::Matrix<T,D,1> sample(std::mt19937& rnd);

  void print() const;

  const Eigen::Matrix<T,D,D>& Sigma() const {return Sigma_;};
  void setSigma(const Eigen::Matrix<T,D,D>& Sigma)
  { Sigma_ = Sigma; SigmaLDLT_.compute(Sigma_);
    logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();};
  T logDetSigma() const {return logDetSigma_;};
  const Eigen::LDLT<Eigen::Matrix<T,D,D> >& SigmaLDLT() const {return SigmaLDLT_;};

  Eigen::Matrix<T,D,1> mu_;
  Eigen::Matrix<T,D,D> Sigma_;
private:

  // helpers for fast computation
  T logDetSigma_;
  Eigen::LDLT<Eigen::Matrix<T,D,D> > SigmaLDLT_;

  std::normal_distribution<T> gauss_;
};

template<typename T, int D>
Normal<T,D>::Normal(const Eigen::Matrix<T,D,1>& mu, const Eigen::Matrix<T,D,D>& Sigma)
  : mu_(mu), Sigma_(Sigma), SigmaLDLT_(Sigma_), gauss_(0,1)
{
  // equivalent to log(det(Sigma)) but more stable for small values
  logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();
};

template<typename T, int D>
Normal<T,D>::Normal(const Eigen::Matrix<T,D,D>& Sigma)
  : mu_(Eigen::Matrix<T,D,1>::Zero()), Sigma_(Sigma), SigmaLDLT_(Sigma_) , gauss_(0,1)
{
  // equivalent to log(det(Sigma)) but more stable for small values
  logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();
};

template<typename T, int D>
Normal<T,D>::Normal()
  : mu_(Eigen::Matrix<T,D,1>::Zero()), Sigma_(Eigen::Matrix<T,D,D>::Identity()), SigmaLDLT_(Sigma_), gauss_(0,1)
{
  // equivalent to log(det(Sigma)) but more stable for small values
  logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();
};

template<typename T, int D>
Normal<T,D>::Normal(const Normal<T,D>& other)
  : mu_(other.mu_), Sigma_(other.Sigma_),
  logDetSigma_(other.logDetSigma_), SigmaLDLT_(Sigma_) , gauss_(0,1)
{};

template<typename T, int D>
Normal<T,D>::~Normal()
{}

template<typename T, int D>
T Normal<T,D>::logPdf(const Eigen::Matrix<T,D,D>& x) const
{
  return -0.5*(LOG_2PI*D + logDetSigma_ +((x-mu_).transpose()*SigmaLDLT_.solve(x-mu_)).sum() );
}

template<typename T, int D>
T Normal<T,D>::logPdfSlower(const Eigen::Matrix<T,D,D>& x) const
{
  return -0.5*(LOG_2PI*D + logDetSigma_
//      +((x-mu_).transpose()*Sigma_.inverse()*(x-mu_)).sum() );
  +((x-mu_).transpose()*Sigma_.fullPivHouseholderQr().solve(x-mu_)).sum() );
}

template<typename T, int D>
T Normal<T,D>::logPdf(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const
{
	Eigen::Matrix<T,1,D> meanT = mean.transpose();
	Eigen::Matrix<T,D,D> CovMu= SigmaLDLT_.solve(mu_);
	return -0.5*((LOG_2PI*D + logDetSigma_)*count
		+ count*(mu_.transpose()*CovMu).sum()
		-2.*count*(meanT*CovMu).sum()
		+(SigmaLDLT_.solve(scatter + mean*meanT*count)).trace());

  //return -0.5*((LOG_2PI*D + logDetSigma_)*count
  //    + count*(mu_.transpose()*SigmaLDLT_.solve(mu_)).sum()
  //    -2.*count*(mean.transpose()*SigmaLDLT_.solve(mu_)).sum()
  //    +(SigmaLDLT_.solve(scatter + mean*mean.transpose()*count )).trace());
}

template<typename T, int D>
T Normal<T,D>::logPdfSlower(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const
{
	Eigen::Matrix<T,1,D> meanT = mean.transpose();
	Eigen::Matrix<T,D,D> CovMu=
    Sigma_.fullPivHouseholderQr().solve(mu_);
	return -0.5*((LOG_2PI*D + logDetSigma_)*count
		+ count*(mu_.transpose()*CovMu).sum()
		-2.*count*(meanT*CovMu).sum()
		+(Sigma_.fullPivHouseholderQr().solve(
      scatter + mean*meanT*count)).trace());
}

template<typename T, int D>
T Normal<T,D>::logPdf(const Eigen::Matrix<T,D,D>& scatter, T count) const
{
  assert(false);
//  cout<<count*(mu_.transpose()*SigmaLDLT_.solve(mu_)).sum()<<endl;
//  cout<<(SigmaLDLT_.solve(scatter)).trace()<<endl;
  return -0.5*((LOG_2PI*D + logDetSigma_)*count
      + count*(mu_.transpose()*SigmaLDLT_.solve(mu_)).sum()
      +(SigmaLDLT_.solve(scatter)).trace());
}

template<typename T, int D>
Eigen::Matrix<T,D,1> Normal<T,D>::sample(std::mt19937& rnd)
{
  // populate the mean
  Eigen::Matrix<T,D,1> x(D);
  for (uint32_t d=0; d<D; d++)
    x[d] = gauss_(rnd); //gsl_ran_gaussian(r,1);
  Eigen::Matrix<T,D,D> sqrtD = Eigen::Matrix<T,D,D>::Zero(D,D);
  sqrtD.diagonal() = SigmaLDLT_.vectorD();
  sqrtD = sqrtD.array().sqrt();
  return (SigmaLDLT_.matrixL()*sqrtD*SigmaLDLT_.matrixU())*x + mu_;
};


/*!
 * @file
 * @brief
 * @copyright alphya 2018-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_DIFF_HPP
#define NYARUGA_UTIL_DIFF_HPP

#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cmath>
#include <limits>
#include <nyaruga_util/get_arg_type.hpp>
#include <type_traits>

namespace nyaruga {

namespace util {

using num_t =
   boost::multiprecision::number<boost::multiprecision::cpp_dec_float<1000>>;

// 微分します
// 7 点近似です
template <typename Number = num_t, typename F>
constexpr auto diff(F && f) noexcept
{
   return [f](auto && x) noexcept(noexcept(f(x))) -> Number {
      auto h = 1e-7;
      if constexpr (std::is_same_v<Number, num_t>)
         h = DBL_EPSILON;

      auto&& y1 = f(static_cast<Number>(x + h));
      auto&& y2 = f(static_cast<Number>(x - h));
      auto&& y3 = f(static_cast<Number>(x + 2 * h));
      auto&& y4 = f(static_cast<Number>(x - 2 * h));
      auto&& y5 = f(static_cast<Number>(x + 3 * h));
      auto&& y6 = f(static_cast<Number>(x - 3 * h));

      return (y5 - 9 * y3 + 45 * y1 - 45 * y2 + 9 * y4 - y6) /
                           (60 * h);
   };
}

// 5 点近似です
template <typename Number = num_t, typename F>
constexpr auto diff_fast(F && f) noexcept
{
   return [f](auto && x) noexcept(noexcept(f(x))) -> Number
   {
      auto h = 1e-7;
      if constexpr (std::is_same_v<Number, num_t>)
         h = DBL_EPSILON;

      auto && y1 = f(static_cast<Number>(x + h));
      auto && y2 = f(static_cast<Number>(x - h));
      auto && y3 = f(static_cast<Number>(x + 2 * h));
      auto && y4 = f(static_cast<Number>(x - 2 * h));

      return (y4 - 8 * y2 + 8 * y1 - y3) / (12 * h);
   };
}

} // namespace util

} // namespace nyaruga

#endif // #ifndef NYARUGA_UTIL_DIFF_HPP


/*
 * Copyright (c) Nuno Alves de Sousa 2019
 *
 * Use, modification and distribution is 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 <iostream>
#include <cstdlib>
#include <cstdint>
#include <algorithm>

#include <celero/Celero.h>

#include <Eigen/Eigen>

#include <mkl.h>

#include <random_vector.hpp>
#include <debug.hpp>

void dgemmTest()
{
    const int m = 3;
    const int n = 3;

    std::cout << "Generating random data...\n\n";
    auto randomDoubles = makeRandomVector(m*n, 0.0, 2.0);

    // Eigen initialization
    Eigen::MatrixXd eA;
    eA = Eigen::Map<Eigen::MatrixXd>(randomDoubles.data(), m, n);

    std::cout << "Eigen matrix:\n";
    EIGEN_DEBUG(eA);

    // MKL initialization
    auto mA = static_cast<double*>(mkl_malloc(m*n*sizeof(double), 64));
    auto mC = static_cast<double*>(mkl_malloc(m*n*sizeof(double), 64));
    if (mA == nullptr || mC == nullptr) {
        std::cerr << "error: memory allocation failed. Aborting...";
        mkl_free(mA);
        mkl_free(mC);
        return;
    }
    std::copy(randomDoubles.begin(), randomDoubles.end(), mA);
    std::fill(mC, mC + n*m, 0);

    std::cout << "\nMKL matrices:\n";
    MKL_DEBUG(mA, m, n);
    MKL_DEBUG(mC, m, n);

    std::cout << "\nEigen A * A\n";
    std::cout << eA * eA << '\n';

    std::cout << "\nMKL A * A\n";
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, m, n, n, 1, mA, m, mA, m, 1, mC, m);
    MKL_DEBUG(mC, m, n);

    // Cleanup
    mkl_free(mA);
    mkl_free(mC);
}

void maddTest()
{
    const int m = 5;
    const int n = 5;

    std::cout << "Generating random data...\n\n";
    auto randomDoubles = makeRandomVector(m*n, 0.0, 2.0);

    // Eigen initialization
    Eigen::MatrixXd eA;
    eA = Eigen::Map<Eigen::MatrixXd>(randomDoubles.data(), m, n);

    // MKL initialization
    auto mA = static_cast<double*>(mkl_malloc(m*n*sizeof(double), 64));
    auto mB = static_cast<double*>(mkl_malloc(m*n*sizeof(double), 64));
    auto mC = static_cast<double*>(mkl_malloc(m*n*sizeof(double), 64));
    if (mA == nullptr || mB == nullptr || mC == nullptr) {
        std::cerr << "error: memory allocation failed. Aborting...";
        mkl_free(mA);
        mkl_free(mB);
        mkl_free(mC);
        return;
    }
    std::copy(randomDoubles.begin(), randomDoubles.end(), mA);
    std::copy(randomDoubles.begin(), randomDoubles.end(), mB);
    std::fill(mC, mC + m*n, 0);

    std::cout << "\nMKL matrices:\n";
    MKL_DEBUG(mA, m, n);
    MKL_DEBUG(mB, m, n);
    MKL_DEBUG(mC, m, n);

    std::cout << "\nEigen A + A\n";
    std::cout << eA + eA << '\n';

    // Arrays A and B cannot overlap
    std::cout << "MKL dmatadd C = A + B\n";
    mkl_domatadd('C', 'N', 'N', m, n, 1, mA, m, 1, mB, m, mC, m);
    MKL_DEBUG(mC, m, n);

    MKL_DEBUG(mA, m, n);

    std::cout << "MKL daxpy A = A + A\n";
    cblas_daxpy(m*n, 1, mA, 1, mA, 1);
    MKL_DEBUG(mA, m, n);

    std::cout << "MKL vdAss B = B + B\n";
    vdAdd(m*n, mB, mB, mB);
    MKL_DEBUG(mB, m, n);

    // Cleanup
    mkl_free(mA);
    mkl_free(mB);
    mkl_free(mC);
}

int main()
{
//    dgemmTest();
    maddTest();
}