/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * 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.
 */

///
/// \file quasi_cauchy_rrd.hpp
///
#ifndef MXPFIT_QUASI_CAUCHY_RRD_HPP
#define MXPFIT_QUASI_CAUCHY_RRD_HPP

#include <cassert>

#include <Eigen/Core>

namespace mxpfit
{
namespace detail
{

//
// Compute exponential z minus one, exp(z) -1
//
// --- for real
template <typename T>
inline auto expm1(T x) -> decltype(std::expm1(x))
{
    return std::expm1(x);
}
// --- for complex
template <typename T>
std::complex<T> expm1(const std::complex<T>& z)
{
    constexpr const T inf = std::numeric_limits<T>::infinity();
    constexpr const T nan = std::numeric_limits<T>::quiet_NaN();

    const T x = std::real(z);
    const T y = std::imag(z);

    if (std::isnan(x))
    {
        return {x, y == T() ? y : x};
    }
    else if (!std::isfinite(y))
    {
        if (x == inf)
        {
            return {-x, nan};
        }
        else if (x == -inf)
        {
            return {-T(1), std::copysign(T(), y)};
        }
        else
        {
            return {nan, nan};
        }
    }
    else
    {
        const auto u = std::expm1(x);
        if (y == T())
        {
            return {u, y};
        }
        else
        {
            const auto v = u + T(1);
            const auto w = std::sin(y / 2);
            const auto re =
                std::isfinite(v) ? u - 2 * v * w * w : v * std::cos(y);
            return {re, v * std::sin(y)};
        }
    }
}

} // namespace: detail

///
/// ### DefaultQuasiCauchyRRDFunctor
///
/// Default Functor class for QuasiCauchyRRD.
///
/// \tparam T Scalar type of matrix elements
///
/// This functor is for a quasi-Cauchy matrix whose element is expressed as
/// \f$C_{ij}=\frac{a_{i}b_{j}}{x_{i}+y_{j}}\f$
///
template <typename T>
struct DefaultQuasiCauchyRRDFunctor
{
    constexpr static T matrix_element(T ai, T bj, T xi, T yj)
    {
        return ai * bj / (xi + yj);
    }

    constexpr static T update_coeff(T xi, T xj, T yj)
    {
        return (xi - xj) / (xi + yj);
    }
};

///
/// ### QuasiCauchyRRDFunctorLogPole
///
/// Functor class for a rank-revealing Cholesky decomposition of Cauchy-like
/// matrix appearing in parameter reduction of exponential sum.
///
/// \tparam T Scalar type of matrix elements
///
/// This functor is for a Cauchy-like matrix whose element is expressed as
/// \f[
///    C_{ij}=\frac{a_{i}b_{j}}{exp(p_{i}) - exp(q_{j})}
/// \f]
///
template <typename T>
struct QuasiCauchyRRDFunctorLogPole
{
    constexpr static T matrix_element(T ai, T bj, T pi, T qj)
    {
        using Eigen::numext::exp;
        return ai * bj / (exp(qj) * detail::expm1(pi - qj));
    }

    constexpr static T update_coeff(T pi, T pj, T qj)
    {
        using Eigen::numext::exp;
        // (1 - exp(pj - pi)) / (1 - exp(qj - pi));
        return detail::expm1(pj - pi) / detail::expm1(qj - pi);
    }
};

namespace detail
{

// Apply row permutation
template <typename IPiv, typename MatX, typename VecWork>
void apply_row_permutation(const Eigen::DenseBase<IPiv>& ipiv,
                           Eigen::DenseBase<MatX>& matX,
                           Eigen::DenseBase<VecWork>& work)
{
    using Index = Eigen::Index;
    for (Index j = 0; j < matX.cols(); ++j)
    {
        for (Index i = 0; i < matX.rows(); ++i)
        {
            work(ipiv(i)) = matX(i, j);
        }

        matX.col(j) = work;
    }
}

//
// Functor class for making dense matrix expression of a quasi-Cauchy matrix
//
template <typename VecA, typename VecB, typename VecX, typename VecY,
          typename FunctorBody>
struct quasi_cauchy_functor
{
    using Index = Eigen::Index;

    quasi_cauchy_functor(const VecA& a, const VecB& b, const VecX& x,
                         const VecY& y)
        : m_a(a), m_b(b), m_x(x), m_y(y)
    {
    }

    auto operator()(Index i, Index j) const
        -> decltype(FunctorBody::matrix_element(typename VecA::Scalar(),
                                                typename VecB::Scalar(),
                                                typename VecX::Scalar(),
                                                typename VecY::Scalar()))
    {
        // m_a(i) * m_b(j) / (m_x(i) + m_y(j));
        return FunctorBody::matrix_element(m_a(i), m_b(j), m_x(i), m_y(j));
    }

private:
    const VecA& m_a;
    const VecB& m_b;
    const VecX& m_x;
    const VecY& m_y;
};

} // namespace detail

///
/// ### QuasiCauchyRRD
///
/// \brief Compute the rank-revealing Cholesky decomposition of a self-adjoint
///   quasi-Cauchy matrix in high relative accuracy.
///
/// \tparam T the scalar type of matrix to be decomposed
/// \tparam Functor Functor class that provides static functions for calculating
///   matrix elements and Cholesky factors, such that,
/// ``` c++
///   /* Compute `ai * bj / (xi + yj)
///   static T matrix_element(T ai, T bj, T xi, T yj);
///   /* Compute `(xi - xj) / (xi + yj)
///   static T update_coeff(T xi, T xj, T yj);
/// ```
///
/// For given arrays of $a_{i},b_{i},x_{i},y_{i} \, (i=1,2,\dots,n)$ a
/// quasi-Cauchy matrix \f$ C \f$ is defined as
///
///   \f[
///     C_{ij} = \frac{a_{i}^{} b_{j}^{}}{x_{i}^{} + y_{j}^{}} \quad
///     (i,j=1,2,\dots,n).
///   \f]
///
/// We assume that the matrix \f$ C \f$ is self-adjoint and positive
/// definite. A quasi-Cauchy matrix is usually rank deficient. Let \f$ m =
/// \text{rank}(C) \f$ then, matrix `C` can have partial Cholesky decomposition
/// of the form
///
///   \f[ C = (PL)D^2(PL)^{\ast}, \f]
///
/// where \f$ L \f$ is \f$ n \times m \f$ unit triangular (trapezoidal) matrix,
/// \f$ D \f$ is \f$ m \times m \f$ diagonal matrix, and \f$ P \f$ is a \f$ m
/// \times n \f$ permutation matrix.
///
///
/// #### Algorithm
///
/// The factorization is made using the modified Gaussian elimination of
/// complete pivoting (GECP) proposed in Ref. [1], which is also described in
/// Algorithm 2 and 3 in Ref.[2]. Following the algorithms described in Ref.
/// [2], we stop the factorization when the diagonal element \f$ D_{mm} \f$
/// becomes smaller than the threshold value, \f$ D_{mm} \leq \delta^2 \epsilon
/// \f$, at certain \f$ m \f$ where \f$ \delta \f$ is mimimal target size of
/// singular values of the systems to be kept in the later step, and \f$
/// \epsilon \f$ is the machine epsilon. Note that the diagonal elements \f$
/// D_{ii} \f$ are sorted in non-increasing order by complete pivoting.
///
///
/// #### Complexity
///
/// The complexity of this algorithm is \f$
/// \mathcal{O}(n(\log(\delta\epsilon)^{-1})^2) \f$.
///
///
/// #### References
///
/// 1. J. Demmel, "ACCURATE SINGULAR VALUE DECOMPOSITIONS OF STRUCTURED
///    MATRICES", SIAM J. Matrix Anal. Appl. **21** (1999) 562-580.
///    [DOI: https://doi.org/10.1137/S0895479897328716]
/// 2. T. S. Haut and G. Beylkin, "FAST AND ACCURATE CON-EIGENVALUE ALGORITHM
///    FOR OPTIMAL RATIONAL APPROXIMATIONS", SIAM J. Matrix Anal. Appl. **33**
///    (2012) 1101-1125.
///    [DOI: https://doi.org/10.1137/110821901]
///

// --- Forward declaration
template <typename T, typename Functor = DefaultQuasiCauchyRRDFunctor<T>>
class QuasiCauchyRRD;
//
// --- Implementation
//
template <typename T, typename Functor>
class QuasiCauchyRRD
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<T>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using StorageIndex  = Eigen::Index;
    using Index         = Eigen::Index;
    using MatrixType    = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using VectorType    = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVectorType = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using IndicesType    = Eigen::Matrix<Index, Eigen::Dynamic, 1>;

    ///
    /// Default constructor
    ///
    QuasiCauchyRRD()
        : m_a(),
          m_b(),
          m_x(),
          m_y(),
          m_work(),
          m_ipiv(),
          m_matPL(),
          m_vecD(),
          m_threshold(Eigen::NumTraits<RealScalar>::epsilon()),
          m_is_initialized()
    {
    }

    ///
    /// Default destructor
    ///
    ~QuasiCauchyRRD() = default;

    ///
    /// Compute RRD of self-adjoint quasi-Cauchy matrix.
    ///
    /// \param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] b  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    /// \param[in] y  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecB, typename VecX, typename VecY>
    void
    compute(const Eigen::EigenBase<VecA>& a, const Eigen::EigenBase<VecB>& b,
            const Eigen::EigenBase<VecX>& x, const Eigen::EigenBase<VecY>& y)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecB);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecY);

        m_a = a.derived();
        m_b = b.derived();
        m_x = x.derived();
        m_y = y.derived();

        m_work.resize(a.derived().size());
        m_ipiv.resize(a.derived().size());

        const Index rank = pivot_order();
        m_matPL.resize(m_a.derived().size(), rank);
        m_vecD.resize(rank);
        factorize();

        // apply row permutations
        detail::apply_row_permutation(m_ipiv, m_matPL, m_work);

        m_is_initialized = true;
    }

    ///
    /// Set threshold value for GECP termination.
    ///
    /// We stop GECP step as soon as the diagonal element of Cholesky factor
    /// \f$D_{mm}\f$ becomes smaller than this value.
    ///
    QuasiCauchyRRD& setThreshold(RealScalar threshold)
    {
        m_threshold = threshold;
        return *this;
    }

    ///
    /// \return the rank of matrix revealed.
    ///
    Index rank() const
    {
        return m_vecD.size();
    }

    ///
    /// \return const reference to the rank revealing factor \f$ X=PL \f$
    ///
    const MatrixType& matrixPL() const
    {
        assert(m_is_initialized && "QuasiCauchyRRD is not initialized");
        return m_matPL;
    }

    ///
    /// \return const reference to the rank revealing factor \f$ D \f$
    ///
    const RealVectorType& vectorD() const
    {
        assert(m_is_initialized && "QuasiCauchyRRD is not initialized");
        return m_vecD;
    }

    ///
    /// Create dense matrix expression of quasi-Cauchy matrix
    ///
    /// \param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] b  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    /// \param[in] y  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecB, typename VecX, typename VecY>
    static Eigen::CwiseNullaryOp<
        detail::quasi_cauchy_functor<VecA, VecB, VecX, VecY, Functor>,
        MatrixType>
    makeDenseExpr(const Eigen::EigenBase<VecA>& a,
                  const Eigen::EigenBase<VecB>& b,
                  const Eigen::EigenBase<VecX>& x,
                  const Eigen::EigenBase<VecY>& y)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecB);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecY);

        assert(b.size() == a.size());
        assert(x.size() == a.size());
        assert(y.size() == a.size());

        using functor_t =
            detail::quasi_cauchy_functor<VecA, VecB, VecX, VecY, Functor>;

        return MatrixType::NullaryExpr(
            a.size(), a.size(),
            functor_t(a.derived(), b.derived(), x.derived(), y.derived()));
    }

