/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/qr.hpp
 *
 * \brief The QR matrix decomposition.
 *
 * Given a matrix \f$A\f$, its QR-decomposition is a matrix decomposition of the
 * form:
 * \f[
 *   A=QR
 * \f]
 * where \f$R\f$ is an m-by-n upper trapezoidal (or, when \f$m \ge n\f$,
 * triangular) matrix and \f$Q\f$ is an m-by-m orthogonal (or unitary) matrix,
 * that is one satisfying:
 * \f[
 *   Q^{T}Q=I,
 * \f]
 * where \f$Q^{T}\f$ is the transpose of \f$Q\f$ and \f$I\f$ is the identity
 * matrix.
 *
 * For the special case of \f$m \ge n\f$, the factorization can be rewritten as:
 * \f[
 *  A=\begin{pmatrix}
 *     Q_1 & Q_2
 *     \end{pmatrix}
 *     \begin{pmatrix}
 *     R_1 \\
 *     R_2 \\
 *     \end{pmatrix}
 *   =\begin{pmatrix}
 *     Q_1 & Q_2
 *     \end{pmatrix}
 *     \begin{pmatrix}
 *     R_1 \\
 *     0 \\
 *     \end{pmatrix}
 *   = Q_1 R_1
 * \f]
 * where \f$Q_1\f$ is an m-by-n matrix, \f$Q_2\f$ is an m-by-(m-n) matrix,
 * \f$R_1\f$ is an n-by-n lower triangular matrix, and \f$R_2\f$ is an
 * (m-n)-by-n zero matrix.
 *
 * This matrix decomposition can be used to solve linear systems of equations,
 * especially the ones involved in the linear least squares problem.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_QR_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_QR_HPP

#include <algorithm>
#include <boost/mpl/and.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/numeric/bindings/lapack/computational/geqrf.hpp>
#include <boost/numeric/bindings/lapack/computational/orgqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ormqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ungqr.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/tag.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>
#include <boost/type_traits/is_same.hpp>
#include <complex>
#include <cstddef>
#include <stdint.h>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

/**
 * \brief Common operations for QR decomposition.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
struct qr_decomposition_impl_common;

/**
 * \brief Type-oriented operations for QR decomposition.
 *
 * \tparam IsComplex Logical parameter telling if the we are doing either a real
 *  or a complex QR decomposition.
 *
 * This class makes distinction between the real and the complex case.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <bool IsComplex>
struct qr_decomposition_impl;

struct qr_decomposition_impl_common
{
    /// Performan QR decomposition of the given input matrix \a A
    /// (row-major case).
    template <typename AMatrixT, typename TauVectorT>
        static void decompose(AMatrixT& A, TauVectorT& tau, row_major_tag)
    {
        matrix<typename matrix_traits<AMatrixT>::value_type, column_major> tmp_A(A);

        decompose(tmp_A, tau, column_major_tag());

        A = tmp_A;
    }

    /// Performan QR decomposition of the given input matrix \a A
    /// (column-major case).
    template <typename AMatrixT, typename TauVectorT>
        static void decompose(AMatrixT& A, TauVectorT& tau, column_major_tag)
    {
        typedef typename matrix_traits<AMatrixT>::size_type size_type;

        size_type m = num_rows(A);
        size_type n = num_columns(A);
        size_type k = ::std::min(m,n);

        if (size(tau) != k)
        {
            tau.resize(k, false);
        }

        ::boost::numeric::bindings::lapack::geqrf(A, tau);
    }

    /**
     * \brief Extract the R matrix from a previously computing QR decomposition
     * (row-major case).
     *
     * Let QR be an m-by-n matrix, then the R matrix is built by taking the
     * min(m,n)-by-n upper trapezoidal (triangular, if m >= n) elements of QR.
     */
    template <typename QRMatrixT, typename RMatrixT>
        static void extract_R(QRMatrixT const& QR, RMatrixT& R, bool full, row_major_tag)
    {
        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<RMatrixT>::value_type, column_major> tmp_R(R);

        extract_R(tmp_QR, tmp_R, full, column_major_tag());

        R = tmp_R;
    }

    /**
     * \brief Extract the R matrix from a previously computing QR decomposition
     * (row-major case).
     *
     * Let QR be an m-by-n matrix, then the R matrix is built by taking the
     * min(m,n)-by-n upper trapezoidal (triangular, if m >= n) elements of QR.
     */
    template <typename QRMatrixT, typename RMatrixT>
        static void extract_R(QRMatrixT const& QR, RMatrixT& R, bool full, column_major_tag)
    {
        typedef typename matrix_traits<RMatrixT>::size_type size_type;
        typedef typename matrix_traits<RMatrixT>::value_type value_type;

        size_type m = num_rows(QR);
        size_type n = num_columns(QR);
        size_type nr = full ? m : ::std::min(m,n);

        if (num_rows(R) != nr && num_columns(R) != n)
        {
            R.resize(nr, n, false);
        }

        if (m >= n)
        {
            // The upper triangle of the submatrix QR(1:n,1:n) contains the
            // min(m,n)-by-n upper triangular matrix R.
            for (size_type row = 0; row < n; ++row)
            {
                for (size_type col = 0; col < n; ++col)
                {
                    if (col >= row)
                    {
                        R(row,col) = QR(row,col);
                    }
                    else
                    {
                        R(row,col) = value_type/*zero*/();
                    }
                }
            }

            // Set to zero the last m-n rows
            if (full)
            {
                subrange(R, n, m, 0, n) = scalar_matrix<value_type>(m-n, n, value_type/*zero*/());
            }
        }
        else
        {
            // The elements on and upper the n-th subdiagonal contain the
            // m-by-n upper trapezoidal matrix R.
            for (size_type row = 0; row < m; ++row)
            {
                for (size_type col = 0; col < n; ++col)
                {
                    if (col >= row)
                    {
                        R(row,col) = QR(row,col);
                    }
                    else
                    {
                        R(row,col) = value_type/*zero*/();
                    }
                }
            }
        }
    }

    /**
     * \brief Multiply the given \a C matrix by the \c Q matrix obtained from
     *  the QR decomposition.
     *
     * \tparam QRMatrixT The type of the \a QR matrix.
     * \tparam TAUMatrixT The type of the \a tau vector.
     * \tparam CMatrixT The type of the \a C matrix.
     *
     * \param QR The matrix obtained by the QR decomposition such that the i-th
     *  column contains the vector which defines the elementary reflector
     *  \f$H(i)\f$, for \f$i = 1,2,\ldots,k\f$.
     * \param tau The vector obtained by the QR decomposition containing the
     *  scalar factors of the elementary reflectors \f$H(i)\f$, for
     *  \f$i=1,2,\ldots,k\f$.
     * \param left_Q A boolean value indicating which side of the product the
     *  matrix \c Q will occupy. A \c true value indicates that \c Q is the left
     *  operand, while a \c false value indicates that \c Q is the right
     *  operand.
     * \param trans_Q A boolean value indicating if the matrix \c Q is to be
     *  transposed. A \c true value indicates that \c Q is to be transposed,
     *  while a \c false value indicates that \c Q is to be taken as-is.
     * \param orientation The matrix orientation fixed to row-major.
     *
     * Let \c Q be the matrix obtained from the QR decomposition represented
     * by the \a QR matrix and the \a tau vector parameters.
     * Then this function computes the following matrix product:
     * \f{equation*}{
     *   \begin{cases}
     *   Q C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{false}}, \\
     *   Q^T C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{true}}, \\
     *   C Q, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{false}}, \\
     *   C Q^T, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{true}}.
     *  \end{cases}
     * \f}
     */
    template <typename QRMatrixT, typename TAUVectorT, typename CMatrixT>
        static void prod(QRMatrixT& QR, TAUVectorT const& tau, CMatrixT& C, bool left_Q, bool trans_Q, row_major_tag)
    {
        //NOTE: QR cannot be const since LAPACK::ORMQR modified it, restoring
        //      it at the end of the function.

        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<CMatrixT>::value_type, column_major> tmp_C(C);

        prod(tmp_QR, tau, tmp_C, left_Q, trans_Q, column_major_tag());

        C = tmp_C;
    }

    /**
     * \brief Multiply the given \a C matrix by the \c Q matrix obtained from
     *  the QR decomposition.
     *
     * \tparam QRMatrixT The type of the \a QR matrix.
     * \tparam TAUMatrixT The type of the \a tau vector.
     * \tparam CMatrixT The type of the \a C matrix.
     *
     * \param QR The matrix obtained by the QR decomposition such that the i-th
     *  column contains the vector which defines the elementary reflector
     *  \f$H(i)\f$, for \f$i = 1,2,\ldots,k\f$.
     * \param tau The vector obtained by the QR decomposition containing the
     *  scalar factors of the elementary reflectors \f$H(i)\f$, for
     *  \f$i=1,2,\ldots,k\f$.
     * \param left_Q A boolean value indicating which side of the product the
     *  matrix \c Q will occupy. A \c true value indicates that \c Q is the left
     *  operand, while a \c false value indicates that \c Q is the right
     *  operand.
     * \param trans_Q A boolean value indicating if the matrix \c Q is to be
     *  transposed. A \c true value indicates that \c Q is to be transposed,
     *  while a \c false value indicates that \c Q is to be taken as-is.
     * \param orientation The matrix orientation fixed to column-major.
     *
     * Let \c Q be the matrix obtained from the QR decomposition represented
     * by the \a QR matrix and the \a tau vector parameters.
     * Then this function computes the following matrix product:
     * \f{equation*}{
     *   \begin{cases}
     *   Q C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{false}}, \\
     *   Q^T C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{true}}, \\
     *   C Q, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{false}}, \\
     *   C Q^T, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{true}}.
     *  \end{cases}
     * \f}
     */
    template <typename QRMatrixT, typename TAUVectorT, typename CMatrixT>
        static void prod(QRMatrixT& QR, TAUVectorT const& tau, CMatrixT& C, bool left_Q, bool trans_Q, column_major_tag /*orientation*/)
    {
        //NOTE: QR cannot be const since LAPACK::ORMQR modified it, restoring
        //      it at the end of the function.

//      typedef typename matrix_traits<QRMatrixT>::value_type value_type;
//      typedef typename matrix_traits<QRMatrixT>::size_type size_type;
//      typedef typename type_traits<value_type>::real_type real_type;
//
//      const ::fortran_int_t m = num_rows(C);
//      const ::fortran_int_t n = num_columns(C);
//      const ::fortran_int_t k = size(tau);
//      const ::fortran_int_t lda = num_rows(QR);
//      const ::fortran_int_t ldc = m;
//      real_type* work;
//      real_type opt_work_size;
//      ::fortran_int_t lwork;
//      ::std::ptrdiff_t info;

        if (left_Q)
        {
            if (trans_Q)
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::left(),
                    ::boost::numeric::bindings::trans(QR),
                    tau,
                    C
                );
            }
            else
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::left(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::left(),
                    QR,
                    tau,
                    C
                );
            }
        }
        else
        {
            if (trans_Q)
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::right(),
                    ::boost::numeric::bindings::trans(QR),
                    tau,
                    C
                );
            }
            else
            {
//              //FIXME: actually (2010-08-13) bindinds::lapack::ormqr has problems
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  &opt_work_size,
//                  -1
//              );
//              lwork = static_cast< ::fortran_int_t >(opt_work_size);
//              work = new real_type[lwork];
//              info = ::boost::numeric::bindings::lapack::detail::ormqr(
//                  ::boost::numeric::bindings::tag::right(),
//                  ::boost::numeric::bindings::tag::no_transpose(),
//                  m,
//                  n,
//                  k,
//                  QR.data().begin(),
//                  lda,
//                  tau.data().begin(),
//                  C.data().begin(),
//                  ldc,
//                  work,
//                  lwork
//              );
//              delete[] work;
                ::boost::numeric::bindings::lapack::ormqr(
                    ::boost::numeric::bindings::tag::right(),
                    QR,
                    tau,
                    C
                );
            }
        }
    }
};

