//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_GGEVX_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_GGEVX_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for ggevx is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t ggevx( const char balanc, const char jobvl,
        const char jobvr, const char sense, const fortran_int_t n, float* a,
        const fortran_int_t lda, float* b, const fortran_int_t ldb,
        float* alphar, float* alphai, float* beta, float* vl,
        const fortran_int_t ldvl, float* vr, const fortran_int_t ldvr,
        fortran_int_t& ilo, fortran_int_t& ihi, float* lscale, float* rscale,
        float& abnrm, float& bbnrm, float* rconde, float* rcondv, float* work,
        const fortran_int_t lwork, fortran_int_t* iwork,
        fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_SGGEVX( &balanc, &jobvl, &jobvr, &sense, &n, a, &lda, b, &ldb,
            alphar, alphai, beta, vl, &ldvl, vr, &ldvr, &ilo, &ihi, lscale,
            rscale, &abnrm, &bbnrm, rconde, rcondv, work, &lwork, iwork,
            bwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t ggevx( const char balanc, const char jobvl,
        const char jobvr, const char sense, const fortran_int_t n, double* a,
        const fortran_int_t lda, double* b, const fortran_int_t ldb,
        double* alphar, double* alphai, double* beta, double* vl,
        const fortran_int_t ldvl, double* vr, const fortran_int_t ldvr,
        fortran_int_t& ilo, fortran_int_t& ihi, double* lscale,
        double* rscale, double& abnrm, double& bbnrm, double* rconde,
        double* rcondv, double* work, const fortran_int_t lwork,
        fortran_int_t* iwork, fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_DGGEVX( &balanc, &jobvl, &jobvr, &sense, &n, a, &lda, b, &ldb,
            alphar, alphai, beta, vl, &ldvl, vr, &ldvr, &ilo, &ihi, lscale,
            rscale, &abnrm, &bbnrm, rconde, rcondv, work, &lwork, iwork,
            bwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
inline std::ptrdiff_t ggevx( const char balanc, const char jobvl,
        const char jobvr, const char sense, const fortran_int_t n,
        std::complex<float>* a, const fortran_int_t lda,
        std::complex<float>* b, const fortran_int_t ldb,
        std::complex<float>* alpha, std::complex<float>* beta,
        std::complex<float>* vl, const fortran_int_t ldvl,
        std::complex<float>* vr, const fortran_int_t ldvr, fortran_int_t& ilo,
        fortran_int_t& ihi, float* lscale, float* rscale, float& abnrm,
        float& bbnrm, float* rconde, float* rcondv, std::complex<float>* work,
        const fortran_int_t lwork, float* rwork, fortran_int_t* iwork,
        fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_CGGEVX( &balanc, &jobvl, &jobvr, &sense, &n, a, &lda, b, &ldb,
            alpha, beta, vl, &ldvl, vr, &ldvr, &ilo, &ihi, lscale, rscale,
            &abnrm, &bbnrm, rconde, rcondv, work, &lwork, rwork, iwork, bwork,
            &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
inline std::ptrdiff_t ggevx( const char balanc, const char jobvl,
        const char jobvr, const char sense, const fortran_int_t n,
        std::complex<double>* a, const fortran_int_t lda,
        std::complex<double>* b, const fortran_int_t ldb,
        std::complex<double>* alpha, std::complex<double>* beta,
        std::complex<double>* vl, const fortran_int_t ldvl,
        std::complex<double>* vr, const fortran_int_t ldvr,
        fortran_int_t& ilo, fortran_int_t& ihi, double* lscale,
        double* rscale, double& abnrm, double& bbnrm, double* rconde,
        double* rcondv, std::complex<double>* work, const fortran_int_t lwork,
        double* rwork, fortran_int_t* iwork, fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_ZGGEVX( &balanc, &jobvl, &jobvr, &sense, &n, a, &lda, b, &ldb,
            alpha, beta, vl, &ldvl, vr, &ldvr, &ilo, &ihi, lscale, rscale,
            &abnrm, &bbnrm, rconde, rcondv, work, &lwork, rwork, iwork, bwork,
            &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to ggevx.
//
template< typename Value, typename Enable = void >
struct ggevx_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct ggevx_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
            typename VectorALPHAI, typename VectorBETA, typename MatrixVL,
            typename MatrixVR, typename VectorLSCALE, typename VectorRSCALE,
            typename VectorRCONDE, typename VectorRCONDV, typename WORK,
            typename IWORK, typename BWORK >
    static std::ptrdiff_t invoke( const char balanc, const char jobvl,
            const char jobvr, const char sense, MatrixA& a, MatrixB& b,
            VectorALPHAR& alphar, VectorALPHAI& alphai, VectorBETA& beta,
            MatrixVL& vl, MatrixVR& vr, fortran_int_t& ilo,
            fortran_int_t& ihi, VectorLSCALE& lscale,
            VectorRSCALE& rscale, real_type& abnrm, real_type& bbnrm,
            VectorRCONDE& rconde, VectorRCONDV& rcondv, detail::workspace3<
            WORK, IWORK, BWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVR >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorALPHAR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorALPHAI >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorBETA >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVL >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorLSCALE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorRSCALE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorRCONDE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorRCONDV >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorALPHAR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorALPHAI >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorBETA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorLSCALE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorRSCALE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorRCONDE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorRCONDV >::value) );
        BOOST_ASSERT( bindings::size(alphai) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(alphar) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( sense, bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(fortran_bool_t())) >=
                min_size_bwork( sense, bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( balanc, jobvl, jobvr, sense,
                bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(vl) == 1 ||
                bindings::stride_minor(vl) == 1 );
        BOOST_ASSERT( bindings::size_minor(vr) == 1 ||
                bindings::stride_minor(vr) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( balanc == 'N' || balanc == 'P' || balanc == 'S' ||
                balanc == 'B' );
        BOOST_ASSERT( jobvl == 'N' || jobvl == 'V' );
        BOOST_ASSERT( jobvr == 'N' || jobvr == 'V' );
        BOOST_ASSERT( sense == 'N' || sense == 'E' || sense == 'V' ||
                sense == 'B' );
        return detail::ggevx( balanc, jobvl, jobvr, sense,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(alphar),
                bindings::begin_value(alphai), bindings::begin_value(beta),
                bindings::begin_value(vl), bindings::stride_major(vl),
                bindings::begin_value(vr), bindings::stride_major(vr), ilo,
                ihi, bindings::begin_value(lscale),
                bindings::begin_value(rscale), abnrm, bbnrm,
                bindings::begin_value(rconde), bindings::begin_value(rcondv),
                bindings::begin_value(work.select(real_type())),
                bindings::size(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())),
                bindings::begin_value(work.select(fortran_bool_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
            typename VectorALPHAI, typename VectorBETA, typename MatrixVL,
            typename MatrixVR, typename VectorLSCALE, typename VectorRSCALE,
            typename VectorRCONDE, typename VectorRCONDV >
    static std::ptrdiff_t invoke( const char balanc, const char jobvl,
            const char jobvr, const char sense, MatrixA& a, MatrixB& b,
            VectorALPHAR& alphar, VectorALPHAI& alphai, VectorBETA& beta,
            MatrixVL& vl, MatrixVR& vr, fortran_int_t& ilo,
            fortran_int_t& ihi, VectorLSCALE& lscale,
            VectorRSCALE& rscale, real_type& abnrm, real_type& bbnrm,
            VectorRCONDE& rconde, VectorRCONDV& rcondv, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< real_type > tmp_work( min_size_work( balanc,
                jobvl, jobvr, sense, bindings::size_column(a) ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( sense, bindings::size_column(a) ) );
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                sense, bindings::size_column(a) ) );
        return invoke( balanc, jobvl, jobvr, sense, a, b, alphar, alphai,
                beta, vl, vr, ilo, ihi, lscale, rscale, abnrm, bbnrm, rconde,
                rcondv, workspace( tmp_work, tmp_iwork, tmp_bwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
            typename VectorALPHAI, typename VectorBETA, typename MatrixVL,
            typename MatrixVR, typename VectorLSCALE, typename VectorRSCALE,
            typename VectorRCONDE, typename VectorRCONDV >
    static std::ptrdiff_t invoke( const char balanc, const char jobvl,
            const char jobvr, const char sense, MatrixA& a, MatrixB& b,
            VectorALPHAR& alphar, VectorALPHAI& alphai, VectorBETA& beta,
            MatrixVL& vl, MatrixVR& vr, fortran_int_t& ilo,
            fortran_int_t& ihi, VectorLSCALE& lscale,
            VectorRSCALE& rscale, real_type& abnrm, real_type& bbnrm,
            VectorRCONDE& rconde, VectorRCONDV& rcondv, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        real_type opt_size_work;
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( sense, bindings::size_column(a) ) );
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                sense, bindings::size_column(a) ) );
        detail::ggevx( balanc, jobvl, jobvr, sense,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(alphar),
                bindings::begin_value(alphai), bindings::begin_value(beta),
                bindings::begin_value(vl), bindings::stride_major(vl),
                bindings::begin_value(vr), bindings::stride_major(vr), ilo,
                ihi, bindings::begin_value(lscale),
                bindings::begin_value(rscale), abnrm, bbnrm,
                bindings::begin_value(rconde), bindings::begin_value(rcondv),
                &opt_size_work, -1, bindings::begin_value(tmp_iwork),
                bindings::begin_value(tmp_bwork) );
        bindings::detail::array< real_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( balanc, jobvl, jobvr, sense, a, b, alphar, alphai,
                beta, vl, vr, ilo, ihi, lscale, rscale, abnrm, bbnrm, rconde,
                rcondv, workspace( tmp_work, tmp_iwork, tmp_bwork ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const char balanc, const char jobvl,
            const char jobvr, const char sense, const std::ptrdiff_t n ) {
        if ( balanc == 'S' || balanc == 'B' || jobvl == 'V' || jobvr == 'V' )
            return std::max< std::ptrdiff_t >( 1, 6*n );
        if ( sense == 'E' )
            return std::max< std::ptrdiff_t >( 1, 10*n );
        if ( sense == 'V' || sense == 'B' )
            return 2*n*n + 8*n + 16;
        return std::max< std::ptrdiff_t >( 1, 2*n );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const char sense,
            const std::ptrdiff_t n ) {
        if ( sense == 'E' )
          return 0;
        else
          return n+6;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array bwork.
    //
    static std::ptrdiff_t min_size_bwork( const char sense,
            const std::ptrdiff_t n ) {
        if ( sense == 'N' )
          return 0;
        else
          return n;
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct ggevx_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHA,
            typename VectorBETA, typename MatrixVL, typename MatrixVR,
            typename VectorLSCALE, typename VectorRSCALE,
            typename VectorRCONDE, typename VectorRCONDV, typename WORK,
            typename RWORK, typename IWORK, typename BWORK >
    static std::ptrdiff_t invoke( const char balanc, const char jobvl,
            const char jobvr, const char sense, MatrixA& a, MatrixB& b,
            VectorALPHA& alpha, VectorBETA& beta, MatrixVL& vl, MatrixVR& vr,
            fortran_int_t& ilo, fortran_int_t& ihi,
            VectorLSCALE& lscale, VectorRSCALE& rscale, real_type& abnrm,
            real_type& bbnrm, VectorRCONDE& rconde, VectorRCONDV& rcondv,
            detail::workspace4< WORK, RWORK, IWORK, BWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVR >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorLSCALE >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorRSCALE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorLSCALE >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorRCONDE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorLSCALE >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorRCONDV >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorALPHA >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorBETA >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVL >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorALPHA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorBETA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorLSCALE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorRSCALE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorRCONDE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorRCONDV >::value) );
        BOOST_ASSERT( bindings::size(alpha) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(beta) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( sense, bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(fortran_bool_t())) >=
                min_size_bwork( sense, bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_rwork( balanc, bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_work( sense, bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(vl) == 1 ||
                bindings::stride_minor(vl) == 1 );
        BOOST_ASSERT( bindings::size_minor(vr) == 1 ||
                bindings::stride_minor(vr) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( balanc == 'N' || balanc == 'P' || balanc == 'S' ||
                balanc == 'B' );
        BOOST_ASSERT( jobvl == 'N' || jobvl == 'V' );
        BOOST_ASSERT( jobvr == 'N' || jobvr == 'V' );
        BOOST_ASSERT( sense == 'N' || sense == 'E' || sense == 'V' ||
                sense == 'B' );
        return detail::ggevx( balanc, jobvl, jobvr, sense,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(alpha),
                bindings::begin_value(beta), bindings::begin_value(vl),
                bindings::stride_major(vl), bindings::begin_value(vr),
                bindings::stride_major(vr), ilo, ihi,
                bindings::begin_value(lscale), bindings::begin_value(rscale),
                abnrm, bbnrm, bindings::begin_value(rconde),
                bindings::begin_value(rcondv),
                bindings::begin_value(work.select(value_type())),
                bindings::size(work.select(value_type())),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())),
                bindings::begin_value(work.select(fortran_bool_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHA,
            typename VectorBETA, typename MatrixVL, typename MatrixVR,
            typename VectorLSCALE, typename VectorRSCALE,
            typename VectorRCONDE, typename VectorRCONDV >
    static std::ptrdiff_t invoke( const char balanc, const char jobvl,
            const char jobvr, const char sense, MatrixA& a, MatrixB& b,
            VectorALPHA& alpha, VectorBETA& beta, MatrixVL& vl, MatrixVR& vr,
            fortran_int_t& ilo, fortran_int_t& ihi,
            VectorLSCALE& lscale, VectorRSCALE& rscale, real_type& abnrm,
            real_type& bbnrm, VectorRCONDE& rconde, VectorRCONDV& rcondv,
            minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< value_type > tmp_work( min_size_work( sense,
                bindings::size_column(a) ) );
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork(
                balanc, bindings::size_column(a) ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( sense, bindings::size_column(a) ) );
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                sense, bindings::size_column(a) ) );
        return invoke( balanc, jobvl, jobvr, sense, a, b, alpha, beta, vl, vr,
                ilo, ihi, lscale, rscale, abnrm, bbnrm, rconde, rcondv,
                workspace( tmp_work, tmp_rwork, tmp_iwork, tmp_bwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHA,
            typename VectorBETA, typename MatrixVL, typename MatrixVR,
            typename VectorLSCALE, typename VectorRSCALE,
            typename VectorRCONDE, typename VectorRCONDV >
    static std::ptrdiff_t invoke( const char balanc, const char jobvl,
            const char jobvr, const char sense, MatrixA& a, MatrixB& b,
            VectorALPHA& alpha, VectorBETA& beta, MatrixVL& vl, MatrixVR& vr,
            fortran_int_t& ilo, fortran_int_t& ihi,
            VectorLSCALE& lscale, VectorRSCALE& rscale, real_type& abnrm,
            real_type& bbnrm, VectorRCONDE& rconde, VectorRCONDV& rcondv,
            optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        value_type opt_size_work;
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork(
                balanc, bindings::size_column(a) ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( sense, bindings::size_column(a) ) );
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                sense, bindings::size_column(a) ) );
        detail::ggevx( balanc, jobvl, jobvr, sense,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(alpha),
                bindings::begin_value(beta), bindings::begin_value(vl),
                bindings::stride_major(vl), bindings::begin_value(vr),
                bindings::stride_major(vr), ilo, ihi,
                bindings::begin_value(lscale), bindings::begin_value(rscale),
                abnrm, bbnrm, bindings::begin_value(rconde),
                bindings::begin_value(rcondv), &opt_size_work, -1,
                bindings::begin_value(tmp_rwork),
                bindings::begin_value(tmp_iwork),
                bindings::begin_value(tmp_bwork) );
        bindings::detail::array< value_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( balanc, jobvl, jobvr, sense, a, b, alpha, beta, vl, vr,
                ilo, ihi, lscale, rscale, abnrm, bbnrm, rconde, rcondv,
                workspace( tmp_work, tmp_rwork, tmp_iwork, tmp_bwork ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const char sense,
            const std::ptrdiff_t n ) {
        if ( sense == 'N' )
            return std::max< std::ptrdiff_t >( 1, 2*n );
        else {
            if ( sense == 'E' )
                return std::max< std::ptrdiff_t >( 1, 4*n );
            else
                return std::max< std::ptrdiff_t >( 1, 2*n*n+2*n );
        }
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array rwork.
    //
    static std::ptrdiff_t min_size_rwork( const char balanc,
            const std::ptrdiff_t n ) {
        if ( balanc == 'S' || balanc == 'B' )
            return std::max< std::ptrdiff_t >( 1, 6*n );
        else
            return std::max< std::ptrdiff_t >( 1, 2*n );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const char sense,
            const std::ptrdiff_t n ) {
        if ( sense == 'E' )
          return 0;
        else
          return n+2;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array bwork.
    //
    static std::ptrdiff_t min_size_bwork( const char sense,
            const std::ptrdiff_t n ) {
        if ( sense == 'N' )
          return 0;
        else
          return n;
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the ggevx_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for ggevx. Its overload differs for
// * User-defined workspace
//
template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
        typename VectorALPHAI, typename VectorBETA, typename MatrixVL,
        typename MatrixVR, typename VectorLSCALE, typename VectorRSCALE,
        typename VectorRCONDE, typename VectorRCONDV, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
ggevx( const char balanc, const char jobvl, const char jobvr,
        const char sense, MatrixA& a, MatrixB& b, VectorALPHAR& alphar,
        VectorALPHAI& alphai, VectorBETA& beta, MatrixVL& vl, MatrixVR& vr,
        fortran_int_t& ilo, fortran_int_t& ihi, VectorLSCALE& lscale,
        VectorRSCALE& rscale, typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type& abnrm,
        typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type& bbnrm, VectorRCONDE& rconde,
        VectorRCONDV& rcondv, Workspace work ) {
    return ggevx_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( balanc, jobvl, jobvr, sense, a, b,
            alphar, alphai, beta, vl, vr, ilo, ihi, lscale, rscale, abnrm,
            bbnrm, rconde, rcondv, work );
}

//
// Overloaded function for ggevx. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
        typename VectorALPHAI, typename VectorBETA, typename MatrixVL,
        typename MatrixVR, typename VectorLSCALE, typename VectorRSCALE,
        typename VectorRCONDE, typename VectorRCONDV >
inline typename boost::disable_if< detail::is_workspace< VectorRCONDV >,
        std::ptrdiff_t >::type
ggevx( const char balanc, const char jobvl, const char jobvr,
        const char sense, MatrixA& a, MatrixB& b, VectorALPHAR& alphar,
        VectorALPHAI& alphai, VectorBETA& beta, MatrixVL& vl, MatrixVR& vr,
        fortran_int_t& ilo, fortran_int_t& ihi, VectorLSCALE& lscale,
        VectorRSCALE& rscale, typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type& abnrm,
        typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type& bbnrm, VectorRCONDE& rconde,
        VectorRCONDV& rcondv ) {
    return ggevx_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( balanc, jobvl, jobvr, sense, a, b,
            alphar, alphai, beta, vl, vr, ilo, ihi, lscale, rscale, abnrm,
            bbnrm, rconde, rcondv, optimal_workspace() );
}

//
// Overloaded function for ggevx. Its overload differs for
// * User-defined workspace
//
template< typename MatrixA, typename MatrixB, typename VectorALPHA,
        typename VectorBETA, typename MatrixVL, typename MatrixVR,
        typename VectorLSCALE, typename VectorRSCALE, typename VectorRCONDE,
        typename VectorRCONDV, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
ggevx( const char balanc, const char jobvl, const char jobvr,
        const char sense, MatrixA& a, MatrixB& b, VectorALPHA& alpha,
        VectorBETA& beta, MatrixVL& vl, MatrixVR& vr, fortran_int_t& ilo,
        fortran_int_t& ihi, VectorLSCALE& lscale, VectorRSCALE& rscale,
        typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type& abnrm, typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type& bbnrm,
        VectorRCONDE& rconde, VectorRCONDV& rcondv, Workspace work ) {
    return ggevx_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( balanc, jobvl, jobvr, sense, a, b,
            alpha, beta, vl, vr, ilo, ihi, lscale, rscale, abnrm, bbnrm,
            rconde, rcondv, work );
}

//
// Overloaded function for ggevx. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixA, typename MatrixB, typename VectorALPHA,
        typename VectorBETA, typename MatrixVL, typename MatrixVR,
        typename VectorLSCALE, typename VectorRSCALE, typename VectorRCONDE,
        typename VectorRCONDV >
inline typename boost::disable_if< detail::is_workspace< VectorRCONDV >,
        std::ptrdiff_t >::type
ggevx( const char balanc, const char jobvl, const char jobvr,
        const char sense, MatrixA& a, MatrixB& b, VectorALPHA& alpha,
        VectorBETA& beta, MatrixVL& vl, MatrixVR& vr, fortran_int_t& ilo,
        fortran_int_t& ihi, VectorLSCALE& lscale, VectorRSCALE& rscale,
        typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type& abnrm, typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type& bbnrm,
        VectorRCONDE& rconde, VectorRCONDV& rcondv ) {
    return ggevx_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( balanc, jobvl, jobvr, sense, a, b,
            alpha, beta, vl, vr, ilo, ihi, lscale, rscale, abnrm, bbnrm,
            rconde, rcondv, optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


//==============================================================================
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/exponential/include/functions/nthroot.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/half.hpp>

NT2_TEST_CASE_TPL ( nthroot,  NT2_SIMD_REAL_TYPES)
{
  using nt2::nthroot;
  using nt2::tag::nthroot_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::as_integer<vT>::type          ivT;
  typedef typename nt2::meta::call<nthroot_(vT,ivT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Inf<vT>(),nt2::splat<ivT>(3)), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Inf<vT>(),nt2::splat<ivT>(4)), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Minf<vT>(),nt2::splat<ivT>(3)), nt2::Minf<r_t>(), 0.5);
   NT2_TEST_ULP_EQUAL(nthroot(nt2::Minf<vT>(),nt2::splat<ivT>(4)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Nan<vT>(),nt2::splat<ivT>(3)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Nan<vT>(),nt2::splat<ivT>(4)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<vT>(),nt2::splat<ivT>(4)), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<vT>(),nt2::splat<ivT>(0)), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One <vT>(),nt2::splat<ivT>(0)), nt2::One<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Half<vT>(),nt2::splat<ivT>(0)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Two <vT>(),nt2::splat<ivT>(0)), nt2::Inf <r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<vT>(),nt2::splat<ivT>(0)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Mone<vT>(),nt2::splat<ivT>(3)), nt2::Mone<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One<vT>(),nt2::splat<ivT>(3)), nt2::One<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::One<vT>(),nt2::splat<ivT>(4)), nt2::One<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<vT>(),nt2::splat<ivT>(3)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::Zero<vT>(),nt2::splat<ivT>(4)), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(-8),nt2::splat<ivT>(3)), nt2::splat<r_t>(-2), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(256),nt2::splat<ivT>(4)), nt2::splat<r_t>(4), 0.5);
  NT2_TEST_ULP_EQUAL(nthroot(nt2::splat<vT>(8),nt2::splat<ivT>(3)), nt2::splat<r_t>(2), 0.5);
}


#ifndef TRIUMF_NMR_SPECTRAL_DENSITY_HPP
#define TRIUMF_NMR_SPECTRAL_DENSITY_HPP

// Author: Ryan M. L. McFadden
// NMR spectral density functions for spin-lattice relaxation in solids.
//
// For additional details see:
//
// N. Bloembergen, E. M. Purcell, and R. V. Pound, "Relaxation Effects in
// Nuclear Magnetic Resonance Absorption", Phys. Rev. 73, 679-712 (1948).
// https://dx.doi.org/10.1103/PhysRev.73.679
//
// P. M. Richards, "Effect of low dimensionality on prefactor anomalies in
// superionic conductors", Solid State Commun. 25, 1019-1021 (1978).
// https://dx.doi.org/10.1016/0038-1098(78)90896-7
//
// C. A Sholl, "Nuclear spin relaxation by translational diffusion in liquids
// and solids: high- and low-frequency limits", J. Phys. C.: Solid State Phys.
// 14, 447-464 (1981). https://dx.doi.org/10.1088/0022-3719/14/4/018
//
// P. A. Beckmann, "Spectral densities and nuclear spin relaxation in solids",
// Phys. Rep. 171, 85-128 (1988).
// https://dx.doi.org/10.1016/0370-1573(88)90073-7
//
// W. Küchler, P. Heitjans, A. Payer, and R. Schöllhorn, "7Li NMR relaxation by
// diffusion in hexagonal and cubic LixTiS2". Solid State Ionics 70-71, Part 1,
// 434-438 (1994). https://dx.doi.org/10.1016/0167-2738(94)90350-6
//

#include <cmath>
#include <iostream>

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

#include <triumf/constants/codata_2018.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// Nuclear Magnetic Resonance (NMR)
namespace nmr {

// Arrhenius correlation time
template <typename T = double>
T tau_c(T temperature, T prefactor, T activation_energy) {
  constexpr T boltzmann_constant =
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  if (temperature <= 0.0 or prefactor <= 0.0 or activation_energy < 0.0) {
    return 0.0;
  } else {
    return prefactor *
           std::exp(activation_energy / (boltzmann_constant * temperature));
  }
}

// Arrhenius correlation rate
template <typename T = double>
T nu_c(T temperature, T prefactor, T activation_energy) {
  constexpr T boltzmann_constant =
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  if (temperature <= 0.0 or prefactor <= 0.0 or activation_energy < 0.0) {
    return 0.0;
  } else {
    return prefactor *
           std::exp(-activation_energy / (boltzmann_constant * temperature));
  }
}

// Bloembergen-Purcell-Pound (i.e., Debye) - isotropic 3D fluctuations
template <typename T = double>
T j_3d(T correlation_time, T nmr_frequency, T interaction_strength,
       T stretching_exponent) {
  if (correlation_time < 0 or nmr_frequency < 0 or interaction_strength <= 0) {
    return 0.0;
  }
  if (stretching_exponent > 2 or stretching_exponent < 1) {
    std::cout << "WARNING : stretching exponent outside of bounds : [1, 2]"
              << std::endl;
    return 0.0;
  }
  return interaction_strength * 2.0 * correlation_time /
         (1.0 +
          std::pow(nmr_frequency * correlation_time, stretching_exponent));
}

// Richards - empirocal function for 2D fluctuations that gives correct
// asymptotic limits
template <typename T = double>
T j_2d(T correlation_time, T nmr_frequency, T interaction_strength,
       T stretching_exponent) {
  if (correlation_time < 0 or nmr_frequency < 0 or interaction_strength <= 0) {
    return 0.0;
  }
  if (stretching_exponent > 2 or stretching_exponent < 1) {
    std::cout << "WARNING : stretching exponent outside of bounds : [1, 2]"
              << std::endl;
    return 0.0;
  }
  return interaction_strength * correlation_time *
         std::log1p(
             std::pow(nmr_frequency * correlation_time, -stretching_exponent));
}

// Cole-Cole - correlated motion (j_cc -> j_bpp as stretching_exponent -> 1)
template <typename T = double>
T j_cc(T correlation_time, T nmr_frequency, T interaction_strength,
       T stretching_exponent) {
  if (correlation_time < 0 or nmr_frequency < 0 or interaction_strength <= 0) {
    return 0.0;
  }
  if (stretching_exponent > 1 or stretching_exponent <= 0) {
    std::cout << "WARNING : stretching exponent outside of bounds : (0, 1]"
              << std::endl;
    return 0.0;
  }
  return interaction_strength * (2.0 / nmr_frequency) *
         std::sin(stretching_exponent * boost::math::constants::pi<T>() / 2.0) *
         (std::pow(nmr_frequency * correlation_time, stretching_exponent) /
          (1.0 +
           std::pow(nmr_frequency * correlation_time,
                    2.0 * stretching_exponent) +
           2.0 *
               std::cos(stretching_exponent * boost::math::constants::pi<T>() /
                        2.0) *
               std::pow(nmr_frequency * correlation_time,
                        stretching_exponent)));
}

// Davidson-Cole - distribution of barriers (j_dc -> j_bpp as
// stretching_exponent -> 1)
template <typename T = double>
T j_dc(T correlation_time, T nmr_frequency, T interaction_strength,
       T stretching_exponent) {
  if (correlation_time < 0 or nmr_frequency < 0 or interaction_strength <= 0) {
    return 0.0;
  }
  if (stretching_exponent > 1 or stretching_exponent <= 0) {
    std::cout << "WARNING : stretching exponent outside of bounds : (0, 1]"
              << std::endl;
    return 0.0;
  }
  return interaction_strength * (2.0 / nmr_frequency) *
         std::sin(stretching_exponent *
                  std::atan(nmr_frequency * correlation_time)) /
         std::pow(1.0 + nmr_frequency * nmr_frequency * correlation_time *
                            correlation_time,
                  stretching_exponent / 2.0);
}

// Fang - mirror image of Davidson-Cole
template <typename T = double>
T j_fang(T correlation_time, T nmr_frequency, T interaction_strength,
         T stretching_exponent) {
  if (correlation_time < 0 or nmr_frequency < 0 or interaction_strength <= 0) {
    return 0.0;
  }
  if (stretching_exponent > 1 or stretching_exponent <= 0) {
    std::cout << "WARNING : stretching exponent outside of bounds : (0, 1]"
              << std::endl;
    return 0.0;
  }
  return interaction_strength * (2.0 / nmr_frequency) *
         std::pow(nmr_frequency * correlation_time, stretching_exponent) *
         std::sin(stretching_exponent *
                  std::atan(1.0 / (nmr_frequency * correlation_time))) /
         std::pow(1.0 + std::pow(nmr_frequency * correlation_time, 2.0),
                  stretching_exponent / 2.0);
}

// Fuoss-Kirkwood - distribution of correlation times
template <typename T = double>
T j_fk(T correlation_time, T nmr_frequency, T interaction_strength,
       T stretching_exponent) {
  if (correlation_time < 0 or nmr_frequency < 0 or interaction_strength <= 0) {
    return 0.0;
  }
  if (stretching_exponent > 1 or stretching_exponent <= 0) {
    std::cout << "WARNING : stretching exponent outside of bounds : (0, 1]"
              << std::endl;
    return 0.0;
  }
  return interaction_strength * (2.0 * stretching_exponent / nmr_frequency) *
         std::pow(nmr_frequency * correlation_time, stretching_exponent) /
         (1.0 + std::pow(nmr_frequency * correlation_time,
                         2.0 * stretching_exponent));
}

// Havriliak-Negami - correlated motion w/ distribution of barriers
// - delta ~ measure of correlations
// - delta*epsilon ~ measure of a spread in barriers
template <typename T = double>
T j_hn(T correlation_time, T nmr_frequency, T interaction_strength, T delta,
       T epsilon) {
  if (correlation_time < 0 or nmr_frequency < 0 or interaction_strength <= 0) {
    return 0.0;
  }
  if (delta > 1 or delta <= 0 or epsilon > 1.0 / delta or epsilon <= 0) {
    std::cout << "WARNING : stretching exponents outside of bounds : delta -> "
                 "(0, 1]\n";
    std::cout << "                                                   epsilon "
                 "-> (0, 1/delta]"
              << std::endl;
    return 0.0;
  }
  return interaction_strength * (2.0 / nmr_frequency) *
         (std::sin(
             epsilon *
             std::atan(
                 (std::pow(nmr_frequency * correlation_time, delta) *
                  std::sin(delta * boost::math::constants::pi<T>() / 2.0)) /
                 (1.0 + std::pow(nmr_frequency * correlation_time, delta) *
                            std::cos(delta * boost::math::constants::pi<T>() /
                                     2.0))))) *
         std::pow(
             1.0 +
                 2.0 * std::pow(nmr_frequency * correlation_time, delta) *
                     std::cos(delta * boost::math::constants::pi<T>() / 2.0) +
                 std::pow(nmr_frequency * correlation_time, 2.0 * delta),
             -epsilon / 2.0);
}

// Bryn-Mawr
// double j_bm(double correlation_time, double nmr_frequency, double
// interaction_strength = 1, double epsilon = 1, double eta = 1);

// Wagner (i.e., log-Gaussian)
// double j_lg(double correlation_time, double nmr_frequency, double
// interaction_strength = 1, double width = 1);

// log-Lorentzian
// double j_ll(double correlation_time, double nmr_frequency, double
// interaction_strength = 1, double width = 1);

// Frolich (i.e., energy box)
// double j_eb(double correlation_time, double nmr_frequency, double
// interaction_strength = 1, ...);

// Power law : power = 1 (e.g., Korringa); power = 2 (e.g., phonon); power = 3
// (e.g., [Dirac] orbital)
template <typename T = double>
T j_pow(T temperature, T intercept, T constant, T power) {
  if (temperature <= 0.0) {
    return 0.0;
  }
  if (constant == 0.0) {
    return intercept;
  }
  return intercept + constant * std::pow(temperature, power);
}

} // namespace nmr

} // namespace triumf

#endif // TRIUMF_NMR_SPECTRAL_DENSITY_HPP


#ifndef STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_DIAG_HPP
#define STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_DIAG_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_vector.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>

namespace stan {
namespace math {

template <typename T1, typename T2, int R, int C>
inline Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                     Eigen::Dynamic, Eigen::Dynamic>
quad_form_diag(const Eigen::Matrix<T1, Eigen::Dynamic, Eigen::Dynamic>& mat,
               const Eigen::Matrix<T2, R, C>& vec) {
  check_vector("quad_form_diag", "vec", vec);
  check_square("quad_form_diag", "mat", mat);
  check_size_match("quad_form_diag", "rows of mat", mat.rows(), "size of vec",
                   vec.size());
  return vec.asDiagonal() * mat * vec.asDiagonal();
}

}  // namespace math
}  // namespace stan
#endif


//    $Id: block_matrix_array.cc 27657 2012-11-21 13:19:08Z bangerth $
//
//    Copyright (C) 2005-2006, 2012 by the deal.II authors
//
//    This file is subject to QPL and may not be distributed without copyright
//    and license information. Please refer to the file
//    deal.II/doc/license.html for the text and further information on this
//    license.
//
//---------------------------------------------------------------------------

// See documentation of BlockMatrixArray for documentation of this example

#include <deal.II/base/logstream.h>
#include <deal.II/lac/block_matrix_array.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_gmres.h>

#include <iostream>
#include <fstream>

using namespace dealii;

double Adata[] =
{
  4., .5, .1, 0.,
  .5, 4., .5, .1,
  .1, .5, 4., .5,
  0., .1, .5, 4.
};

double B1data[] =
{
  .5, .1,
  .4, .2,
  .3, .3,
  .2, .4
};

double B2data[] =
{
  .3, 0., -.3, 0.,
  -.3, 0., .3, 0.
};

double Cdata[] =
{
  8., 1.,
  1., 8.
};

int main ()
{
  FullMatrix<float> A(4,4);
  FullMatrix<float> B1(4,2);
  FullMatrix<float> B2(2,4);
  FullMatrix<float> C(2,2);

  A.fill(Adata);
  B1.fill(B1data);
  B2.fill(B2data);
  C.fill(Cdata);

  GrowingVectorMemory<Vector<double> > simple_mem;

  BlockMatrixArray<double> matrix(2, 2, simple_mem);

  matrix.enter(A,0,0,2.);
  matrix.enter(B1,0,1,-1.);
  matrix.enter(B2,0,1,1., true);
  matrix.enter(B2,1,0,1.);
  matrix.enter(B1,1,0,-1., true);
  matrix.enter(C,1,1);
  matrix.print_latex(deallog);

  std::vector<unsigned int> block_sizes(2);
  block_sizes[0] = 4;
  block_sizes[1] = 2;

  BlockVector<double> result(block_sizes);
  BlockVector<double> x(block_sizes);
  BlockVector<double> y(block_sizes);
  for (unsigned int i=0; i<result.size(); ++i)
    result(i) = i;

  matrix.vmult(y, result);

  SolverControl control(100,1.e-10);
  GrowingVectorMemory<BlockVector<double> > mem;
  PreconditionIdentity id;

  SolverCG<BlockVector<double> > cg(control, mem);
  cg.solve(matrix, x, y, id);
  x.add(-1., result);
  deallog << "Error " << x.l2_norm() << std::endl;

  deallog << "Error A-norm "
          << std::sqrt(matrix.matrix_norm_square(x))
          << std::endl;

  FullMatrix<float> Ainv(4,4);
  Ainv.invert(A);
  FullMatrix<float> Cinv(2,2);
  Cinv.invert(C);

  BlockTrianglePrecondition<double>
  precondition(2, simple_mem);
  precondition.enter(Ainv,0,0,.5);
  precondition.enter(Cinv,1,1);

  cg.solve(matrix, x, y, precondition);
  x.add(-1., result);
  deallog << "Error " << x.l2_norm() << std::endl;

  precondition.enter(B1,1,0,-1., true);
  precondition.enter(B2,1,0,1.);

  SolverGMRES<BlockVector<double> > gmres(control, mem);
  gmres.solve(matrix, x, y, precondition);
  x.add(-1., result);
  deallog << "Error " << x.l2_norm() << std::endl;

  return 0;
}


/**
 * @file EigenMathUtil.hpp
 * @author Adam Li (adamli@umich.edu)
 * @date 2021-11-12
 *
 * @brief Includes a variety of utility functions for Eigen objects of the same type.
 *
 */

#include <Eigen/Core>

namespace sel_map::core::EigenMathUtil {

    template <typename Derived1, typename Derived2>
    inline double crossProduct2D(const Eigen::EigenBase<Derived1>& x, const Eigen::EigenBase<Derived2>& y){
        return x.derived()(0)*y.derived()(1) - x.derived()(1)*y.derived()(0);
    }

    // Convert boolean masks to a sequence usable for slicing vectors
    template <typename Derived>
    inline std::vector<unsigned int>& seqLogical(const Eigen::ArrayBase<Derived>& mask, std::vector<unsigned int>& sequence){
        if (mask.cols() != 1) throw std::invalid_argument("Mask should be single dim column array!");

        // Create the result
        // std::vector<unsigned int> sequence;
        sequence.clear();
        sequence.resize(mask.count());

        unsigned int accumulator = 0;
        auto itr = sequence.begin();

        for (auto val : mask){
            if(val) *(itr++) = (accumulator);
            ++accumulator;
        }

        // Make a sequence
        // Eigen::Array<unsigned int, Eigen::Dynamic, 1>::Map(&sequence[0], sequence.size())
        //      = mask.template cast<bool>().template cast<unsigned int>() * Eigen::Array<unsigned int, Eigen::Dynamic, 1>::LinSpaced(mask.rows(), 1, mask.rows());

        // // remove the 0's
        // auto seq_end = std::remove(sequence.begin(), sequence.end(), 0);

        // // Isolate the actual values
        // sequence.resize(seq_end - sequence.begin());

        // // Subtract 1 to make it valid
        // Eigen::Array<unsigned int, Eigen::Dynamic, 1>::Map(&sequence[0], sequence.size()) -= 1;

        return sequence;
    }
}


/* Boost example/rational.cpp
 * example program of how to use interval< rational<> >
 *
 * Copyright Sylvain Pion, Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors make any representation about the
 * suitability of this software for any purpose. It is provided "as
 * is" without express or implied warranty.
 *
 * $Id: rational.cpp,v 1.2 2003/02/05 17:34:35 gmelquio Exp $
 */

// it would have been enough to only include:
//   <boost/numeric/interval.hpp>
// but it's a bit overkill to include processor intrinsics
// and transcendental functions, so we do it by ourselves

#include <boost/numeric/interval/interval.hpp>      // base class
#include <boost/numeric/interval/rounded_arith.hpp> // default arithmetic rounding policy
#include <boost/numeric/interval/checking.hpp>      // default checking policy
#include <boost/numeric/interval/arith.hpp>         // += *= -= etc
#include <boost/numeric/interval/policies.hpp>      // default policy

#include <boost/rational.hpp>
#include <iostream>

typedef boost::rational<int> Rat;
typedef boost::numeric::interval<Rat> Interval;

std::ostream& operator<<(std::ostream& os, const Interval& r) {
  os << "[" << r.lower() << "," << r.upper() << "]";
  return os;
}

int main() {
  Rat p(2, 3), q(3, 4);
  Interval z(4, 5);
  Interval a(p, q);
  a += z;
  z *= q;
  a -= p;
  a /= q;
  std::cout << z << std::endl;
  std::cout << a << std::endl;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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_

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_bin_float.hpp>

#include "libs/multiprecision/test/test_arithmetic.hpp"

template <unsigned D>
struct related_type<boost::multiprecision::number< boost::multiprecision::cpp_bin_float<D> > >
{
   typedef boost::multiprecision::number< boost::multiprecision::cpp_bin_float<(D / 2 > std::numeric_limits<long double>::digits10 ? D / 2 : D)> > type;
};

int main()
{
   //test<boost::multiprecision::cpp_bin_float_50>();
   //test<boost::multiprecision::number<boost::multiprecision::cpp_bin_float<1000, boost::multiprecision::digit_base_10, std::allocator<void> > > >();
   test<boost::multiprecision::cpp_bin_float_quad>();
   return boost::report_errors();
}


#include "RayTracingOpenMP.h"

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

#include "Camera.h"
#include "KdTree.h"

namespace math = boost::math::constants;

const Color BACKGROUND_COLOR(0, 0, 0);

RayTracingOpenMP::RayTracingOpenMP() {
    lights = new Light[20];
    Ia = Color(0.2, 0.2, 0.2);
}

RayTracingOpenMP::~RayTracingOpenMP() {
    delete[] data;
}

Image RayTracingOpenMP::render() {
    Light light(Point(0, 0, -1), Color(1, 1, 1), Color(1, 1, 1));
    lights[0] = light;
    numberOfLights = 1;

    Resolution resolution = Resolution(width, height);
    Camera camera(Point(0, 0, -1), Point(0, math::pi<float>(), 0),
                  math::pi<float>() / 2, resolution, 1);
#pragma omp parallel for
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            Vector vector = camera.getPrimaryVector(x, y);
            Color color = trace(vector, 0);
            camera.update(x, y, color);
        }
    }

    data = new Color[width * height];
#pragma omp parallel for
    for (int y = 0; y < resolution.height; ++y) {
        for (int x = 0; x < resolution.width; ++x) {
            data[width * y + x] = camera.getPixelColor(x, y);
        }
    }
    return Image(resolution.width, resolution.height, data);
}

Color RayTracingOpenMP::trace(Vector vector, int depth, int ignoredTriangle, float weight) {
    if (depth > MAX_DEPTH || weight < MINIMUM_WEIGHT) {
        return BACKGROUND_COLOR;
    }

    int triangleIndex = kdTree->getNearestTriangle(vector, ignoredTriangle);
    if (triangleIndex == -1) {
        return BACKGROUND_COLOR;
    }

    Triangle &triangle = scene->getTriangles()[triangleIndex];
    Vector reflectionVector = triangle.getReflectedVector(vector);
    reflectionVector.normalize();

    Point reflectionPoint = reflectionVector.startPoint;

    Vector normal = scene->getTriangles()[triangleIndex].getNormal();

    Material material = scene->getMaterial((scene->getTriangles()[triangleIndex]).materialCode);

    Vector toViewer = vector.mul(-1);
    Color refractionColor(0, 0, 0);
    Color reflectionColor = Ia * material.ambient;
    float refractivity = 0;

    if (material.dissolve < FULLY_OPAQUE_RATIO) {
        float ior = material.refractiveIndex;
        float reflectivity = fresnel(vector, normal, ior);
        refractivity = (1 - reflectivity) * (1 - material.dissolve);
        Vector refractionVector = refract(vector, triangle.getNormal(), ior);

        refractionVector.startPoint = reflectionPoint;
        refractionColor =
                trace(refractionVector, depth + 1, triangleIndex, weight * refractivity) *
                material.transparent;
    }

    if (material.dissolve > FULLY_TRANSPARENT_RATIO) {
        for (int light = 0; light < numberOfLights; ++light) {
            Vector toLight = Vector(reflectionPoint, lights[light].point);
            toLight.normalize();

            // Check if the light is blocked out
            if (normal.isObtuse(toLight)) {
                continue;
            }

            // Cast shadow ray, take refraction into account (simplified version)
            float intensity = 1;
            float dist = 0;
            float lightDistance = lights[light].point.getDist(reflectionPoint);
            int lightTriangleIndex = triangleIndex;
            for (int lightDepth = depth;
                 lightDepth < MAX_DEPTH && intensity > 0.01f; ++lightDepth) {
                lightTriangleIndex = kdTree->getNearestTriangle(toLight, lightTriangleIndex);

                if (lightTriangleIndex == -1) {
                    break;
                }
                const Intersection &intersection =
                        scene->getTriangles()[lightTriangleIndex].intersect(toLight);
                dist += intersection.distance;
                if (dist >= lightDistance) {
                    break;
                }

                toLight.startPoint = intersection.point;
                intensity *= (1 - scene->getMaterial(triangle.materialCode).dissolve);
            }
            if (intensity <= 0.01f) {
                continue;
            }

            // Calculate reflection color
            Vector fromLight(lights[light].point, reflectionPoint);
            Vector fromLightReflected = scene->getTriangles()[triangleIndex].getReflectedVector(
                    fromLight);
            fromLightReflected.normalize();

            reflectionColor +=
                    lights[light].diffuse * intensity *
                    std::max(0.f, normal.dot(toLight)) * material.diffuse;
            reflectionColor +=
                    lights[light].specular * intensity *
                    powf(std::max(0.f, toViewer.dot(fromLightReflected)),
                         material.specularExponent) * material.specular;
        }

        reflectionColor +=
                trace(reflectionVector, depth + 1, triangleIndex, weight * (1 - refractivity)) *
                powf(std::max(0.f, toViewer.dot(normal)), material.specularExponent) *
                material.specular;
    }

    if (material.dissolve >= FULLY_OPAQUE_RATIO) {
        return reflectionColor;
    } else if (material.dissolve <= FULLY_TRANSPARENT_RATIO) {
        return refractionColor;
    }

    return reflectionColor * (1 - refractivity) + refractionColor * refractivity;
}

Vector RayTracingOpenMP::refract(const Vector &vector, const Vector &normal, float ior) const {
    float dot = vector.dot(normal);
    float eta1 = 1;
    float eta2 = ior;
    Vector localNormal = normal;

    if (dot < 0) {
        // Ray entering the object
        dot *= -1;
    } else {
        // Ray going out of the object
        localNormal = normal.mul(-1);
        std::swap(eta1, eta2);
    }

    float eta = eta1 / eta2;
    float k = 1 - eta * eta * (1 - dot * dot);
    if (k < 0) {
        // Total internal reflection
        return Vector::ZERO;
    }

    Vector returnVector = vector.mul(eta).add(localNormal.mul(eta * dot - sqrtf(k)));
    returnVector.normalize();
    return returnVector;
}

float RayTracingOpenMP::fresnel(const Vector &vector, const Vector &normal, float ior) const {
    float cos1 = vector.dot(normal);
    float eta1 = 1;
    float eta2 = ior;

    if (cos1 > 0) {
        std::swap(eta1, eta2);
    }

    float sin2 = eta1 / eta2 * sqrtf(std::max(0.f, 1 - cos1 * cos1));
    if (sin2 >= 1.f) {
        // Total internal reflection
        return 1;
    }

    float cos2 = sqrtf(1 - sin2 * sin2);
    cos1 = fabsf(cos1);
    float reflectS = (eta1 * cos1 - eta2 * cos2) / (eta1 * cos1 + eta2 * cos2);
    float reflectP = (eta1 * cos2 - eta2 * cos1) / (eta1 * cos2 + eta2 * cos1);
    return (reflectS * reflectS + reflectP * reflectP) / 2;
}

void RayTracingOpenMP::setScene(std::unique_ptr<Scene> scene) {
    Backend::setScene(std::move(scene));
    kdTree = std::make_unique<KdTree>(this->scene.get());
}


#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>

#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include "orcvio/euroc_gt.h"

namespace orcvio
{

    // this subscribes to the estimated pose
    void EuRoCPub::gt_odom_path_cb(const nav_msgs::Odometry::ConstPtr &est_odom_ptr){

        double timestamp = est_odom_ptr->header.stamp.toSec();
        // Our groundtruth state
        Eigen::Matrix<double,17,1> state_gt;

        // Check that we have the timestamp in our GT file [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel]
        if(!DatasetReader::get_gt_state(timestamp, state_gt, gt_states)) {
            return;
        }

        Eigen::Vector4d q_gt;
        Eigen::Vector3d t_gt;

        q_gt << state_gt(1,0),state_gt(2,0),state_gt(3,0),state_gt(4,0);
        t_gt << state_gt(5,0),state_gt(6,0),state_gt(7,0);

        Eigen::Quaterniond q_in = Eigen::Quaterniond(q_gt);

        if (first_pose_flag)
        {
            t0 = t_gt;
            // convert to rotation matrix
            R0 = q_in.normalized().toRotationMatrix();

            // align the estimated pose and groundtruth pose
            t0 = slamTgt.linear() * t0 + slamTgt.translation();
            R0 = slamTgt.linear() * R0;

            first_pose_flag = false;
        }

        Eigen::Vector3d t1, t1_new;
        t1 = t_gt;

        // convert to rotation matrix
        Eigen::Matrix3d R1, R_new;
        R1 = q_in.normalized().toRotationMatrix();

        // align the estimated pose and groundtruth pose
        t1_new = slamTgt.linear() * t1 + slamTgt.translation();
        // initial position is 0
        t1_new = t1_new - t0;
        // NOTE, rotation is not identity since orcvio initializes using gravity
        R_new = slamTgt.linear() * R1;

        // convert to quaternion
        Eigen::Quaterniond q1_normalized = Eigen::Quaterniond(R_new);
        // normalize the quaternion
        q1_normalized = q1_normalized.normalized();

        geometry_msgs::PoseStamped cur_pose;
        cur_pose.header = est_odom_ptr->header;
        cur_pose.header.frame_id = "/global";

        cur_pose.pose.position.x = t1_new(0,0);
        cur_pose.pose.position.y = t1_new(1,0);
        cur_pose.pose.position.z = t1_new(2,0);

        cur_pose.pose.orientation.x = q1_normalized.x();
        cur_pose.pose.orientation.y = q1_normalized.y();
        cur_pose.pose.orientation.z = q1_normalized.z();
        cur_pose.pose.orientation.w = q1_normalized.w();

        path.header = cur_pose.header;
        path.header.frame_id = "/global";
        path.poses.push_back(cur_pose);

        pub_gt_path.publish(path);

        // save the pose to txt for trajectory evaluation
        // ============================
        // TUM format
        // timestamp tx ty tz qx qy qz qw
        fStateToSave << std::fixed << std::setprecision(3) << cur_pose.header.stamp.toSec();
        fStateToSave << " "
            << cur_pose.pose.position.x << " " << cur_pose.pose.position.y << " " << cur_pose.pose.position.z << " "
            << cur_pose.pose.orientation.x << " " << cur_pose.pose.orientation.y << " " << cur_pose.pose.orientation.z << " " << cur_pose.pose.orientation.w << std::endl;

        return;

    }

}


#include "rv/Math.h"
#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iostream>
#include <list>
#include <sstream>
#include "rv/IOError.h"
#include "rv/Random.h"
#include "rv/string_utils.h"

namespace rv {

namespace Math {

struct SortElement {
  SortElement(unsigned int ind, double val) : index(ind), value(val) {}

  bool operator<(const SortElement& other) { return (value < other.value); }

  uint32_t index;
  double value;
};

Eigen::MatrixXf cov(const Eigen::MatrixXf& X, const int32_t N) {
  int n = std::min(1, std::max(N, 0));
  if (X.rows() == 1) n = 1;

  // calculate the mean

  Eigen::VectorXf mean = X.colwise().sum();
  mean = mean / X.rows();

  Eigen::MatrixXf X_mean = Eigen::MatrixXf::Zero(X.rows(), X.cols());
  for (uint32_t i = 0; i < X.rows(); ++i)
    for (uint32_t j = 0; j < X.cols(); ++j) X_mean(i, j) = X(i, j) - mean[j];

  Eigen::MatrixXf covariance = 1.0 / (double)(X.rows() - 1 + n) * X_mean.transpose() * X_mean;

  return covariance;
}

// double min(const legacy_fkie::Vector& V)
//{
//  return V.min();
//}
//

Eigen::MatrixXf min(const Eigen::MatrixXf& M) { return M.colwise().minCoeff(); }

//
// double max(const legacy_fkie::Vector& V)
//{
//  return V.max();
//}
//
Eigen::MatrixXf max(const Eigen::MatrixXf& M) { return M.colwise().maxCoeff(); }

void sort(const Eigen::MatrixXf& M, Eigen::MatrixXf& result, Eigen::MatrixXf& indices) {
  std::list<SortElement> temp;

  for (uint32_t c = 0; c < M.cols(); ++c) {
    for (uint32_t r = 0; r < M.rows(); ++r) temp.push_back(SortElement(r, M(r, c)));

    temp.sort();

    uint32_t r = 0;
    while (!temp.empty()) {
      indices(r, c) = temp.front().index;
      result(r, c) = temp.front().value;
      temp.pop_front();  // clear the list
      ++r;
    }
  }
}
//
// legacy_fkie::Vector prod(const legacy_fkie::Matrix& M, int dim)
//{
//  legacy_fkie::Vector result;
//  if (dim == 0)
//  {
//    result = legacy_fkie::Vector(M.cols());
//    for (unsigned int c = 0; c < M.cols(); ++c)
//    {
//      double product = 1.0;
//      for (unsigned int r = 0; r < M.rows(); ++r)
//        product *= M(r, c);
//      result[c] = product;
//    }
//  }
//  else if (dim == 1)
//  {
//    result = legacy_fkie::Vector(M.rows());
//
//    for (unsigned int r = 0; r < M.rows(); ++r)
//    {
//      double product = 1.0;
//      for (unsigned int c = 0; c < M.cols(); ++c)
//        product *= M(r, c);
//      result[r] = product;
//    }
//  }
//
//  return result;
//}
//
// legacy_fkie::Matrix importdata(const std::string& filename,
//    const std::string& delimiter, const unsigned int headerline)
//{
//  const unsigned int MAX_LENGTH = 64000;
//  char line[MAX_LENGTH];
//  memset(line, 0, MAX_LENGTH * sizeof(char));
//  char* token = 0;
//
//  std::ifstream file(filename.c_str());
//  if (!file.good()) throw legacy_fkie::IOError(
//      "importdata: failed opening " + filename);
//
//  // determine the rows and column count
//  unsigned int lineno = 0;
//  unsigned int rows = 0;
//  unsigned int cols = 0;
//  while (!file.eof())
//  {
//    file.getline(line, 64000);
//    ++lineno;
//    if (lineno <= headerline) continue;
//
//    // count columns
//    if (rows == 0)
//    {
//      token = strtok(line, delimiter.c_str());
//      while (token != 0)
//      {
//        ++cols;
//        token = strtok(0, delimiter.c_str());
//      }
//    }
//    ++rows;
//  }
//  file.close();
//
//  legacy_fkie::Matrix result(rows, cols);
//  file.open(filename.c_str());
//  unsigned int r = 0;
//  unsigned int c = 0;
//  lineno = 0;
//  while (!file.eof())
//  {
//    c = 0;
//    file.getline(line, 64000);
//    ++lineno;
//    if (lineno <= headerline) continue;
//    // split line
//    token = strtok(line, delimiter.c_str());
//    while (token != 0)
//    {
//      result(r, c) = atof(token);
//      token = strtok(0, delimiter.c_str());
//      ++c;
//    }
//    ++r;
//  }
//
//  file.close();
//
//  return result;
//}
//
// legacy_fkie::Matrix sqrt(const legacy_fkie::Matrix& M)
//{
//  legacy_fkie::Matrix Z = M;
//  std::vector<double> ev;
//
//  int info = legacy_fkie::LA::dsyev('V', 'U', Z, ev);
//  if (info < 0)
//  {
//    throw legacy_fkie::MathError(
//        "failed to determine the eigenvalues and eigenvectors of M");
//  }
//  else if (info > 0)
//  {
//    throw legacy_fkie::MathError("failed to converge!");
//  }
//
////  std::cout << ev.size() << std::endl;
//  /* calculate D^(1/2) */
//  legacy_fkie::Matrix D(ev.size(), ev.size());
//  for (unsigned int i = 0; i < ev.size(); ++i)
//  {
//    // epsilon test for values near zero.
//    if (std::abs(ev[i]) < 0.0000001)
//    {
//      D(i, i) = 0.0;
//      continue;
//    }
//    if (ev[i] < 0)
//    {
//      std::stringstream sstr;
//      sstr
//          << "Matrix M is not positive semi-definite. Eigenvalue less than zero: "
//          << ev[i];
//      throw legacy_fkie::MathError(sstr.str());
//    }
//    D(i, i) = std::sqrt(ev[i]);
//  }
//
//  return Z * D * Z.transpose();
//}

// legacy_fkie::Vector randperm(const unsigned int n)
//{
//  static legacy_fkie::Random rand;
//  legacy_fkie::Vector v(n);
//
//  for (unsigned int i = 1; i <= n; ++i)
//    v[i - 1] = i;
//
//  std::random_shuffle(v.ptr(), v.ptr() + n, rand);
//
//  return v;
//}

void save(const std::string& filename, const std::string& varname, const Eigen::MatrixXf& mat) {
  std::ofstream out(filename.c_str());
  assert(out.is_open());

  /** outputting the header information. **/
  out << "# name: " << varname << std::endl;
  out << "# type: matrix" << std::endl;
  out << "# rows: " << mat.rows() << std::endl;
  out << "# columns: " << mat.cols() << std::endl;

  for (uint32_t r = 0; r < mat.rows(); ++r) {
    for (uint32_t c = 0; c < mat.cols(); ++c) {
      out << " " << mat(r, c);
    }
    out << std::endl;
  }

  out.close();
}

bool load(const std::string& filename, Eigen::MatrixXf& M) {
  std::ifstream in(filename.c_str());
  if (!in.is_open()) {
    std::cerr << "unable to open '" << filename << "'" << std::endl;
    return false;
  }

  std::string line;
  std::vector<std::string> tokens;

  bool rows_read = false, cols_read = false;
  uint32_t rows = 0, cols = 0;

  in.peek();
  while ((!rows_read || !cols_read) && !in.eof()) {
    std::getline(in, line);
    if (line[0] != '#') break;
    tokens = split(line, ":");
    // trim = remove "# " in the begining.
    std::string name = trim(tokens[0], " #");
    std::string value = trim(tokens[1]);

    if (name == "type") {
      if (value != "matrix") {
        std::cerr << "unknown type '" << tokens[1] << "'!" << std::endl;
        return false;
      }
    }

    if (name == "rows") {
      rows_read = true;
      // trim = remove leading spaces
      rows = boost::lexical_cast<uint32_t>(value);
    }

    if (name == "columns") {
      cols_read = true;
      // trim = remove leading spaces
      cols = boost::lexical_cast<uint32_t>(value);
    }
    in.peek();
  }

  if (!rows_read) {
    std::cerr << "missing 'rows' entry!" << std::endl;
    return false;
  }
  if (!cols_read) {
    std::cerr << "missing 'cols' entry!" << std::endl;
    return false;
  }

  M = Eigen::MatrixXf(rows, cols);

  in.peek();
  for (uint32_t r = 0; r < rows; ++r) {
    if (in.eof()) return false;

    std::getline(in, line);
    // trim = remove leading space.
    tokens = split(trim(line));
    if (tokens.size() < cols) return false;

    for (uint32_t c = 0; c < cols; ++c) M(r, c) = boost::lexical_cast<float>(tokens[c]);

    in.peek();
  }

  in.close();

  return true;
}

void save(const std::string& filename, const std::string& varname, const Eigen::MatrixXd& mat) {
  std::ofstream out(filename.c_str());
  assert(out.is_open());

  /** outputting the header information. **/
  out << "# name: " << varname << std::endl;
  out << "# type: matrix" << std::endl;
  out << "# rows: " << mat.rows() << std::endl;
  out << "# columns: " << mat.cols() << std::endl;

  for (uint32_t r = 0; r < mat.rows(); ++r) {
    for (uint32_t c = 0; c < mat.cols(); ++c) {
      out << " " << mat(r, c);
    }
    out << std::endl;
  }

  out.close();
}

bool load(const std::string& filename, Eigen::MatrixXd& M) {
  std::ifstream in(filename.c_str());
  if (!in.is_open()) {
    std::cerr << "unable to open '" << filename << "'" << std::endl;
    return false;
  }

  std::string line;
  std::vector<std::string> tokens;

  bool rows_read = false, cols_read = false;
  uint32_t rows = 0, cols = 0;

  in.peek();
  while ((!rows_read || !cols_read) && !in.eof()) {
    std::getline(in, line);
    if (line[0] != '#') break;
    tokens = split(line, ":");
    // trim = remove "# " in the begining.
    std::string name = trim(tokens[0], " #");
    std::string value = trim(tokens[1]);

    if (name == "type") {
      if (value != "matrix") {
        std::cerr << "unknown type '" << tokens[1] << "'!" << std::endl;
        return false;
      }
    }

    if (name == "rows") {
      rows_read = true;
      // trim = remove leading spaces
      rows = boost::lexical_cast<uint32_t>(value);
    }

    if (name == "columns") {
      cols_read = true;
      // trim = remove leading spaces
      cols = boost::lexical_cast<uint32_t>(value);
    }
    in.peek();
  }

  if (!rows_read) {
    std::cerr << "missing 'rows' entry!" << std::endl;
    return false;
  }
  if (!cols_read) {
    std::cerr << "missing 'cols' entry!" << std::endl;
    return false;
  }

  M = Eigen::MatrixXd(rows, cols);

  in.peek();
  for (uint32_t r = 0; r < rows; ++r) {
    if (in.eof()) return false;

    std::getline(in, line);
    // trim = remove leading space.
    tokens = split(trim(line));
    if (tokens.size() < cols) return false;

    for (uint32_t c = 0; c < cols; ++c) M(r, c) = boost::lexical_cast<double>(tokens[c]);

    in.peek();
  }

  in.close();

  return true;
}

double normal_pdf(double x, double mu, double sigma) {
  static const double sqrt2pi = std::sqrt(2.0 * PI);

  double value = 1. / (sigma * sqrt2pi);
  value *= std::exp(-0.5 * (x - mu) * (x - mu) / (sigma * sigma));

  return value;
}

double exponential_pdf(double x, double lambda) { return lambda * exp(-lambda * x); }

double sigmoid2(double x, double loc, double scale) { return 1.0 / (1.0 + std::exp(-scale * x + scale * loc)); }

Eigen::Matrix2f diag(float val0, float val1) {
  Eigen::Matrix2f m = Eigen::Matrix2f::Zero();

  m(0, 0) = val0;
  m(1, 1) = val1;

  return m;
}

Eigen::Matrix3f diag(float val0, float val1, float val2) {
  Eigen::Matrix3f m = Eigen::Matrix3f::Zero();

  m(0, 0) = val0;
  m(1, 1) = val1;
  m(2, 2) = val2;

  return m;
}
}
}


// -*- mode: C++ -*-
/**
   Dietrich Bollmann, Kamakura, 2015/01/01

   DVector-test.h

   Unit tests for class: DVector

   Copyright (c) 2015 Dietrich Bollmann

   This software may be modified and distributed under the terms
   of the MIT license.  See the LICENSE file for details.
*/

#define BOOST_TEST_DYN_LINK
#ifdef STAND_ALONE
#   define BOOST_TEST_MODULE Main
#endif
#include <boost/test/unit_test.hpp>

#include "DVector.h"

namespace nsl {

BOOST_AUTO_TEST_SUITE(TestSuite_DVector)

// Test equality operator
bool test_equality(const std::vector<double> &left,
		   const std::vector<double> &right)
{
  DVector vLeft(left);
  DVector vRight(right);

  return (vLeft == vRight);
}

BOOST_AUTO_TEST_CASE(Test_DVector_equality_operator)
{
  // equal
  BOOST_REQUIRE(   test_equality( {},        {}        ) );
  BOOST_REQUIRE(   test_equality( {1},       {1}       ) );
  BOOST_REQUIRE(   test_equality( {1, 2},    {1, 2}    ) );
  BOOST_REQUIRE(   test_equality( {1, 2, 3}, {1, 2, 3} ) );

  // different size
  BOOST_REQUIRE( ! test_equality( {},        {1}       ) );
  BOOST_REQUIRE( ! test_equality( {1},       {}        ) );
  BOOST_REQUIRE( ! test_equality( {1},       {1, 2}    ) );
  BOOST_REQUIRE( ! test_equality( {1, 2},    {1}       ) );

  // different elements
  BOOST_REQUIRE( ! test_equality( {1},       {2}       ) );
  BOOST_REQUIRE( ! test_equality( {1, 2},    {3, 2}    ) );
  BOOST_REQUIRE( ! test_equality( {1, 2},    {1, 3}    ) );
  BOOST_REQUIRE( ! test_equality( {1, 2, 3}, {4, 2, 3} ) );
  BOOST_REQUIRE( ! test_equality( {1, 2, 3}, {1, 4, 3} ) );
  BOOST_REQUIRE( ! test_equality( {1, 2, 3}, {1, 2, 4} ) );
}

// Test multiplication
BOOST_AUTO_TEST_CASE(Test_DVector_multiplication_operator)
{
  BOOST_REQUIRE( DVector( (std::vector<double>){4} ) * DVector( (std::vector<double>){7} ) == 28 );
  BOOST_REQUIRE( DVector(    {3, 4} ) * DVector(    {6, 7} ) == 46 );
  BOOST_REQUIRE( DVector( {2, 3, 4} ) * DVector( {5, 6, 7} ) == 56 );
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace nsl

/* fin */