protected:
    VectorType m_a;
    VectorType m_b;
    VectorType m_x;
    VectorType m_y;
    VectorType m_work;
    IndicesType m_ipiv;
    MatrixType m_matPL;
    RealVectorType m_vecD;

    RealScalar m_threshold;
    bool m_is_initialized;

    Index pivot_order();
    void factorize();
};

template <typename T, typename Functor>
typename QuasiCauchyRRD<T, Functor>::Index
QuasiCauchyRRD<T, Functor>::pivot_order()
{
    using Eigen::numext::abs;

    const Index n = m_a.size();

    assert(m_b.size() == n);
    assert(m_x.size() == n);
    assert(m_y.size() == n);
    assert(m_work.size() == n);
    assert(m_ipiv.size() == n);

    //
    // Form vector g(i) = a(i) * b(i) / (x(i) + y(i))
    //
    VectorType& g = m_work;

    for (Index i = 0; i < n; ++i)
    {
        g[i] = Functor::matrix_element(m_a[i], m_b[i], m_x[i], m_y[i]);
    }

    // Initialize rows transposition matrix
    for (Index i = 0; i < n; ++i)
    {
        m_ipiv[i] = i;
    }

    // GECP iteration
    Index m = 0;
    while (m < n)
    {
        //
        // Find m <= l < n such that |g(l)| = max_{m<=k<n}|g(k)|
        //
        Index l;
        auto max_diag = RealScalar();
        for (Index k = m; k < n; ++k)
        {
            const auto abs_gk = abs(g[k]);
            if (abs_gk > max_diag)
            {
                max_diag = abs_gk;
                l        = k;
            }
        }

        if (max_diag < m_threshold)
        {
            break;
        }

        if (l != m)
        {
            // Swap elements
            std::swap(m_ipiv[l], m_ipiv[m]);
            std::swap(g[l], g[m]);
            std::swap(m_a[l], m_a[m]);
            std::swap(m_b[l], m_b[m]);
            std::swap(m_x[l], m_x[m]);
            std::swap(m_y[l], m_y[m]);
        }

        // Update diagonal of Schur complement
        const auto xm = m_x[m];
        const auto ym = m_y[m];

        for (Index k = m + 1; k < n; ++k)
        {
            // g[k] *= (m_x[k] - xm) / (m_x[k] + ym);
            g[k] *= Functor::update_coeff(m_x[k], xm, ym);
            // g[k] *= (m_y[k] - ym) / (m_y[k] + xm);
            g[k] *= Functor::update_coeff(m_y[k], ym, xm);
        }
        ++m;
    }
    //
    // Returns the rank of input matrix
    //
    return m;
}

template <typename T, typename Functor>
void QuasiCauchyRRD<T, Functor>::factorize()
{
    using Eigen::numext::real;
    using Eigen::numext::sqrt;

    const auto n = m_matPL.rows();
    const auto m = m_matPL.cols();

    m_matPL.setZero();
    const auto b0 = m_b[0];
    const auto y0 = m_y[0];

    for (Index l = 0; l < n; ++l)
    {
        // m_matPL(l, 0) = m_a[l] * b0 / (m_x[l] + y0);
        m_matPL(l, 0) = Functor::matrix_element(m_a[l], b0, m_x[l], y0);
    }

    for (Index k = 1; k < m; ++k)
    {
        // Upgrade generators
        const auto xkm1 = m_x[k - 1];
        const auto ykm1 = m_y[k - 1];
        for (Index l = k; l < n; ++l)
        {
            // m_a[l] *= (m_x[l] - xkm1) / (m_x[l] + ykm1);
            m_a[l] *= Functor::update_coeff(m_x[l], xkm1, ykm1);
        }
        for (Index l = k; l < n; ++l)
        {
            // m_b[l] *= (m_y[l] - ykm1) / (m_y[l] + xkm1);
            m_b[l] *= Functor::update_coeff(m_y[l], ykm1, xkm1);
        }
        // Extract k-th column for Cholesky factors
        const auto bk = m_b[k];
        const auto yk = m_y[k];
        for (Index l = k; l < n; ++l)
        {
            m_matPL(l, k) = Functor::matrix_element(m_a[l], bk, m_x[l], yk);
        }
    }
    //
    // Scale strictly lower triangular part of G
    //   - diagonal part of G contains D**2
    //   - L = tril(G) * D^{-2} + I
    //
    for (Index j = 0; j < m; ++j)
    {
        const auto djj   = real(m_matPL(j, j));
        const auto scale = RealScalar(1) / djj;

        m_matPL(j, j) = RealScalar(1);
        m_vecD[j] = sqrt(djj);
        for (Index i = j + 1; i < n; ++i)
        {
            m_matPL(i, j) *= scale;
        }
    }

    return;
}

namespace detail
{

template <typename VecA, typename VecX>
struct self_adjoint_quasi_cauchy_helper
{
    using Scalar =
        typename Eigen::ScalarBinaryOpTraits<typename VecA::Scalar,
                                             typename VecX::Scalar>::ReturnType;
    using MatrixType =
        Eigen::Matrix<Scalar, VecA::SizeAtCompileTime, VecA::SizeAtCompileTime,
                      Eigen::ColMajor, VecA::MaxSizeAtCompileTime,
                      VecA::MaxSizeAtCompileTime>;
};

template <typename VecA, typename VecX>
struct self_adjoint_quasi_cauchy_functor
{
    using Scalar =
        typename Eigen::ScalarBinaryOpTraits<typename VecA::Scalar,
                                             typename VecX::Scalar>::ReturnType;
    using Index = Eigen::Index;
    using MatrixType =
        Eigen::Matrix<Scalar, VecA::SizeAtCompileTime, VecA::SizeAtCompileTime,
                      Eigen::ColMajor, VecA::MaxSizeAtCompileTime,
                      VecA::MaxSizeAtCompileTime>;

    self_adjoint_quasi_cauchy_functor(const VecA& a, const VecX& x)
        : m_a(a), m_x(x)
    {
    }

    Scalar operator()(Index i, Index j) const
    {
        using Eigen::numext::conj;
        return m_a(i) * conj(m_a(j)) / (m_x(i) + conj(m_x(j)));
    }

private:
    const VecA& m_a;
    const VecX& m_x;
};

} // namespace detail

///
/// ### SelfAdjointQuasiCauchyRRD
///
/// \brief  Compute the rank-revealing decomposition (RRD) of a self-adjoint
///   quasi-Cauchy matrix in high relative accuracy.
///
/// \tparam T  the scalar type of matrix to be decomposed
///
/// For given arrays of $a_{i},x_{i} \, (i=1,2,\dots,n)$ a self-adjoint
/// quasi-Cauchy matrix \f$ C \f$ is defined as
///
///   \f[
///     C_{ij} = \frac{a_{i}^{} a_{j}^{\ast}}{x_{i}^{} + x_{j}^{\ast}} \quad
///     (i,j=1,2,\dots,n).
///   \f]
///
/// We assume that the matrix \f$ C \f$ is also positive definite. A
/// quasi-Cauchy matrix is usually rank deficient. Let \f$ m = \text{rank}(C)
/// \f$ then, matrix `C` can have partial Cholesky decomposition of the form
///
///   \f[ C = (PL)D^2(PL)^{\ast}, \f]
///
/// where \f$ L \f$ is \f$ n \times m \f$ unit triangular (trapezoidal) matrix,
/// \f$ D \f$ is \f$ m \times m \f$ diagonal matrix, and \f$ P \f$ is a \f$ m
/// \times n \f$ permutation matrix.
///
/// #### Algorithm
///
/// The factorization is made using the modified Gaussian elimination of
/// complete pivoting (GECP) proposed in Ref. [1], which is also described in
/// Algorithm 2 and 3 in Ref.[2]. Following the algorithms described in Ref.
/// [2], we stop the factorization when the diagonal element \f$ D_{mm} \f$
/// becomes smaller than the threshold value, \f$ D_{mm} \leq \delta^2 \epsilon
/// \f$, at certain \f$ m \f$ where \f$ \delta \f$ is mimimal target size of
/// singular values of the systems to be kept in the later step, and \f$
/// \epsilon \f$ is the machine epsilon. Note that the diagonal elements \f$
/// D_{ii} \f$ are sorted in non-increasing order by complete pivoting.
///
/// #### Complexity
///
/// The complexity of this algorithm is \f$
/// \mathcal{O}(n(\log(\delta\epsilon)^{-1})^2) \f$.
///
///
/// #### References
///
/// 1. J. Demmel, "ACCURATE SINGULAR VALUE DECOMPOSITIONS OF STRUCTURED
///    MATRICES", SIAM J. Matrix Anal. Appl. **21** (1999) 562-580.
///    [DOI: https://doi.org/10.1137/S0895479897328716]
/// 2. T. S. Haut and G. Beylkin, "FAST AND ACCURATE CON-EIGENVALUE ALGORITHM
///    FOR OPTIMAL RATIONAL APPROXIMATIONS", SIAM J. Matrix Anal. Appl. **33**
///    (2012) 1101-1125.
///    [DOI: https://doi.org/10.1137/110821901]
///
template <typename T>
class SelfAdjointQuasiCauchyRRD
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<T>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using StorageIndex  = Eigen::Index;
    using Index         = Eigen::Index;
    using MatrixType    = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using VectorType    = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVectorType = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using IndicesType    = Eigen::Matrix<Index, Eigen::Dynamic, 1>;

    ///
    /// Default constructor
    ///
    SelfAdjointQuasiCauchyRRD()
        : m_a(),
          m_x(),
          m_work(),
          m_ipiv(),
          m_matPL(),
          m_vecD(),
          m_threshold(Eigen::NumTraits<RealScalar>::epsilon()),
          m_is_initialized(false)
    {
    }

    ///
    /// Default destructor
    ///
    ~SelfAdjointQuasiCauchyRRD() = default;

    ///
    /// Compute RRD of self-adjoint quasi-Cauchy matrix.
    ///
    /// @param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// @param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecX>
    void compute(const Eigen::EigenBase<VecA>& a,
                 const Eigen::EigenBase<VecX>& x)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);

        m_a = a.derived();
        m_x = x.derived();
        m_work.resize(a.derived().size());
        m_ipiv.resize(a.derived().size());

        const Index rank = pivot_order();
        m_matPL.resize(m_a.derived().size(), rank);
        m_vecD.resize(rank);
        factorize();

        // apply row permutations
        detail::apply_row_permutation(m_ipiv, m_matPL, m_work);

        m_is_initialized = true;
    }

    ///
    /// Set threshold value for GECP termination.
    ///
    /// We stop GECP step as soon as the diagonal element of Cholesky factor
    /// \f$D_{mm}\f$ becomes smaller than this value.
    ///
    SelfAdjointQuasiCauchyRRD& setThreshold(RealScalar threshold)
    {
        m_threshold = threshold;
        return *this;
    }

    ///
    /// \return the rank of matrix revealed.
    ///
    Index rank() const
    {
        return m_vecD.size();
    }
    ///
    /// \return const reference to the rank revealing factor \f$ X=PL \f$
    ///
    const MatrixType& matrixPL() const
    {
        assert(m_is_initialized &&
               "SelfAdjointQuasiCauchyRRD is not initialized");
        return m_matPL;
    }

    ///
    /// \return const reference to the rank revealing factor \f$ D \f$
    ///
    const RealVectorType& vectorD() const
    {
        assert(m_is_initialized &&
               "SelfAdjointQuasiCauchyRRD is not initialized");
        return m_vecD;
    }

    ///
    /// Create dense matrix expression of quasi-Cauchy matrix
    ///
    /// \param[in] a  vector of length @f$ n @f$ defining matrix @f$ C. @f$.
    /// \param[in] x  vector of length @f$ n @f$ defining matrix @f$ C. @f$
    ///
    template <typename VecA, typename VecX>
    static Eigen::CwiseNullaryOp<
        detail::self_adjoint_quasi_cauchy_functor<VecA, VecX>, MatrixType>
    makeDenseExpr(const Eigen::DenseBase<VecA>& a,
                  const Eigen::DenseBase<VecX>& x)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecA);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecX);
        using functor_t = detail::self_adjoint_quasi_cauchy_functor<VecA, VecX>;

        assert(a.size() == x.size());

        return MatrixType::NullaryExpr(a.size(), a.size(),
                                       functor_t(a.derived(), x.derived()));
    }

