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

/**
 * \file boost/numeric/ublasx/operation/lu.hpp
 *
 * \brief LU decomposition and solver.
 *
 * Computes an LU factorization of a general m-by-n matrix \f$A\f$ optionally
 * using partial pivoting with row interchanges.
 * The factorization has the form
 * \f[
 *  A = L U
 * \f]
 * or, if partial pivoting is used:
 * \f[
 *  A = P L U
 * \f]
 * where \f$P\f$ is a permutation matrix, \f$L\f$ is lower triangular with unit
 * diagonal elements (lower trapezoidal if \f$m > n\f$), and \f$U\f$ is upper
 * triangular (upper trapezoidal if \f$m < n\f$).
 * If matrix \f$A\f$ is rectangular \f$L and \f$P are square matrices each
 * having the same number of rows as \f$A\f$, while \f$U\f$ is exactly the same
 * shape as \f$A\f$.
 *
 * \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_LU_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LU_HPP

//TODO: Use LAPACK functions in order to handle different types of matrices.
//TODO: How about full pivoting?
//TODO: Create a \c lu_decomposition class (e.g., \sa qr.hpp).

#include <boost/numeric/ublas/detail/temporary.hpp>
#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/traits.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>

namespace boost { namespace numeric { namespace ublasx {

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

/**
 * \brief LU decomposition without pivoting of the given matrix \a A.
 * \param A The matrix to be decomposed.
 * \return Zero if decomposition succeed; non-zero if decompositon fails (the
 *  value is 1 + the numer of the failing row). The input matrix \a A is
 *  modified in order to contain the L*U matrix.
 *
 * Perform LU decomposition of matrix \a A and replaces the strict lower
 * triangular part of \a m with the computed matrix L and the upper triangular
 * part of \a m is replaced by the computed matrix U.
 * For obtaining the single matrices L and U proceed as follows:
 * - for L: extract the strict lower-triangular part (i.e., without the main
 *   diagonal) from the computed matrix \a m and add to it the identity matrix
 *   of the same order of \a m;
 * - for U: extract the upper-triangular part (with the main diagonal) from the
 *   computer matrix \a m.
 */
template <typename MatrixT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixT>::size_type lu_decompose_inplace(matrix_container<MatrixT>& A)
{
    return lu_factorize(A());
}

/**
 * \brief LU decomposition with partial pivoting of the matrix \a A.
 *
 * \param A The matrix to be decomposed.
 * \param P The permutation matrix reporting permutated rows of \a A after
 *  the decomposition.
 * \return Zero if decomposition succeed; non-zero if decompositon fails (the
 *  value is 1 + the numer of the failing row). The input matrix \a A is
 *  modified in order to contain the L*U matrix.
 *
 * Perform LUP decomposition of matrix \a A and replaces the strict lower
 * triangular part of \a A with the computed matrix L and the upper triangular
 * part of \a A is replaced by the computed matrix U.
 * For obtaining the single matrices L and U proceed as follows:
 * - for L: extract the strict lower-triangular part (i.e., without the main
 *   diagonal) from the computed matrix \a m and add to it the identity matrix
 *   of the same order of \a A;
 * - for U: extract the upper-triangular part (with the main diagonal) from the
 *   computer matrix \a A.
 * .
 */
template <typename MatrixT, typename PermutationMatrixT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixT>::size_type lu_decompose_inplace(matrix_container<MatrixT>& A, PermutationMatrixT& P)
{
    typedef typename matrix_traits<MatrixT>::size_type size_type;

    // Safety check: P is squared && size(P) == num_rows(A)
    //BOOST_UBLAS_CHECK( size(P) == num_columns(P) && size(P) == num_rows(A), bad_size() );
    size_type nr_A = num_rows(A);
    if (size(P) != nr_A)
    {
        P.resize(nr_A, false);
    }

    return lu_factorize(A(), P);
//
//  // postcondition: P is squared && size(P) == num_rows(A)
//  BOOST_UBLAS_CHECK( size(P) == num_columns(P) && size(P) == num_rows(A), bad_size() );
}

/**
 * \brief LU decomposition without pivoting of the matrix \a A.
 *
 * \tparam MatrixT The type of the matrix to be decomposed.
 * \param A The matrix to be decomposed.
 * \return Zero if decomposition succeed; non-zero if decompositon fails (the
 *  value is 1 + the numer of the failing row). The input matrix \a A is
 *  modified in order to contain the L*U matrix.
 *
 * Perform LU decomposition of matrix \a A and replaces the strict lower
 * triangular part of \a A with the computed matrix L and the upper triangular
 * part of \a A is replaced by the computed matrix U.
 * For obtaining the single matrices L and U proceed as follows:
 * - for L: extract the strict lower-triangular part (i.e., without the main
 *   diagonal) from the computed matrix \a A and add to it the identity matrix
 *   of the same order of \a A;
 * - for U: extract the upper-triangular part (with the main diagonal) from the
 *   computer matrix \a A.
 * .
 */
template <typename AMatrixExprT, typename LUMatrixT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixExprT>::size_type lu_decompose(matrix_expression<AMatrixExprT> const& A, matrix_container<LUMatrixT>& LU)
{
    LU = A;

    return lu_decompose_inplace(LU);
}

/**
 * \brief LU decomposition with partial pivoting of the matrix \a A.
 *
 * \pre Matrix \a A must be squared.
 * \tparam MatrixT The type of the matrix to be decomposed.
 * \param A The matrix to be decomposed.
 * \param P The permutation matrix reporting permutated rows of \a A after
 *  the decomposition.
 * \return The LU matrix.
 *
 * Perform LUP decomposition of matrix \a A and return the LU matrix.
 * For obtaining the single matrices L and U proceed as follows:
 * - for L: extract the strict lower-triangular part (i.e., without the main
 *   diagonal) from the computed matrix \c LU and add to it the identity matrix
 *   of the same order of \c LU;
 * - for U: extract the upper-triangular part (with the main diagonal) from the
 *   computer matrix \c LU.
 * .
 */
template <typename AMatrixExprT, typename PermutationMatrixT, typename LUMatrixT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixExprT>::size_type lu_decompose(matrix_expression<AMatrixExprT> const& A, PermutationMatrixT& P, matrix_container<LUMatrixT>& LU)
{
    typedef typename matrix_traits<AMatrixExprT>::size_type size_type;

    // Safety check: size(P) == num_rows(A)
    //BOOST_UBLAS_CHECK( size(P) == num_rows(A), bad_size() );
    size_type nr_A = num_rows(A);
    if (size(P) != nr_A)
    {
        P.resize(nr_A, false);
    }

    LU = A;

    return lu_decompose_inplace(LU, P);
}

/**
 * \brief Complete the LU forward/backward substitution for solving the system
 *  \f$LUx=b\f$.
 *
 * \pre The size of \a b must be the same as the number of rows of \a LU.
 * \tparam MatrixExprT The type of the matrix obtained by the LU decomposition.
 * \tparam VectorT The type of the constant terms vector.
 * \param LU A matrix representing an LU decomposition.
 * \param b The vector of coefficients.
 * \return Nothing. However, the vector \a b is replaced with the value of
 *  unknowns \f$x_i\f$ satisfying the system \f$LUx=b\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=b\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LU\f$, where \f$L\f$ is a lower-triangular matrix with ones on the
 * diagonal, and \f$U\f$ is an upper-triangular matrix). Solving the system
 * \f$Ax=b\f$ is then equivalent to solving two simpler systems \f$Ly=b\f$ and
 * \f$Ux=y\f$. Since \f$L\f$ is lower-triangular, the system \f$Ly=b\f$ can be
 * solved by forward substitution. Moreover, since \f$U\f$ is upper-triangular,
 * the system \f$Ux=y\f$ can be solved by backward substitution.
 */
template <typename MatrixExprT, typename VectorT>
BOOST_UBLAS_INLINE
void lu_apply_inplace(matrix_expression<MatrixExprT> const& LU, vector_container<VectorT>& b)
{
    // pre: size(b) == size(P)
    BOOST_UBLAS_CHECK( size(b) == num_rows(LU), bad_size() );

    lu_substitute(LU(), b());
}

/**
 * \brief Complete the LU forward/backward substitution for solving the system
 *  \f$LUX=B\f$.
 *
 * \pre The number of rows of \a LU and \a b must be the same.
 * \tparam LUMatrixExprT The type of the matrix obtained by the LU decomposition.
 * \tparam BMatrixT The type of the constant terms vector.
 * \param LU A matrix representing an LU decomposition.
 * \param B The matrix of coefficients.
 * \return Nothing. However, the matrix \a B is replaced with the value of
 *  unknowns \f$X_{ij}\f$ satisfying the system \f$LUX=B\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=B\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LU\f$, where \f$L\f$ is a lower-triangular matrix with ones on the
 * diagonal, and \f$U\f$ is an upper-triangular matrix). Solving the system
 * \f$AX=B\f$ is then equivalent to solving two simpler systems \f$LY=B\f$ and
 * \f$UX=Y\f$. Since \f$L\f$ is lower-triangular, the system \f$LY=B\f$ can be
 * solved by forward substitution. Moreover, since \f$U\f$ is upper-triangular,
 * the system \f$UX=Y\f$ can be solved by backward substitution.
 */
template <typename LUMatrixExprT, typename BMatrixT>
BOOST_UBLAS_INLINE
void lu_apply_inplace(matrix_expression<LUMatrixExprT> const& LU, matrix_container<BMatrixT>& B)
{
    // pre: num_rows(B) == num_rows(LU)
    BOOST_UBLAS_CHECK( num_rows(B) == num_rows(LU), bad_size() );

    lu_substitute(LU(), B());
}

/**
 * \brief Complete the LUP forward/backward substitution for solving the system
 *  \f$LU*X=P*b\f$.
 *
 * \pre The size of \a P must be the same as the number of rows of \a LU.
 * \pre The size of \a b must be the same as the number of rows of \a LU.
 * \param LU A matrix representing an LU decomposition.
 * \param P A permutation matrix.
 * \param b The vector constant terms.
 * \return Nothing. However, the vector \a b is replaced with the value
 *  of unknowns \f$x_i\f$ satisfying the system \f$LU*x=P*b\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=b\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LUP\f$, where \f$L\f$ is a lower-triangular matrix with ones on
 * the diagonal, \f$U\f$ is an upper-triangular matrix, and \f$P\f$ is a row
 * permutation matrix that is used to rearrange the rows of \f$A\f$ before so
 * that it can be decomposed). Solving the system \f$Ax=b\f$ is then equivalent
 * to solving two simpler systems \f$Ly=Pb\f$ and \f$Ux=y\f$. Since \f$L\f$ is
 * lower-triangular, the system \f$Ly=Pb\f$ can be solved by forward
 * substitution. Moreover, since \f$U\f$ is upper-triangular, the system
 * \f$Ux=y\f$ can be solved by backward substitution.
 */
template <typename LUMatrixExprT, typename PermutationMatrixT, typename BVectorT>
BOOST_UBLAS_INLINE
void lu_apply_inplace(matrix_expression<LUMatrixExprT> const& LU, PermutationMatrixT const& P, vector_container<BVectorT>& b)
{
    // precondition: size(P) == num_rows(LU)
    BOOST_UBLAS_CHECK( size(P) == num_rows(LU), bad_size() );
    // precondition: size(b) == num_rows(LU)
    BOOST_UBLAS_CHECK( size(b) == num_rows(LU), bad_size() );

    lu_substitute(LU(), P, b());
}