template <>
struct qr_decomposition_impl<false>: public qr_decomposition_impl_common
{
    /// Extract the Q matrix from a previously computing QR decomposition
    /// (row-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT const& tau, QMatrixT& Q, bool full, row_major_tag)
    {
        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<QMatrixT>::value_type, column_major> tmp_Q(Q);

        extract_Q(tmp_QR, tau, tmp_Q, full, column_major_tag());

        Q = tmp_Q;
    }

    /// Extract the Q matrix from a previously computing QR decomposition
    /// (column-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT& tau, QMatrixT& Q, bool full, column_major_tag)
    {
        typedef typename matrix_traits<QMatrixT>::size_type size_type;
        typedef typename matrix_traits<QMatrixT>::value_type value_type;

        size_type m = num_rows(QR);
        size_type n = num_columns(QR);
        size_type nc = full ? m : ::std::min(m,n);

        if (num_rows(Q) != m || num_columns(Q) != nc)
        {
            Q.resize(m, nc, false);
        }

        if (m > n)
        {
            if (full)
            {
                subrange(Q, 0, m, 0, n) = QR;
                subrange(Q, 0, m, n, m) = scalar_matrix<value_type>(m, m-n, value_type/*zero*/());
            }
            else
            {
                Q = QR;
            }
        }
        else if (m < n)
        {
            Q = subrange(QR, 0, m, 0, nc);
        }
        else
        {
            Q = QR;
        }

       ::boost::numeric::bindings::lapack::orgqr(Q, tau);
    }
};

template <>
struct qr_decomposition_impl<true>: public qr_decomposition_impl_common
{
    /// Extract the Q matrix from a previously computing QR decomposition
    /// (row-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT const& tau, QMatrixT& Q, bool full, row_major_tag)
    {
        matrix<typename matrix_traits<QRMatrixT>::value_type, column_major> tmp_QR(QR);
        matrix<typename matrix_traits<QMatrixT>::value_type, column_major> tmp_Q(Q);

        extract_Q(tmp_QR, tau, tmp_Q, full, column_major_tag());

        Q = tmp_Q;
    }

    /// Extract the Q matrix from a previously computing QR decomposition
    /// (column-major case).
    template <typename QRMatrixT, typename TauVectorT, typename QMatrixT>
        static void extract_Q(QRMatrixT const& QR, TauVectorT const& tau, QMatrixT& Q, bool full, column_major_tag)
    {
        typedef typename matrix_traits<QMatrixT>::size_type size_type;
        typedef typename matrix_traits<QMatrixT>::value_type value_type;

        size_type m = num_rows(QR);
        size_type n = num_columns(QR);
        size_type nc = full ? m : ::std::min(m,n);

        if (num_rows(Q) != m || num_columns(Q) != nc)
        {
            Q.resize(m, nc, false);
        }

        if (m > n)
        {
            if (full)
            {
                subrange(Q, 0, m, 0, n) = QR;
                subrange(Q, 0, m, n, m) = scalar_matrix<value_type>(m, m-n, value_type/*zero*/());
            }
            else
            {
                Q = QR;
            }
        }
        else if (m < n)
        {
            Q = subrange(QR, 0, m, 0, nc);
        }
        else
        {
            Q = QR;
        }

        ::boost::numeric::bindings::lapack::ungqr(Q, tau);
    }
};

/// Free function performing the QR decomposition of the given matrix expression \a A.
template<typename MatrixExprT, typename QMatrixT, typename RMatrixT, typename OrientationT>
void qr_decompose_impl(matrix_expression<MatrixExprT> const& A, QMatrixT& Q, RMatrixT& R, bool full, OrientationT orientation)
{
    typedef typename matrix_traits<MatrixExprT>::value_type value_type;

    matrix<value_type, typename layout_type<MatrixExprT>::type> tmp_QR(A);
    vector<value_type> tmp_tau;

    qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template decompose(tmp_QR, tmp_tau, orientation);

    qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template extract_Q(tmp_QR, tmp_tau, Q, full, orientation);

    qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template extract_R(tmp_QR, R, full, orientation);
}

} // Namespace detail