protected:
    VectorType m_a;
    VectorType m_x;
    VectorType m_work;
    IndicesType m_ipiv;
    MatrixType m_matPL;
    RealVectorType m_vecD;

    RealScalar m_threshold;
    bool m_is_initialized;

    Index pivot_order();
    void factorize();
};

template <typename T>
Eigen::Index SelfAdjointQuasiCauchyRRD<T>::pivot_order()
{
    using Eigen::numext::abs;
    using Eigen::numext::abs2;
    using Eigen::numext::conj;

    const Index n = m_a.size();

    assert(m_x.size() == n);
    assert(m_work.size() == n);
    assert(m_ipiv.size() == n);

    //
    // Form vector g(i) = a(i) * b(i) / (x(i) + y(i))
    //
    VectorType& g = m_work;

    for (Index i = 0; i < n; ++i)
    {
        g[i] = m_a[i] * conj(m_a[i]) / (m_x[i] + conj(m_x[i]));
    }

    // Initialize rows transposition matrix
    for (Index i = 0; i < n; ++i)
    {
        m_ipiv(i) = i;
    }

    // GECP iteration
    Index m = 0;
    while (m < n)
    {
        //
        // Find m <= l < n such that |g(l)| = max_{m<=k<n}|g(k)|
        //
        Index l;
        auto max_diag = RealScalar();
        for (Index k = m; k < n; ++k)
        {
            const auto abs_gk = abs(g[k]);
            if (abs_gk > max_diag)
            {
                max_diag = abs_gk;
                l        = k;
            }
        }

        if (max_diag < m_threshold)
        {
            break;
        }

        if (l != m)
        {
            // Swap elements
            std::swap(m_ipiv[l], m_ipiv[m]);
            std::swap(g[l], g[m]);
            std::swap(m_a[l], m_a[m]);
            std::swap(m_x[l], m_x[m]);
        }

        // Update diagonal of Schur complement
        const auto xm = m_x[m];
        const auto ym = conj(m_x[m]);

        for (Index k = m + 1; k < n; ++k)
        {
            const auto s1 = (m_x[k] - xm) / (m_x[k] + ym);
            g[k] *= s1 * conj(s1);
        }
        ++m;
    }
    //
    // Returns the rank of input matrix
    //
    return m;
}

template <typename T>
void SelfAdjointQuasiCauchyRRD<T>::factorize()
{
    using Eigen::numext::conj;
    using Eigen::numext::real;
    using Eigen::numext::sqrt;

    const auto n = m_matPL.rows();
    const auto m = m_matPL.cols();

    m_matPL.setZero();
    const auto b0 = conj(m_a(0));
    const auto y0 = conj(m_x(0));

    for (Index l = 0; l < n; ++l)
    {
        m_matPL(l, 0) = m_a[l] * b0 / (m_x[l] + y0);
    }

    for (Index k = 1; k < m; ++k)
    {
        // Upgrade generators
        const auto xkm1 = m_x[k - 1];
        const auto ykm1 = conj(xkm1);
        for (Index l = k; l < n; ++l)
        {
            m_a[l] *= (m_x[l] - xkm1) / (m_x[l] + ykm1);
        }
        // Extract k-th column for Cholesky factors
        const auto bk = conj(m_a[k]);
        const auto yk = conj(m_x[k]);
        for (Index l = k; l < n; ++l)
        {
            m_matPL(l, k) = m_a[l] * bk / (m_x[l] + yk);
        }
    }
    //
    // Scale strictly lower triangular part of G
    //   - diagonal part of G contains D**2
    //   - L = tril(G) * D^{-2} + I
    //
    for (Index j = 0; j < m; ++j)
    {
        const auto djj   = real(m_matPL(j, j));
        const auto scale = RealScalar(1) / djj;

        m_matPL(j, j) = RealScalar(1);
        m_vecD[j] = sqrt(djj);
        for (Index i = j + 1; i < n; ++i)
        {
            m_matPL(i, j) *= scale;
        }
    }

    return;
}

} // namespace: mxpfit

#endif /* MXPFIT_QUASI_CAUCHY_RRD_HPP */


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Currently needs -DMTL_DEEP_COPY_CONSTRUCTOR !!!

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace std;

    mtl::compressed2D<double> m(3, 3);
    {
	mtl::mat::inserter<mtl::compressed2D<double> > ins(m);
	ins(0, 1) << 2.0; ins(1, 0) << 1.0;
	ins(1, 1) << 4.0; ins(2, 2) << 5.0;
    }

    mtl::dense_vector<double> x(3), y(3);
    for (unsigned i= 0; i < size(x); i++) x[i]= double(i+1);

    y = trans(m) * x;
    cout << y << '\n';

    MTL_THROW_IF(y[0] != 2.0, mtl::runtime_error("y[0] should be 2.0!\n"));

    return 0;
}


// Author(s): Jeroen van der Wulp
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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 sort_expression_test.cpp
/// \brief Basic regression test for sort expressions.

#include <boost/test/minimal.hpp>
#include <iostream>

#include "mcrl2/data/bool.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/rewriter.h"
#include "mcrl2/data/standard_utility.h"

using namespace mcrl2;
using namespace mcrl2::data;

template < typename Rewriter >
void representation_check(Rewriter& R, data_expression const& input, data_expression const& expected, const data_specification& spec)
{
  data_expression output(R(normalize_sorts(input,spec)));

  BOOST_CHECK(normalize_sorts(expected,spec) == output);

  if (output != normalize_sorts(expected,spec))
  {
    std::clog << "--- test failed --- " << data::pp(input) << " ->* " << data::pp(expected) << std::endl
              << "input    " << data::pp(input) << std::endl
              << "expected " << data::pp(expected) << std::endl
              << "R(input) " << data::pp(output) << std::endl
              << " -- term representations -- " << std::endl
              << "input    " << input << std::endl
              << "expected " << normalize_sorts(expected,spec)<< std::endl
              << "R(input) " << normalize_sorts(output,spec) << std::endl;
  }
}

void number_test()
{
  using namespace sort_bool;
  using namespace sort_pos;
  using namespace sort_nat;
  using namespace sort_int;
  using namespace sort_real;

  BOOST_CHECK(data::detail::as_decimal_string(1) == "1");
  BOOST_CHECK(data::detail::as_decimal_string(2) == "2");
  BOOST_CHECK(data::detail::as_decimal_string(3) == "3");
  BOOST_CHECK(data::detail::as_decimal_string(4) == "4");
  BOOST_CHECK(data::detail::as_decimal_string(144) == "144");

  // Test character array arithmetic
  std::vector< char > numbers;
  numbers = data::detail::string_to_vector_number("1");
  BOOST_CHECK(data::detail::vector_number_to_string(numbers) == "1");

  data::detail::decimal_number_multiply_by_two(numbers);
  BOOST_CHECK(data::detail::vector_number_to_string(numbers) == "2");

  data::detail::decimal_number_increment(numbers);
  BOOST_CHECK(data::detail::vector_number_to_string(numbers) == "3");

  data::detail::decimal_number_increment(numbers);
  BOOST_CHECK(data::detail::vector_number_to_string(numbers) == "4");

  data::detail::decimal_number_multiply_by_two(numbers);
  BOOST_CHECK(data::detail::vector_number_to_string(numbers) == "8");

  data::detail::decimal_number_multiply_by_two(numbers);
  BOOST_CHECK(data::detail::vector_number_to_string(numbers) == "16");

  data::detail::decimal_number_divide_by_two(numbers);
  BOOST_CHECK(data::detail::vector_number_to_string(numbers) == "8");

  BOOST_CHECK(sort_pos::positive_constant_as_string(number(sort_pos::pos(), "1")) == "1");
  BOOST_CHECK(sort_pos::positive_constant_as_string(number(sort_pos::pos(), "10")) == "10");
  BOOST_CHECK(sort_nat::natural_constant_as_string(number(sort_nat::nat(), "0")) == "0");
  BOOST_CHECK(sort_nat::natural_constant_as_string(number(sort_nat::nat(), "1")) == "1");
  BOOST_CHECK(sort_nat::natural_constant_as_string(number(sort_nat::nat(), "10")) == "10");
  BOOST_CHECK(sort_int::integer_constant_as_string(number(sort_int::int_(), "-10")) == "-10");
  BOOST_CHECK(sort_int::integer_constant_as_string(number(sort_int::int_(), "10")) == "10");

  data_specification specification = parse_data_specification("sort A = Real;");

  mcrl2::data::rewriter R(specification);

  representation_check(R, number(sort_pos::pos(), "1"), sort_pos::c1(),specification);
  representation_check(R, number(sort_nat::nat(), "1"), R(normalize_sorts(pos2nat(sort_pos::c1()),specification)),specification);
  representation_check(R, number(sort_int::int_(), "-1"), R(cneg(sort_pos::c1())),specification);
  representation_check(R, normalize_sorts(number(sort_real::real_(), "1"),specification), R(normalize_sorts(pos2real(sort_pos::c1()),specification)),specification);

  representation_check(R, pos("11"), cdub(true_(), cdub(true_(), cdub(false_(), c1()))),specification);
  representation_check(R, pos(12), cdub(false_(), cdub(false_(), cdub(true_(), c1()))),specification);
  representation_check(R, nat("18"), R(normalize_sorts(pos2nat(cdub(false_(), cdub(true_(), cdub(false_(), cdub(false_(), c1()))))),specification)),specification);
  representation_check(R, nat(12), R(normalize_sorts(pos2nat(cdub(false_(), cdub(false_(), cdub(true_(), c1())))),specification)),specification);
  representation_check(R, int_("0"), R(nat2int(c0())),specification);
  representation_check(R, int_("-1"), cneg(c1()),specification);
  representation_check(R, int_(-2), cneg(cdub(false_(), c1())),specification);
  representation_check(R, real_("0"), R(normalize_sorts(nat2real(c0()),specification)),specification);
  representation_check(R, real_("-1"), R(normalize_sorts(int2real(cneg(c1())),specification)),specification);
  representation_check(R, real_(-2), R(normalize_sorts(int2real(cneg(cdub(false_(), c1()))),specification)),specification);

}