/**
 * \brief Complete the LUP forward/backward substitution for solving the system
 *  \f$LU*X=P*B\f$.
 *
 * \pre The size of \a P must be the same as the number of rows of \a LU.
 * \pre The number of rows of \a LU and of \a B must be the same.
 * \param LU A matrix representing an LU decomposition.
 * \param P A permutation matrix.
 * \param B The matrix of constant terms.
 * \return Nothing. However, the matrix \a B is replaced with the value
 *  of unknowns \f$X_{ij}\f$ satisfying the system \f$LU*X=P*B\f$.
 *
 * An \f$n \times n\f$ linear system \f$AX=B\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LUP\f$, where \f$L\f$ is a lower-triangular matrix with ones on
 * the diagonal, \f$U\f$ is an upper-triangular matrix, and \f$P\f$ is a row
 * permutation matrix that is used to rearrange the rows of \f$A\f$ before so
 * that it can be decomposed). Solving the system \f$AX=B\f$ is then equivalent
 * to solving two simpler systems \f$LY=Pb\f$ and \f$UX=y\f$. Since \f$L\f$ is
 * lower-triangular, the system \f$LY=PB\f$ can be solved by forward
 * substitution. Moreover, since \f$U\f$ is upper-triangular, the system
 * \f$UX=Y\f$ can be solved by backward substitution.
 */
template <typename LUMatrixExprT, typename PermutationMatrixT, typename BMatrixT>
BOOST_UBLAS_INLINE
void lu_apply_inplace(matrix_expression<LUMatrixExprT> const& LU, PermutationMatrixT const& P, matrix_container<BMatrixT>& B)
{
    // pre: size(P) == num_rows(LU)
    BOOST_UBLAS_CHECK( size(P) == num_rows(LU), bad_size() );
    // pre: num_rows(b) == num_rows(LU)
    BOOST_UBLAS_CHECK( num_rows(B) == num_rows(LU), bad_size() );

    lu_substitute(LU(), P, B());
}

/**
 * \brief Complete the LU forward/backward substitution for solving the system
 *  \f$LUx=b\f$.
 *
 * \param LU A matrix representing an LU decomposition.
 * \param b The vector of coefficients.
 * \return The vector of unknowns \f$x\f$ satisfying the system \f$LUPx=b\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=b\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LU\f$, where \f$L\f$ is a lower-triangular matrix with ones on the
 * diagonal, and \f$U\f$ is an upper-triangular matrix). Solving the system
 * \f$Ax=b\f$ is then equivalent to solving two simpler systems \f$Ly=b\f$ and
 * \f$Ux=y\f$. Since \f$L\f$ is lower-triangular, the system \f$Ly=b\f$ can be
 * solved by forward substitution. Moreover, since \f$U\f$ is upper-triangular,
 * the system \f$Ux=y\f$ can be solved by backward substitution.
 */
template <typename LUMatrixExprT, typename BVectorExprT>
BOOST_UBLAS_INLINE
typename vector_temporary_traits<BVectorExprT>::type lu_apply(matrix_expression<LUMatrixExprT> const& LU, vector_expression<BVectorExprT> const& b)
{
    //  preconditions check delegated to lu_apply_inplace

    typedef typename vector_temporary_traits<BVectorExprT>::type out_vector_type;

    out_vector_type x(b);

    lu_apply_inplace(LU, x);

    return x;
}

/**
 * \brief Complete the LU forward/backward substitution for solving the system
 *  \f$LU*X=B\f$.
 *
 * \param LU A matrix representing an LU decomposition.
 * \param B The matrix of constant terms.
 * \return The matrix of unknowns \f$X\f$ satisfying the system \f$LU*X=B\f$.
 */
template <typename LUMatrixExprT, typename BMatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_temporary_traits<BMatrixExprT>::type lu_apply(matrix_expression<LUMatrixExprT> const& LU, matrix_expression<BMatrixExprT> const& B)
{
    //  preconditions check delegated to lu_apply_inplace

    typedef typename matrix_temporary_traits<BMatrixExprT>::type out_matrix_type;

    out_matrix_type X(B);

    lu_apply_inplace(LU, X);

    return X;
}

/**
 * \brief Complete the LUP forward/backward substitution for solving the system
 *  \f$LUx=b\f$.
 *
 * \param LU A matrix representing an LU decomposition.
 * \param P A permutation matrix.
 * \param b The vector of coefficients.
 * \return The vector of unknowns \f$x\f$ satisfying the system \f$LUPx=b\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=b\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LUP\f$, where \f$L\f$ is a lower-triangular matrix with ones on
 * the diagonal, \f$U\f$ is an upper-triangular matrix, and \f$P\f$ is a row
 * permutation matrix that is used to rearrange the rows of A before so that it
 * can be decomposed). Solving the system \f$Ax=b\f$ is then equivalent to
 * solving two simpler systems \f$Ly=Pb\f$ and \f$Ux=y\f$. Since \f$L\f$ is
 * lower-triangular, the system \f$Ly=Pb\f$ can be solved by forward
 * substitution. Moreover, since \f$U\f$ is upper-triangular, the system
 * \f$Ux=y\f$ can be solved by backward substitution.
 */
template <typename LUMatrixExprT, typename PermutationMatrixT, typename BVectorExprT>
BOOST_UBLAS_INLINE
typename vector_temporary_traits<BVectorExprT>::type lu_apply(matrix_expression<LUMatrixExprT> const& LU, PermutationMatrixT const& P, vector_expression<BVectorExprT> const& b)
{
    //  preconditions check delegated to lu_apply_inplace

    typedef typename vector_temporary_traits<BVectorExprT>::type out_vector_type;

    out_vector_type x(b);

    lu_apply_inplace(LU, P, x);

    return x;
}

/**
 * \brief Complete the LUP forward/backward substitution for solving the system
 *  \f$LUP*X=B\f$.
 *
 * \param LU A matrix representing an LU decomposition.
 * \param P A permutation matrix.
 * \param B The matrix of constant terms.
 * \return The matrix of unknowns \f$X\f$ satisfying the system \f$LUP*X=B\f$.
 */
template <typename LUMatrixExprT, typename PermutationMatrixT, typename BMatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_temporary_traits<BMatrixExprT>::type lu_apply(matrix_expression<LUMatrixExprT> const& LU, PermutationMatrixT const& P, matrix_expression<BMatrixExprT> const& B)
{
    //  preconditions check delegated to lu_apply_inplace

    typedef typename matrix_temporary_traits<BMatrixExprT>::type out_matrix_type;

    out_matrix_type X(B);

    lu_apply_inplace(LU, P, X);

    return X;
}

/**
 * \brief Solve the linear system \f$Ax=b\f$ by LUP decomposition.
 *
 * \pre The size of \a b must be the same as the number of rows of \a A.
 * \param A The input matrix representing the coefficients of the unknowns.
 * \param b The vector of coefficients.
 * \return A zero value if the system is solvable; a number greater than zero if
 *  it is not. Moreover, the vector \a b is replaced with the value of unknowns
 *  \f$x_i\f$ satisfying the system \f$Ax=b\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=b\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LUP\f$, where \f$L\f$ is a lower-triangular matrix with ones on
 * the diagonal, \f$U\f$ is an upper-triangular matrix, and \f$P\f$ is a row
 * permutation matrix that is used to rearrange the rows of \f$A\f$ before so
 * that it can be decomposed). Solving the system \f$Ax=b\f$ is then equivalent
 * to solving two simpler systems \f$Ly=Pb\f$ and \f$Ux=y\f$. Since \f$L\f$ is
 * lower-triangular, the system \f$Ly=Pb\f$ can be solved by forward
 * substitution. Moreover, since \f$U\f$ is upper-triangular, the system
 * \f$Ux=y\f$ can be solved by backward substitution.
 */
template <typename MatrixExprT, typename VectorT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::size_type lu_solve_inplace(matrix_expression<MatrixExprT> const& A, vector_container<VectorT>& b)
{
    // precondition: size(b) == num_rows(A)
    BOOST_UBLAS_CHECK( size(b) == num_rows(A), bad_size() );

    typedef typename matrix_traits<MatrixExprT>::size_type size_type;
    typedef typename matrix_traits<MatrixExprT>::value_type value_type;
    typedef typename layout_type<MatrixExprT>::type layout_type;

    // Ax=b ==> LUx=b ==> Ly=b AND Ux=y

    matrix<value_type, layout_type> LU(A);
    permutation_matrix<size_type> P(num_rows(LU));

    size_type singular;
    singular = lu_decompose_inplace(LU, P);

    if (!singular)
    {
        lu_apply_inplace(LU, P, b());
    }

    return singular;
}

/**
 * \brief Solve the linear system \f$AX=B\f$ by LUP decomposition.
 *
 * \pre The number of rows of \a A and \a B must be the same.
 * \param A The input matrix representing the coefficients of the unknowns.
 * \param B The matrix of coefficients.
 * \return A zero value if the system is solvable; a number greater than zero if
 *  it is not. Moreover, the matrix \a B is replaced with the value of unknowns
 *  \f$X_{ij}\f$ satisfying the system \f$AX=B\f$.
 *
 * An \f$n \times n\f$ linear system \f$AX=B\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LUP\f$, where \f$L\f$ is a lower-triangular matrix with ones on
 * the diagonal, \f$U\f$ is an upper-triangular matrix, and \f$P\f$ is a row
 * permutation matrix that is used to rearrange the rows of \f$A\f$ before so
 * that it can be decomposed). Solving the system \f$AX=B\f$ is then equivalent
 * to solving two simpler systems \f$LY=PB\f$ and \f$UX=Y\f$. Since \f$L\f$ is
 * lower-triangular, the system \f$LY=PB\f$ can be solved by forward
 * substitution. Moreover, since \f$U\f$ is upper-triangular, the system
 * \f$UX=Y\f$ can be solved by backward substitution.
 */
template <typename AMatrixExprT, typename BMatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixExprT>::size_type lu_solve_inplace(matrix_expression<AMatrixExprT> const& A, matrix_container<BMatrixExprT>& B)
{
    // pre: num_rows(B) == num_rows(A)
    BOOST_UBLAS_CHECK( num_rows(B) == num_rows(A), bad_size() );

    typedef typename matrix_traits<AMatrixExprT>::size_type size_type;
    typedef typename matrix_traits<AMatrixExprT>::value_type value_type;
    typedef typename layout_type<AMatrixExprT>::type layout_type;

    // Ax=B ==> LUx=B ==> Ly=B AND Ux=y

    matrix<value_type, layout_type> LU(A());
    permutation_matrix<size_type> P(num_rows(LU));

    size_type singular;
    singular = lu_decompose_inplace(LU, P);

    if (!singular)
    {
        lu_apply_inplace(LU, P, B());
    }

    return singular;
}

/**
 * \brief Solve the linear system \f$Ax=b\f$ by LUP decomposition.
 *
 * \pre The size of \a b must be the same as the number of rows of \a A.
 * \param A The input matrix representing the coefficients of the unknowns..
 * \param b The vector of coefficients.
 * \return The vector of unknowns \f$x\f$ satisfying the system \f$Ax=b\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=b\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LUP\f$, where \f$L\f$ is a lower-triangular matrix with ones on
 * the diagonal, \f$U\f$ is an upper-triangular matrix, and \f$P\f$ is a row
 * permutation matrix that is used to rearrange the rows of A before so that it
 * can be decomposed). Solving the system \f$Ax=b\f$ is then equivalent to
 * solving two simpler systems \f$Ly=Pb\f$ and \f$Ux=y\f$. Since \f$L\f$ is
 * lower-triangular, the system \f$Ly=Pb\f$ can be solved by forward
 * substitution. Moreover, since \f$U\f$ is upper-triangular, the system
 * \f$Ux=y\f$ can be solved by backward substitution.
 */