/**
 * \brief QR decomposition.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename ValueT>
class qr_decomposition
{
    public: typedef ValueT value_type;
    private: typedef matrix<value_type, column_major> work_matrix_type;
    public: typedef work_matrix_type QR_matrix_type;
    public: typedef work_matrix_type Q_matrix_type;
    public: typedef work_matrix_type R_matrix_type;
    private: typedef vector<value_type> tau_vector_type;

    public: qr_decomposition()
    {
        // empty
    }

    public: template <typename MatrixExprT>
        qr_decomposition(matrix_expression<MatrixExprT> const& A)
        : QR_(A)
    {
        decompose();
    }

    public: template <typename MatrixExprT>
        void decompose(matrix_expression<MatrixExprT> const& A)
    {
        QR_ = A;

        decompose();
    }

    public: Q_matrix_type Q(bool full = true) const
    {
        Q_matrix_type tmp_Q;

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template extract_Q(QR_, tau_, tmp_Q, full, column_major_tag());

        return tmp_Q;
    }

    public: R_matrix_type R(bool full = true) const
    {
        R_matrix_type tmp_R;

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template extract_R(QR_, tmp_R, full, column_major_tag());

        return tmp_R;
    }

    /// Perform the product \f$Q C\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void lprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        lprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$C Q\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void rprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        rprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$Q^T C\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void tlprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        tlprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$C Q^T\f$ and store the result in \a C.
    public: template <typename CMatrixT>
        void trprod_inplace(CMatrixT& C) const
    {
        typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

        trprod_inplace(C, orientation_category());
    }

    /// Perform the product \f$Q C\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type lprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        lprod_inplace(tmp_C);

        return tmp_C;
    }

    /// Perform the product \f$C Q\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type rprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        rprod_inplace(tmp_C);

        return tmp_C;
    }

    /// Perform the product \f$Q^T C\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type tlprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        tlprod_inplace(tmp_C);

        return tmp_C;
    }

    /// Perform the product \f$C Q^T\f$ and return the result.
    public: template <typename CMatrixExprT>
        typename matrix_temporary_traits<CMatrixExprT>::type trprod(matrix_expression<CMatrixExprT> const& C) const
    {
        typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

        trprod_inplace(tmp_C);

        return tmp_C;
    }

    private: void decompose()
    {
        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template decompose(QR_, tau_, column_major_tag());
    }

    /// Perform the product \f$Q C\f$ and store the result in \a C (column-major
    /// case).
    private: template <typename CMatrixT>
        void lprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, C, true, false, column_major_tag());
    }

    /// Perform the product \f$Q C\f$ and store the result in \a C (row-major
    /// case).
    private: template <typename CMatrixT>
        void lprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, true, false, column_major_tag());

        C = tmp_C;
    }

    /// Perform the product \f$C Q\f$ and store the result in \a C (column-major
    /// case).
    private: template <typename CMatrixT>
        void rprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template prod(QR_, tau_, C, false, false, column_major_tag());
    }

    /// Perform the product \f$C Q\f$ and store the result in \a C (row-major
    /// case).
    private: template <typename CMatrixT>
        void rprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, false, false, column_major_tag());

        C = tmp_C;
    }

    /// Perform the product \f$Q^T C\f$ and store the result in \a C
    /// (column-major case).
    private: template <typename CMatrixT>
        void tlprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
            ::boost::is_complex<value_type>::value
        >::template prod(QR_, tau_, C, true, true, column_major_tag());
    }

    /// Perform the product \f$Q^T C\f$ and store the result in \a C
    /// (row-major case).
    private: template <typename CMatrixT>
        void tlprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, true, true, column_major_tag());

        C = tmp_C;
    }

    /// Perform the product \f$C Q^T\f$ and store the result in \a C
    /// (column-major case).
    private: template <typename CMatrixT>
        void trprod_inplace(CMatrixT& C, column_major_tag) const
    {
        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, C, false, true, column_major_tag());
    }

    /// Perform the product \f$C Q^T\f$ and store the result in \a C
    /// (row-major case).
    private: template <typename CMatrixT>
        void trprod_inplace(CMatrixT& C, row_major_tag) const
    {
        work_matrix_type tmp_C(C);

        detail::qr_decomposition_impl<
                ::boost::is_complex<value_type>::value
            >::template prod(QR_, tau_, tmp_C, false, true, column_major_tag());

        C = tmp_C;
    }

    // NOTE: the 'mutable' keyword is needed in order to make 'const' the
    //       '?prod' methods ('lprod', 'tlprod', 'rprod', 'trprod').
    //       Indeed, these methods call the respective '?prod_inplace' methods
    //       which, in turns, call the LAPACK::ORMQR function which temporarily
    //       changes the QR matrix (and restores it before returning).
    private: mutable QR_matrix_type QR_;
    private: tau_vector_type tau_;
};

/// Free function performing the QR decomposition of the given matrix expression \a A.
template<typename MatrixExprT, typename OutMatrix1T, typename OutMatrix2T>
BOOST_UBLAS_INLINE
void qr_decompose(matrix_expression<MatrixExprT> const& A, OutMatrix1T& Q, OutMatrix2T& R, bool full = true)
{
    typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category1;
    typedef typename matrix_traits<OutMatrix1T>::orientation_category orientation_category2;
    typedef typename matrix_traits<OutMatrix2T>::orientation_category orientation_category3;

    // precondition: same orientation category
    BOOST_MPL_ASSERT(
        (::boost::mpl::and_<
            ::boost::is_same<orientation_category1,orientation_category2>,
            ::boost::is_same<orientation_category1,orientation_category3>
        >)
    );

    detail::qr_decompose_impl(A, Q, R, full, orientation_category1());
}

/// Free function performing the QR decomposition of the given matrix expression \a A.
template<typename MatrixExprT>
BOOST_UBLAS_INLINE
qr_decomposition<typename matrix_traits<MatrixExprT>::value_type> qr_decompose(matrix_expression<MatrixExprT> const& A)
{
    typedef typename matrix_traits<MatrixExprT>::value_type value_type;

    return qr_decomposition<value_type>(A);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_QR_HPP


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestRotation3DX
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <cmath>
#include "minimath/rotation3d.hpp"
#include "minimath/point3d.hpp"

#include "Defines.h"
#include "TestRotation3DUtils.h"

using namespace minimath;

BOOST_AUTO_TEST_SUITE(TestRotation3DX)

BOOST_AUTO_TEST_CASE(testInstantiation)
{
  rotation3dx<double> rot1, rot2;
}

BOOST_AUTO_TEST_CASE(testDefaultEquality)
{
  BOOST_CHECK(rotation3dx<double>() == rotation3dx<double>());
}

BOOST_AUTO_TEST_CASE(testCopyConstruction)
{
  rotation3dx<double> rot1;
  rotation3dx<double> rot2(rot1);
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testAssignment)
{
  rotation3dx<double> rot1, rot2;
  rot2 = rot1;
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testRotatePoint0)
{
  rotation3dx<double> rotx4(0);
  pointxyzd pTest = rotx4*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotx4*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rotx4*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45)
{
  rotation3dx<double> rotx6(PI/4.); // 45 degree rotation about X
  pointxyzd pTest = rotx6*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotx6*p010;
  BOOST_CHECK(pTest == pointxyzd(0., cos45, cos45));

  pTest = rotx6*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -cos45, cos45));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90)
{
  rotation3dx<double> rotx4(PI/2.); // 90 degree rotation about X
  pointxyzd pTest = rotx4*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotx4*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));

  pTest = rotx4*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180)
{
  rotation3dx<double> rotx5(PI); // 180 degree rotation about X

  pointxyzd pTest = rotx5*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rotx5*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = rotx5*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testInverse)
{
  BOOST_CHECK(TestUtils::testInverse<rotation3dx<double> >());
}

BOOST_AUTO_TEST_CASE(testInvert)
{
  BOOST_CHECK(TestUtils::testInvert<rotation3dx<double> >());
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef INCLUDE_MIMKL_MODELS_UMKL_KNN_HPP_
#define INCLUDE_MIMKL_MODELS_UMKL_KNN_HPP_

#include <dlib/optimization.h>
#include <mimkl/definitions.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/model.hpp>
#include <queue>
#include <spdlog/spdlog.h>
#include <thread>
#include <type_traits>
#include <utility>

using dlib::mat;
using mimkl::definitions::Index;
using mimkl::kernels_handler::KernelsHandler;
using mimkl::utilities::sort_indices_decending;

namespace mimkl
{
namespace models
{

static std::shared_ptr<spdlog::logger> logger_umkl_knn =
spdlog::stdout_color_mt("UMKLKNN");

template <typename Scalar, typename Kernel>
void find_and_update_neighbours_using_kernels(
KernelsHandler<Scalar, Kernel> &kernels_handler,
const Index number_of_kernels,
const Index number_of_support_vectors,
const Index k,
MATRIX(Scalar) & W)
{
// for each kernel, get k-NN and add it to upper triangular part of W
// TODO: consider allocation of W_local outside of the loop
// currently not in place given SegFault in development
#pragma omp parallel for shared(kernels_handler, W)
    for (Index m = 0; m < number_of_kernels; ++m)
    {
        // set W_local copy
        MATRIX(Scalar)
        W_local = MATRIX(Scalar)::Zero(number_of_support_vectors,
                                       number_of_support_vectors);
        // TODO: think about how to avoid this copy
        MATRIX(Scalar) kernel = kernels_handler[m];
        for (Index c = 0; c < number_of_support_vectors; ++c)
        {
            // no self loops admitted in W we have to treat the handle the case
            // i==c see original implementation
            // https://github.com/cran/mixKernel/blob/master/R/combine.kernels.R
            // starting by filling a neighbors heap
            std::priority_queue<std::pair<Scalar, Index>,
                                std::vector<std::pair<Scalar, Index>>,
                                std::greater<std::pair<Scalar, Index>>>
            nn_heap;
            Index i = 0;
            // initialize initial k elements
            while (nn_heap.size() < k)
            {
                if (i != c)
                {
                    nn_heap.push(std::pair<Scalar, Index>(kernel(i, c), i));
                }
                // keep track of the index
                ++i;
            }
            // compute k nearest neighbors for c
            for (; i < number_of_support_vectors; ++i)
            {
                if (i != c && nn_heap.top().first < kernel(i, c))
                {
                    nn_heap.pop();
                    nn_heap.push(std::pair<Scalar, Index>(kernel(i, c), i));
                }
            }
            // consuming the neighbors heap and populating the knn graph
            while (!nn_heap.empty())
            {
                Index neighbor = nn_heap.top().second;
                neighbor > c ? W_local(c, neighbor) = 1 :
                               W_local(neighbor, c) = 1;
                nn_heap.pop();
            }
        }
// update W with W_local
#pragma omp critical
        {
            W.template triangularView<Eigen::Upper>() += W_local;
        }
    }
}

//! Unsupervised Multiple Kernel Learning (2017), notion of original topology
//! from k-NN of kernels
/*! (sparse version from paper)
 *
 */
template <typename Scalar, typename Kernel>
class UMKLKNN : public Model<Scalar, Kernel>
{

    private:
    // inheritance of templatized base members
    using Model<Scalar, Kernel>::_kernels_handler;
    using Model<Scalar, Kernel>::_trained;
    using Model<Scalar, Kernel>::_precompute;
    using Model<Scalar, Kernel>::_trace_normalization;
    using Model<Scalar, Kernel>::_number_of_support_vectors;
    using Model<Scalar, Kernel>::_number_of_kernels;

    std::shared_ptr<spdlog::logger> _logger = spdlog::get("UMKLKNN");

    typedef MATRIX(Scalar) Matrix;
    typedef COLUMN(Scalar) Column;
    typedef Eigen::Map<const Column> MapColumn;
    typedef dlib::matrix<Scalar, 0, 1, dlib::default_memory_manager, dlib::column_major_layout>
    DlibColumn;

    Index _k = 5; // for k nearest neighbors (k-NN)
    double _epsilon = 0.0001;
    Index _maxiter_qp = 100000;

    Matrix _W;
    Matrix _S;

    DlibColumn _beta_dlib;
    Column _beta;

    void compute_w_matrix();
    void compute_s_matrix();
    void optimize_beta();
    void fit();

    public:
    UMKLKNN(const std::vector<Kernel> & = std::vector<Kernel>(),
            const bool precompute = true,
            const bool trace_normalization = true,
            const Index k = 5,
            const double epsilon = 0.0001,
            const Index maxiter_qp = 100000);
    UMKLKNN(const std::vector<Matrix> & = std::vector<Matrix>(),
            const bool precompute = true,
            const bool trace_normalization = true,
            const Index k = 5,
            const double epsilon = 0.0001,
            const Index maxiter_qp = 100000);

    void fit(const Matrix &);
    void fit(const std::vector<Matrix> &);
    Matrix predict(const Matrix &);
    Matrix predict(const std::vector<Matrix> &);

    Index get_k() const;
    double get_epsilon() const;
    Index get_maxiter_qp() const;
    Column get_beta() const;
    Matrix get_optimal_kernel();

    void set_k(const Index);
    void set_epsilon(const double);
    void set_maxiter_qp(const Index);
    void set_parameters(const Index,
                        const double epsilon = 0.0001,
                        const Index maxiter_qp = 100000);
    void set_beta(const Column &);
};

template <typename Scalar, typename Kernel>
UMKLKNN<Scalar, Kernel>::UMKLKNN(const std::vector<Kernel> &kernel_functions,
                                 const bool precompute,
                                 const bool trace_normalization,
                                 const Index k,
                                 const double epsilon,
                                 const Index maxiter_qp)
: Model<Scalar, Kernel>(kernel_functions, precompute, trace_normalization)
{
    _beta = Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
    _beta_dlib = mat(_beta);
    set_parameters(k, epsilon, maxiter_qp);
}