void list_construction_test()
{
  using namespace mcrl2::data::sort_list;
  using namespace mcrl2::data::sort_bool;

  data_expression_vector expressions;

  expressions.push_back(true_());
  expressions.push_back(false_());
  expressions.push_back(true_());
  expressions.push_back(false_());

  data_specification specification;

  mcrl2::data::rewriter R(specification, jitty);

  representation_check(R, sort_list::list(bool_(), expressions),
                       R(cons_(bool_(), expressions[0], cons_(bool_(), expressions[1],
                               cons_(bool_(), expressions[2], cons_(bool_(), expressions[3], empty(bool_())))))),specification);
}

void convert_test()
{
  std::vector< data_expression > l;

  l.push_back(sort_bool::true_());

  atermpp::aterm_list al(l.begin(),l.end());

  BOOST_CHECK(l.size() == al.size());
}

int test_main(int argc, char** argv)
{
  number_test();

  list_construction_test();

  convert_test();

  return EXIT_SUCCESS;
}


/*
 * tsdf_difference_statistics.hpp
 *
 *  Created on: Mar 15, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

#pragma once

//stdlib
#include <iostream>

//libraries
#include <Eigen/Eigen>

//local
#include "../math/typedefs.hpp"

namespace eig = Eigen;

namespace telemetry {
/**
 * A structure for logging statistics pertaining to numerical differences between corresponding locations in the
 * canonical and the live (target and source) TSDF fields after optimization
 */
template<typename Coordinates>
struct TsdfDifferenceStatistics {
	float difference_min = 0.0f;
	float difference_max = 0.0f;
	float difference_mean = 0.0f;
	float difference_standard_deviation = 0.0f;
	Coordinates biggest_difference_location = Coordinates(0);

	TsdfDifferenceStatistics() = default;
	TsdfDifferenceStatistics(
			float difference_min,
			float difference_max,
			float difference_mean,
			float difference_standard_deviation,
			Coordinates biggest_difference_location
			);

	eig::VectorXf to_array();

	bool operator==(const TsdfDifferenceStatistics& rhs);
	bool operator!=(const TsdfDifferenceStatistics& rhs);
};
template<typename Coordinates>
std::ostream &operator<<(std::ostream &ostr, const TsdfDifferenceStatistics<Coordinates> &ts);

template<typename Coordinates, typename ScalarContainer>
TsdfDifferenceStatistics<Coordinates> build_tsdf_difference_statistics(const ScalarContainer& canonical_field,
		const ScalarContainer& live_field);

typedef TsdfDifferenceStatistics<math::Vector2i> TsdfDifferenceStatistics2d;
typedef TsdfDifferenceStatistics<math::Vector3i> TsdfDifferenceStatistics3d;

} //namespace telemetry


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

#include <iostream>
#include <stdint.h>
#include <vector>
#include <Eigen/Dense>

#include <dpMM/dpMM.hpp>
#include <dpMM/cat.hpp>
#include <dpMM/niw.hpp>

using namespace Eigen;
using std::endl;
using std::cout; using std::vector; using std::string;

/*
 * DP mixture model
 * following Neal [[http://www.stat.purdue.edu/~rdutta/24.PDF]]
 * Algo 3
 */
template <class Dist>
class DpStickMM : public DpMM<double>
{
public:
  DpStickMM(double alpha, const Dist& H)
    : alpha_(alpha), H_(H)
   {};
  ~DpStickMM()
  {};

  virtual void initialize(const MatrixXd& x);
  virtual void initialize(const shared_ptr<ClGMMData<double> >& cld)
    {cout<<"not supported"<<endl; assert(false);};
  virtual void sampleLabels();
  virtual void sampleParameters(){;};
  virtual const VectorXu & getLabels() {return z_;};
  virtual uint32_t getK() const { return z_.maxCoeff()+1;};

  VectorXd getCounts();

private:
  void removeEmptyClusters();

  double alpha_; //
  vector<Dist> models_;

  MatrixXd x_;
  VectorXu z_; // indicators
  Dist H_; //
};

// ---------------- impl -----------------------------------------------------
template <class Dist>
void DpStickMM<Dist>::initialize(const MatrixXd& x)
{
  uint32_t K0=1;
  x_ = x;
  z_.setZero(x.cols(),1);
  for (uint32_t k=0; k<K0; ++k)
    models_.push_back(Dist(H_));
  cout<<models_.size()<<endl;
  //cout<<x_<<endl;
  cout<<alpha_<<endl;
  if (K0>1)
  {
    VectorXd pi(K0);
    pi.setOnes();
    pi /= static_cast<double>(K0);
    Catd cat(pi,H_.pRndGen_);
    for (uint32_t i=0; i<z_.size(); ++i)
      z_(i) = cat.sample();
  }
};

template <class Dist>
VectorXd DpStickMM<Dist>::getCounts()
{
  VectorXd counts(getK());
  counts.setZero();
//TODO
//  for(uint32_t k=0; k<getK(); ++k)
//    counts(k) = models_[k]->count();
  return counts;
};

template <class Dist>
void DpStickMM<Dist>::sampleLabels()
{
  double N = z_.size();
  for(uint32_t i=0; i<z_.size(); ++i)
  {
    // compute clustercounts
    VectorXd Nk(models_.size()); Nk.setZero(models_.size());
    for(uint32_t ii=0; ii<z_.size(); ++ii)
      Nk(z_(ii))++;
    // compute distribution pi over indicators
    VectorXd pi(models_.size()+1);
    for (uint32_t k=0; k<models_.size(); ++k)
      pi(k) = log(Nk(k))-log(N+alpha_) +models_[k].logPosteriorProb(x_,z_,k,i);
    pi(models_.size()) = log(alpha_)-log(N+alpha_)+H_.logProb(x_.col(i));
    // normalize pi and exponentiate it
    double pi_max = pi.maxCoeff();
    pi = (pi.array()-(pi_max + log((pi.array() - pi_max).exp().sum()))).exp().matrix();
    // sample new indicator
    z_(i) = Catd(pi,H_.pRndGen_).sample();
    // if z_i was a new cluster
    if(z_(i)==models_.size())
      models_.push_back(H_);

    // -------- outputs --------------
    if(i%100 ==0)
    {
      cout<<" @i="<<i
        <<"\tcounts="<<Nk.transpose()<<endl;
    }
  }
  this->removeEmptyClusters();
};

template <class Dist>
void DpStickMM<Dist>::removeEmptyClusters()
{
  for(uint32_t k=models_.size()-1; k>=0; --k)
  {
    bool haveCluster_k = false;
    for(uint32_t i=0; i<z_.size(); ++i)
      if(z_(i)==k)
      {
        haveCluster_k = true;
        break;
      }
    if (!haveCluster_k)
    {
      for (uint32_t i=0; i<z_.size(); ++i)
        if(z_(i) >= k) z_(i) --;
      models_.erase(models_.begin()+k);
    }
  }
}