template <typename MatrixExprT, typename VectorExprT, typename OutVectorT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::size_type lu_solve(matrix_expression<MatrixExprT> const& A, vector_expression<VectorExprT> const& b, OutVectorT& x)
{
    // precondition: size(b) == num_rows(A)
    BOOST_UBLAS_CHECK( size(b) == num_rows(A), bad_size() );

    // Ax=b ==> LUx=b ==> Ly=b AND Ux=y

    typedef typename matrix_traits<MatrixExprT>::value_type size_type;
    typedef typename matrix_traits<MatrixExprT>::value_type value_type;
    typedef typename layout_type<MatrixExprT>::type layout_type;

    matrix<value_type,layout_type> LU(A);
    permutation_matrix<size_type> P(num_rows(LU));

    size_type singular;
    singular = lu_decompose_inplace(LU, P);

    if (!singular)
    {
        x = b;

        lu_apply_inplace(LU, P, x);
    }

    return singular;
}

/**
 * \brief Solve the linear system \f$Ax=b\f$ by LUP decomposition.
 *
 * \pre The size of \a b must be the same as the number of rows of \a A.
 * \param A The input matrix representing the coefficients of the unknowns..
 * \param b The vector of coefficients.
 * \return The vector of unknowns \f$x\f$ satisfying the system \f$Ax=b\f$.
 *
 * An \f$n \times n\f$ linear system \f$Ax=b\f$ can often be solved efficiently
 * by \f$LU\f$ decomposition (that is by decomposing matrix \f$A\f$ into a
 * product \f$LUP\f$, where \f$L\f$ is a lower-triangular matrix with ones on
 * the diagonal, \f$U\f$ is an upper-triangular matrix, and \f$P\f$ is a row
 * permutation matrix that is used to rearrange the rows of A before so that it
 * can be decomposed). Solving the system \f$Ax=b\f$ is then equivalent to
 * solving two simpler systems \f$Ly=Pb\f$ and \f$Ux=y\f$. Since \f$L\f$ is
 * lower-triangular, the system \f$Ly=Pb\f$ can be solved by forward
 * substitution. Moreover, since \f$U\f$ is upper-triangular, the system
 * \f$Ux=y\f$ can be solved by backward substitution.
 */
template <typename AMatrixExprT, typename BMatrixExprT, typename OutMatrixT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixExprT>::size_type lu_solve(matrix_expression<AMatrixExprT> const& A, matrix_expression<BMatrixExprT> const& B, OutMatrixT& X)
{
    // precondition: size(b) == num_rows(A)
    BOOST_UBLAS_CHECK( num_rows(B) == num_rows(A), bad_size() );

    // Ax=b ==> LUx=b ==> Ly=b AND Ux=y

    typedef typename matrix_traits<AMatrixExprT>::value_type size_type;
    typedef typename matrix_traits<AMatrixExprT>::value_type value_type;
    typedef typename layout_type<AMatrixExprT>::type layout_type;

    matrix<value_type,layout_type> LU(A);
    permutation_matrix<size_type> P(num_rows(LU));

    size_type singular;
    singular = lu_decompose_inplace(LU, P);

    if (!singular)
    {
        X = B;

        lu_apply_inplace(LU, P, X);
    }

    return singular;
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LU_HPP


#pragma once
#include <cmath>
#include <omp.h>
#include <vector>
#include <string>
#include <cstdio>
#include <random>
#include <fstream>
#include <algorithm>
#include <unordered_set>
#include <parallel/algorithm>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "n2/hnsw.h"

using namespace std;
using namespace Eigen;

namespace parallel
{

struct nn_t
{
    int key;
    float val;
    nn_t() : key(-1), val(-987654321.0) {}
    bool operator < (const float& that){
        return this->val > that;
    }

    bool operator < (const nn_t& that){
        return this->val > that.val;
    }
};

struct topn_t
{
    nn_t* nns;
    int K;
    topn_t() : K(0) {}
    void alloc(int _K){
        K = _K;
        nns = new nn_t[K];
    }
    void free() {
        if(K){
            delete[] nns;
        }
    }
    void update(int& key, float& val){
        nn_t* ptr = lower_bound(nns, nns + K, val);
        int idx = (int)(ptr - nns);
        if(idx >= K)
            return;
        if(idx + 1 == K){
            nns[idx].key = key;
            nns[idx].val = val;
        }
        else{
            memmove((void*)&nns[idx + 1], (void*)&nns[idx], sizeof(nn_t) * (K - idx - 1));
            nns[idx].key = key;
            nns[idx].val = val;
        }
    }
};

void quickselect(float* scores, int rows, int cols, int32_t* result, int k, bool sorted, int num_threads){
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> _scores(scores, rows, cols);
    Map<Matrix<int, Dynamic, Dynamic, RowMajor>> _result(result, rows, k);
    omp_set_num_threads(num_threads);
    #pragma omp parallel for schedule(dynamic, 4)
    for (int i=0; i<rows; ++i){
        vector<int> ranks(cols);
        iota(ranks.begin(), ranks.end(), 0);  // initialize ranks as {0, 1, 2, ...}
        // high score has a priority
        nth_element(ranks.begin(), ranks.begin() + k - 1, ranks.end(),
                    [&](int lhs, int rhs){return _scores(i, lhs) > _scores(i, rhs);});
        // sort ranks[: k], since it is not guaranteed to be sorted
        if (sorted)
            sort(ranks.begin(), ranks.begin() + k,
                    [&](int lhs, int rhs){return _scores(i, lhs) > _scores(i, rhs);});
        copy(ranks.begin(), ranks.begin() + k, &_result(i, 0));
    }
}

void dot_topn(
        int32_t* indexes, int num_queries,
        float* _P, int p_rows, int p_cols,
        float* _Q, int q_rows, int q_cols,
        float* _Qb, int qb_rows,
        int32_t* _out_keys, float* _out_scores,
        int32_t* _pool, int pool_size,
        int k, int num_threads)
{
    bool is_same = _P == _Q;
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> P(_P, p_rows, p_cols);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> Q(_Q, q_rows, q_cols);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> Qb(_Qb, qb_rows, 1);
    Map<Matrix<int, Dynamic, Dynamic, RowMajor>> out_keys(_out_keys, num_queries, k);
    Map<Matrix<float , Dynamic, Dynamic, RowMajor>> out_scores(_out_scores, num_queries, k);

    unordered_set<int32_t> pool;
    for (int i=0; i < pool_size; ++i)
        pool.insert(_pool[i]);

    int correct_k = min(q_rows, k);
    if (pool_size)
        correct_k = min(pool_size, correct_k);

    omp_set_num_threads(num_threads);
    #pragma omp parallel for schedule(guided)
    for (int i=0; i < num_queries; ++i){
        topn_t topn;
        topn.alloc(correct_k);
        float last_one = -987654321.0f;
        int q = indexes[i];
        for (int j=0; j < q_rows; ++j) {
            if (is_same and q == j)
                continue;
            if (pool_size and pool.find(j) == pool.end())
                continue;
            float score = P.row(q).dot(Q.row(j));
            if (qb_rows)
                score += Qb(j);
            if (score > last_one) {
                topn.update(j, score);
                last_one = topn.nns[correct_k - 1].val;
            }
        }
        for (int j=0; j < correct_k; ++j) {
            out_keys(i, j) = topn.nns[j].key;
            out_scores(i, j) = topn.nns[j].val;
        }
        for (int j=correct_k; j < k; ++j) {
            out_keys(i, j) = -1;
            out_scores(i, j) = 0.0;
        }
        topn.free();
    }
}

void ann_search(
        string index_path,
        int ef_search,
        bool use_mmap,
        int32_t* indexes, int num_queries,
        float* _P, int p_rows, int p_cols,
        float* _Q, int q_rows, int q_cols,
        float* _Qb, int qb_rows,
        int32_t* _out_keys, float* _out_scores,
        int32_t* _pool, int pool_size,
        int k, int num_threads)
{
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> P(_P, p_rows, p_cols);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> Q(_Q, q_rows, q_cols);
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>> Qb(_Qb, qb_rows, 1);
    Map<Matrix<int, Dynamic, Dynamic, RowMajor>> out_keys(_out_keys, num_queries, k);
    Map<Matrix<float , Dynamic, Dynamic, RowMajor>> out_scores(_out_scores, num_queries, k);

    unordered_set<int32_t> pool;
    for (int i=0; i < pool_size; ++i)
        pool.insert(_pool[i]);

    int correct_k = min(q_rows, k);
    if (pool_size)
        correct_k = min(pool_size, correct_k);

    omp_set_num_threads(num_threads);
    n2::Hnsw hnsws[num_threads];
    for (int i=0; i < num_threads; ++i) {
        hnsws[i].LoadModel(index_path.c_str(), use_mmap);
    }

    #pragma omp parallel
    {
        int worker_id = omp_get_thread_num();
        #pragma omp for schedule(guided)
        for (int i=0; i < num_queries; ++i)
        {
            auto& hnsw = hnsws[worker_id];
            int q = indexes[i];
            vector<pair<int, float>> result;
            hnsw.SearchById(q, correct_k, ef_search, result);
            for (int j=0; j < (int)result.size(); ++j) {
                out_keys(i, j) = result[j].first;
                out_scores(i, j) = result[j].second;
            }
            for (int j=(int)result.size(); j < k; ++j) {
                out_keys(i, j) = -1;
                out_scores(i, j) = 0.0;
            }
        }
    }
    for (int i=0; i < num_threads; ++i) {
        hnsws[i].UnloadModel();
    }
}

}


#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/core/robust_kernel_impl.h>
#include <opencv2/core/core.hpp>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <cmath>
#include <chrono>
#include <vector>

using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<6, Eigen::Matrix<double,6,1>>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl()
    {
		_estimate << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0;
	}

    virtual void oplusImpl( const double* update )
    {
        _estimate += Eigen::Matrix<double,6,1> (update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) { return false; }
    virtual bool write( ostream& out ) const { return false; }
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double ax, double ay, double az ): BaseUnaryEdge(), ax_(ax), ay_(ay), az_(az) {}
    // 计算曲线模型误差
    virtual void computeError()
    {
    	// para = [sx, sy, sz, bx, by, bz]
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Matrix<double,6,1> para = v->estimate();
        //cout<<para(0,0)<<", "<<para(1,0)<<", "<<para(2,0)<<", "<<para(3,0)<<", "<<para(4,0)<<", "<<para(5,0)<<endl;
        _error(0,0) = _measurement - ( (para(0,0) * (ax_ + para(3,0))) * (para(0,0) * (ax_ + para(3,0))) +
									   (para(1,0) * (ay_ + para(4,0))) * (para(1,0) * (ay_ + para(4,0))) +
									   (para(2,0) * (az_ + para(5,0))) * (para(2,0) * (az_ + para(5,0)))
   									 );
		// cout << _error(0,0) << endl;
    }

    virtual void linearizeOplus()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Matrix<double,6,1> para = v->estimate();
        _jacobianOplusXi[0] = -2.0*para(0,0)*( (ax_ + para(3,0)) * (ax_ + para(3,0)) );
        _jacobianOplusXi[1] = -2.0*para(1,0)*( (ay_ + para(4,0)) * (ay_ + para(4,0)) );
        _jacobianOplusXi[2] = -2.0*para(2,0)*( (az_ + para(5,0)) * (az_ + para(5,0)) );
        _jacobianOplusXi[3] = -2.0*para(0,0)*para(0,0)*(ax_ + para(3,0));
        _jacobianOplusXi[4] = -2.0*para(1,0)*para(1,0)*(ay_ + para(4,0));
        _jacobianOplusXi[5] = -2.0*para(2,0)*para(2,0)*(az_ + para(5,0));

    }

    virtual bool read( istream& in ) { return false; }
    virtual bool write( ostream& out ) const { return false; }
public:
    const double ax_, ay_, az_;  // acc 值， 9.8^2 值为 _measurement
};