template <typename Scalar, typename Kernel>
UMKLKNN<Scalar, Kernel>::UMKLKNN(const std::vector<Matrix> &kernel_matrices,
                                 const bool precompute,
                                 const bool trace_normalization,
                                 const Index k,
                                 const double epsilon,
                                 const Index maxiter_qp)
: Model<Scalar, Kernel>(kernel_matrices, precompute, trace_normalization)
{
    _beta = Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
    _beta_dlib = mat(_beta);
    set_parameters(k, epsilon, maxiter_qp);
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::fit()
{
    _number_of_support_vectors = _kernels_handler.get_lhs_size();
    _number_of_kernels = _kernels_handler.get_number_of_kernels();
    compute_w_matrix();
    compute_s_matrix();
    optimize_beta();
    _trained = true;
    _logger->debug("all done");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::fit(const std::vector<Matrix> &kernel_matrices)
{
    _kernels_handler.set_matrices(kernel_matrices, true);
    fit();
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::fit(const Matrix &X)
{
    _kernels_handler.set_lhs(X);
    fit();
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Matrix
UMKLKNN<Scalar, Kernel>::predict(const Matrix &X)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    _kernels_handler.set_rhs(X);
    return get_optimal_kernel();
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Matrix
UMKLKNN<Scalar, Kernel>::predict(const std::vector<Matrix> &kernel_matrices)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    if (kernel_matrices[0].rows() != _number_of_support_vectors)
        throw std::length_error("Similarities must be provided for all support "
                                "vectors; matrices have wrong number of rows.");
    if (kernel_matrices.size() != _number_of_kernels)
        throw std::length_error(
        "Same number of kernels as on training is required");
    _kernels_handler.set_matrices(kernel_matrices);
    return get_optimal_kernel();
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::compute_w_matrix()
{
    // ensure a clean status
    _W.resize(_number_of_support_vectors, _number_of_support_vectors);
    _W.setZero();

    // find k-nearest neighbors filling upper part of _W
    find_and_update_neighbours_using_kernels(_kernels_handler, _number_of_kernels,
                                             _number_of_support_vectors, _k, _W);

    // symmetrize _W
    _W.template triangularView<Eigen::Lower>() = _W.transpose();
    // check if we handled properly the self loops
    if (_W.diagonal().sum() > 0.0)
    {
        _logger->critical("W diagonal contains non zero elements");
    }
    _logger->trace("Matrix W:\n{}", _W);
    _logger->debug("W done");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::compute_s_matrix()
{
    _S.resize(_number_of_kernels, _number_of_kernels);
    _S.setZero();
    Matrix K_st(_number_of_support_vectors, _number_of_support_vectors);
    for (Index s = 0; s < _number_of_kernels; ++s)
    {
        for (Index t = s; t < _number_of_kernels; ++t)
        { // compute
            K_st = _kernels_handler[s] * _kernels_handler[t];
            _S(s, t) =
            ((_W * K_st.diagonal()).sum() - _W.cwiseProduct(K_st).sum());
            if (t > s)
            {
                _S(t, s) = _S(s, t);
            }
        }
    }
    _logger->trace("Matrix S:\n{}", _S);
    _logger->debug("S done");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::optimize_beta()
{
    _beta = Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
    _beta_dlib = mat(_beta);
    Index iterations = dlib::solve_qp_using_smo(
    mat(_S), dlib::zeros_matrix<Scalar>(0, _number_of_kernels), _beta_dlib,
    _epsilon, _maxiter_qp); // 2*mat(_S) would be correct, but not needed
    if (iterations > _maxiter_qp)
    {
        _logger->critical("the qp-solver did not finish in {} iterations",
                          _maxiter_qp);
    }
    _logger->debug("{} qp iterations", iterations);
    _beta = mimkl::linear_algebra::dlib_to_eigen(_beta_dlib);
}

template <typename Scalar, typename Kernel>
Index UMKLKNN<Scalar, Kernel>::get_k() const
{
    return _k;
}

template <typename Scalar, typename Kernel>
double UMKLKNN<Scalar, Kernel>::get_epsilon() const
{
    return _epsilon;
}

template <typename Scalar, typename Kernel>
Index UMKLKNN<Scalar, Kernel>::get_maxiter_qp() const
{
    return _maxiter_qp;
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Column UMKLKNN<Scalar, Kernel>::get_beta() const
{
    _logger->debug(
    "The model has not been fit. Maybe the parameters were changed?");
    return _beta;
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Matrix
UMKLKNN<Scalar, Kernel>::get_optimal_kernel()
{
    return _kernels_handler.sum(_beta);
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_k(const Index k)
{
    _k = k;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_epsilon(const double epsilon)
{
    _epsilon = epsilon;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_maxiter_qp(const Index maxiter_qp)
{
    _maxiter_qp = maxiter_qp;
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_parameters(const Index k,
                                             const double epsilon,
                                             const Index maxiter_qp)
{
    _k = k;
    _epsilon = epsilon;
    _maxiter_qp = maxiter_qp;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
    _logger->debug("k: {}", _k);
    _logger->debug("epsilon: {}", _epsilon);
    _logger->debug("maxiter_qp: {}", _maxiter_qp);
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_beta(const Column &beta)
{
    if (beta.rows() != _number_of_kernels)
        throw std::length_error(
        "passed beta does not have one weight for each kernel");
    _beta = beta;
    _beta_dlib = mat(_beta);
}

} // namespace models
} // namespace mimkl

#endif /* INCLUDE_MIMKL_MODELS_UMKL_KNN_HPP_ */


/* sse_expf_test.cc
   Jeremy Barnes, 18 January 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.
   This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

   Test for the SSE2 expf function.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include "mldb/arch/sse2_exp.h"
#include "mldb/arch/sse2_log.h"
#include "mldb/arch/demangle.h"
#include "mldb/utils/vector_utils.h"

#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <vector>
#include <set>
#include <iostream>
#include <cmath>
#include "mldb/arch/tick_counter.h"
#include "mldb/utils/string_functions.h"
#include <sys/time.h>
#include "mldb/utils/floating_point.h"

using namespace std;
using namespace MLDB;
using namespace MLDB::SIMD;

using boost::unit_test::test_suite;

float extract_scalar(v4sf i)
{
    float vals[4];
    *((v4sf *)vals) = i;
    if (std::isnan(vals[0])) {
        BOOST_CHECK(std::isnan(vals[1]));
        BOOST_CHECK(std::isnan(vals[2]));
        BOOST_CHECK(std::isnan(vals[3]));
    }
    else {
        BOOST_CHECK_EQUAL(vals[0], vals[1]);
        BOOST_CHECK_EQUAL(vals[0], vals[2]);
        BOOST_CHECK_EQUAL(vals[0], vals[3]);
    }
    return vals[0];
}

int extract_scalar(v4si i)
{
    int vals[4];
    *((v4si *)vals) = i;
    BOOST_CHECK_EQUAL(vals[0], vals[1]);
    BOOST_CHECK_EQUAL(vals[0], vals[2]);
    BOOST_CHECK_EQUAL(vals[0], vals[3]);
    return vals[0];
}

double extract_scalar(v2df i)
{
    double vals[2];
    *((v2df *)vals) = i;
    if (std::isnan(vals[0])) {
        BOOST_CHECK(std::isnan(vals[1]));
    }
    else {
        BOOST_CHECK_EQUAL(vals[0], vals[1]);
    }
    return vals[0];
}

BOOST_AUTO_TEST_CASE( ldexp_test )
{
    BOOST_CHECK_EQUAL(ldexp(1.0, 0), extract_scalar(ldexp(vec_splat(1.0), vec_splat(0))));
    BOOST_CHECK_EQUAL(ldexp(1.0, 1), extract_scalar(ldexp(vec_splat(1.0), vec_splat(1))));
}

void test_functions(double val)
{
    cerr << format("%5.2f  %5.2f  %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f\n",
                   val,
                   floor(val + 0.5),
                   round(val),
                   trunc(val),
                   floor(val),
                   ceil(val),
                   rint(val),
                   double(int(val)));
}

BOOST_AUTO_TEST_CASE( value_test )
{
    cerr << "  val   func  round trunc floor  ceil  rint  conv" << endl;
    test_functions(0.0);
    cerr << endl;
    test_functions(0.1);
    test_functions(0.49);
    test_functions(0.50);
    test_functions(0.51);
    test_functions(0.99);
    test_functions(1.00);
    test_functions(1.01);
    test_functions(1.50);
    test_functions(1.51);
    cerr << endl;
    test_functions(-0.1);
    test_functions(-0.49);
    test_functions(-0.50);
    test_functions(-0.51);
    test_functions(-0.99);
    test_functions(-1.00);
    test_functions(-1.01);
    test_functions(-1.50);
    test_functions(-1.51);
    cerr << endl;
}

#define test_floor_value(input) \
{ \
    float in2 = float(input); \
    float output1 = floorf(in2); \
    float output2 = extract_scalar(sse2_floor(vec_splat(in2))); \
    if (std::isnan(output1)) \
        BOOST_CHECK(std::isnan(output2));            \
    else BOOST_CHECK_EQUAL(floorf(float(input)), output2);      \
} \
{ \
    double in2 = double(input); \
    double output1 = floorf(in2); \
    double output2 = extract_scalar(sse2_floor(vec_splat(in2))); \
    if (std::isnan(output1)) \
        BOOST_CHECK(std::isnan(output2));            \
    else BOOST_CHECK_EQUAL(floorf(double(input)), output2);      \
}

static const float NaN = std::numeric_limits<float>::quiet_NaN();

BOOST_AUTO_TEST_CASE( floor_test )
{
    test_floor_value(0.0);
    test_floor_value(-0.0);
    test_floor_value(1.0);
    test_floor_value(2.0);
    test_floor_value(2.5);
    test_floor_value(-0.49);
    test_floor_value(-0.5);
    test_floor_value(-0.51);
    test_floor_value(-0.99);
    test_floor_value(-1.0);
    test_floor_value(-1.01);
    test_floor_value(2.5);
    test_floor_value(NaN);
    test_floor_value(-NaN);
    test_floor_value(INFINITY);
    test_floor_value(-INFINITY);
}

#define test_trunc_value(input) \
{ \
    float in2 = float(input); \
    float output1 = truncf(in2); \
    float output2 = extract_scalar(sse2_trunc(vec_splat(in2))); \
    if (std::isnan(output1)) \
        BOOST_CHECK(std::isnan(output2));            \
    else BOOST_CHECK_EQUAL(truncf(float(input)), output2);      \
}

BOOST_AUTO_TEST_CASE( trunc_test )
{
    test_trunc_value(0.0);
    test_trunc_value(-0.0);
    test_trunc_value(1.0);
    test_trunc_value(2.0);
    test_trunc_value(2.5);
    test_trunc_value(-0.49);
    test_trunc_value(-0.5);
    test_trunc_value(-0.51);
    test_trunc_value(-0.99);
    test_trunc_value(-1.0);
    test_trunc_value(-1.01);
    test_trunc_value(2.5);
    test_trunc_value(NaN);
    test_trunc_value(-NaN);
    test_trunc_value(INFINITY);
    test_trunc_value(-INFINITY);
}

#define test_expf_value(input)                                          \
    {                                                                   \
        float in2 = float(input);                                       \
        float output1 = expf(in2);                                      \
        float output2 = extract_scalar(sse2_expf(vec_splat(in2)));      \
        if (std::isnan(output1)) {                                           \
            if (!std::isnan(output2)) {                                      \
                cerr << "input = " << in2 << " output1 = " << output1 << " output2 = " << output2 \
                 << endl;                                               \
            }                                                           \
            BOOST_CHECK(std::isnan(output2));                                \
        }                                                               \
        else if (output1 != output2) {                                  \
            int i1 = reinterpret_as_int(output1);                       \
            int i2 = reinterpret_as_int(output2);                       \
            if (abs(i1 - i2) > 1) {                                     \
                cerr << format("%12.8f: %14.9f != %14.9f: %08x != %08x (%4d ulps)\n", \
                               in2, output1, output2, i1, i2, (i1 - i2)); \
                BOOST_CHECK_EQUAL(expf(float(input)), output2);         \
            }                                                           \
        }                                                               \
    }

BOOST_AUTO_TEST_CASE( test_expf )
{
    test_expf_value(0.0);
    test_expf_value(1.0);
    test_expf_value(2.0);
    test_expf_value(3.0);
    test_expf_value(-0.0);
    test_expf_value(-1.0);
    test_expf_value(-2.0);
    test_expf_value(-3.0);

    test_expf_value(-10.0);
    test_expf_value(-20.0);
    test_expf_value(-30.0);
    test_expf_value(-50.0);
    test_expf_value(-100.0);
    test_expf_value(-1000.0);

    test_expf_value(10.0);
    test_expf_value(20.0);
    test_expf_value(30.0);
    test_expf_value(50.0);
    test_expf_value(100.0);
    test_expf_value(1000.0);

    test_expf_value(NaN);
    test_expf_value(-NaN);
    test_expf_value(INFINITY);
    test_expf_value(-INFINITY);

    int nvals = 65536;
    for (int i = 0;  i < nvals;  ++i) {
        float f = 105.0 * (2 * i - nvals) / (1.0 * nvals);
        test_expf_value(f);
    }
}

#define test_exp_value(input)                                          \
    {                                                                   \
        double in2 = double(input);                                       \
        double output1 = exp(in2);                                      \
        double output2 = extract_scalar(sse2_exp(vec_splat(in2)));      \
        if (std::isnan(output1)) {                                           \
            if (!std::isnan(output2)) {                                      \
                cerr << "input = " << in2 << " output1 = " << output1 << " output2 = " << output2 \
                 << endl;                                               \
            }                                                           \
            BOOST_CHECK(std::isnan(output2));                                \
        }                                                               \
        else if (output1 != output2) {                                  \
            int i1 = reinterpret_as_int(output1);                       \
            int i2 = reinterpret_as_int(output2);                       \
            /* Allow 2 ulps as there are a very few values that */      \
            /* have this error */                                       \
            if (abs(i1 - i2) > 2) {                                     \
                cerr << format("%12.8f: %14.9f != %14.9f: %08x != %08x (%4d ulps)\n", \
                               in2, output1, output2, i1, i2, (i1 - i2)); \
                BOOST_CHECK_EQUAL(exp(double(input)), output2);         \
            }                                                           \
        }                                                               \
    }

BOOST_AUTO_TEST_CASE( test_exp )
{
    test_exp_value(0.0);
    test_exp_value(1.0);
    test_exp_value(2.0);
    test_exp_value(3.0);
    test_exp_value(-0.0);
    test_exp_value(-1.0);
    test_exp_value(-2.0);
    test_exp_value(-3.0);

    test_exp_value(-10.0);
    test_exp_value(-20.0);
    test_exp_value(-30.0);
    test_exp_value(-50.0);
    test_exp_value(-100.0);
    test_exp_value(-1000.0);

    test_exp_value(10.0);
    test_exp_value(20.0);
    test_exp_value(30.0);
    test_exp_value(50.0);
    test_exp_value(100.0);
    test_exp_value(1000.0);

    test_exp_value(NaN);
    test_exp_value(-NaN);
    test_exp_value(INFINITY);
    test_exp_value(-INFINITY);

    // Test over the whole range
    int nvals = 65536;
    for (int i = 0;  i < nvals;  ++i) {
        double f = 1000.0 * (2 * i - nvals) / (1.0 * nvals);
        test_exp_value(f);
    }

    // Test more closely over (-10, 10)
    for (int i = 0;  i < nvals;  ++i) {
        double f = 10.0 * (2 * i - nvals) / (1.0 * nvals);
        test_exp_value(f);
    }
}

inline double pow2(int input)
{
    return pow(2.0, double(input));
}

#define test_pow2_value(input)                                         \
    {                                                                   \
        double output1 = pow2(input);                                      \
        double output2 = extract_scalar(sse2_pow2(vec_splat(input)));      \
        if (std::isnan(output1)) {                                           \
            if (!std::isnan(output2)) {                                      \
                cerr << "input = " << input << " output1 = " << output1 << " output2 = " << output2 \
                 << endl;                                               \
            }                                                           \
            BOOST_CHECK(std::isnan(output2));                                \
        }                                                               \
        else if (output1 != output2) {                                  \
            int i1 = reinterpret_as_int(output1);                       \
            int i2 = reinterpret_as_int(output2);                       \
            if (abs(i1 - i2) > 1) {                                     \
                cerr << format("%12.8f: %14.9f != %14.9f: %08x != %08x (%4d ulps)\n", \
                               (double)input, output1, output2, i1, i2, (i1 - i2)); \
                BOOST_CHECK_EQUAL(pow2(double(input)), output2);         \
            }                                                           \
        }                                                               \
    }

BOOST_AUTO_TEST_CASE( test_pow2 )
{
    test_pow2_value(0);
    test_pow2_value(1);
    test_pow2_value(2);
    test_pow2_value(3);
    test_pow2_value(-0);
    test_pow2_value(-1);
    test_pow2_value(-2);
    test_pow2_value(-3);

    test_pow2_value(-10);
    test_pow2_value(-20);
    test_pow2_value(-30);
    test_pow2_value(-50);
    test_pow2_value(-100);
    test_pow2_value(-1000);

    test_pow2_value(10);
    test_pow2_value(20);
    test_pow2_value(30);
    test_pow2_value(50);
    test_pow2_value(100);
    test_pow2_value(1000);

#if 0
    test_pow2_value(NaN);
    test_pow2_value(-NaN);
    test_pow2_value(INFINITY);
    test_pow2_value(-INFINITY);

    // Test over the whole range
    int nvals = 65536;
    for (int i = 0;  i < nvals;  ++i) {
        double f = 1000.0 * (2 * i - nvals) / (1.0 * nvals);
        test_pow2_value(f);
    }

    // Test more closely over (-10, 10)
    for (int i = 0;  i < nvals;  ++i) {
        double f = 10.0 * (2 * i - nvals) / (1.0 * nvals);
        test_pow2_value(f);
    }
#endif
}

namespace {

// TODO: use clock_gettime
double elapsed_since(const timeval & tv_start)
{
    struct timeval tv_end;
    gettimeofday(&tv_end, 0);

    double start_sec = tv_start.tv_sec + (tv_start.tv_usec / 1000000.0);
    double end_sec = tv_end.tv_sec + (tv_end.tv_usec / 1000000.0);

    return (end_sec - start_sec);
}

} // file scope

void warm_up_cpu(double seconds = 1.0)
{
    // One second of activity to push up the CPU speed if boost is enabled
    struct timeval tv;
    gettimeofday(&tv, 0);

    while (elapsed_since(tv) < seconds);

}

double builtin_expf_array(float * vals, int nvals)
{
    sched_yield();
    size_t before = ticks();

    for (unsigned i = 0;  i < nvals;  ++i)
        vals[i] = expf(vals[i]);

    size_t after = ticks();

    return (after - before);
}

double builtin_exp_array(float * vals, int nvals)
{
    sched_yield();
    size_t before = ticks();

    for (unsigned i = 0;  i < nvals;  ++i)
        vals[i] = exp(double(vals[i]));

    size_t after = ticks();

    return (after - before);
}

double sse2_expf_array(float * vals, int nvals)
{
    sched_yield();
    size_t before = ticks();

    int nvecs = nvals / 4;
    v4sf * vvals = (v4sf *)vals;

    for (unsigned i = 0;  i < nvecs;  ++i)
        vvals[i] = sse2_expf(vvals[i]);

    size_t after = ticks();
    return (after - before);
}

double sse2_exp_array(float * vals_, int nvals)
{
    double vals[nvals];
    std::copy(vals_, vals_ + nvals, vals);

    sched_yield();
    size_t before = ticks();

    int nvecs = nvals / 2;
    v2df * vvals = (v2df *)vals;

    for (unsigned i = 0;  i < nvecs;  ++i)
        vvals[i] = sse2_exp(vvals[i]);

    size_t after = ticks();
    return (after - before);
}

typedef double (*profile_function) (float *, int);

void profile_expf(int nvals, const std::string & desc,
                  profile_function function)
{
    double overhead = calc_ticks_overhead();
    double tps = calc_ticks_per_second();

    // First, warm it up
    float vals[nvals];
    for (int i = 0;  i < nvals;  ++i)
        vals[i] = 10.0 * (2 * i - nvals) / (1.0 * nvals);

    function(vals, nvals);

    vector<double> timings(20);
    for (unsigned trial = 0;  trial < 20;  ++trial) {
        for (int i = 0;  i < nvals;  ++i)
            vals[i] = 10.0 * (2 * i - nvals) / (1.0 * nvals);
        timings[trial] = function(vals, nvals);
    }

    std::sort(timings.begin(), timings.end());

    //cerr << "timings = " << timings << endl;

    cerr << format("%-30s %8.2f %10.1f %8.5f\n", desc.c_str(),
                   (timings[10] - overhead) / (1.0 * nvals),
                   tps, overhead);
}

BOOST_AUTO_TEST_CASE( profile_expf_test )
{
    cerr << endl;
    cerr << "profiling expf" << endl;

    static const int NVALS = 131072;

    warm_up_cpu();

    profile_expf(NVALS, "builtin", &builtin_expf_array);
    profile_expf(NVALS, "builtin double", &builtin_exp_array);
    profile_expf(NVALS, "sse2 discrete", &sse2_expf_array);
    profile_expf(NVALS, "sse2 discrete double", &sse2_exp_array);
}

#define test_frexpf_value(input)                                          \
    {                                                                   \
        float in2 = float(input);                                       \
        int exp1 = 0, exp2 = 0;                                         \
        float output1 = frexpf(in2, &exp1);                              \
        v4si eexp2;                                                     \
        float output2 = extract_scalar(sse2_frexpf(vec_splat(in2), eexp2)); \
        exp2 = extract_scalar(eexp2);                                   \
        if (std::isnan(output1)) {                                           \
            if (!std::isnan(output2)) {                                      \
                cerr << "input = " << in2 << " output1 = " << output1 << " output2 = " << output2 \
                 << endl;                                               \
            }                                                           \
            BOOST_CHECK(std::isnan(output2));                                \
        }                                                               \
        else if (output1 != output2) {                                  \
            int i1 = reinterpret_as_int(output1);                       \
            int i2 = reinterpret_as_int(output2);                       \
            if (abs(i1 - i2) > 1) {                                     \
                cerr << format("%12.8f: %14.9f != %14.9f: %08x != %08x (%4d ulps)\n", \
                               in2, output1, output2, i1, i2, (i1 - i2)); \
                BOOST_CHECK_EQUAL(frexpf(float(input), &exp1), output2); \
            }                                                           \
        }                                                               \
        if (exp1 != exp2 && isfinite(input)) {                          \
            BOOST_CHECK_EQUAL(make_pair(input, exp1).second, exp2);     \
        }                                                               \
    }

BOOST_AUTO_TEST_CASE( test_frexpf )
{
    test_frexpf_value(0.0);
    test_frexpf_value(1.0);
    test_frexpf_value(2.0);
    test_frexpf_value(3.0);
    test_frexpf_value(-0.0);
    test_frexpf_value(-1.0);
    test_frexpf_value(-2.0);
    test_frexpf_value(-3.0);

    test_frexpf_value(-10.0);
    test_frexpf_value(-20.0);
    test_frexpf_value(-30.0);
    test_frexpf_value(-50.0);
    test_frexpf_value(-100.0);
    test_frexpf_value(-1000.0);

    test_frexpf_value(10.0);
    test_frexpf_value(20.0);
    test_frexpf_value(30.0);
    test_frexpf_value(50.0);
    test_frexpf_value(100.0);
    test_frexpf_value(1000.0);

    test_frexpf_value(-0.0);

    test_frexpf_value(NaN);
    test_frexpf_value(-NaN);
    test_frexpf_value(INFINITY);
    test_frexpf_value(-INFINITY);

#if 0
    int nvals = 65536;
    for (int i = 0;  i < nvals;  ++i) {
        float f = 105.0 * (2 * i - nvals) / (1.0 * nvals);
        test_frexp_value(f);
    }
#endif
}

#define test_logf_value(input)                                          \
    {                                                                   \
        float in2 = float(input);                                       \
        float output1 = logf(in2);                                      \
        float output2 = extract_scalar(sse2_logf(vec_splat(in2)));      \
        if (std::isnan(output1)) {                                           \
            if (!std::isnan(output2)) {                                      \
                cerr << "input = " << in2 << " output1 = " << output1 << " output2 = " << output2 \
                 << endl;                                               \
            }                                                           \
            BOOST_CHECK(std::isnan(output2));                                \
        }                                                               \
        else if (output1 != output2) {                                  \
            int i1 = reinterpret_as_int(output1);                       \
            int i2 = reinterpret_as_int(output2);                       \
            if (abs(i1 - i2) > 1) {                                     \
                cerr << format("%12.8f: %14.9f != %14.9f: %08x != %08x (%4d ulps)\n", \
                               in2, output1, output2, i1, i2, (i1 - i2)); \
                BOOST_CHECK_EQUAL(logf(float(input)), output2);         \
            }                                                           \
        }                                                               \
    }

BOOST_AUTO_TEST_CASE( test_logf )
{
    test_logf_value(0.0);
    test_logf_value(1.0);
    test_logf_value(2.0);
    test_logf_value(3.0);
    test_logf_value(-0.0);
    test_logf_value(-1.0);
    test_logf_value(-2.0);
    test_logf_value(-3.0);

    test_logf_value(-10.0);
    test_logf_value(-20.0);
    test_logf_value(-30.0);
    test_logf_value(-50.0);
    test_logf_value(-100.0);
    test_logf_value(-1000.0);

    test_logf_value(10.0);
    test_logf_value(20.0);
    test_logf_value(30.0);
    test_logf_value(50.0);
    test_logf_value(100.0);
    test_logf_value(1000.0);

    test_logf_value(-0.0);
    test_logf_value(NaN);
    test_logf_value(-NaN);
    test_logf_value(INFINITY);
    test_logf_value(-INFINITY);

    int nvals = 65536;
    for (int i = 0;  i < nvals;  ++i) {
        float f = 105.0 * (2 * i - nvals) / (1.0 * nvals);
        test_logf_value(f);
    }
}

double builtin_logf_array(float * vals, int nvals)
{
    sched_yield();
    size_t before = ticks();

    for (unsigned i = 0;  i < nvals;  ++i)
        vals[i] = logf(vals[i]);

    size_t after = ticks();

    return (after - before);
}

double builtin_log_array(float * vals, int nvals)
{
    sched_yield();
    size_t before = ticks();

    for (unsigned i = 0;  i < nvals;  ++i)
        vals[i] = log(double(vals[i]));

    size_t after = ticks();

    return (after - before);
}

double sse2_logf_array(float * vals, int nvals)
{
    sched_yield();
    size_t before = ticks();

    int nvecs = nvals / 4;
    v4sf * vvals = (v4sf *)vals;

    for (unsigned i = 0;  i < nvecs;  ++i)
        vvals[i] = sse2_logf(vvals[i]);

    size_t after = ticks();
    return (after - before);
}

double sse2_log_array(float * vals_, int nvals)
{
    double vals[nvals];
    std::copy(vals_, vals_ + nvals, vals);

    sched_yield();
    size_t before = ticks();

    int nvecs = nvals / 2;
    v2df * vvals = (v2df *)vals;

    for (unsigned i = 0;  i < nvecs;  ++i)
        vvals[i] = sse2_log(vvals[i]);

    size_t after = ticks();
    return (after - before);
}

typedef double (*profile_function) (float *, int);

void profile_logf(int nvals, const std::string & desc,
                  profile_function function)
{
    double overhead = calc_ticks_overhead();
    double tps = calc_ticks_per_second();

    // First, warm it up
    float vals[nvals];
    for (int i = 0;  i < nvals;  ++i)
        vals[i] = 10.0 * (2 * i - nvals) / (1.0 * nvals);

    function(vals, nvals);

    vector<double> timings(20);
    for (unsigned trial = 0;  trial < 20;  ++trial) {
        for (int i = 0;  i < nvals;  ++i)
            vals[i] = 10.0 * (2 * i - nvals) / (1.0 * nvals);
        timings[trial] = function(vals, nvals);
    }

    std::sort(timings.begin(), timings.end());

    //cerr << "timings = " << timings << endl;

    cerr << format("%-30s %8.2f %10.1f %8.5f\n", desc.c_str(),
                   (timings[10] - overhead) / (1.0 * nvals),
                   tps, overhead);
}

BOOST_AUTO_TEST_CASE( profile_logf_test )
{
    cerr << endl;
    cerr << "profiling logf" << endl;

    static const int NVALS = 131072;

    warm_up_cpu();

    profile_logf(NVALS, "builtin", &builtin_logf_array);
    profile_logf(NVALS, "builtin double", &builtin_log_array);
    profile_logf(NVALS, "sse2 discrete", &sse2_logf_array);
    profile_logf(NVALS, "sse2 discrete double", &sse2_log_array);
}


#include "fbstab/components/dense_data.h"

#include <Eigen/Dense>
#include <cmath>
#include <stdexcept>

namespace fbstab {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;

void DenseData::gemvH(const Eigen::VectorXd &x, double a, double b,
                      Eigen::VectorXd *y) const {
  *y = a * H_ * x + b * (*y);
}

void DenseData::gemvG(const Eigen::VectorXd &x, double a, double b,
                      Eigen::VectorXd *y) const {
  *y = a * G_ * x + b * (*y);
}

void DenseData::gemvGT(const Eigen::VectorXd &x, double a, double b,
                       Eigen::VectorXd *y) const {
  *y = a * G_.transpose() * x + b * (*y);
}

void DenseData::gemvA(const Eigen::VectorXd &x, double a, double b,
                      Eigen::VectorXd *y) const {
  *y = a * A_ * x + b * (*y);
}

void DenseData::gemvAT(const Eigen::VectorXd &x, double a, double b,
                       Eigen::VectorXd *y) const {
  *y = a * A_.transpose() * x + b * (*y);
}

void DenseData::axpyf(double a, Eigen::VectorXd *y) const { *y += a * f_; }

void DenseData::axpyh(double a, Eigen::VectorXd *y) const { *y += a * h_; }

void DenseData::axpyb(double a, Eigen::VectorXd *y) const { *y += a * b_; }

}  // namespace fbstab


#include "../params.h"
#include "../sigmaplus_prover.h"
#include "../sigmaplus_verifier.h"

#include <boost/test/unit_test.hpp>

#include "../../test/fixtures.h"

BOOST_FIXTURE_TEST_SUITE(sigma_protocol_tests, ZerocoinTestingSetup200)

BOOST_AUTO_TEST_CASE(one_out_of_n)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 0;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(uint64_t(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }
    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(params);

    prover.proof(commits, index, r, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    BOOST_CHECK(verifier.verify(commits, proof));
}

BOOST_AUTO_TEST_CASE(prove_and_verify_in_different_set)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 0;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(uint64_t(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(params);

    prover.proof(commits, index, r, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    // Add more commit
    secp_primitives::GroupElement c;
    secp_primitives::Scalar zero(uint64_t(0));
    c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
    commits.push_back(c);

    BOOST_CHECK(!verifier.verify(commits, proof));
}

BOOST_AUTO_TEST_CASE(prove_coin_out_of_index)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        commits.push_back(secp_primitives::GroupElement());
        commits[i].randomize();
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(params);

    prover.proof(commits, commits.size(), r, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);
    BOOST_CHECK(!verifier.verify(commits,proof));
}

BOOST_AUTO_TEST_CASE(prove_coin_not_in_set)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 0;
    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        commits.push_back(secp_primitives::GroupElement());
        commits[i].randomize();
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(params);

    prover.proof(commits, index, r, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);
    BOOST_CHECK(!verifier.verify(commits,proof));
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/plot.hpp>
#include "g2o/solvers/eigen/linear_solver_eigen.h"

#include "data.hpp"
#include "helper.hpp"

#include "stereo_processor/pose_estimater.hpp"
#include "stereo_processor/g2o_edges/edge_se3m.h"

class LocalKFOptimizer{
private:
	PoseEstimater pose_estimater;

	double getTransformBetweenKF(const KeyFrame& KF_from, const KeyFrame& KF_to, const CameraModel& cam0, Eigen::Isometry3d& T);
public:
	bool optimize(std::vector<KeyFrame>& keyframes, int KF_count, const CameraModel& cam0);

};


#ifndef _ITERATIVE_CLOSEST_POINT_HPP_
#define _ITERATIVE_CLOSEST_POINT_HPP_

#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <cmath>
#include <vector>
#include <iostream>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

#include "hornRegistration.hpp"
#include "TerminationCriteria.hpp"

/// Finds the nearest point on a line segment to a point in space.  Called by the function OutsideOfTriangle()
/// to determine where the nearest point is to each side of the triangle.
/// @param a is the point in space
/// @param p is one end of the line segment represented by an Eigen::Vector3d
/// @param q is the other end of the line segment represented by an Eigen::Vector3d
/// @see page 8 of PointPairs.pdf from the notes
Eigen::Vector4d ProjectOnSegment(const Eigen::Vector3d& c, const Eigen::Vector3d& p, const Eigen::Vector3d& q)
{
    Eigen::Vector3d pMinusQ = q - p;
    double lambda = (c-p).dot(pMinusQ)/(pMinusQ.dot(pMinusQ));
    double zero = 0.0;
    double one = 1.0;
    double vertexlogic = one;
    if (lambda <= 0 || lambda >= 1){
        vertexlogic = zero;
    }
    lambda = std::max(zero,std::min(lambda,one));
    Eigen::Vector4d cNew;
    cNew.block<3,1>(0,0) = p+lambda*pMinusQ;
    cNew(3) = vertexlogic;
    return cNew;
}

/// Determines if two points are equal.  Used by the function OutsideOfTriangle to determine if the nearest
/// point on the triangle lies on a vertice
/// @param a is the point in space
/// @param p is the first vertice of the triangle represented by an Eigen::Vector3d
bool PointEqualityCheck(const Eigen::Vector3d& a, const Eigen::Vector3d& b){
    bool tf = (a(0) == b(0) && a(1) == b(1) && a(2) == b(2));
    return tf;
}

/// Finds the closest point on the triangle to a point in space if the closest point lies on an edge or vertice
/// @param a is the point in space
/// @param p is the first vertice of the triangle represented by an Eigen::Vector3d
/// @param q is the second vertice of the triangle represented by an Eigen::Vector3d
/// @param r is the third vertice of the triangle represented by an Eigen::Vector3d
/// @see page 8 of PointPairs.pdf from the notes
Eigen::Vector3d OutsideOfTriangle(const Eigen::Vector3d& a, const Eigen::Vector3d& p, const Eigen::Vector3d& q, const Eigen::Vector3d& r)
{
    Eigen::MatrixXd c(3,4);
    c.block<1,4>(0,0) = ProjectOnSegment(a,r,p).transpose();
    c.block<1,4>(1,0) = ProjectOnSegment(a,p,q).transpose();
    c.block<1,4>(2,0) = ProjectOnSegment(a,q,r).transpose();
    Eigen::Vector3d cnew;
    if (PointEqualityCheck(c.block<1,3>(0,0),c.block<1,3>(1,0))) cnew = c.block<1,3>(0,0);
    else if (PointEqualityCheck(c.block<1,3>(0,0),c.block<1,3>(2,0))) cnew = c.block<1,3>(0,0);
    else if (PointEqualityCheck(c.block<1,3>(1,0),c.block<1,3>(2,0))) cnew = c.block<1,3>(1,0);
    else{
        for (int i=0; i<3; i++){
            if (c(i,3) == 1){
                cnew = c.block<1,3>(i,0);
            }
        }
    }
    return cnew;
}

/// Finds the closest point on the triangle to a point in space.  If the closest point lies with in triangle,
/// then the function finds the nearest point internally.  Else if the closest point lies on an edge or vertice,
/// the function OutsideOfTriangle() is called to find the nearest point
/// @param a is the point in space represented by an Eigen::Vector3d
/// @param vertices are the three vertices of the triangle represented by standard vector of Eigen::Vector3d
/// @see page 7 of PointPairs.pdf from the notes
Eigen::Vector3d FindClosestPoint(const Eigen::Vector3d& p, const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, const Eigen::Vector3d& p3)
{
    Eigen::Vector3d u = p2-p1;
    Eigen::Vector3d v = p3-p1;
    Eigen::Vector3d w = p-p1;
    Eigen::Vector3d n = u.cross(v);
    double gamma = u.cross(w).dot(n)/(n.dot(n));
    double beta = w.cross(v).dot(n)/(n.dot(n));
    double alpha = 1-gamma-beta;
    Eigen::Vector3d projectedPoint;
    if ((alpha < 0 || alpha > 1) || (beta < 0 || beta > 1) || (gamma < 0 || gamma > 1)){
        projectedPoint = OutsideOfTriangle(p,p1,p2,p3);
    }
    else projectedPoint = alpha*p1+beta*p2+gamma*p3;

    return projectedPoint;
}

Eigen::Vector3d FindClosestPoint(const Eigen::Vector3d& p, const std::vector<Eigen::Vector3d>& vertices, const Eigen::VectorXd& triangle) {

    Eigen::Vector3d p1 = vertices[triangle(0)];
    Eigen::Vector3d p2 = vertices[triangle(1)];
    Eigen::Vector3d p3 = vertices[triangle(2)];

    return FindClosestPoint(p,p1,p2,p3);
}

typedef Eigen::Vector3d EV3d;
typedef bg::cs::cartesian bgcscart;

#define ZEROINDEX operator()(0)
#define ONEINDEX operator()(1)
#define TWOINDEX operator()(2)

BOOST_GEOMETRY_REGISTER_POINT_3D(EV3d, double, bgcscart, ZEROINDEX, ONEINDEX, TWOINDEX)

namespace cis {

    //typedef bg::model::point<double, 3, bg::cs::cartesian> point;
    typedef Eigen::Vector3d point;
    typedef bg::model::box<point> box;
    typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
    typedef std::pair<box, polygon> value;
}

/// perform ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh.
///
/// @pre NA and NB vectors, rather than data, must be of equal length. Corresponding to the same quantity of measured sample time points.
///
/// @param[in]  NA list of sensor data point sets for pointer A
/// @param[in]  NB list of sensor data point sets for fiducial B
/// @param[in]  Atip location of tip of pointerA in pointer A body coordinates
/// @param[in]  bodyAmarkerLEDs location of LED markers on pointer A in pointer A body coordinates
/// @param[in]  bodyAmarkerLEDs location of LED markers on fiducial B in fiducial B body coordinatesnd 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @return dkList location of Atip in fiducial B body coordinates, nx3 matrix of transposed vectors
Eigen::MatrixXd
dkKnownMeshPointsBaseFrame(const std::vector<Eigen::MatrixXd>& NA,
                                const std::vector<Eigen::MatrixXd>& NB,
                                const Eigen::Vector3d& Atip,
                                const Eigen::MatrixXd& bodyAmarkerLEDs,
                                const Eigen::MatrixXd& bodyBmarkerLEDs){

    Eigen::MatrixXd dkList(NA.size(),3);

    for (int i=0; i<NA.size(); i++){
        Eigen::Affine3d FaAffine(hornRegistration(bodyAmarkerLEDs,NA[i])); // a: PA3-A-Debug-SampleReadingsTest A: Problem3-BodyA
        Eigen::Affine3d FbInverseAffine(hornRegistration(NB[i], bodyBmarkerLEDs)); // b: PA3-A-Debug-SampleReadingsTest B: Problem3-BodyB

        // Atip: Problem3-BodyA (last line)
        Eigen::Vector3d dk_i(Eigen::Vector3d(FbInverseAffine*FaAffine*Atip));
        dkList.block<1,3>(i,0) = dk_i.transpose();
    }

    return dkList;
}

/// Perform one iteration of ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh.
///
/// @param[out] dkList location of Atip in fiducial B body coordinates, nx3 matrix of transposed vectors
/// @param[in]  vertices list of vertices on mesh, corresponding to bone surface
/// @param[in]  vertexTriangleNeighborIndex list of 1x6 vectors. First 3 Elements are indices into vertices list, Second 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
void ICPwithSimpleSearchStep(
                              const Eigen::MatrixXd& dkList,
                              const std::vector<Eigen::Vector3d>& vertices,
                              const std::vector<Eigen::VectorXd>& vertexTriangleNeighborIndex,
                              Eigen::Affine3d& Freg,
                              Eigen::MatrixXd& skList,
                              Eigen::MatrixXd& ckList,
                              std::vector<double>& errork){

    errork.clear();
    ckList.resize(dkList.rows(),3);
    skList.resize(dkList.rows(),3);

    for (int i=0; i<dkList.rows(); i++){
        double errorMin=std::numeric_limits<double>::max();
	    Eigen::Vector3d ckMin;
        Eigen::Vector3d dk_i(dkList.block<1,3>(i,0).transpose());

        Eigen::Vector3d sk(Freg*dk_i);
        skList.block<1,3>(i,0) = sk.transpose();

        //if(i % NA.size() == 3) std::cout << "\n\nsk[3]\n\n" << sk << "\n\n";
        for (auto&& triangle : vertexTriangleNeighborIndex){
            Eigen::Vector3d ckTemp = FindClosestPoint(sk, vertices, triangle);
            double errorTemp = (sk-ckTemp).norm();
            if (errorTemp < errorMin){
                ckMin = ckTemp;
                errorMin = errorTemp;
            }
        }
		ckList.block<1,3>(i,0) = ckMin.transpose();
        errork.push_back(errorMin);
    }

	Freg = hornRegistration(dkList,ckList);
}

/// perform ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh. Uses brute force iteration.
/// Runs many iterations and stops
/// optimization based on the input terminationCriteria object, which also
/// returns statistics of the execution by default.
///
/// @param[in] dkList location of Atip in fiducial B body coordinates, n x 3 matrix of transposed vectors
/// @param[in]  vertices list of vertices on mesh, corresponding to bone surface
/// @param[in]  vertexTriangleNeighborIndex list of 1x6 vectors. First 3 Elements are indices into vertices list, Second 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @param[in,out] terminationCriteria  Collects statistics and determines when algorithm should stop. C++ concept matches TerminationCriteria class.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
template<typename TerminationType = TerminationCriteria>
void ICPwithSimpleSearch(
                              const Eigen::MatrixXd& dkList,
                              const std::vector<Eigen::Vector3d>& vertices,
                              const std::vector<Eigen::VectorXd>& vertexTriangleNeighborIndex,
                              TerminationType& terminationCriteria,
                              Eigen::Affine3d& Freg,
                              Eigen::MatrixXd& skList,
                              Eigen::MatrixXd& ckList,
                              std::vector<double>& errork,
                              bool debug = false){

    Freg.setIdentity();

    for(int i = 0; !terminationCriteria.shouldTerminate(); i++){

        if(debug) std::cout << "\n\nFreg before iteration " << i << ":\n\n" << Freg.matrix() << "\n\n";

        if(i) terminationCriteria.nextIteration();
		ICPwithSimpleSearchStep(dkList,vertices,vertexTriangleNeighborIndex,Freg,skList,ckList,errork);

        for(auto && err : errork){
            terminationCriteria(err);
        }

	}
}

/// Perform one iteration of ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh. Uses a spatial index to accelerate
/// nearest neighbor lookup of triangles.
///
/// @tparam boost::geometry::rtree data structure type configured with the triangle set
///
/// @param[in] dkList location of Atip in fiducial B body coordinates, n x 3 matrix of transposed vectors
/// @param[in] vertices list of vertices on mesh, corresponding to bone surface
/// @param[in] rtree actual triangle data index
/// @param[in,out] terminationCriteria  Collects statistics and determines when algorithm should stop. C++ concept matches TerminationCriteria class.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
/// @param[out] errork norm between ck and dk
/// @param[in]  initialQuerySize is the number of boxes that will initially be selected in spatialIndex. Affects algorithm performance.
template<typename RTREE>
void ICPwithSpatialIndexStep(
                      const Eigen::MatrixXd& dkList,
                      const RTREE& rtree,
                      Eigen::Affine3d& Freg,
                      Eigen::MatrixXd& skList,
                      Eigen::MatrixXd& ckList,
                      std::vector<double>& errork,
                      const std::size_t initialQuerySize = 4){

    errork.clear();
    ckList.resize(dkList.rows(),3);
    skList.resize(dkList.rows(),3);

    for (int i=0; i<dkList.rows(); ++i){
        double minErrorAKAdistanceToClosestTriangle=std::numeric_limits<double>::max();
        Eigen::Vector3d ckClosestPointOnMesh;
        Eigen::Vector3d dk_i(dkList.block<1,3>(i,0).transpose());

        Eigen::Vector3d sk(Freg*dk_i);
        skList.block<1,3>(i,0) = sk.transpose();

        // starting query size will affect performance
        /// @todo consider making querySize a parameter

        bool closestFound = false;
        std::size_t querySize = initialQuerySize;
        while(!closestFound){
            // If the distance to the next box was greater than to the nearest triangle
            // or the distance to the triangle was equal to 0
            // break the loop.

            // find 2 nearest values to a point
            std::vector<cis::value> result_n;
            rtree.query(bgi::nearest(sk, querySize), std::back_inserter(result_n));

            // results should be listed in order from closest box to furthest box,
            // but the boxes can overlap so untl the closest point on the nearest triangle
            // is closer than boxes later in the list, there may be closer triangle points
            // than the current one.
            for(auto&& result : result_n){
                /// @todo Eliminate redundantly checking first querySize boxes in result_n on subsequent iterations of while(!closestFound == false).
                Eigen::Vector3d ckClosestPointOnCurrentTriangle = FindClosestPoint(sk, result.second.outer()[0], result.second.outer()[1], result.second.outer()[2]);
                double distanceToCurrentBox = bg::distance(sk,result.first);
                double distanceToCurrentTriangle = bg::distance(sk,ckClosestPointOnCurrentTriangle);

                if (distanceToCurrentTriangle < minErrorAKAdistanceToClosestTriangle){
                    // put dk (qk)  into A put ck into B
                    ckClosestPointOnMesh = ckClosestPointOnCurrentTriangle;
                    minErrorAKAdistanceToClosestTriangle = distanceToCurrentTriangle;
                }

                // leave the loop if the distance to the box is larger than the distance to the polygon
                if( minErrorAKAdistanceToClosestTriangle < distanceToCurrentBox || distanceToCurrentTriangle == 0) {
                    closestFound = true;
                    break;
                }
            }

            // increase the query size and rerun it if we didn't find the closest point for certain
            querySize *=2;
        }

        ckList.block<1,3>(i,0) = ckClosestPointOnMesh.transpose();
        BOOST_VERIFY(!boost::math::isnan(minErrorAKAdistanceToClosestTriangle));
        errork.push_back(minErrorAKAdistanceToClosestTriangle);
    }

    Freg = hornRegistration(dkList,ckList);
}

/// perform ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh.  Uses a spatial index to accelerate
/// nearest neighbor lookup of triangles. Runs many iterations and stops
/// optimization based on the input terminationCriteria object, which also
/// returns statistics of the execution by default.
///
/// @param[in] dkList location of Atip in fiducial B body coordinates, n x 3 matrix of transposed vectors
/// @param[in]  vertices list of vertices on mesh, corresponding to bone surface
/// @param[in]  vertexTriangleNeighborIndex list of 1x6 vectors. First 3 Elements are indices into vertices list, Second 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @param[in,out] terminationCriteria  Collects statistics and determines when algorithm should stop. C++ concept matches TerminationCriteria class.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
template<typename TerminationType = TerminationCriteria>
void ICPwithSpatialIndex(
                  const Eigen::MatrixXd& dkList,
                  const std::vector<Eigen::Vector3d>& vertices,
                  const std::vector<Eigen::VectorXd>& vertexTriangleNeighborIndex,
                  TerminationType& terminationCriteria,
                  Eigen::Affine3d& Freg,
                  Eigen::MatrixXd& skList,
                  Eigen::MatrixXd& ckList,
                  std::vector<double>& errork,
                  bool debug = false){

    Freg.setIdentity();

    // polygons
    std::vector<cis::polygon> polygons;

    // create some polygons
    for ( auto&& triangle : vertexTriangleNeighborIndex )
    {
        // create a polygon
        cis::polygon p;

        Eigen::Vector3d p1 = vertices[triangle(0)];
        Eigen::Vector3d p2 = vertices[triangle(1)];
        Eigen::Vector3d p3 = vertices[triangle(2)];
        p.outer().push_back(p1);
        p.outer().push_back(p2);
        p.outer().push_back(p3);

        // add polygon
        polygons.push_back(p);
    }

    // create the rtree using default constructor
    bgi::rtree< cis::value, bgi::rstar<16, 4> > rtree;

    // fill the spatial index
    for ( auto && polygon : polygons )
    {

        // calculate polygon bounding box
        cis::box b = bg::return_envelope<cis::box>(polygon);
        // insert new value
        rtree.insert(std::make_pair(b, polygon));
    }

    for(int i = 0; !terminationCriteria.shouldTerminate(); i++){

        if(debug) std::cout << "\n\nFreg before iteration " << i << ":\n\n" << Freg.matrix() << "\n\n";
        if(i) terminationCriteria.nextIteration();
        ICPwithSpatialIndexStep(dkList,rtree,Freg,skList,ckList,errork);

        for(auto && err : errork){
            terminationCriteria(err);
        }
    }
}

#endif // _ITERATIVE_CLOSEST_POINT_HPP_