/**
 * @file test-jump.cpp
 *
 * @brief test jump function.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include <time.h>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/ZZ.h>
#include "dSFMT-calc-jump.hpp"
#include "dSFMT-jump.h"

using namespace NTL;
using namespace std;
using namespace dsfmt;

static void read_file(GF2X& characteristic, int line_no, const string& file);
static int test(dsfmt_t * dsfmt, GF2X& poly);
static int check(dsfmt_t *a, dsfmt_t *b, int verbose);
static void print_state(dsfmt_t *a, dsfmt_t * b);
static void print_state_line(w128_t *a, w128_t *b);
static void print_sequence(dsfmt_t *a, dsfmt_t * b);
static int speed(dsfmt_t * dsfmt, GF2X& characteristic);

int main(int argc, char * argv[]) {
    if (argc <= 1) {
	printf("%s -s|-c [prefix]\n", argv[0]);
	return -1;
    }
    string prefix = "poly.";
    if (argc >= 3) {
	prefix = argv[2];
    }
    stringstream ss_file;
    GF2X characteristic;
    ss_file << prefix << DSFMT_MEXP << ".txt";
    read_file(characteristic, 0, ss_file.str());
    dsfmt_t dsfmt;
    if (argv[1][1] == 's') {
	return speed(&dsfmt, characteristic);
    } else {
	return test(&dsfmt, characteristic);
    }
}

static int speed(dsfmt_t * dsfmt, GF2X& characteristic)
{
    uint32_t seed = 1234;
    long step = 10000;
    int exp = 4;
    ZZ test_count;
    string jump_string;
    clock_t start;
    double elapsed1;
    double elapsed2;

    dsfmt_init_gen_rand(dsfmt, seed);
    test_count = step;
    for (int i = 0; i < 10; i++) {
	start = clock();
	calc_jump(jump_string, test_count, characteristic);
	elapsed1 = clock() - start;
	elapsed1 = elapsed1 * 1000 / CLOCKS_PER_SEC;
	cout << "mexp "
	     << setw(5)
	     << DSFMT_MEXP
	     << " jump 10^"
	     << setfill('0') << setw(2)
	     << exp
	     << " steps  calc_jump:"
	     << setfill(' ') << setiosflags(ios::fixed)
	     << setw(6) << setprecision(3)
	     << elapsed1
	     << "ms"
	     << endl;
	start = clock();

	for (int j = 0; j < 10; j++) {
	    dSFMT_jump(dsfmt, jump_string.c_str());
	}
	elapsed2 = clock() - start;
	elapsed2 = elapsed2 * 1000 / 10 / CLOCKS_PER_SEC;
	cout << "mexp "
	     << setw(5)
	     << DSFMT_MEXP
	     << " jump 10^"
	     << setfill('0') << setw(2)
	     << exp
	     << " steps dSFMT_jump:"
	     << setfill(' ') << setiosflags(ios::fixed)
	     << setw(6) << setprecision(3)
	     << elapsed2
	     << "ms"
	     << endl;
	test_count *= 100;
	exp += 2;
    }
    return 0;
}

static void read_file(GF2X& characteristic, int line_no, const string& file)
{
    ifstream ifs(file.c_str());
    string line;
    for (int i = 0; i < line_no; i++) {
	ifs >> line;
	ifs >> line;
    }
    if (ifs) {
	ifs >> line;
	line = "";
	ifs >> line;
    }
    stringtopoly(characteristic, line);
#if defined(DEBUG)
    cout << "line = " << line << endl;
    cout << "deg = " << deg(characteristic) << endl;
    cout << "cha = " << characteristic << endl;
    string x;
    polytostring(x, characteristic);
    cout << "x = " << x << endl;
#endif
}

static int check(dsfmt_t *a, dsfmt_t *b, int verbose)
{
    int check = 0;
    for (int i = 0; i < 100; i++) {
	double x = dsfmt_genrand_close_open(a);
	double y = dsfmt_genrand_close_open(b);
	if (x != y) {
	    print_state(a, b);
	    print_sequence(a, b);
	    check = 1;
	    break;
	}
    }
    if (check == 0) {
	if (verbose) {
	    printf("OK!\n");
	}
    } else {
	printf("NG!\n");
    }
    return check;
}

static void print_state_line(w128_t *a, w128_t *b)
{
    printf("[");
    for (int j = 0; j < 2; j++) {
	printf("%016"PRIx64, a->u[j]);
	if (j == 0) {
	    printf(" ");
	} else {
	    printf("]");
	}
    }
    printf("[");
    for (int j = 0; j < 2; j++) {
	printf("%016"PRIx64, b->u[j]);
	if (j == 0) {
	    printf(" ");
	} else {
	    printf("]");
	}
    }
    printf("\n");
}

static void print_state(dsfmt_t *a, dsfmt_t * b)
{
    printf("idx = %d                            idx = %d\n",
	   a->idx, b->idx);
    for (int i = 0; (i < 10) && (i < DSFMT_N); i++) {
	print_state_line(&a->status[i], &b->status[i]);
    }
    print_state_line(&a->status[DSFMT_N], &b->status[DSFMT_N]);
}

static void print_sequence(dsfmt_t *a, dsfmt_t * b)
{
    for (int i = 0; i < 25; i++) {
	double c, d;
	c = dsfmt_genrand_close_open(a);
	d = dsfmt_genrand_close_open(b);
	printf("[%1.15f %1.15f]\n", c, d);
    }
}

static int test(dsfmt_t * dsfmt, GF2X& characteristic)
{
    dsfmt_t new_dsfmt_z;
    dsfmt_t * new_dsfmt = &new_dsfmt_z;
    uint32_t seed[20] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    long steps[] = {1, 2, DSFMT_N + 1,
		    DSFMT_N * 128 - 1,
		    DSFMT_N * 128 + 1,
		    3003,
		    3004,
		    200004,
		    200005,
		    10000005,
		    10000006};
    int steps_size = sizeof(steps) / sizeof(long);
    ZZ test_count;
    string jump_string;

    dsfmt_init_gen_rand(dsfmt, seed[0]);
    dsfmt_genrand_close_open(dsfmt);
    /* plus jump */
    for (int index = 0; index < steps_size; index++) {
//	dsfmt_init(dsfmt, seed[index]);
	test_count = steps[index];
	cout << "mexp " << dec << DSFMT_MEXP << " jump "
	     << test_count << " steps" << endl;
	*new_dsfmt = *dsfmt;
	for (long i = 0; i < steps[index] * 2; i++) {
	    dsfmt_genrand_uint32(dsfmt);
	}
	calc_jump(jump_string, test_count, characteristic);
#if defined(DEBUG)
	cout << "jump string:" << jump_string << endl;
	cout << "before jump:" << endl;
	print_state(new_dsfmt, dsfmt);
#endif
	dSFMT_jump(new_dsfmt, jump_string.c_str());
#if defined(DEBUG)
	cout << "after jump:" << endl;
	print_state(new_dsfmt, dsfmt);
#endif
	if (check(new_dsfmt, dsfmt, 1)) {
	    return 1;
	}
    }
    dsfmt_t rnd;
    dsfmt_init_gen_rand(&rnd, (uint32_t)clock());
    for (int index = 0; index < 100; index++) {
	dsfmt_init_gen_rand(dsfmt, dsfmt_genrand_uint32(&rnd));
	test_count = dsfmt_genrand_uint32(&rnd) % 100000;
	*new_dsfmt = *dsfmt;
	for (long i = 0; i < test_count * 2; i++) {
	    dsfmt_genrand_uint32(dsfmt);
	}
	calc_jump(jump_string, test_count, characteristic);
	dSFMT_jump(new_dsfmt, jump_string.c_str());
	if (check(new_dsfmt, dsfmt, 0)) {
	    cout << "mexp " << dec << DSFMT_MEXP << " jump "
		 << test_count << " steps ";
	    cout << "check NG!" << endl;
	    return 1;
	}
    }
    return 0;
}


#pragma once

#include <Eigen/Dense>
#include <tuple>
#include <vector>
#include <type_traits>
#include <utility>
#include <algorithm>

namespace mtao { namespace eigen {
    template <bool Rows, typename... Args, int... N>
        auto _stack(std::integer_sequence<int,N...>, const Args&... args) {
            using namespace Eigen;
            using Scalar = typename std::tuple_element<0,std::tuple<Args...>>::type::Scalar;

            constexpr static int minCompileRows = std::min<int>({Args::RowsAtCompileTime...});
            constexpr static int maxCompileRows = std::max<int>({Args::RowsAtCompileTime...});
            constexpr static int minCompileCols = std::min<int>({Args::ColsAtCompileTime...});
            constexpr static int maxCompileCols = std::max<int>({Args::ColsAtCompileTime...});

            constexpr static int sumCompileRows = (Args::RowsAtCompileTime + ... + 0);
            constexpr static int sumCompileCols = (Args::ColsAtCompileTime + ... + 0);

            //constexpr static int myCompRows = (minCompileRows==Dynamic)?Dynamic:(Rows?S:1)*maxCompileRows;
            //constexpr static int myCompCols = (minCompileCols==Dynamic)?Dynamic:(Rows?1:S)*maxCompileCols;
            constexpr static int myCompRows = (minCompileRows==Dynamic)?Dynamic:(Rows?sumCompileRows:maxCompileRows);
            constexpr static int myCompCols = (minCompileCols==Dynamic)?Dynamic:(Rows?maxCompileCols:sumCompileCols);
            int rows;
            int cols;
            std::vector<int> offset(1,0);
            auto push_sum = [&](int size) {
                offset.push_back(offset.back() + size);
            };
            if constexpr(Rows) {
                rows = (args.rows() + ... + 0);
                cols = std::max( {args.cols()...}) ;
                (push_sum(args.rows()),...);
            } else {
                rows = std::max( {args.rows()...}) ;
                cols = (args.cols() + ... + 0);
                (push_sum(args.cols()),...);
            }

            using Matf = Matrix<Scalar,myCompRows,myCompCols>;
            Matf A = Matf::Constant(rows,cols,0);

            if constexpr(Rows) {
                (A.block(offset[N],0,args.rows(),args.cols()).operator=(args),...);
            } else {
                (A.block(0,offset[N],args.rows(),args.cols()).operator=(args),...);
            }

            return A;
        }

    template <typename... Args>
        auto vstack(const Args&... args) {
            return _stack<true>(std::make_integer_sequence<int,sizeof...(Args)>(), std::forward<const Args&>(args)...);
        }
    template <typename... Args>
        auto hstack(const Args&... args) {
            return _stack<false>(std::make_integer_sequence<int,sizeof...(Args)>(), std::forward<const Args&>(args)...);
        }

    template <typename BeginIt, typename EndIt>
        auto hstack_iter(BeginIt beginit, EndIt endit) {
            using CDerived = typename std::decay_t<decltype(*beginit)>;

            constexpr static int CRows = CDerived::RowsAtCompileTime;
            using Index = typename CDerived::Scalar;
            using RetCells = Eigen::Matrix<Index,CRows,Eigen::Dynamic>;
            int ccols = 0;
            int crows = 0;

            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                crows = std::max<int>(crows,c.rows());
                ccols += c.cols();
                }
            }
            RetCells mC(crows,ccols);
            ccols = 0;
            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                mC.block(0,ccols,c.rows(),c.cols()) = c;
                ccols += c.cols();
                }
            }
            return mC;
        }
    template <typename BeginIt, typename EndIt>
        auto vstack_iter(BeginIt beginit, EndIt endit) {
            using CDerived = typename std::decay_t<decltype(*beginit)>;

            constexpr static int CCols = CDerived::ColsAtCompileTime;
            using Index = typename CDerived::Scalar;
            using RetCells = Eigen::Matrix<Index,Eigen::Dynamic,CCols>;
            int ccols = 0;
            int crows = 0;

            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                ccols = std::max<int>(ccols,c.cols());
                crows += c.rows();
                }
            }
            RetCells mC(crows,ccols);
            crows = 0;
            for(auto it = beginit; it != endit; ++it) {
                auto&& c = *it;
                if(c.size() > 0) {
                mC.block(crows,0,c.rows(),c.cols()) = c;
                crows += c.rows();
                }
            }
            return mC;
        }
}}


#include <iostream>
#include <vector>
#include <fstream>
#include <algorithm>
#include <boost/date_time.hpp>
#include "../mylib/heap.hpp"
#include "../mylib/graph.hpp"
#include "../mylib/dijkstra.hpp"

void load_vertices(const std::string &filename, bool get_r,
                my::Graph &vertices, my::Graph &vertices_r,
                u_int64_t &vertice_count, u_int64_t &edge_count){

    std::cout << "reading file " << filename << std::endl;
    std::fstream file(filename, std::ios::in);

    file >> vertice_count >> edge_count;

    vertices.reserve(static_cast<u_int64_t>(vertice_count));
    for(u_int64_t i = 0; i < vertice_count; i++){
        vertices.emplace_back(my::Vertice(i));
        if (get_r) vertices_r.emplace_back(my::Vertice(i));
    }

    u_int64_t fro, to, weight;
    while (file >> fro >> to >> weight) {
        vertices[fro].edges.emplace_back(to, weight);
        if (get_r) vertices_r[to].edges.emplace_back(fro, weight);
    }
    file.close();
}