int main( int argc, char** argv )
{
	vector<vector<double>> matrix;
	//readfile
	ifstream file;
	cout << "----- Fetching .csv file ..." << endl;
	file.open("../imu_data.csv");
	if(!file.is_open()){
		cerr << "ERROR: failed to open the file!" << endl;
		return 1;
	}
	string line;
	while (getline( file, line,'\n'))  //讀檔讀到跳行字元
	{
		vector<double> row_data;
		stringstream templine(line); // string 轉換成 stream
		string data;
		 while (getline( templine, data,',')) //讀檔讀到逗號
		 {
			 row_data.push_back(stof(data));  //string 轉換成數字
		 }
		matrix.push_back(row_data);
	}
	file.close();
	cout << "----- Finish!" << endl << endl;
	cout << "Whether to show the raw data on the SCREEN? 'y' or 'n': ";
	char resp;
	cin >> resp;
	if(resp == 'y'){
		cout << "row_data: " << endl;
		for(int i=0; i<matrix.size(); i++){
			for(int j=0; j<matrix[i].size(); j++){
				cout << matrix[i][j] << "  ";
			}
			cout << endl;
		}
	}

	// number of rows of the raw data
	int N = matrix.size();

    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,1> > Block;  // 每个误差项优化变量维度为6，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
	Eigen::Matrix<double,6,1> para_initial;
	//Vector6d << 1.05, 0.95, 0.98, 1.5, -1.2, 0.7;
    para_initial << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0;
	v->setEstimate( para_initial );
    v->setId(0);
    optimizer.addVertex( v );

    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( matrix[i][0], matrix[i][1], matrix[i][2] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( 9.8*9.8 );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity() ); // 信息矩阵：协方差矩阵之逆
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;  // Huber function (kill outlier data)
        // rk->setDelta(1.0);
        cout << rk->delta() << endl;
        edge->setRobustKernel(rk);
        optimizer.addEdge( edge );
    }

    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(50);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出优化值
    Eigen::Matrix<double,6,1> para_estimate = v->estimate();
    cout << "----- Output the result:" << endl;
	cout << "	The mathematical model: " << endl;
	cout << "	ax_cal = sx * (ax_raw + ba)" << endl
		 << "	ay_cal = sy * (ay_raw + by)" << endl
		 << "	az_cal = sz * (az_raw + bz)" << endl << endl;
	cout << "	sx = " << para_estimate(0,0) << endl;
	cout << "	sy = " << para_estimate(1,0) << endl;
	cout << "	sz = " << para_estimate(2,0) << endl;
	cout << "	bx = " << para_estimate(3,0) << endl;
	cout << "	by = " << para_estimate(4,0) << endl;
	cout << "	bz = " << para_estimate(5,0) << endl;

    return 0;
}


#ifndef SKYLARK_CONDEST_HPP
#define SKYLARK_CONDEST_HPP

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

#include "../base/base.hpp"
#include "../utility/typer.hpp"
#include "../utility/external/print.hpp"

extern "C" {

void EL_BLAS(dbdsqr)(const char *, const El::Int *, const El::Int *,
    const El::Int *, const El::Int *, double *, double *, double *,
    const El::Int *, double *, const El::Int *,
    double *, const El::Int *, double *, El::Int *);

}

namespace skylark {
namespace nla {

struct condest_params_t : public base::params_t {

    int iter_lim;

    // See paper for meaning of these.
    int powerits;
    double c1, c2, c3, c4, c1t;

    condest_params_t(int iter_lim = 1000,
        bool am_i_printing = 0,
        int log_level = 0,
        std::ostream &log_stream = std::cout,
        std::string prefix = "",
        int debug_level = 0) :
        base::params_t(am_i_printing, log_level, log_stream, prefix, debug_level),
        iter_lim(iter_lim) {

        const double em = std::numeric_limits<double>::epsilon();
        c1 = 8 * em; c2 = 1e-3; c3 = 64.0 / em;
        c4 = std::sqrt(em); c1t = 4 * em;
        powerits = 300;
  }

};

/**
 * Estimates the condition number (with certificates) of a matrix
 * using an iterative algorith. Based on the following paper:
 *
 * Haim Avron, Alex Druinsky and Sivan Toledo
 * Spectral Condition-Number Estimation of Large Sparse Matrices
 *
 * \param A Input matrix
 * \param cond Output condition number estimation
 * \param sigma_max,v_max,u_max Estiamte of largest singular value and
 *                               right, left certificates. That is,
 *                               sigma_max * u_max = A * v_max / sigma_max,
 *                               ||v_max|| = ||u_max|| = 1,
 * \param sigma_min Best estimate for smallest singular value.
 * \param sigma_min_c,v_min,u_min Estimate of smallest singular value with
 *                                certificate
 * \param context Skylark context
 * \param params Parameters.
 */
template<typename MatrixType, typename LeftType, typename RightType>
int CondEst(const MatrixType& A, double &cond,
    double &sigma_max, RightType &v_max, LeftType &u_max,
    double &sigma_min, double &sigma_min_c, RightType &v_min, LeftType &u_min,
    base::context_t &context, condest_params_t params = condest_params_t()) {

    typedef typename utility::typer_t<MatrixType>::value_type value_t;
    typedef typename utility::typer_t<MatrixType>::index_type index_t;

    typedef MatrixType matrix_type;
    typedef RightType right_type;        // Also serves as "long" vector type.
    typedef LeftType left_type;        // Also serves as "short" vector type.

    typedef utility::print_t<right_type> rhs_print_t;
    typedef utility::print_t<left_type> sol_print_t;

    bool log_lev1 = params.am_i_printing && params.log_level >= 1;
    bool log_lev2 = params.am_i_printing && params.log_level >= 2;

    /** Throughout, we will use m, n to denote the problem dimensions */
    index_t m = base::Height(A);
    index_t n = base::Width(A);

    double c1 = params.c1, c2 = params.c2, c3 = params.c3;
    double c4 = params.c4, c1t = params.c1t;

    /** Estimate the largest singular vector using power-iteration */
    base::GaussianMatrix(v_max, n, 1, context);
    PowerIteration(El::NORMAL, El::NORMAL, El::NORMAL,
        A, v_max, u_max, params.powerits, true);
    sigma_max = El::Nrm2(u_max);
    El::Scale(1.0 / sigma_max, u_max);

    sigma_min = sigma_max;
    u_min = u_max;
    v_min = v_max;

    if (log_lev2)
        params.log_stream << params.prefix
                          << "CondEst: sigma_max = " << sigma_max
                          << std::endl;

    /** Generate xhat, and figure out tau */
    right_type xhat;
    base::GaussianMatrix(xhat, n, 1, context);
    double nrm_xhat = El::Nrm2(xhat);
    double tau = std::sqrt(2) * boost::math::erf_inv(c2) / nrm_xhat;
    El::Scale(1.0 / nrm_xhat, xhat);

    if (log_lev2)
        params.log_stream << params.prefix
                          << "CondEst: tau = " << tau << std::endl;

    /** Generate b, and iteration x */
    left_type b(m, 1);
    base::Gemm(El::NORMAL, El::NORMAL, 1.0, A, xhat, b);
    right_type x(n, 1);
    double nrm_b = El::Nrm2(b);

    /** Initialize everything */
    right_type u(b);
    double beta, i_beta;
    beta = El::Nrm2(u);
    El::Scale(1.0 / beta, u);
    rhs_print_t::apply(u, "u Init", params.am_i_printing, params.debug_level);

    left_type v(n, 1);
    base::Gemm(El::ADJOINT, El::NORMAL, 1.0, A, u, v);
    double alpha;
    alpha = El::Nrm2(v);
    El::Scale(1.0 / alpha, v);
    sol_print_t::apply(v, "v Init", params.am_i_printing, params.debug_level);

    /* Create w=v and x=0 */
    El::Zero(x);
    left_type w(v);
    double phibar = beta, rhobar = alpha, nrm_r;

    /* Reset the iteration limit if none was specified */
    if (0>params.iter_lim) params.iter_lim = std::max(20, 2*std::min(m,n));

    /* More varaibles */
    left_type Au(n, 1);
    double minus_beta, rho;
    double cs, sn, theta, phi;
    double phi_by_rho, minus_theta_by_rho;
    right_type d(n, 1);
    left_type Ad(m, 1);
    std::vector<double> Rdiag, Rsub;

    /** Main iteration loop */
    index_t T = params.iter_lim;
    int retval = -6;
    for (index_t itn=0; itn < T; ++itn) {

        /** 1. Update u and beta */
        alpha = -alpha;
        El::Scale(alpha, u);
        base::Gemm(El::NORMAL, El::NORMAL, 1.0, A, v, 1.0, u);
        beta = El::Nrm2(u);
        El::Scale(1.0 / beta, u);

        /** 2. Update v */
        minus_beta = -beta;
        El::Scale(minus_beta, v);
        base::Gemm(El::ADJOINT, El::NORMAL, 1.0, A, u, Au);
        base::Axpy(1.0, Au, v);
        alpha = El::Nrm2(v);
        El::Scale(1.0 / alpha, v);

       /** 3. Update variables, store parts of R */
        rho = sqrt((rhobar*rhobar) + (beta*beta));

        Rdiag.push_back(rho);
        if (itn > 0)
            Rsub.push_back(theta);

        cs = rhobar/rho;
        sn =  beta/rho;
        theta = sn*alpha;
        rhobar = -cs*alpha;
        phi = cs*phibar;
        phibar =  sn*phibar;

        /** 4. Update x and w */
        phi_by_rho = phi/rho;
        base::Axpy(phi_by_rho, w, x);
        sol_print_t::apply(x, "x", params.am_i_printing, params.debug_level);

        minus_theta_by_rho = -theta/rho;
        El::Scale(minus_theta_by_rho, w);
        base::Axpy(1.0, v, w);
        sol_print_t::apply(w, "w", params.am_i_printing, params.debug_level);

        /** 5. Compute forward error */
        d = xhat;
        El::Axpy(-1.0, x, d);
        double nrm_d = El::Nrm2(d);
        if (nrm_d == 0.0) {
            cond = 1.0;
            sigma_min = sigma_max;
            u_min = u_max;
            v_min = v_max;
            if (log_lev1)
                params.log_stream << params.prefix
                                  << "CondEst: Detected condition number 1"
                                  << std::endl;
            return -1;
        }

        /** 6. Compute current estimate of sigma_min using d, and compare it */
        base::Gemm(El::NORMAL, El::NORMAL, 1.0, A, d, Ad);
        double nrm_ad = El::Nrm2(Ad);
        if (nrm_ad <= sigma_min * nrm_d) {
            sigma_min = nrm_ad / nrm_d;
            v_min = d;
            u_min = Ad;
            El::Scale(1.0 / nrm_ad, u_min);
        }

        /** 7. Check if parameters need to be tuned */
        if (c1 != c1t && sigma_min / sigma_max <= c4 ) {
            if (log_lev1)
                params.log_stream
                    << params.prefix
                    << "CondEst: Highly ill-conditioned, C1 adjusted (C4)"
                    << std::endl;
            c1 = c1t;
        }

        /** 8. Test various stopping criteria */
        double nrm_x = El::Nrm2(x);
        if (T == params.iter_lim &&
            nrm_ad <= c1 * (sigma_max * nrm_x + nrm_b)) {
            if (log_lev1)
                params.log_stream << params.prefix
                                  << "CondEst: Convergence detected (C1)"
                                  << std::endl;
            T = 1.25 * itn + 1;
            retval = -2;
        }

        if (T == params.iter_lim && nrm_d <= tau) {
            if (log_lev1)
                params.log_stream << params.prefix
                                  << "CondEst: Convergence detected (C2)"
                                  << std::endl;
            T = 1.25 * itn + 1;
            retval = -3;
        }

        if (T == params.iter_lim && sigma_max / sigma_min >= c3) {
            if (log_lev1)
                params.log_stream << params.prefix
                                  << "CondEst: Singular?, stopping (C3)"
                                  << std::endl;
            T = 1.25 * itn + 1;
            retval = -4;
        }

        if (log_lev2)
            params.log_stream << params.prefix
                              << "CondEst: Iteration " << itn
                              << " sigma_min = " << sigma_min
                              << " cond = " << sigma_max / sigma_min
                              << " nrm_d = " << nrm_d
                              << std::endl;
    }

    if (log_lev1 && retval == -6)
        params.log_stream << params.prefix
                          << "CondEst: No convergence within iteration limit."
                          << std::endl;

    /** Estimate condition number using R */
    El::Int N = Rdiag.size(), izero = 0, ione = 1, info = 0;
    std::vector<double> workspace(4 * N);
    EL_BLAS(dbdsqr)("Upper", &N, &izero, &izero, &izero, &Rdiag[0], &Rsub[0],
        nullptr, &ione, nullptr, &ione, nullptr, &ione, &workspace[0], &info);
    double sigma_min_R = Rdiag[Rdiag.size() - 1];

    if (log_lev2)
        params.log_stream << params.prefix
                          << "CondEst: R sigma_min = " << sigma_min_R
                          << std::endl;

    sigma_min_c = sigma_min;
    if (sigma_min_R < sigma_min)
        sigma_min = sigma_min_R;

    cond = sigma_max / sigma_min;

    return retval;
}

} } /** namespace skylark::nla */

#endif // SKYLARK_NLA_HPP


#include <NTL/ZZ.h>
#include <cstdint>
#include <cmath>

#define NUM_BITS_REAL_MANTISSA 1024
#define IGNORE_DECODING_COST 0
// #define EXPLORE_REPRS

#include "binomials.hpp"
#include "isd_cost_estimate.hpp"
#include <cmath>

int main(int argc, char* argv[]){
  if(argc != 6){
     std::cout << "Work factor computation for ISD" << std::endl << " Usage "
               << argv[0] << " <codeword_size> <code_dimension> <number_of_errors> <qc_block_size> <is_kra>" << std::endl <<
               "<qc_block_size> = 1 implies a non QC code " << std::endl <<
               "<is_kra> = the attack is a key recovery attack on a QC-[L|M]DPC " << std::endl;
    return -1;
  }

  InitBinomials();
  NTL::RR::SetPrecision(NUM_BITS_REAL_MANTISSA);
  pi = NTL::ComputePi_RR();
  uint32_t n = atoi(argv[1]);
  uint32_t k = atoi(argv[2]);
  uint32_t t = atoi(argv[3]);
  uint32_t qc_block_size = atoi(argv[4]);
  uint32_t is_kra = atoi(argv[5]);

  /* reduce by a factor matching the QC block size */
  std::cout << "Minimum classic cost :" << c_isd_log_cost(n,k,t,qc_block_size,is_kra) << " Minimum quantum cost :" << q_isd_log_cost(n,k,t,qc_block_size,is_kra);
  if(qc_block_size !=1) std::cout << " (including qc_effects) ";
  std::cout << std::endl;
  return 0;
}


#ifndef HEADER_Bezier
#define HEADER_Bezier

#include <armadillo>
#include "ControlPoint.hpp"
#include "Element.hpp"

#include <boost/math/special_functions/factorials.hpp>
#include <memory>
#include <iostream>
#include <RigidBodyKinematics.hpp>

#include <set>
#include <map>

class ControlPoint;

template <class PointType>
class ShapeModel;

class Bezier : public Element{

public:

	/**
	Constructor
	@param vertices pointer to vector storing the vertices owned by this facet
	*/
	Bezier(std::vector<int> vertices,ShapeModel<ControlPoint> * owning_shape);

	/**
	Get neighbors
	@param if false, only return neighbors sharing an edge. Else, returns all neighbords
	@return Pointer to neighboring facets, plus the calling facet
	*/
	virtual std::set< int > get_neighbors(bool all_neighbors) const;

	std::set < int > get_neighbors(double u, double v) const;

	/**
	Returns pointer to the first vertex owned by $this that is
	neither $v0 and $v1. When $v0 and $v1 are on the same edge,
	this method returns a pointer to the vertex of $this that is not
	on the edge but still owned by $this
	@param v0 Pointer to first vertex to exclude
	@param v1 Pointer to first vertex to exclude
	@return Pointer to the first vertex of $this that is neither $v0 and $v1
	*/
	int vertex_not_on_edge(
		int v0,
		int v1) const ;

	/**
	Returns patch degree
	@param degree
	*/
	unsigned int get_degree() const;

	/**
	Evaluates the bezier patch at the barycentric
	coordinates (u,v). Note that 0<= u + v <= 1
	@param u first barycentric coordinate
	@param v second barycentric coordinate
	@return point at the surface of the bezier patch
	*/
	arma::vec::fixed<3> evaluate(const double u, const double v) const;

	/**
	Evaluates the normal of the bezier patch at the barycentric
	coordinates (u,v). Note that 0<= u + v <= 1
	@param u first barycentric coordinate
	@param v second barycentric coordinate
	@return point at the surface of the bezier patch
	*/
	arma::vec get_normal_coordinates(const double u, const double v) const;

	/**
	Get index of the queried point
	@param i first index
	@param j second index
	@return local index to control point
	*/
	int get_point_local_index(unsigned int i, unsigned int j) const;

	int get_point_global_index(unsigned int i, unsigned int j) const;

	/**
	Returns the control point given its i and j indices (k = n - i - j)
	@param i first index
	@param j second index
	@return global index of control point
	*/
	int get_point(unsigned int i, unsigned int j) const;

	/**
	Returns the tuple of local indices (i,j,k) of a control point within a bezier patch
	@param local_index local index of considered point
	@return local_indices (i,j,k)
	*/
	std::tuple<int,int,int> get_local_indices(int local_index) const;

	/**
	Returns the coordinates of a control point given its i and j indices (k = n - i - j)
	@param i first index
	@param j second index
	@return coordinats of contorl point
	*/
	const arma::vec::fixed<3> & get_point_coordinates(unsigned int i, unsigned int j) const;

	/**
	Returns P_X
	@return P_X matrix
	*/
	arma::mat get_P_X() const;

	/**
	Evaluates the partial derivative of Sum( B^n_{i,j,k}C_{ijk}) with respect to (u,v) evaluated
	at (u,v)
	@param u first coordinate
	@param v second coordinate
	*/
	arma::mat::fixed<3,2> partial_bezier(const double u,const double v) const;

	/**
	Returns the 3x3 covariance matrix
	tracking the uncertainty in the location of
	a surface point given uncertainty in the patch's control
	points
	@param u mean of first coordinate
	@param v mean of second coordinate
	@param dir direction of ray
	@param P_X covariance on the position
	of the control points
	@return 3x3 covariance
	*/
	arma::mat covariance_surface_point_deprecated(
		const double u,
		const double v,
		const arma::vec & dir,
		const arma::mat & P_X);

	/**
	Sets patch covariance to prescribed value
	@param P_X prescribed value of patch covariance
	*/

	void set_P_X(arma::mat P_X){this -> P_X = P_X;}

	/**
	Returns the 3x3 covariance matrix
	tracking the uncertainty in the location of
	a surface point given uncertainty in the patch's control
	points using an alternative formulation
	@param u mean of first coordinate
	@param v mean of second coordinate
	@param dir direction of ray
	of the control points
	@return 3x3 covariance
	*/
	arma::mat::fixed<3,3> covariance_surface_point(
		const double u,
		const double v,
		const arma::vec & dir) const;

	/**
	Returns the 3x3 covariance matrix
	tracking the uncertainty in the location of
	a surface point given uncertainty in the patch's control
	points using an alternative formulation
	@param u mean of first coordinate
	@param v mean of second coordinate
	@param dir direction of ray
	@param P_X covariance on the position
	of the control points
	@return 3x3 covariance
	*/
	arma::mat::fixed<3,3> covariance_surface_point(
		const double u,
		const double v,
		const arma::vec & dir,
		const arma::mat & P_X) const;

	/**
	Sets the covariance parametrization to the prescribed values
	@param covariance_param unique covariance parameters
	*/
	void set_patch_covariance(const std::vector<double> & covariance_param);

	/**
	Returns the triple product of points i_ = (i,j), j_ = = (k,l) and k_ = = (m,p), e.g Ci_^T(Cj_ x Ck_)
	@param i first index of first point
	@param j second index of first point
	@param k first index of second point
	@param l second index of second point
	@param m first index of third point
	@param p second index of third point
	*/
	double triple_product(const int i ,const int j ,const int k ,const int l ,const int m ,const int p ) const;

	/**
	Returns the triple product of points i_ = (i,j), j_ = = (k,l) and k_ = = (m,p), e.g Ci_^T(Cj_ x Ck_)
	@param i first index of first point
	@param j second index of first point
	@param k first index of second point
	@param l second index of second point
	@param m first index of third point
	@param p second index of third point
	*/
	double triple_product(const int i ,const int j ,const int k ,const int l ,const int m ,const int p ,
		const arma::vec & deviation) const;

	/**
	Computes the quadruple product of points i_ = (i,j), j_ = (k,l), k_ = (m,p), l_ = (q,r)  e.g (Ci_^T Cj_) * (Ck_ x Cl_)
	@param result container storing result of computation
	@param i first index of first point
	@param j second index of first point
	@param k first index of second point
	@param l second index of second point
	@param m first index of third point
	@param p second index of third point
	@param q first index of fourth point
	@param r second index of fourth point
	*/
	void quadruple_product(double * result,const int i ,const int j ,const int k ,const int l ,const int m ,const int p, const int q, const int r ) const;

	// Returns the partial derivative d^2P/(dchi dv)
	arma::mat::fixed<3,2> partial_bezier_dv(const double u,const double v) const;

	// Returns the partial derivative d^2P/(dchi du)
	arma::mat::fixed<3,2> partial_bezier_du(const double u,const double v) const;

	/**
	Evaluates the Berstein polynomial
	@param u first barycentric coordinate
	@param v first barycentric coordinate
	@param i first index
	@param j second index
	@param n polynomial order
	@return evaluated bernstein polynomial
	*/
	static double bernstein(
		const double u,
		const double v,
		const int i,
		const int j,
		const int n) ;

	/**
	Computes the partial derivative of the unit normal vector at the queried point
	with respect to a given control point
	@param u first barycentric coordinate
	@param v first barycentric coordinate
	@param i first index
	@param j second index
	@param n polynomial order
	*/
	arma::mat::fixed<3,3> partial_n_partial_Ck(
		const double u,
		const double v,
		const int i ,
		const int j,
		const int n) const;

	/**
	Returns the coefficient alpha_ijk for volume computation
	@param i first index of first triplet
	@param j second index of first triplet
	@param k first index of second triplet
	@param l second index of second triplet
	@param m first index of third triplet
	@param p second index of third triplet
	@param n patch degree
	@returm computed coefficient
	*/
	static double alpha_ijk(const int i, const int j, const int k, const int l, const int m, const int p,const int n);

	/**
	Returns the coefficient gamma_ijkl for center of mass computation
	@param i first index of first triplet
	@param j second index of first triplet
	@param k first index of second triplet
	@param l second index of second triplet
	@param m first index of third triplet
	@param p second index of third triplet
	@param q first index of fourth triplet
	@param r second index of fourth triplet
	@param n patch degree
	@returm computed coefficient
	*/
	static double gamma_ijkl(const int i, const int j, const int k, const int l, const int m, const int p,const int q, const int r, const int n);

	/**
	Returns the coefficient kappa_ijkl for inertia of mass computation
	@param i first index of first triplet
	@param j second index of first triplet
	@param k first index of second triplet
	@param l second index of second triplet
	@param m first index of third triplet
	@param p second index of third triplet
	@param q first index of fourth triplet
	@param r second index of fourth triplet
	@param s first index of fifth triplet
	@param t second index of fifth triplet
	@param n patch degree
	@returm computed coefficient
	*/
	static double kappa_ijklm(const int i, const int j, const int k, const int l,
		const int m, const int p,const int q, const int r,
		const int s, const int t, const int n);