std::vector<unsigned long> getpath(std::vector<unsigned long> ancestors, my::Vertice start, my::Vertice end){
    std::vector<unsigned long> path;
    for(auto i = start.index; i != ULONG_MAX; i = ancestors[i]){
        path.emplace_back(i);
    }
    return path;
}

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

    std::string filename = "../graphs/vg1";
    if (argc > 1)
        filename = argv[1];

    my::Graph vertices;
    my::Graph vertices_r;
    u_int64_t vertice_count = 0;
    u_int64_t edge_count = 0;

    boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();
    load_vertices(filename, true, vertices, vertices_r, vertice_count, edge_count);
    boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time();
    std::cout << "loaded " << vertices.size() << " vertices in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    start = boost::posix_time::microsec_clock::local_time();
    auto res = my::dijkstra(vertices, 0);
    auto ancestors = res.first;
    auto distances = res.second;
    end = boost::posix_time::microsec_clock::local_time();
    std::cout << "found dijkstra in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    if (ancestors.size() < 10000000){
        std::string tab = "\t\t\t";
        std::cout << "Node" << "\t" << "ancestor" << "\t" << "distance" << std::endl;
        for(u_int64_t i = 0; i < ancestors.size() and i < 100; i++){
            std::cout << i << tab;
            if (ancestors[i] != ULONG_MAX) {
                std::cout << ancestors[i];
            } else {
                std::cout << " ";
            }
            std::cout << tab;
            if (distances.at(i) != ULONG_MAX) {
                 std::cout << distances[i];
            } else {
                std::cout << "unreachable";
            }
            std::cout << std::endl;
        }
    }

    std::cout << "Path from " << 50 << " to " << 0 << std::endl;
    for(auto i : getpath(ancestors, 50, 0)){
        std::cout << i;
        if (i != 0) std::cout << ", ";
    }
    std::cout << std::endl;

    return EXIT_SUCCESS;

}


//////////////////////////////////////////////////////////////////////////////
// random::location_scale.hpp                                               //
//                                                                          //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_LOCATION_SCALE_HPP_ER_2009
#define BOOST_RANDOM_LOCATION_SCALE_HPP_ER_2009
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
//#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/chi_squared.hpp>
namespace boost{
namespace random{

    // Samples from a location-scale distribution
    //
    // X = sigma Z + mu
    //
    // TODO Z shouldn't be allowed to be a ref because ref_distribution already
    // exists
    template<typename Z>
    class location_scale_distribution{
        public:
            typedef typename remove_cv<
                typename remove_reference<Z>::type
            >::type z_type;
            typedef typename z_type::input_type input_type;
            typedef typename z_type::result_type result_type;

        location_scale_distribution(){}
        location_scale_distribution(
            const result_type& mu,
            const result_type& sigma,
            typename call_traits<Z>::param_type z
        )
        :mu_(mu),sigma_(sigma),z_(z){}

        template<typename U>
        result_type operator()(U& urng){ return (this->impl(urng)); }

        template<typename U>
        result_type operator()(U& urng)const{ return (this->impl(urng)); }

        const result_type& mu()const{ return this->mu_; }
        const result_type& sigma()const{ return this->sigma_; }
        const Z& z(){ return this->z_ ;}

        private:
        result_type mu_;
        result_type sigma_;
        typename call_traits<Z>::value_type z_;
        template<typename U>
        result_type impl(U& urng){
            return (this->mu()) + (this->sigma()) * (this->z_)(urng);
        }

    };

}// random
}// boost

#endif


//  (C) Copyright John Maddock 2015.
//  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.hpp>

#ifndef BOOST_NO_CXX11_HDR_TUPLE

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <iostream>
#include <iomanip>
#include <tuple>
#include "table_type.hpp"

// No derivatives - using TOMS748 internally.
struct cbrt_functor_noderiv
{ //  cube root of x using only function - no derivatives.
   cbrt_functor_noderiv(double to_find_root_of) : a(to_find_root_of)
   { // Constructor just stores value a to find root of.
   }
   double operator()(double x)
   {
      double fx = x*x*x - a; // Difference (estimate x^3 - a).
      return fx;
   }
private:
   double a; // to be 'cube_rooted'.
}; // template <class T> struct cbrt_functor_noderiv

// Using 1st derivative only Newton-Raphson
struct cbrt_functor_deriv
{ // Functor also returning 1st derviative.
   cbrt_functor_deriv(double const& to_find_root_of) : a(to_find_root_of)
   { // Constructor stores value a to find root of,
      // for example: calling cbrt_functor_deriv<double>(x) to use to get cube root of x.
   }
   std::pair<double, double> operator()(double const& x)
   { // Return both f(x) and f'(x).
      double fx = x*x*x - a; // Difference (estimate x^3 - value).
      double dx = 3 * x*x; // 1st derivative = 3x^2.
      return std::make_pair(fx, dx); // 'return' both fx and dx.
   }
private:
   double a; // to be 'cube_rooted'.
};
// Using 1st and 2nd derivatives with Halley algorithm.
struct cbrt_functor_2deriv
{ // Functor returning both 1st and 2nd derivatives.
   cbrt_functor_2deriv(double const& to_find_root_of) : a(to_find_root_of)
   { // Constructor stores value a to find root of, for example:
      // calling cbrt_functor_2deriv<double>(x) to get cube root of x,
   }
   std::tuple<double, double, double> operator()(double const& x)
   { // Return both f(x) and f'(x) and f''(x).
      double fx = x*x*x - a; // Difference (estimate x^3 - value).
      double dx = 3 * x*x; // 1st derivative = 3x^2.
      double d2x = 6 * x; // 2nd derivative = 6x.
      return std::make_tuple(fx, dx, d2x); // 'return' fx, dx and d2x.
   }
private:
   double a; // to be 'cube_rooted'.
};

template <class T, class Policy>
struct ibeta_roots_1   // for first order algorithms
{
   ibeta_roots_1(T _a, T _b, T t, bool inv = false)
      : a(_a), b(_b), target(t), invert(inv) {}

   T operator()(const T& x)
   {
      return boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
   }
private:
   T a, b, target;
   bool invert;
};

template <class T, class Policy>
struct ibeta_roots_2   // for second order algorithms
{
   ibeta_roots_2(T _a, T _b, T t, bool inv = false)
      : a(_a), b(_b), target(t), invert(inv) {}

   boost::math::tuple<T, T> operator()(const T& x)
   {
      typedef boost::math::lanczos::lanczos<T, Policy> S;
      typedef typename S::type L;
      T f = boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
      T f1 = invert ?
         -boost::math::detail::ibeta_power_terms(b, a, 1 - x, x, L(), true, Policy())
         : boost::math::detail::ibeta_power_terms(a, b, x, 1 - x, L(), true, Policy());
      T y = 1 - x;
      if (y == 0)
         y = boost::math::tools::min_value<T>() * 8;
      f1 /= y * x;

      // make sure we don't have a zero derivative:
      if (f1 == 0)
         f1 = (invert ? -1 : 1) * boost::math::tools::min_value<T>() * 64;

      return boost::math::make_tuple(f, f1);
   }
private:
   T a, b, target;
   bool invert;
};

template <class T, class Policy>
struct ibeta_roots_3   // for third order algorithms
{
   ibeta_roots_3(T _a, T _b, T t, bool inv = false)
      : a(_a), b(_b), target(t), invert(inv) {}

   boost::math::tuple<T, T, T> operator()(const T& x)
   {
      typedef typename boost::math::lanczos::lanczos<T, Policy>::type L;
      T f = boost::math::detail::ibeta_imp(a, b, x, Policy(), invert, true) - target;
      T f1 = invert ?
         -boost::math::detail::ibeta_power_terms(b, a, 1 - x, x, L(), true, Policy())
         : boost::math::detail::ibeta_power_terms(a, b, x, 1 - x, L(), true, Policy());
      T y = 1 - x;
      if (y == 0)
         y = boost::math::tools::min_value<T>() * 8;
      f1 /= y * x;
      T f2 = f1 * (-y * a + (b - 2) * x + 1) / (y * x);
      if (invert)
         f2 = -f2;

      // make sure we don't have a zero derivative:
      if (f1 == 0)
         f1 = (invert ? -1 : 1) * boost::math::tools::min_value<T>() * 64;

      return boost::math::make_tuple(f, f1, f2);
   }
private:
   T a, b, target;
   bool invert;
};

BOOST_AUTO_TEST_CASE( test_main )
{
   int newton_limits = static_cast<int>(std::numeric_limits<double>::digits * 0.6);

   double arg = 1e-50;
   boost::uintmax_t iters;
   double guess;
   double dr;

   while(arg < 1e50)
   {
      double result = boost::math::cbrt(arg);
      //
      // Start with a really bad guess 5 times below the result:
      //
      guess = result / 5;
      iters = 1000;
      // TOMS algo first:
      std::pair<double, double> r = boost::math::tools::bracket_and_solve_root(cbrt_functor_noderiv(arg), guess, 2.0, true, boost::math::tools::eps_tolerance<double>(), iters);
      BOOST_CHECK_CLOSE_FRACTION((r.first + r.second) / 2, result, std::numeric_limits<double>::epsilon() * 4);
      BOOST_CHECK_LE(iters, 14);
      // Newton next:
      iters = 1000;
      dr = boost::math::tools::newton_raphson_iterate(cbrt_functor_deriv(arg), guess, guess / 2, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 12);
      // Halley next:
      iters = 1000;
      dr = boost::math::tools::halley_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 7);
      // Schroder next:
      iters = 1000;
      dr = boost::math::tools::schroder_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 11);
      //
      // Over again with a bad guess 5 times larger than the result:
      //
      iters = 1000;
      guess = result * 5;
      r = boost::math::tools::bracket_and_solve_root(cbrt_functor_noderiv(arg), guess, 2.0, true, boost::math::tools::eps_tolerance<double>(), iters);
      BOOST_CHECK_CLOSE_FRACTION((r.first + r.second) / 2, result, std::numeric_limits<double>::epsilon() * 4);
      BOOST_CHECK_LE(iters, 14);
      // Newton next:
      iters = 1000;
      dr = boost::math::tools::newton_raphson_iterate(cbrt_functor_deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 12);
      // Halley next:
      iters = 1000;
      dr = boost::math::tools::halley_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 7);
      // Schroder next:
      iters = 1000;
      dr = boost::math::tools::schroder_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 11);
      //
      // A much better guess, 1% below result:
      //
      iters = 1000;
      guess = result * 0.9;
      r = boost::math::tools::bracket_and_solve_root(cbrt_functor_noderiv(arg), guess, 2.0, true, boost::math::tools::eps_tolerance<double>(), iters);
      BOOST_CHECK_CLOSE_FRACTION((r.first + r.second) / 2, result, std::numeric_limits<double>::epsilon() * 4);
      BOOST_CHECK_LE(iters, 12);
      // Newton next:
      iters = 1000;
      dr = boost::math::tools::newton_raphson_iterate(cbrt_functor_deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 5);
      // Halley next:
      iters = 1000;
      dr = boost::math::tools::halley_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 3);
      // Schroder next:
      iters = 1000;
      dr = boost::math::tools::schroder_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 4);
      //
      // A much better guess, 1% above result:
      //
      iters = 1000;
      guess = result * 1.1;
      r = boost::math::tools::bracket_and_solve_root(cbrt_functor_noderiv(arg), guess, 2.0, true, boost::math::tools::eps_tolerance<double>(), iters);
      BOOST_CHECK_CLOSE_FRACTION((r.first + r.second) / 2, result, std::numeric_limits<double>::epsilon() * 4);
      BOOST_CHECK_LE(iters, 12);
      // Newton next:
      iters = 1000;
      dr = boost::math::tools::newton_raphson_iterate(cbrt_functor_deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 5);
      // Halley next:
      iters = 1000;
      dr = boost::math::tools::halley_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 3);
      // Schroder next:
      iters = 1000;
      dr = boost::math::tools::schroder_iterate(cbrt_functor_2deriv(arg), guess, result / 10, result * 10, newton_limits, iters);
      BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 2);
      BOOST_CHECK_LE(iters, 4);

      arg *= 3.5;
   }

   //
   // Test ibeta as this triggers all the pathological cases!
   //