	/**
	Returns the coefficient beta_ijkl for center of mass computation
	@param i first index of first triplet
	@param j second index of first triplet
	@param k first index of second triplet
	@param l second index of second triplet
	@param n patch degree
	@returm computed coefficient
	*/
	static double beta_ijkl( const int i,  const int j,  const int k, const  int l,  const int n);

	/**
	Returns the stacked crossed products
	@param i first index of first triplet
	@param j second index of first triplet
	@param k first index of second triplet
	@param l second index of second triplet
	@param m first index of third triplet
	@param p second index of third triplet
	@returm computed stacked cross-product
	*/
	arma::vec get_cross_products(const int i, const int j, const int k, const int l, const int m,const int p) const;

	/**
	Returns the augmented stacked crossed products
	@paran mat reference to matrix holding the stacked crossed products
	@param i first index of first triplet
	@param j second index of first triplet
	@param k first index of second triplet
	@param l second index of second triplet
	@param m first index of third triplet
	@param p second index of third triplet
	@param q first index of fourth triplet
	@param r second index of fourth triplet
	@returm computed stacked cross-product
	*/
	void get_augmented_cross_products(arma::mat::fixed<12,3> & mat,const int i, const int j, const int k, const int l, const int m,const int p,
		const int q, const int r) const;

	/**
	Generates the forward table associating a local index l to the corrsponding triplet (i,j,k)
	@param n degree
	@return forward look up table
	*/
	static std::vector<std::tuple< int,  int,  int> > forward_table( int n);

	/**
	Generates the reverse table associating a local index triplet (i,j,k) to the corresponding
	global index l
	@param n degree
	@return reverse look up table
	*/
	static std::map< std::tuple< int,  int,  int> , int> reverse_table( int n);

	/**
	Returns the number of combinations of k items among n.
	Returns 0 if k < 0 or k > n
	@param k subset size
	@param n set size
	@return number of combinations
	*/
	static int combinations(int k, int n);

	/**
	Returns the vector parametrization of the element covariance
	@return parametrization of the element covariance
	*/
	const arma::vec & get_P_X_param() const{return this -> P_X_param;}

protected:

	virtual void compute_normal();
	virtual void compute_area();
	virtual void compute_center();

	static double Sa_b(const int a, const int b);
	static double bernstein_coef(const int i , const int j , const int n);

	void construct_index_tables();

	/**
	Returns the partial derivative of the Bernstein polynomial B^n_{i,j,k} evaluated
	at (u,v)
	@param u first coordinate
	@param v second coordinate
	@param i first index
	@param j second index
	@param n polynomial degree
	@return evaluated partial derivative of the Bernstein polynomial
	*/
	static arma::rowvec::fixed<2> partial_bernstein(
		const double u,
		const double v,
		const int i,
		const int j,
		const int n) ;

	static arma::rowvec::fixed<2> partial_bernstein_dv(
		const double u,
		const double v,
		const int i ,
		const int j,
		const int n) ;

	static arma::rowvec::fixed<2> partial_bernstein_du(
		const double u,
		const double v,
		const int i ,
		const int j,
		const int n) ;

	int n;
	double fitting_residuals = 0;
	double fitting_residuals_mean = 0;

	std::vector<std::tuple< int,  int,  int> > forw_table;
	std::map< std::tuple< int,  int,  int> , int> rev_table;

	arma::mat P_X;
	std::vector<double> epsilons;
	arma::vec P_X_param;

};
#endif


/***************************************************************************
LibBandit - Multi-Armed Bandit Library
Written in 2015 by Tor Lattimore tor.lattimore@gmail.com

To the extent possible under law, the author(s) have dedicated all
copyright and related and neighboring rights to this software to the
public domain worldwide. This software is distributed without any warranty.

You should have received a copy of the CC0 Public Domain Dedication
along with this software. If not,
see http://creativecommons.org/publicdomain/zero/1.0/
***************************************************************************/
#include "algs.h"
#include "bandit.h"
#include "arm.h"
#include "gittins_table.h"

#include <cfloat>
#include <cmath>
#include <algorithm>
#include <iostream>
#include <limits>
#include <queue>
#include <list>

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

using namespace std;

/*************************************************************
GENERIC SIMULATOR
*************************************************************/
double IndexAlgorithm::sim(BanditProblem &bp, uint64_t horizon) {
  K = bp.K;
  n = horizon;

  bp.reset();
  arms.clear();

  for (uint64_t i = 0;i != K;++i) {
    arms.push_back(Arm(i, std::numeric_limits<double>::max()));
    arms.rbegin()->max_idx = std::numeric_limits<double>::max();
  }

  uint64_t t = 0;
  for (;t != K && t !=n;++t) {
    arms[t].pull(bp.choose(arms[t].i));
  }

  for (;t != n;++t) {
    auto best_idx = -numeric_limits<double>::max();
    auto best = arms.begin();
    auto last = arms.begin();

    for (auto a = arms.begin();a!=arms.end();++a) {
      if (a->max_idx < best_idx) {
        break;
      }
      last = a;
      set_index(a, t);

      if (a->idx > best_idx) {
        best_idx = a->idx;
        best = a;
      }
    }
    best->max_idx = numeric_limits<double>::max();
    best->pull(bp.choose(best->i));

    update(best);

    sort(arms.begin(), last+1, [](const Arm &a1, const Arm &a2) {return a1.max_idx > a2.max_idx;});
    inplace_merge(arms.begin(), last+1, arms.end(), [](const Arm &a1, const Arm &a2) {return a1.max_idx > a2.max_idx;});
  }
  return bp.get_regret();
}

void UCB::set_index(vector<Arm>::iterator a, uint64_t t) {
  a->idx = a->mean() + sqrt(alpha / a->T * log(t));
  a->max_idx = a->mean() + sqrt(alpha / a->T * log(n));
}

void MOSS::set_index(vector<Arm>::iterator a, uint64_t t) {
  a->idx = a->mean() + sqrt(2.0 / a->T * log(max(1.0, (double)n / (a->T * K))));
  a->max_idx = a->idx;
}

void OCUCB::set_index(vector<Arm>::iterator a, uint64_t t) {
  a->idx = a->mean() + sqrt(alpha / a->T * log(psi * (double)n / t));
//  a->max_idx = std::numeric_limits<double>::max();
  a->max_idx = a->idx;
}

void AnytimeOCUCB::set_index(vector<Arm>::iterator a, uint64_t t) {
  const double EULER = exp(1.0);
  a->idx =     a->mean() + sqrt(alpha / a->T * log(max(max(EULER, log(t+1.0)), log(t+1.0)  * (t+1.0) / lookup.lookup(a->i))));
  a->max_idx = a->mean() + sqrt(alpha / a->T * log(max(max(EULER, log(n+1.0)), log(n+1.0)  * (n+1.0) / lookup.lookup(a->i))));
}

void AnytimeOCUCB::update(vector<Arm>::iterator a) {
  lookup.update(a->i);
}

void OptAnytimeOCUCB::set_index(vector<Arm>::iterator a, uint64_t t) {
  a->idx =     a->mean() + sqrt(alpha / a->T * log(max(1.0, (t+1.0) / lookup.lookup(a->i))));
  a->max_idx =     a->mean() + sqrt(alpha / a->T * log(max(1.0, (n+1.0) / lookup.lookup(a->i))));
}

void OptAnytimeOCUCB::update(vector<Arm>::iterator a) {
  lookup.update(a->i);
}

void AOCUCB::set_index(vector<Arm>::iterator a, uint64_t t) {
  a->idx = a->mean() + sqrt(alpha / a->T * log((double)t / a->T));
  a->max_idx = a->mean() + sqrt(alpha / a->T * log((double)n / a->T));
}

void GaussianTS::set_index(vector<Arm>::iterator a, uint64_t t) {
  a->idx = a->mean() + dist(gen) / sqrt(a->T);
  a->max_idx = std::numeric_limits<double>::max();
}

void GaussianGittins::set_index(vector<Arm>::iterator a, uint64_t t) {
  a->idx = a->mean() + table.get_idx(n - t, a->T);
  a->max_idx = a->idx;
}

void GaussianGittinsApprox::set_index(vector<Arm>::iterator a, uint64_t t) {
  uint64_t m = n - t;
  double beta = max(1.0, min(m / pow(log(m), 1.5) / 4.0, m / 4.0 / a->T / pow(log(m/a->T), 0.5)));
  a->idx = a->mean() + sqrt(2.0 / a->T * log(beta));
  a->max_idx = a->idx;
}


/*! @file pitts_tensortrain_from_dense_classical.hpp
* @brief conversion of a dense tensor to the tensor-train format, classical TT-SVD algorithm
* @author Melven Roehrig-Zoellner <Melven.Roehrig-Zoellner@DLR.de>
* @date 2020-06-19
* @copyright Deutsches Zentrum fuer Luft- und Raumfahrt e. V. (DLR), German Aerospace Center
*
**/

// include guard
#ifndef PITTS_TENSORTRAIN_FROM_DENSE_CLASSICAL_HPP
#define PITTS_TENSORTRAIN_FROM_DENSE_CLASSICAL_HPP

// includes
#include <cstddef>
#include <functional>
#include <iterator>
#include <numeric>
#include <type_traits>
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options
#include "pitts_tensortrain.hpp"
#include "pitts_timer.hpp"

//! namespace for the library PITTS (parallel iterative tensor train solvers)
namespace PITTS
{
  //! calculate tensor-train decomposition of a tensor stored in fully dense format (slow classical TT-SVD)
  //!
  //! @tparam Iter      contiguous input iterator to access the dense data
  //! @tparam T         underlying data type (double, complex, ...)
  //!
  //! @param first          input iterator that points to the first index, e.g. std::begin(someContainer)
  //! @param last           input iterator that points behind the last index, e.g. std::end(someContainer)
  //! @param dimensions     tensor dimensions, input is interpreted in Fortran storage order (first index changes the fastest)
  //! @param rankTolerance  approximation accuracy, used to reduce the TTranks of the resulting tensor train
  //! @param maxRank        maximal TTrank (bond dimension), unbounded by default
  //! @return               resulting tensor train
  //!
  template<class Iter, typename T = std::iterator_traits<Iter>::value_type>
  TensorTrain<T> fromDense_classical(const Iter first, const Iter last, const std::vector<int>& dimensions, T rankTolerance = std::sqrt(std::numeric_limits<T>::epsilon()), int maxRank = -1)
  {
    // timer
    const auto timer = PITTS::timing::createScopedTimer<TensorTrain<T>>();

    // check that the input is contiguous in memory
    //static_assert(std::is_base_of< std::contiguous_iterator_tag, typename std::iterator_traits<Iter>::iterator_category >::value, "fromDense only works with contiguous iterators!");
    static_assert(std::is_base_of< std::random_access_iterator_tag, typename std::iterator_traits<Iter>::iterator_category >::value, "fromDense only works with contiguous iterators!");

    // abort early for zero dimensions
    if( dimensions.size() == 0 )
    {
      if( last - first != 0 )
        throw std::out_of_range("Mismatching dimensions in TensorTrain<T>::fromDense");
      return TensorTrain<T>{dimensions};
    }

    const auto totalSize = std::accumulate(begin(dimensions), end(dimensions), (std::ptrdiff_t)1, std::multiplies<std::ptrdiff_t>());
    if( totalSize != last - first )
      throw std::out_of_range("Mismatching dimensions in TensorTrain<T>::fromDense");

    TensorTrain<T> result(dimensions);

    using EigenMatrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
    EigenMatrix tmp = Eigen::Map<const EigenMatrix>(&(*first), 1, totalSize);
    for(int iDim = 0; iDim+1 < dimensions.size(); iDim++)
    {
      tmp.resize(tmp.rows()*dimensions[iDim], tmp.cols()/dimensions[iDim]);
      Eigen::BDCSVD<EigenMatrix> svd(tmp, Eigen::ComputeThinU | Eigen::ComputeThinV);
      svd.setThreshold(rankTolerance);
      int rank = svd.rank();
      if( maxRank > 0 )
        rank = std::min(rank, maxRank);

      auto& subT = result.editableSubTensors()[iDim];
      subT.resize(tmp.rows()/dimensions[iDim], dimensions[iDim], rank);
      for(int i = 0; i < subT.r1(); i++)
        for(int j = 0; j < subT.n(); j++)
          for(int k = 0; k < subT.r2(); k++)
            subT(i,j,k) = svd.matrixU()(i+j*subT.r1(), k);

      tmp.resize(rank, tmp.cols());
      tmp = svd.singularValues().topRows(rank).asDiagonal() * svd.matrixV().leftCols(rank).adjoint();
    }
    int lastDim = dimensions.size()-1;
    auto& lastSubT = result.editableSubTensors()[lastDim];
    tmp.resize(tmp.size()/dimensions[lastDim], dimensions[lastDim]);
    lastSubT.resize(tmp.rows(), tmp.cols(), 1);
    for(int j = 0; j < tmp.cols(); j++)
      for(int i = 0; i < tmp.rows(); i++)
        lastSubT(i, j, 0) = tmp(i, j);

    return result;
  }

}

#endif // PITTS_TENSORTRAIN_FROM_DENSE_CLASSICAL_HPP


/* Copyright 2020 Oinam Romesh Meitei

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>
#include <cmath>
#include <vector>

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>

#include "getham.h"

namespace py = pybind11;

Eigen::MatrixXcd solve_trotter(
			       std::vector<double> &tlist,
			       std::vector<std::complex<double> > &ini_vec,
			       pulsec pobj,
			       std::vector< std::vector< Eigen::SparseMatrix<double,0,ptrdiff_t> > > hdrive,
			       std::vector< std::complex<double> > dsham){

  Eigen::SparseMatrix<std::complex<double> > H_ ;
  Eigen::MatrixXcd H1_;
  int dsham_len = dsham.size();
  Eigen::SparseMatrix<std::complex<double> >
    matexp_(dsham_len, dsham_len);

  Eigen::Map<Eigen::VectorXcd> trot_(ini_vec.data(), ini_vec.size());
  int tlen = tlist.size();
  double tau = tlist[tlen-1] / tlen;
  std::complex<double> im(0.0,-tau);

  for (int t=0; t<tlen; t++){
    H_ = getham(tlist[t], pobj, hdrive, dsham, dsham_len, matexp_);
    H1_ = im * Eigen::MatrixXcd(H_);

    trot_ = H1_.exp() * trot_;
  }

  return trot_;
}

Eigen::MatrixXcd solve_trotter2(
			std::vector<double> &tlist,
			std::vector<std::complex<double> > &ini_vec,
			pulsec pobj,
			std::vector< std::vector< Eigen::SparseMatrix
			<double,0,ptrdiff_t> > > hdrive,
			std::vector< std::complex<double> > dsham){

  Eigen::SparseMatrix<std::complex<double> > H_ ;
  Eigen::MatrixXcd H1_;

  int dsham_len = dsham.size();
  int tlen = tlist.size();

  Eigen::SparseMatrix<std::complex<double> > hamdr;
  Eigen::SparseMatrix<std::complex<double> > hamdR;
  std::complex<double> hcoef;

  Eigen::SparseMatrix<std::complex<double> >
    matexp_(dsham_len, dsham_len);

  Eigen::Map<Eigen::VectorXcd> trot_(ini_vec.data(), ini_vec.size());

  double tau = tlist[tlen-1] / tlen;
  std::complex<double> im(0.0,-tau);

  for (int t=0; t<tlen; t++){
    H_ = getham3( tlist[t], t, pobj, hdrive, dsham, dsham_len, matexp_,
		  hamdr, hamdR, hcoef);
    H1_ = im * Eigen::MatrixXcd(H_);
    trot_ = H1_.exp() * trot_;
  }
  return trot_;
}

PYBIND11_MODULE(trotter,m){
  m.def("solve_trotter", &solve_trotter, "trotter");
  m.def("solve_trotter2", &solve_trotter2, "trotter2");
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2010 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Andrea Bonito, Sebastian Pauletti.
 */

// @sect3{Include files}

// 如果你读过 step-4 和 step-7 ，你会认识到我们已经在那里使用了以下所有的包含文件。因此，我们不会在这里再次解释它们的含义。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>

#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/mapping_q.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

#include <fstream>
#include <iostream>

namespace Step38
{
  using namespace dealii;
// @sect3{The LaplaceBeltramiProblem class template}

//这个类几乎与 step-4 中的 LaplaceProblem 类完全相似。

//本质上的区别是这样的。

// - 模板参数现在表示嵌入空间的维度，它不再与域和我们计算的三角形的维度相同。我们通过调用参数 @p spacedim, 并引入一个等于域的维度的常数 @p dim 来表明这一点--这里等于 spacedim-1  。

// - 所有具有几何特征的成员变量现在都需要知道它们自己的维度以及嵌入空间的维度。因此，我们需要指定它们的模板参数，一个是网格的维度 @p dim, ，另一个是嵌入空间的维度， @p spacedim.  这正是我们在 step-34 中所做的，请看那里有更深的解释。

// - 我们需要一个对象来描述从参考单元到三角形组成的单元所使用的哪种映射。从Mapping基类派生出来的类正是这样做的。在deal.II的大部分时间里，如果你不做任何事情，图书馆会假定你想要一个使用（双，三）线性映射的MappingQ1对象。在许多情况下，这就足够了，这就是为什么这些对象的使用大多是可选的：例如，如果你有一个二维空间中的多边形二维域，参考单元到三角形单元的双线性映射会产生该域的精确表示。如果你有一个弯曲的域，你可能想对那些位于域的边界的单元使用一个高阶映射--例如，这就是我们在 step-11 中所做的。然而，在这里我们有一个弯曲的域，而不仅仅是一个弯曲的边界，虽然我们可以用双线性映射的单元来近似它，但对所有单元使用高阶映射才是真正谨慎的。因此，这个类有一个MappingQ类型的成员变量；我们将选择映射的多项式程度等于计算中使用的有限元的多项式程度，以确保最佳近似，尽管这种等参数性不是必须的。

  template <int spacedim>
  class LaplaceBeltramiProblem
  {
  public:
    LaplaceBeltramiProblem(const unsigned degree = 2);
    void run();

  private:
    static constexpr unsigned int dim = spacedim - 1;

    void make_grid_and_dofs();
    void assemble_system();
    void solve();
    void output_results() const;
    void compute_error() const;

    Triangulation<dim, spacedim> triangulation;
    FE_Q<dim, spacedim>          fe;
    DoFHandler<dim, spacedim>    dof_handler;
    MappingQ<dim, spacedim>      mapping;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> system_rhs;
  };
// @sect3{Equation data}

// 接下来，让我们定义描述问题的精确解和右手边的类。这与 step-4 和 step-7 相类似，在那里我们也定义了此类对象。鉴于介绍中的讨论，实际的公式应该是不言自明的。值得关注的一点是，我们是如何使用一般模板的明确特化，分别定义2D和3D情况下的值和梯度函数的。另一种方法是定义通用模板，并为空间维度的每个可能的值设置一个 switch 语句（或一串 if s）。

  template <int dim>
  class Solution : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

    virtual Tensor<1, dim>
    gradient(const Point<dim> & p,
             const unsigned int component = 0) const override;
  };

  template <>
  double Solution<2>::value(const Point<2> &p, const unsigned int) const
  {
    return (-2. * p(0) * p(1));
  }

  template <>
  Tensor<1, 2> Solution<2>::gradient(const Point<2> &p,
                                     const unsigned int) const
  {
    Tensor<1, 2> return_value;
    return_value[0] = -2. * p(1) * (1 - 2. * p(0) * p(0));
    return_value[1] = -2. * p(0) * (1 - 2. * p(1) * p(1));

    return return_value;
  }

  template <>
  double Solution<3>::value(const Point<3> &p, const unsigned int) const
  {
    return (std::sin(numbers::PI * p(0)) * std::cos(numbers::PI * p(1)) *
            exp(p(2)));
  }

  template <>
  Tensor<1, 3> Solution<3>::gradient(const Point<3> &p,
                                     const unsigned int) const
  {
    using numbers::PI;

    Tensor<1, 3> return_value;

    return_value[0] = PI * cos(PI * p(0)) * cos(PI * p(1)) * exp(p(2));
    return_value[1] = -PI * sin(PI * p(0)) * sin(PI * p(1)) * exp(p(2));
    return_value[2] = sin(PI * p(0)) * cos(PI * p(1)) * exp(p(2));

    return return_value;
  }

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };

  template <>
  double RightHandSide<2>::value(const Point<2> &p,
                                 const unsigned int /*component*/) const
  {
    return (-8. * p(0) * p(1));
  }

  template <>
  double RightHandSide<3>::value(const Point<3> &p,
                                 const unsigned int /*component*/) const
  {
    using numbers::PI;

    Tensor<2, 3> hessian;

    hessian[0][0] = -PI * PI * sin(PI * p(0)) * cos(PI * p(1)) * exp(p(2));
    hessian[1][1] = -PI * PI * sin(PI * p(0)) * cos(PI * p(1)) * exp(p(2));
    hessian[2][2] = sin(PI * p(0)) * cos(PI * p(1)) * exp(p(2));

    hessian[0][1] = -PI * PI * cos(PI * p(0)) * sin(PI * p(1)) * exp(p(2));
    hessian[1][0] = -PI * PI * cos(PI * p(0)) * sin(PI * p(1)) * exp(p(2));

    hessian[0][2] = PI * cos(PI * p(0)) * cos(PI * p(1)) * exp(p(2));
    hessian[2][0] = PI * cos(PI * p(0)) * cos(PI * p(1)) * exp(p(2));

    hessian[1][2] = -PI * sin(PI * p(0)) * sin(PI * p(1)) * exp(p(2));
    hessian[2][1] = -PI * sin(PI * p(0)) * sin(PI * p(1)) * exp(p(2));

    Tensor<1, 3> gradient;
    gradient[0] = PI * cos(PI * p(0)) * cos(PI * p(1)) * exp(p(2));
    gradient[1] = -PI * sin(PI * p(0)) * sin(PI * p(1)) * exp(p(2));
    gradient[2] = sin(PI * p(0)) * cos(PI * p(1)) * exp(p(2));

    Point<3> normal = p;
    normal /= p.norm();

    return (-trace(hessian) + 2 * (gradient * normal) +
            (hessian * normal) * normal);
  }
// @sect3{Implementation of the LaplaceBeltramiProblem class}

// 如果你知道  step-4  ，程序的其余部分实际上是很不引人注目的。我们的第一步是定义构造函数，设置有限元和映射的多项式程度，并将DoF处理程序与三角形关联。

  template <int spacedim>
  LaplaceBeltramiProblem<spacedim>::LaplaceBeltramiProblem(
    const unsigned degree)
    : fe(degree)
    , dof_handler(triangulation)
    , mapping(degree)
  {}
// @sect4{LaplaceBeltramiProblem::make_grid_and_dofs}

// 下一步是创建网格，分配自由度，并设置描述线性系统的各种变量。所有这些步骤都是标准的，只有如何创建一个描述曲面的网格除外。我们可以为我们感兴趣的领域生成一个网格，用一个网格生成器生成一个三角形，然后用GridIn类将其读入。或者，就像我们在这里做的那样，我们使用GridGenerator命名空间的设施来生成网格。