#ifndef SC_
#define SC_(x) x
#endif
#define T double

#  include "ibeta_small_data.ipp"

   for (unsigned i = 0; i < ibeta_small_data.size(); ++i)
   {
      //
      // These inverse tests are thrown off if the output of the
      // incomplete beta is too close to 1: basically there is insuffient
      // information left in the value we're using as input to the inverse
      // to be able to get back to the original value.
      //
      if (ibeta_small_data[i][5] == 0)
      {
         iters = 1000;
         dr = boost::math::tools::newton_raphson_iterate(ibeta_roots_2<double, boost::math::policies::policy<> >(ibeta_small_data[i][0], ibeta_small_data[i][1], ibeta_small_data[i][5]), 0.5, 0.0, 1.0, 53, iters);
         BOOST_CHECK_EQUAL(dr, 0.0);
         BOOST_CHECK_LE(iters, 27);
         iters = 1000;
         dr = boost::math::tools::halley_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(ibeta_small_data[i][0], ibeta_small_data[i][1], ibeta_small_data[i][5]), 0.5, 0.0, 1.0, 53, iters);
         BOOST_CHECK_EQUAL(dr, 0.0);
         BOOST_CHECK_LE(iters, 10);
      }
      else if ((1 - ibeta_small_data[i][5] > 0.001)
         && (fabs(ibeta_small_data[i][5]) > 2 * boost::math::tools::min_value<double>()))
      {
         iters = 1000;
         double result = ibeta_small_data[i][2];
         dr = boost::math::tools::newton_raphson_iterate(ibeta_roots_2<double, boost::math::policies::policy<> >(ibeta_small_data[i][0], ibeta_small_data[i][1], ibeta_small_data[i][5]), 0.5, 0.0, 1.0, 53, iters);
         BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 200);
#if defined(BOOST_MSVC) && (BOOST_MSVC == 1600)
         BOOST_CHECK_LE(iters, 40);
#else
         BOOST_CHECK_LE(iters, 27);
#endif
         iters = 1000;
         result = ibeta_small_data[i][2];
         dr = boost::math::tools::halley_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(ibeta_small_data[i][0], ibeta_small_data[i][1], ibeta_small_data[i][5]), 0.5, 0.0, 1.0, 53, iters);
         BOOST_CHECK_CLOSE_FRACTION(dr, result, std::numeric_limits<double>::epsilon() * 200);
         BOOST_CHECK_LE(iters, 40);
      }
      else if (1 == ibeta_small_data[i][5])
      {
         iters = 1000;
         dr = boost::math::tools::newton_raphson_iterate(ibeta_roots_2<double, boost::math::policies::policy<> >(ibeta_small_data[i][0], ibeta_small_data[i][1], ibeta_small_data[i][5]), 0.5, 0.0, 1.0, 53, iters);
         BOOST_CHECK_EQUAL(dr, 1.0);
         BOOST_CHECK_LE(iters, 27);
         iters = 1000;
         dr = boost::math::tools::halley_iterate(ibeta_roots_3<double, boost::math::policies::policy<> >(ibeta_small_data[i][0], ibeta_small_data[i][1], ibeta_small_data[i][5]), 0.5, 0.0, 1.0, 53, iters);
         BOOST_CHECK_EQUAL(dr, 1.0);
         BOOST_CHECK_LE(iters, 10);
      }
   }

}

#else

int main() { return 0; }

#endif


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/**
 * @file eqtesting.cpp
 * @brief Useful fucntions for equality testing...
 */
#include <NTL/lzz_pXFactoring.h>
#include "timing.h"
#include "EncryptedArray.h"

#include <cstdio>

namespace helib {

// Map all non-zero slots to 1, leaving zero slots as zero.
// Assumes that r=1, and that all the slot contain elements from GF(p^d).
//
// We compute x^{p^d-1} = x^{(1+p+...+p^{d-1})*(p-1)} by setting y=x^{p-1}
// and then outputting y * y^p * ... * y^{p^{d-1}}, with exponentiation to
// powers of p done via Frobenius.

// FIXME: the computation of the "norm" y * y^p * ... * y^{p^{d-1}}
// can be done using O(log d) automorphisms, rather than O(d).

void mapTo01(const EncryptedArray& ea, Ctxt& ctxt)
{
  long p = ctxt.getPtxtSpace();
  if (p != ea.getPAlgebra().getP()) // ptxt space is p^r for r>1
    throw helib::LogicError("mapTo01 not implemented for r>1");

  if (p>2)
    ctxt.power(p-1); // set y = x^{p-1}

  long d = ea.getDegree();
  if (d>1) { // compute the product of the d automorphisms
    std::vector<Ctxt> v(d, ctxt);
    for (long i=1; i<d; i++)
      v[i].frobeniusAutomorph(i);
    totalProduct(ctxt, v);
  }
}

// computes ctxt^{2^d-1} using a method that takes
// O(log d) automorphisms and multiplications
void fastPower(Ctxt& ctxt, long d)
{
  //OLD: assert(ctxt.getPtxtSpace()==2);
  helib::assertEq(ctxt.getPtxtSpace(), 2l, "ptxtSpace must be 2");
  if (d <= 1) return;

  Ctxt orig = ctxt;

  long k = NTL::NumBits(d);
  long e = 1;

  for (long i = k-2; i >= 0; i--) {
    Ctxt tmp1 = ctxt;
    tmp1.smartAutomorph(1L << e);
    ctxt.multiplyBy(tmp1);
    e = 2*e;

    if (NTL::bit(d, i)) {
      ctxt.smartAutomorph(2);
      ctxt.multiplyBy(orig);
      e += 1;
    }
  }
}

// ===> This function only works for p=2, r=1 <===
// Test if prefixes of bits in slots are all zero: Set slot j of res[i] to 0
// if bits 0..i of j'th slot in ctxt are all zero, else it is set to 1
// It is assumed that res and the res[i]'s are initialized by the caller.
// Complexity: O(d + n log d) smart automorphisms
//             O(n d)
void incrementalZeroTest(Ctxt* res[], const EncryptedArray& ea,
			 const Ctxt& ctxt, long n)
{
  FHE_TIMER_START;
  long nslots = ea.size();
  long d = ea.getDegree();

  // compute linearized polynomial coefficients

  std::vector< std::vector<NTL::ZZX> > Coeff;
  Coeff.resize(n);

  for (long i = 0; i < n; i++) {
    // coeffients for mask on bits 0..i
    // L[j] = X^j for j = 0..i, L[j] = 0 for j = i+1..d-1

    std::vector<NTL::ZZX> L;
    L.resize(d);

    for (long j = 0; j <= i; j++)
      SetCoeff(L[j], j);

    std::vector<NTL::ZZX> C;

    ea.buildLinPolyCoeffs(C, L);

    Coeff[i].resize(d);
    for (long j = 0; j < d; j++) {
      // Coeff[i][j] = to the encoding that has C[j] in all slots
      // FIXME: maybe encrtpted array should have this functionality
      //        built in
      std::vector<NTL::ZZX> T;
      T.resize(nslots);
      for (long s = 0; s < nslots; s++) T[s] = C[j];
      ea.encode(Coeff[i][j], T);
    }
  }

  std::vector<Ctxt> Conj(d, ctxt);
  // initialize Cong[j] to ctxt^{2^j}
  for (long j = 0; j < d; j++) {
    Conj[j].smartAutomorph(1L << j);
  }

  for (long i = 0; i < n; i++) {
    res[i]->clear();
    for (long j = 0; j < d; j++) {
      Ctxt tmp = Conj[j];
      tmp.multByConstant(Coeff[i][j]);
      *res[i] += tmp;
    }

    // *res[i] now has 0..i in each slot
    // next, we raise to the power 2^d-1

    fastPower(*res[i], d);
  }
  FHE_TIMER_STOP;
}

}


//#include <opencv/cv.h>
//#include <opencv2/imgproc.hpp>
//#include <opencv2/calib3d.hpp>

#include <Eigen/Core>

#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PointIndices.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <apriltagscpp/TagDetection.h>
#include <apriltagscpp/AprilTypes.h>

#include <geometry_msgs/Pose.h>

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
            tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

/*
void GetMarkerTransformUsingOpenCV(const TagDetection& detection, Eigen::Matrix4d& transform, cv::Mat& rvec, cv::Mat& tvec)
{
    // Check if fx,fy or cx,cy are not set
    if ((camera_info_.K[0] == 0.0) || (camera_info_.K[4] == 0.0) || (camera_info_.K[2] == 0.0) || (camera_info_.K[5] == 0.0))
    {
        ROS_WARN("Warning: Camera intrinsic matrix K is not set, can't recover 3D pose");
    }

    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);

    cv::Vec4f distortion_coeff(camera_info_.D[0], camera_info_.D[1], camera_info_.D[2], camera_info_.D[3]);

    // Estimate 3D pose of tag
    // Methods:
    //   CV_ITERATIVE
    //     Iterative method based on Levenberg-Marquardt optimization.
    //     Finds the pose that minimizes reprojection error, being the sum of squared distances
    //     between the observed projections (image_points) and the projected points (object_pts).
    //   CV_P3P
    //     Based on: Gao et al, "Complete Solution Classification for the Perspective-Three-Point Problem"
    //     Requires exactly four object and image points.
    //   CV_EPNP
    //     Moreno-Noguer, Lepetit & Fua, "EPnP: Efficient Perspective-n-Point Camera Pose Estimation"
    int method = CV_ITERATIVE;
    bool use_extrinsic_guess = false; // only used for ITERATIVE method
    cv::solvePnP(object_pts, image_pts, intrinsics, distortion_coeff, rvec, tvec, use_extrinsic_guess, method);

    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
            r(1,0), r(1,1), r(1,2),
            r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
                     tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    transform = T;
}
*/