// 具体来说，我们要做的是这样的（在下面的大括号中）：我们使用 spacedim 函数为半圆盘（2D）或半球（3D）生成一个 GridGenerator::half_hyper_ball 维度的网格。这个函数将位于圆盘/球周边的所有面的边界指标设置为零，而在将整个圆盘/球分成两半的直线部分设置为零。下一步是主要的一点。 GridGenerator::extract_boundary_mesh 函数创建的网格是由那些作为前一个网格的面的单元组成的，也就是说，它描述了原始（体积）网格的surface单元。然而，我们不需要所有的面：只需要那些在圆盘或球的周边，边界指示器为零的面；我们可以使用一组边界指示器来选择这些单元，并传递给 GridGenerator::extract_boundary_mesh. 。

// 有一点需要提及。为了在流形是弯曲的情况下适当地细化表面网格（类似于细化与弯曲边界相邻的单元面），三角形必须要有一个对象附加在上面，描述新顶点应该位于何处。如果你不附加这样的边界对象，它们将位于现有顶点之间的中间位置；如果你有一个具有直线边界的域（例如多边形），这是很合适的，但如果像这里一样，流形具有曲率，则不合适。因此，为了让事情正常进行，我们需要将流形对象附加到我们的（表面）三角形上，其方式与我们在1d中为边界所做的大致相同。我们创建这样一个对象，并将其附加到三角剖面上。

// 创建网格的最后一步是对其进行多次细化。该函数的其余部分与之前的教程程序中相同。

  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::make_grid_and_dofs()
  {
    {
      Triangulation<spacedim> volume_mesh;
      GridGenerator::half_hyper_ball(volume_mesh);

      std::set<types::boundary_id> boundary_ids;
      boundary_ids.insert(0);

      GridGenerator::extract_boundary_mesh(volume_mesh,
                                           triangulation,
                                           boundary_ids);
    }
    triangulation.set_all_manifold_ids(0);
    triangulation.set_manifold(0, SphericalManifold<dim, spacedim>());

    triangulation.refine_global(4);

    std::cout << "Surface mesh has " << triangulation.n_active_cells()
              << " cells." << std::endl;

    dof_handler.distribute_dofs(fe);

    std::cout << "Surface mesh has " << dof_handler.n_dofs()
              << " degrees of freedom." << std::endl;

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }
// @sect4{LaplaceBeltramiProblem::assemble_system}

// 下面是这个程序的中心函数，即组装与表面拉普拉斯（Laplace-Beltrami算子）相对应的矩阵。也许令人惊讶的是，它实际上与例如在  step-4  中讨论的普通拉普拉斯算子看起来完全一样。关键是 FEValues::shape_grad() 函数发挥了魔力：它返回 $i$ 第1个形状函数在 $q$ 第1个正交点的表面梯度 $\nabla_K \phi_i(x_q)$ 。其余的也不需要任何改变。

  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::assemble_system()
  {
    system_matrix = 0;
    system_rhs    = 0;

    const QGauss<dim>       quadrature_formula(2 * fe.degree);
    FEValues<dim, spacedim> fe_values(mapping,
                                      fe,
                                      quadrature_formula,
                                      update_values | update_gradients |
                                        update_quadrature_points |
                                        update_JxW_values);

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     cell_rhs(dofs_per_cell);

    std::vector<double>                  rhs_values(n_q_points);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    RightHandSide<spacedim> rhs;

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0;
        cell_rhs    = 0;

        fe_values.reinit(cell);

        rhs.value_list(fe_values.get_quadrature_points(), rhs_values);

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
              cell_matrix(i, j) += fe_values.shape_grad(i, q_point) *
                                   fe_values.shape_grad(j, q_point) *
                                   fe_values.JxW(q_point);

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
            cell_rhs(i) += fe_values.shape_value(i, q_point) *
                           rhs_values[q_point] * fe_values.JxW(q_point);

        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              system_matrix.add(local_dof_indices[i],
                                local_dof_indices[j],
                                cell_matrix(i, j));

            system_rhs(local_dof_indices[i]) += cell_rhs(i);
          }
      }

    std::map<types::global_dof_index, double> boundary_values;
    VectorTools::interpolate_boundary_values(
      mapping, dof_handler, 0, Solution<spacedim>(), boundary_values);

    MatrixTools::apply_boundary_values(
      boundary_values, system_matrix, solution, system_rhs, false);
  }

//  @sect4{LaplaceBeltramiProblem::solve}

// 下一个函数是解决线性系统的函数。在这里，也不需要做任何改变。

  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::solve()
  {
    SolverControl solver_control(solution.size(), 1e-7 * system_rhs.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    PreconditionSSOR<SparseMatrix<double>> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve(system_matrix, solution, system_rhs, preconditioner);
  }

//  @sect4{LaplaceBeltramiProblem::output_result}

// 这是一个从解决方案中生成图形输出的函数。它的大部分都是模板代码，但有两点值得指出。

// -  DataOut::add_data_vector() 函数可以接受两种向量。  一种是之前通过 DataOut::attach_dof_handler(); 连接的DoFHandler对象定义的每个自由度有一个值的向量，另一种是三角测量的每个单元有一个值的向量，例如，输出每个单元的估计误差。通常，DataOut类知道如何区分这两种向量：自由度几乎总是比单元格多，所以我们可以通过两种向量的长度来区分。我们在这里也可以这样做，但只是因为我们很幸运：我们使用了一个半球体。如果我们用整个球体作为域和 $Q_1$ 元素，我们将有相同数量的单元格作为顶点，因此这两种向量将有相同数量的元素。为了避免由此产生的混乱，我们必须告诉 DataOut::add_data_vector() 函数我们有哪种矢量。DoF数据。这就是该函数的第三个参数的作用。

// -  DataOut::build_patches() 函数可以生成细分每个单元的输出，这样可视化程序可以更好地解决弯曲流形或更高的多项式程度的形状函数。在这里，我们在每个坐标方向上对每个元素进行细分，细分的次数与使用的有限元的多项式程度相同。

  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::output_results() const
  {
    DataOut<dim, spacedim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution,
                             "solution",
                             DataOut<dim, spacedim>::type_dof_data);
    data_out.build_patches(mapping, mapping.get_degree());

    const std::string filename =
      "solution-" + std::to_string(spacedim) + "d.vtk";
    std::ofstream output(filename);
    data_out.write_vtk(output);
  }

//  @sect4{LaplaceBeltramiProblem::compute_error}

// 这是最后一块功能：我们要计算数值解的误差。它是之前在  step-7  中展示和讨论的代码的逐字复制。正如介绍中提到的， Solution 类提供了解决方案的（切向）梯度。为了避免只评估超收敛点的误差，我们选择一个足够高阶的正交规则。

  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::compute_error() const
  {
    Vector<float> difference_per_cell(triangulation.n_active_cells());
    VectorTools::integrate_difference(mapping,
                                      dof_handler,
                                      solution,
                                      Solution<spacedim>(),
                                      difference_per_cell,
                                      QGauss<dim>(2 * fe.degree + 1),
                                      VectorTools::H1_norm);

    double h1_error = VectorTools::compute_global_error(triangulation,
                                                        difference_per_cell,
                                                        VectorTools::H1_norm);
    std::cout << "H1 error = " << h1_error << std::endl;
  }

//  @sect4{LaplaceBeltramiProblem::run}

// 最后一个函数提供了顶层的逻辑。它的内容是不言自明的。

  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::run()
  {
    make_grid_and_dofs();
    assemble_system();
    solve();
    output_results();
    compute_error();
  }
} // namespace Step38
// @sect3{The main() function}

// 该程序的其余部分由 main() 函数占据。它完全遵循首次在 step-6 中介绍的一般布局，并在随后的所有教程程序中使用。

int main()
{
  try
    {
      using namespace Step38;

      LaplaceBeltramiProblem<3> laplace_beltrami;
      laplace_beltrami.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include "robot_interface.hpp"

#include <iostream>
#include <cmath>
#include <ur5/rcg/declarations.h>
#include <ur5/rcg/forward_dynamics.h>

#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/integrate/integrate.hpp>
#include <boost/numeric/odeint/integrate/integrate_n_steps.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/external/eigen/eigen.hpp>

using namespace ur5;
using namespace ur5::rcg;
namespace odeint = boost::numeric::odeint;

using state = Eigen::Matrix<double, 12, 1>;
#define X  block<6,1>(0,0)
#define XD block<6,1>(6,0)

/* define a structure for holding the block local state. By assigning an
 * instance of this struct to the block private_data pointer (see init), this
 * information becomes accessible within the hook functions.
 */
struct robot_interface_info
{
	InertiaProperties ip;
	MotionTransforms xm;
 	ForwardDynamics fd;

	robot_interface_info() : ip(), xm(), fd(ip,xm) {
	    reset();
	}

	void reset() {
	    st0.setZero();
	    tau.setZero();
	    time = 0;
	}

	int joint_state_size = 0;
	state st0;
	JointState tau;

    double time = 0;

	/* this is to have fast access to ports for reading and writing, without
	 * needing a hash table lookup */
	struct robot_interface_port_cache ports;

	/* The rhs of x' = f(x) */
    void dyn( const state& st, state& dst_dt, const double time )
    {
        JointState qdd;
        fd.fd(qdd, st.X, st.XD, tau);
        dst_dt.X  = st.XD;
        dst_dt.XD = qdd;
    }
};

inline robot_interface_info& private_data(ubx_block_t *b)
{
	return *static_cast<robot_interface_info*>(b->private_data);
}

/* init */
int robot_interface_init(ubx_block_t *b)
{
	int ret = -1;
	long len = 0;
	robot_interface_info *inf;

	/* allocate memory for the block local state */
	if ((inf = new robot_interface_info())  == NULL)
	{
		ubx_err(b, "robot_interface: failed to alloc memory");
		ret=EOUTOFMEM;
		goto out;
	}
	b->private_data=inf;
	update_port_cache(b, &inf->ports);

	const int* joint_state_size;
	len = cfg_getptr_int(b, "joint_state_size", &joint_state_size);
	if (len < 0) goto out;

	inf->joint_state_size = (len > 0) ? *joint_state_size : 1;

	/* resize ports */
	if (
	    ubx_outport_resize(inf->ports.qd, inf->joint_state_size) ||
	    ubx_outport_resize(inf->ports.q , inf->joint_state_size) ||
	    ubx_inport_resize (inf->ports.tau,inf->joint_state_size) )
	{
		goto out;
	}

	ret=0;
out:
	return ret;
}

int robot_interface_start(ubx_block_t *b)
{
	int ret = 0;
	return ret;
}

void robot_interface_stop(ubx_block_t *b)
{
    robot_interface_info& data = private_data(b);
    data.reset();
}

void robot_interface_cleanup(ubx_block_t *b)
{
	delete(static_cast<robot_interface_info*>(b->private_data));
}

void normalize_angle(double& angle)
{
    angle = atan2(sin(angle), cos(angle));
}

namespace pl = std::placeholders;
void robot_interface_step(ubx_block_t *b)
{
	robot_interface_info& data = private_data(b);

	auto ptr_dyn = std::bind( &robot_interface_info::dyn , data , pl::_1 , pl::_2 , pl::_3 );
    double dt = 0.001;

    read_double_array(data.ports.tau, data.tau.data(), data.joint_state_size);
    odeint::runge_kutta4<state> stepper;
    /*size_t steps = */odeint::integrate_n_steps(stepper, ptr_dyn, data.st0, data.time, /*data.time+dt,*/ dt/10, 10);
    //std::cout << steps << std::endl;
    data.time += dt;
    normalize_angle(data.st0.X(0));
    normalize_angle(data.st0.X(1));
    normalize_angle(data.st0.X(2));
    normalize_angle(data.st0.X(3));
    normalize_angle(data.st0.X(4));
    normalize_angle(data.st0.X(5));
    write_double_array(data.ports.q , data.st0.X.data() , data.joint_state_size);
    write_double_array(data.ports.qd, data.st0.XD.data(), data.joint_state_size);

    write_double(data.ports.time, &(data.time));
}