class KinectPoseImprovement{
private:
    size_t m_num_samples;//recommended 9
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr m_cloud;
    at::Mat m_tag_space_samples;// 3 by n matrix stores n points in each row

public:
    KinectPoseImprovement(
            size_t num_tag_samples,
            pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input_cloud
    ):
            m_num_samples(num_tag_samples),
            m_cloud(input_cloud)
    {
        gen_tag_samples();
    }

    void localize_2d(TagDetection& detection, geometry_msgs::Pose& out_pose){
        Eigen::Matrix4d pose;
        cv::Mat rvec;
        cv::Mat tvec;
        GetMarkerTransformUsingOpenCV(detection, pose, rvec, tvec);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

        out_pose.position.x = pose(0,3);
        out_pose.position.y = pose(1,3);
        out_pose.position.z = pose(2,3);
        out_pose.orientation.x = q.x();
        out_pose.orientation.y = q.y();
        out_pose.orientation.z = q.z();
        out_pose.orientation.w = q.w();
    }

    int localize(TagDetection& detection, geometry_msgs::Pose& out_pose){
        // sample(segment) a tag corresponding to this detection.
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr tag_sample_cloud;
        int result = sample_cloud(detection, tag_sample_cloud);
        if(result != 0){
            return result;
        }
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr corners_3D;
        result = extract_corners(detection, corners_3D);
        if(result != 0){
            return result;
        }

        //fit plane on sampled cloud
        pcl::PointIndices::Ptr inlier_idxs=boost::make_shared<pcl::PointIndices>();
        pcl::ModelCoefficients coeffs;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr inliers(new pcl::PointCloud<pcl::PointXYZRGB>());

        pcl::SACSegmentation<pcl::PointXYZRGB> seg;
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setDistanceThreshold(0.005);

        seg.setInputCloud(tag_sample_cloud);
        seg.segment(*inlier_idxs, coeffs);

        pcl::ExtractIndices<pcl::PointXYZRGB> extracter;
        extracter.setInputCloud(tag_sample_cloud);
        extracter.setIndices(inlier_idxs);
        extracter.setNegative(false);
        extracter.filter(*inliers);

        //set center as the mean of inliers
        out_pose.position = centroid(*inliers);

        //get orientation
        Eigen::Matrix3d R;
        extractFrame(coeffs, *corners_3D, R);
        Eigen::Quaternion<double> q(R);
        q.normalize();

        out_pose.orientation.x = q.x();
        out_pose.orientation.y = q.y();
        out_pose.orientation.z = q.z();
        out_pose.orientation.w = q.w();
        return 0;
    }

private:
    void gen_tag_samples(){
        m_tag_space_samples = at::Mat::zeros(3, int(m_num_samples*m_num_samples));
        float step = 2.0f/ float(m_num_samples - 1);
        for(size_t y=0; y < m_num_samples; y++){
            for(size_t x=0; x < m_num_samples; x++){
                size_t idx = y*m_num_samples + x;
                m_tag_space_samples[0][idx] = at::real(x) * step - 1.0f;
                m_tag_space_samples[1][idx] = at::real(y) * step - 1.0f;
                m_tag_space_samples[2][idx] = 1.0f;
            }
        }
    }

    int sample_cloud(TagDetection& detection, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& tag_sample_cloud){
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());

        // calcuate points in image coordinate.
        at::Mat img_idx_mat = detection.homography * m_tag_space_samples;
        size_t x = 0; // col id in image(0,0) at center of image(according to pcl_conversions.h)
        size_t y = 0; // row id in image
        size_t count = 0;
        for(size_t i = 0; i < m_num_samples * m_num_samples; i++){
            x = size_t(img_idx_mat[0][i]/img_idx_mat[2][i] + detection.hxy.x);
            y = size_t(img_idx_mat[1][i]/img_idx_mat[2][i] + detection.hxy.y);

            const pcl::PointXYZRGB& pt = (*m_cloud)(x, y);

            //check if pt is NaN
            if (pt_is_nan(pt)){
                //ROS_INFO("    Skipping (%.4f, %.4f, %.4f)", pt.x, pt.y, pt.z);
            }else{
                count++;
                out_cloud->points.push_back(pt);
            }
        }
        if(count < m_num_samples * m_num_samples / 2){
            //more than half is Nan
            return -1;
        }

        tag_sample_cloud = out_cloud;
        return 0;
    }

    int extract_corners(TagDetection& detection, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& corners_3D){
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
        pcl::PointXYZRGB temp_pt;
        int result;
        size_t window_size = 5;
        for(size_t i = 0; i < 4; i++){
            result = find_non_nan(size_t(detection.p[i].x), size_t(detection.p[i].y), window_size, temp_pt);
            if(result != 0){
                return result;
            }

            out_cloud->points.push_back(temp_pt);
        }

        corners_3D = out_cloud;
        return 0;
    }

    bool pt_is_nan(pcl::PointXYZRGB pt){
        return std::isnan(pt.x) || std::isnan(pt.y) || std::isnan(pt.z);
    }

    int find_non_nan(size_t x, size_t y, size_t window_size, pcl::PointXYZRGB& pt){
        pt = (*m_cloud)(x, y);
        if(! pt_is_nan(pt)){
            //original point is not Nan
            return 0;
        }

        //find average in windows_size by window_size reigon

        size_t half_dim = window_size/2;
        if(x < half_dim || x + half_dim + 1 >= (m_cloud->width) || y < half_dim || y + half_dim + 1 >= (m_cloud->height)){
            //window is invalid
            return -1;
        }

        pcl::PointXYZRGB temp_pt;
        float avg_x = 0.0f;
        float avg_y = 0.0f;
        float avg_z = 0.0f;
        size_t count = 0;
        for(size_t temp_y = y - half_dim; temp_y < y + half_dim; temp_y++){
            for(size_t temp_x = x - half_dim; temp_x < x + half_dim; temp_x++){
                temp_pt = (*m_cloud)(temp_x, temp_y);
                if(! pt_is_nan(temp_pt)){
                    avg_x += temp_pt.x;
                    avg_y += temp_pt.y;
                    avg_z += temp_pt.z;
                    count ++;
                }
            }
        }

        if(count == 0){
            //all neighbors are Nan
            return -1;
        }

        float num = (float) count;
        pt.x = avg_x / num;
        pt.y = avg_y / num;
        pt.z = avg_z / num;

        return 0;
    }

    geometry_msgs::Point centroid (const pcl::PointCloud<pcl::PointXYZRGB>& points)
    {
        // find the mean of all point coordinate.
        geometry_msgs::Point sum;
        sum.x = 0;
        sum.y = 0;
        sum.z = 0;
        //for (const Point& p : points)
        for(size_t i=0; i<points.size(); i++)
        {
            sum.x += points[i].x;
            sum.y += points[i].y;
            sum.z += points[i].z;
        }

        geometry_msgs::Point center;
        const size_t n = points.size();
        center.x = sum.x/double(n);
        center.y = sum.y/double(n);
        center.z = sum.z/double(n);
        return center;
    }

    Eigen::Vector3d project(const pcl::PointXYZRGB& p, const double a, const double b,
                         const double c, const double d)
    {
        const double t = a*p.x + b*p.y + c*p.z + d;
        return Eigen::Vector3d(p.x-t*a, p.y-t*b, p.z-t*c);
    }

    int getCoeffs (const pcl::ModelCoefficients& coeffs, double& a, double& b,
                   double& c, double& d)
    {
        if(coeffs.values.size() != 4)
            return -1;
        const double s = coeffs.values[0]*coeffs.values[0] +
                         coeffs.values[1]*coeffs.values[1] + coeffs.values[2]*coeffs.values[2];
        if(fabs(s) < 1e-6)
            return -1;
        a = coeffs.values[0]/s;
        b = coeffs.values[1]/s;
        c = coeffs.values[2]/s;
        d = coeffs.values[3]/s;
        return 0;
    }

    int extractFrame (const pcl::ModelCoefficients& coeffs,
                      pcl::PointCloud<pcl::PointXYZRGB>& corners,
                      Eigen::Matrix3d &retmat){
        double a=0, b=0, c=0, d=0;
        if(getCoeffs(coeffs, a, b, c, d) < 0){
            return -1;
        }

        const Eigen::Vector3d q1 = project(corners.points[0], a, b, c, d);
        const Eigen::Vector3d q2 = project(corners.points[1], a, b, c, d);
        //const Eigen::Vector3d q3 = project(corners.points[0], a, b, c, d);
        //const Eigen::Vector3d q4 = project(corners.points[3], a, b, c, d);

        const Eigen::Vector3d v = (q2-q1).normalized();
        const Eigen::Vector3d n(a, b, c);
        const Eigen::Vector3d w = -v.cross(n);
        Eigen::Matrix3d m;
        m << v[0], v[1], v[2],
             w[0], w[1], w[2],
             n[0], n[1], n[2];

        retmat = m.inverse();

        return 0;
    }
};


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

#include <Eigen/Dense>

//#include <boost/random/uniform_int_distribution.hpp>
//#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/sampler.hpp>

using namespace Eigen;
using std::cout;
using std::endl;

#ifdef WIN32
	using boost::math::lgamma;
#endif

template<typename T>
class Mult : public Distribution<T>
{
public:
  uint32_t K_;
  Matrix<T,Dynamic,1> pdf_;

  /* constructor from pdf */
  Mult(const Matrix<T,Dynamic,1>& pdf, boost::mt19937 *pRndGen);
  /* constructor from indicators - estimates from counts */
  Mult(const VectorXu& z, boost::mt19937 *pRndGen);
  /* copy constructor */
  Mult(const Mult& other);
  virtual ~Mult();

  uint32_t sample();
  void sample(VectorXu& z);

  T logPdf(const Matrix<T,Dynamic,1>& x) const;
  T logPdfOfSS(const Matrix<T,Dynamic,1>& x) const {return logPdf(x);};

  const Matrix<T,Dynamic,1>& pdf() const {return pdf_;};
  void pdf(const Matrix<T,Dynamic,1>& pdf){
    pdf_ = pdf;
  };

  void print() const;

private:
  boost::uniform_01<T> unif_;
};

typedef Mult<float> Multf;
typedef Mult<double> Multd;


/*
 * Copyright (c) 2017, Adrian Michel
 * http://www.amichel.com
 *
 * This software is released under the 3-Clause BSD License
 *
 * The complete terms can be found in the attached LICENSE file
 * or at https://opensource.org/licenses/BSD-3-Clause
 */

#pragma once

#include <boost/math/special_functions/round.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace amichel {
namespace de {

inline double genrand(double min = 0, double max = 1) {
  static boost::random::mt19937 gen;
  boost::random::uniform_real_distribution<> dist(min, max);
  boost::variate_generator<boost::random::mt19937&,
                           boost::random::uniform_real_distribution<double> >
      value(gen, dist);

  return value();
}

inline int genintrand(double min, double max, bool upperexclusive = false) {
  assert(min < max);
  int ret = 0;
  do
    ret = boost::math::round(genrand(min, max));
  while (ret < min || ret > max || upperexclusive && ret == max);
  return ret;
}
}  // namespace de
}  // namespace amichel