/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RW_MATH_EAA_HPP
#define RW_MATH_EAA_HPP

/**
 * @file EAA.hpp
 */
#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/math/Constants.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Rotation3DVector.hpp>
#include <rw/math/Vector3D.hpp>

#include <Eigen/Core>
#endif
namespace rw { namespace math {
    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A class for representing an equivalent angle-axis rotation
     *
     * This class defines an equivalent-axis-angle orientation vector also known
     * as an @f$ \thetak @f$ vector or "axis+angle" vector
     *
     * The equivalent-axis-angle vector is the product of a unit vector @f$
     * \hat{\mathbf{k}} @f$ and an angle of rotation around that axis @f$ \theta
     * @f$
     *
     * @note given two EAA vectors @f$ \theta_1\mathbf{\hat{k}}_1 @f$ and @f$
     * \theta_2\mathbf{\hat{k}}_2 @f$ it is generally not possible to subtract
     * or add these vectors, except for the special case when @f$
     * \mathbf{\hat{k}}_1 == \mathbf{\hat{k}}_2 @f$ this is why this class does
     * not have any subtraction or addition operators
     */
    template< class T = double > class EAA : public rw::math::Rotation3DVector< T >
    {
      public:
        /**
         * @brief Extracts Equivalent axis-angle vector from Rotation matrix
         *
         * @param R [in] A 3x3 rotation matrix @f$ \mathbf{R} @f$
         *
         * @f$
         * \theta = arccos(\frac{1}{2}(Trace(\mathbf{R})-1)=arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2})
         * @f$
         *
         * @f$
         * \thetak=log(\mathbf{R})=\frac{\theta}{2 sin \theta}(\mathbf{R}-\mathbf{R}^T) =
         * \frac{\theta}{2 sin \theta}
         * \left[
         * \begin{array}{c}
         * r_{32}-r_{23}\\
         * r_{13}-r_{31}\\
         * r_{21}-r_{12}
         * \end{array}
         * \right]
         * @f$
         *
         * @f$
         * \thetak=
         * \left[
         * \begin{array}{c}
         * 0\\
         * 0\\
         * 0
         * \end{array}
         * \right]
         * @f$ if @f$ \theta = 0 @f$
         *
         * @f$
         * \thetak=\pi
         * \left[
         * \begin{array}{c}
         * \sqrt{(R(0,0)+1.0)/2.0}\\
         * \sqrt{(R(1,1)+1.0)/2.0}\\
         * \sqrt{(R(2,2)+1.0)/2.0}
         * \end{array}
         * \right]
         * @f$ if @f$ \theta = \pi @f$
         *
         */
        explicit EAA (const rw::math::Rotation3D< T >& R);

        /**
         * @brief Constructs an EAA vector initialized to \f$\{0,0,0\}\f$
         */
        EAA () : _eaa (0, 0, 0) {}

        /**
         * @brief Constructs an initialized EAA vector
         * @param axis [in] \f$ \mathbf{\hat{k}} \f$
         * @param angle [in] \f$ \theta \f$
         * @pre norm_2(axis) = 1
         */
        EAA (const rw::math::Vector3D< T >& axis, T angle) : _eaa (axis * angle) {}

        /**
         * @brief Constructs an initialized EAA vector
         * @f$ \thetak =
         * \left[\begin{array}{c}
         *    \theta k_x\\
         *    \theta k_y\\
         *    \theta k_z
         * \end{array}\right]
         * @f$
         * @param thetakx [in] @f$ \theta k_x @f$
         * @param thetaky [in] @f$ \theta k_y @f$
         * @param thetakz [in] @f$ \theta k_z @f$
         */
        EAA (T thetakx, T thetaky, T thetakz) :
            _eaa (rw::math::Vector3D< T > (thetakx, thetaky, thetakz))
        {}

        /**
         * @brief Constructs an EAA vector that will rotate v1 into
         * v2. Where v1 and v2 are normalized and described in the same reference frame.
         * @param v1 [in] normalized vector
         * @param v2 [in] normalized vector
         */
        EAA (const rw::math::Vector3D< T >& v1, const rw::math::Vector3D< T >& v2);

        /**
         * @brief Constructs an initialized EAA vector
         *
         * The angle of the EAA are \f$\|eaa\|\f$ and the axis is \f$\frac{eaa}{\|eaa\|}\f$
         * @param eaa [in] Values to initialize the EAA
         */
        explicit EAA (rw::math::Vector3D< T > eaa) : _eaa (eaa) {}

        /**
         * @brief Copy Constructor
         * @param eaa [in] Values to initialize the EAA
         */
        EAA (const rw::math::EAA< T >& eaa) : _eaa (eaa._eaa) {}

        /**
         * @brief Constructs an initialized EAA vector
         *
         * The angle of the EAA are \f$\|eaa\|\f$ and the axis is \f$\frac{eaa}{\|eaa\|}\f$
         * @param eaa [in] Values to initialize the EAA
         */
        template< class R > explicit EAA (const Eigen::MatrixBase< R >& r) : _eaa (r) {}

        //! @brief destructor
        virtual ~EAA () {}

        /**
         * @brief Get the size of the EAA.
         * @return the size (always 3).
         */
        size_t size () const { return 3; }

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################
#if !defined(SWIGJAVA)
        /**
         * @copydoc Rotation3DVector::toRotation3D()
         *
         * @f$
         * \mathbf{R} = e^{[\mathbf{\hat{k}}],\theta}=\mathbf{I}^{3x3}+[\mathbf{\hat{k}}]
         * sin\theta+[{\mathbf{\hat{k}}}]^2(1-cos\theta) = \left[ \begin{array}{ccc}
         *      k_xk_xv\theta + c\theta & k_xk_yv\theta - k_zs\theta & k_xk_zv\theta + k_ys\theta \\
         *      k_xk_yv\theta + k_zs\theta & k_yk_yv\theta + c\theta & k_yk_zv\theta - k_xs\theta\\
         *      k_xk_zv\theta - k_ys\theta & k_yk_zv\theta + k_xs\theta & k_zk_zv\theta + c\theta
         *    \end{array}
         *  \right]
         * @f$
         *
         * where:
         * - @f$ c\theta = cos \theta @f$
         * - @f$ s\theta = sin \theta @f$
         * - @f$ v\theta = 1-cos \theta @f$
         */

#endif
        virtual const rw::math::Rotation3D< T > toRotation3D () const;

        /**
         * @brief Extracts the angle of rotation @f$ \theta @f$
         * @return @f$ \theta @f$
         */
        T angle () const { return _eaa.norm2 (); }

        /**
         * @brief change the angle of the EAA
         * @param angle [in] the new angle
         * @return this object
         */
        EAA< T >& setAngle (const T& angle)
        {
            (*this) = EAA< T > (this->axis (), angle);
            return (*this);
        }

        /**
         * @brief Extracts the axis of rotation vector @f$ \mathbf{\hat{\mathbf{k}}} @f$
         * @return @f$ \mathbf{\hat{\mathbf{k}}} @f$
         */
        const rw::math::Vector3D< T > axis () const
        {
            T theta = angle ();
            if (theta < 1e-6)
                return rw::math::Vector3D< T > (0, 0, 0);
            else
                return _eaa / theta;
        }

        /**
         * @brief get the underling Vector
         * @return the vector
         */
        rw::math::Vector3D< T >& toVector3D () { return this->_eaa; }

        /**
         * @brief get the underling Vector
         * @return the vector
         */
        rw::math::Vector3D< T > toVector3D () const { return this->_eaa; }

        /**
         * @brief get as eigen vector
         * @return Eigenvector
         */
        Eigen::Matrix< T, 3, 1 >& e () { return this->_eaa.e (); }

        /**
         * @brief get as eigen vector
         * @return Eigenvector
         */
        Eigen::Matrix< T, 3, 1 > e () const { return this->_eaa.e (); }

#if !defined(SWIG)
        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        const T& operator[] (size_t i) const
        {
            assert (i < 3);
            return _eaa[i];
        }

        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        T& operator[] (size_t i)
        {
            assert (i < 3);
            return _eaa[i];
        }

        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        const T& operator() (size_t i) const
        {
            assert (i < 3);
            return _eaa[i];
        }

        /**
         * @brief Returns element of EAA
         * @param i [in] index (@f$ 0 < i < 3 @f$)
         * @return the @f$ i @f$'th element
         */
        T& operator() (size_t i)
        {
            assert (i < 3);
            return _eaa[i];
        }
#else
        ARRAYOPERATOR (T);
#endif
        // ###################################################
        // #                 Math Operators                  #
        // ###################################################

        // ########## Eigen Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        template< class R > EAA< T > elemDivide (const Eigen::MatrixBase< R >& rhs) const
        {
            EAA< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._eaa[i] /= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        template< class R > EAA< T > elemMultiply (const Eigen::MatrixBase< R >& rhs) const
        {
            EAA< T > ret = *this;
            for (size_t i = 0; i < size (); i++) {
                ret._eaa[i] *= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > EAA< T > operator- (const Eigen::MatrixBase< R >& rhs) const
        {
            return EAA< T > (_eaa - rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend EAA< T > operator- (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs - rhs.e ());
        }

        /**
         * @brief Vector addition.
         */
        template< class R > EAA< T > operator+ (const Eigen::MatrixBase< R >& rhs) const
        {
            return EAA< T > (_eaa + rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend EAA< T > operator+ (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs + rhs.e ());
        }

        // ########### EAA Operators

        /**
         * @brief Unary minus.
         * @brief negative version
         */
        EAA< T > operator- () const { return EAA< T > (-_eaa); }

        /**
         * @brief element wise addition
         * @param rhs [in] the EAA to be added
         * @return the sum of the two EAA's
         */
        EAA< T > elemAdd (const EAA< T >& rhs) const { return EAA< T > (this->_eaa + rhs._eaa); }

        /**
         * @brief element wise subtraction
         * @param rhs [in] the EAA to be subtracted
         * @return the difference between the two EAA's
         */
        EAA< T > elemSubtract (const EAA< T >& rhs) const
        {
            return EAA< T > (this->_eaa - rhs._eaa);
        }

        /**
         * @brief element wise devision ( \b this / \b rhs )
         * @param rhs [in] the EAA to be devided with
         * @return the result of division
         */
        EAA< T > elemDivide (const EAA< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemDivide (rhs._eaa));
        }

        /**
         * @brief element wise multiplication
         * @param rhs [in] the EAA to be multiplyed with
         * @return the result of division
         */
        EAA< T > elemMultiply (const EAA< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemMultiply (rhs._eaa));
        }

        /**
         * @brief This is rotation multiplcation, and it is multiplication of two EAA's first
         * converted to a Rotation3D
         * @param rhs [in] the eaa to multiply with
         * @return the new rotation
         */
        EAA< T > operator* (const EAA< T >& rhs) const
        {
            return EAA< T > (this->toRotation3D () * rhs.toRotation3D ());
        }

        // ########### Rotation3D Operators
#if !defined(SWIG)
        /**
         * @brief Calculates \f$ \robabx{a}{c}{\thetak} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{\thetak}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bTKc [in] \f$ \robabx{b}{c}{\thetak} \f$
         * @return \f$ \robabx{a}{c}{\thetak} \f$
         */
        friend EAA< T > operator* (const rw::math::Rotation3D< T >& aRb, const EAA< T >& bTKc)
        {
            return EAA (aRb * bTKc._eaa);
        }
#endif

        /**
         * @brief matrix multiplication converting EAA to rotation
         * @param rhs [in] the roation matrix to multiply with
         * @return EAA ( this->toRotation3D() * rhs)
         */
        template< class R > EAA< T > operator* (const rw::math::Rotation3D< R >& rhs)
        {
            return EAA (this->toRotation3D () * rhs);
        }

        // ########### Scalar operators

        /**
         * @brief scalar multiplication
         * @param rhs [in] the scalar to multiply with
         * @return the product
         */
        EAA< T > elemMultiply (const T& rhs) const { return EAA< T > (this->_eaa * rhs); }

        /**
         * @brief scalar devision
         * @param rhs [in] the scalar to devide with
         * @return the resulting EAA
         */
        EAA< T > elemDivide (const T& rhs) const { return EAA< T > (this->_eaa / rhs); }

        /**
         * @brief Scalar subtraction.
         */
        EAA< T > elemSubtract (const T rhs) const { return EAA< T > (_eaa.elemSubtract (rhs)); }

        /**
         * @brief Scalar addition.
         */
        EAA< T > elemAdd (const T rhs) const { return EAA< T > (_eaa.elemAdd (rhs)); }

        /**
         * @brief scale the angle, keeping the axis the same
         * @param scale [in] how much the angle should change
         * @return a new EAA with the scaled angle
         */
        EAA< T > scaleAngle (const T& scale)
        {
            return EAA< T > (this->axis (), this->angle () * scale);
        }

        // ############ Vector3D Operators

        /**
         * @brief element wise multiplication.
         * @param rhs [in] the vector being devided with
         * @return the resulting EAA
         */
        EAA< T > operator+ (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa + rhs);
        }

#if !defined(SWIG)
        /**
         * @brief Vector addition
         * @param lhs [in] left side value
         * @param rhs [in] right side value
         * @return the resulting EAA
         */
        friend EAA< T > operator+ (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs + rhs._eaa);
        }
#endif

        /**
         * @brief Vector addition
         * @param rhs [in] the vector being added
         * @return the resulting EAA
         */
        EAA< T > operator- (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa - rhs);
        }

#if !defined(SWIG)
        /**
         * @brief Vector addition
         * @param lhs [in] left side value
         * @param rhs [in] right side value
         * @return the resulting EAA
         */
        friend EAA< T > operator- (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return EAA< T > (lhs - rhs._eaa);
        }
#endif
        /**
         * @brief element wise devision ( \b this / \b rhs )
         * @param rhs [in] the Vector to be devided with
         * @return the result of division
         */
        EAA< T > elemDivide (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemDivide (rhs));
        }

        /**
         * @brief element wise multiplication
         * @param rhs [in] the Vector to be multiplyed with
         * @return the result of division
         */
        EAA< T > elemMultiply (const rw::math::Vector3D< T >& rhs) const
        {
            return EAA< T > (this->_eaa.elemMultiply (rhs));
        }

        // ############ Ostream operators

#if !defined(SWIG)
        /**
         * @brief Ouputs EAA to stream
         * @param os [in/out] stream to use
         * @param eaa [in] equivalent axis-angle
         * @return the resulting stream
         */
        friend std::ostream& operator<< (std::ostream& os, const EAA< T >& eaa)
        {
            return os << " EAA( " << eaa (0) << ", " << eaa (1) << ", " << eaa (2) << ")";
        }
#else
        TOSTRING (rw::math::EAA< T >);
#endif
        // ############ Math Operations

        /**
         * @brief Calculates the cross product and returns the result
         * @param v [in] a Vector3D
         * @return the resulting 3D vector
         */
        rw::math::Vector3D< T > cross (const rw::math::Vector3D< T >& v) const
        {
            return rw::math::cross (this->_eaa, v);
        }

        /**
         * @brief Calculates the cross product and returns the result
         * @param eaa [in] a EAA
         * @return the resulting 3D vector
         */
        EAA< T > cross (const EAA< T >& eaa) const
        {
            return EAA< T > (rw::math::cross (this->_eaa, eaa._eaa));
        }

        /**
         * @brief Calculates the dot product and returns the result
         * @param v [in] a Vector3D
         * @return the resulting scalar
         */
        T dot (const rw::math::Vector3D< T >& v) { return rw::math::dot (this->_eaa, v); }

        /**
         * @brief Calculates the cross product and returns the result
         * @param eaa [in] a EAA
         * @return the resulting 3D vector
         */
        T dot (const EAA< T >& eaa) { return rw::math::dot (this->_eaa, eaa._eaa); }

        /**
         * @brief Returns the Euclidean norm (2-norm) of the vector
         * @return the norm
         */
        T norm2 () const { return _eaa.norm2 (); }

        /**
         * @brief Returns the Manhatten norm (1-norm) of the vector
         * @return the norm
         */
        T norm1 () const { return _eaa.norm1 (); }

        /**
         * @brief Returns the infinte norm (\f$\inf\f$-norm) of the vector
         * @return the norm
         */
        T normInf () const { return _eaa.normInf (); }

        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief copy operator
         * @param rhs [in] the EAA to be copied
         * @return reference to this EAA
         */
        EAA< T >& operator= (const EAA< T >& rhs)
        {
            this->_eaa = rhs._eaa;
            return *this;
        }

        /**
         * @brief assign vector to EAA
         * @param rhs [in] the vector to asign
         * @return reference to this EAA
         */
        EAA< T >& operator= (const rw::math::Vector3D< T >& rhs)
        {
            this->_eaa = rhs;
            return *this;
        }

        /**
         * @brief addition operator
         * @param rhs [in] the right hand side of the operation
         * @return reference to this EAA
         */
        EAA< T >& operator+= (const rw::math::Vector3D< T >& rhs)
        {
            this->_eaa += rhs;
            return *this;
        }

        /**
         * @brief subtraction operator
         * @param rhs [in] the right hand side of the operation
         * @return reference to this EAA
         */
        EAA< T >& operator-= (const rw::math::Vector3D< T >& rhs)
        {
            this->_eaa -= rhs;
            return *this;
        }
#if !defined(SWIG)
        /**
         * @brief Implicit converter to Vector3D
         */
        operator rw::math::Vector3D< T > () const { return _eaa; }

        /**
         * @brief Implicit converter to Vector3D
         */
        operator rw::math::Vector3D< T > & () { return _eaa; }
#endif
        /**
         * @brief copy a vector from eigen type
         * @param r [in] an Eigen Vector
         */
        template< class R > EAA< T >& operator= (const Eigen::MatrixBase< R >& r)
        {
            _eaa = r;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        template< class R > EAA< T >& operator+= (const Eigen::MatrixBase< R >& r)
        {
            _eaa += r;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > EAA< T >& operator-= (const Eigen::MatrixBase< R >& r)
        {
            _eaa -= r;
            return *this;
        }
#if !defined(SWIG)
        /**
         * @brief implicit conversion to EigenVector
         */
        operator Eigen::Matrix< T, 3, 1 > () const { return this->e (); }

        /**
         * @brief implicit conversion to EigenVector
         */
        operator Eigen::Matrix< T, 3, 1 > & () { return this->e (); }
#endif

        /**
         * @brief copy operator
         * @param rhs [in] the Rotation3D to be copied
         * @return reference to this EAA
         */
        EAA< T >& operator= (const rw::math::Rotation3D< T >& rhs)
        {
            return (*this) = EAA< T > (rhs);
        }

        // ###################################################
        // #                    Comparetors                  #
        // ###################################################

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const rw::math::Vector3D< T >& rhs) const { return _eaa == rhs; }

#if !defined(SWIG)
        /**
         * @brief Compare with \b rhs for equality.
         * @param lhs [in] first Vector
         * @param rhs [in] second vector.
         * @return True if a equals b, false otherwise.
         */
        friend bool operator== (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return lhs == rhs._eaa;
        }
#endif

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param rhs [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        bool operator!= (const rw::math::Vector3D< T >& rhs) const { return _eaa != rhs; }

#if !defined(SWIG)
        /**
         *  @brief Compare with \b rhs for inequality.
         * @param lhs [in] first Vector
         * @param rhs [in] second vector.
         *  @return True if a and b are different, false otherwise.
         */
        friend bool operator!= (const rw::math::Vector3D< T >& lhs, const EAA< T >& rhs)
        {
            return lhs != rhs._eaa;
        }
#endif

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const EAA< T >& rhs) const { return _eaa == rhs._eaa; }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param rhs [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        bool operator!= (const EAA< T >& rhs) const { return _eaa != rhs._eaa; }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R > bool operator== (const Eigen::MatrixBase< R >& rhs) const
        {
            return this->_eaa == rhs;
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R >
        friend bool operator== (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return lhs == rhs._eaa;
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R > bool operator!= (const Eigen::MatrixBase< R >& rhs) const
        {
            return !(*this == rhs);
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R >
        friend bool operator!= (const Eigen::MatrixBase< R >& lhs, const EAA< T >& rhs)
        {
            return !(lhs == rhs);
        }

      private:
        rw::math::Vector3D< T > _eaa;
    };

    template< class T >
    rw::math::Vector3D< T > cross (const rw::math::Vector3D< T >& v1, const EAA< T >& v2)
    {
        return rw::math::cross (v1, v2.axis () * v2.angle ());
    }

    /**
     * @brief Casts EAA<T> to EAA<Q>
     * @param eaa [in] EAA with type T
     * @return EAA with type Q
     */
    template< class Q, class T > const EAA< Q > cast (const EAA< T >& eaa)
    {
        return EAA< Q > (
            static_cast< Q > (eaa (0)), static_cast< Q > (eaa (1)), static_cast< Q > (eaa (2)));
    }

    using EAAd = EAA< double >;
    using EAAf = EAA< float >;

    /*@}*/

}}    // namespace rw::math

#if !defined(SWIG)
extern template class rw::math::EAA< double >;
extern template class rw::math::EAA< float >;
#else
SWIG_DECLARE_TEMPLATE (EAAd, rw::math::EAA< double >);
SWIG_DECLARE_TEMPLATE (EAAf, rw::math::EAA< float >);
#endif

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::EAA
         */
        template<>
        void write (const rw::math::EAA< double >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::EAA
         */
        template<>
        void write (const rw::math::EAA< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::EAA
         */
        template<>
        void read (rw::math::EAA< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::EAA
         */
        template<>
        void read (rw::math::EAA< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


/************************************************************************/
/*                   QR-PCA-FaceRec  by John Hany                       */
/*																		*/
/*	A face recognition algorithm using QR based PCA.              		*/
/*																		*/
/*	Released under MIT license.											*/
/*																		*/
/*	Contact me at johnhany@163.com										*/
/*																		*/
/*	Welcome to my blog http://johnhany.net/, if you can read Chinese:)	*/
/*																		*/
/************************************************************************/

#include <opencv2/opencv.hpp>
#include <armadillo>
#include <iostream>

using namespace std;

int component_num = 7;

string orl_path = "G:\\Datasets\\orl_faces";

enum distance_type {ECULIDEAN = 0, MANHATTAN, MAHALANOBIS};
//double distance_criterion[3] = { 10.0, 30.0, 3.0};
double distance_criterion[3] = { 1000.0, 1000.0, 1000.0};

bool compDistance(pair<int, double> a, pair<int, double> b);
double calcuDistance(const arma::vec vec1, const arma::vec vec2, distance_type dis_type);
double calcuDistance(const arma::vec vec1, const arma::vec vec2, const arma::mat cov2, distance_type dis_type);

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

	int class_num = 40;
	int sample_num = 10;

	int img_cols = 92;
	int img_rows = 112;
	cv::Size sample_size(img_cols, img_rows);

	arma::mat mat_sample(img_rows*img_cols, sample_num*class_num);

	//Load samples in one matrix `mat_sample`.

	for(int class_idx = 0; class_idx < class_num; class_idx++) {
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {

			string filename = orl_path + "\\s" + to_string(class_idx+1) + "\\" + to_string(sample_idx+1) + ".pgm";
			cv::Mat img_frame = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
			cv::Mat img_sample;
			cv::resize(img_frame, img_sample, sample_size);

			for(int i = 0; i < img_rows; i++) {
				uchar* pframe = img_sample.ptr<uchar>(i);
				for(int j = 0; j < img_cols; j++) {
					mat_sample(i*img_cols+j, class_idx*sample_num+sample_idx) = (double)pframe[j]/255.0;
				}
			}
		}
	}
//	cout <<	mat_sample.n_rows << endl << mat_sample.n_cols << endl << mat_sample(img_rows*img_cols/2, 0) << endl;

	//Calculate PCA transform matrix `mat_pca`.

	arma::mat H = mat_sample;
	arma::mat mean_x = arma::mean(mat_sample, 1);

	for(int j = 0; j < class_num * sample_num; j++) {
		H.col(j) -= mean_x.col(0);
	}
	H *= 1.0/sqrt(sample_num-1);

	arma::mat Q, R;
	arma::qr_econ(Q, R, H);

	arma::mat U, V;
	arma::vec d;
	arma::svd_econ(U, d, V, R.t());

//	cout << "d" << endl << d << endl;

//	arma::rowvec vec_eigen = d.head(component_num).t();
//	cout << "vec_eigen" << endl << vec_eigen << endl;

	arma::mat V_h(V.n_rows, component_num);
	if(component_num == 1) {
		V_h = V.col(0);
	}else {
		V_h = V.cols(0, component_num-1);
	}

	arma::mat mat_pca = Q * V_h;

	//Calculate eigenfaces `mat_eigen_vec`.

	arma::mat mat_eigen = mat_pca.t() * mat_sample;
//	cout << "mat_eigen" << endl << mat_eigen << endl;
	arma::mat mat_eigen_vec(component_num, class_num, arma::fill::zeros);
	vector<arma::mat> mat_cov_list;

	for(int class_idx = 0; class_idx < class_num; class_idx++) {

		arma::vec eigen_sum(component_num, arma::fill::zeros);
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {
			eigen_sum += mat_eigen.col(class_idx*sample_num+sample_idx);
		}
		eigen_sum /= (double)sample_num;
		mat_eigen_vec.col(class_idx) = eigen_sum;

		mat_cov_list.push_back(arma::cov((mat_eigen.cols(class_idx*sample_num, class_idx*sample_num+sample_num-1)).t()));

//		cout << mat_cov_list[class_idx] << endl;

	}

//	cout << "mat_eigen_vec" << endl << mat_eigen_vec << endl;

/*
	cout << "dis within class" << endl;
	for(int class_idx = 0; class_idx < class_num; class_idx++) {
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {
			double dis = calcuDistance(mat_eigen.col(class_idx*sample_num+sample_idx), mat_eigen_vec.col(class_idx), mat_cov_list[class_idx], distance_type::MAHALANOBIS);
			cout << dis << " ";
		}
		cout << endl;
	}

	cout << "dis between classes" << endl;
	for(int class_idx = 0; class_idx < class_num; class_idx++) {
		for(int sample_idx = 0; sample_idx < class_num; sample_idx++) {
			double dis = calcuDistance(mat_eigen.col(sample_idx*sample_num), mat_eigen_vec.col(class_idx), mat_cov_list[class_idx], distance_type::MAHALANOBIS);
			cout << dis << " ";
		}
		cout << endl;
	}
*/

	//Classify new sample.

	int correct_count = 0;

	double max_dis = 0.0;

	for(int class_idx = 0; class_idx < class_num; class_idx++){
		for(int sample_idx = 0; sample_idx < sample_num; sample_idx++) {
			arma::mat mat_new_sample(img_rows*img_cols, 1);

			string filename = orl_path + "\\s" + to_string(class_idx+1) + "\\" + to_string(sample_idx+1) + ".pgm";
			cv::Mat img_new_frame = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
			cv::Mat img_new_sample;
			cv::resize(img_new_frame, img_new_sample, sample_size);

			for(int i = 0; i < img_rows; i++) {
				uchar* pframe = img_new_sample.ptr<uchar>(i);
				for(int j = 0; j < img_cols; j++) {
					mat_new_sample(i*img_cols+j, 0) = (double)pframe[j]/255.0;
				}
			}

			arma::mat mat_new_eigen = mat_pca.t() * mat_new_sample;

			vector<pair<int, double>> dis_list;
			for(int new_class_idx = 0; new_class_idx < class_num; new_class_idx++) {
				double dis = calcuDistance(mat_new_eigen.col(0), mat_eigen_vec.col(new_class_idx), mat_cov_list[new_class_idx], distance_type::MAHALANOBIS);
				dis_list.push_back(make_pair(new_class_idx, dis));
			}
			sort(dis_list.begin(), dis_list.end(), compDistance);

			if(dis_list[0].first == class_idx && dis_list[0].second <= distance_criterion[distance_type::MAHALANOBIS]) {
				correct_count++;
			}

			if(dis_list.back().second > max_dis) {
				max_dis = dis_list.back().second;
			}
		}
	}

	cout << "Maximum distance: " << max_dis << endl;

	double correct_ratio = (double)correct_count / (class_num * sample_num);
	cout << "Correctness ratio: " << correct_ratio * 100.0 << "%" << endl;

	cin.get();

	return 0;
}

bool compDistance(pair<int, double> a, pair<int, double> b) {
	return (a.second < b.second);
}

double calcuDistance(const arma::vec vec1, const arma::vec vec2, distance_type dis_type) {

	if(dis_type == ECULIDEAN) {
		return arma::norm(vec1-vec2, 2);
	}else if(dis_type == MANHATTAN) {
		return arma::norm(vec1-vec2, 1);
	}else if(dis_type == MAHALANOBIS) {
		arma::mat tmp = (vec1-vec2).t() * (vec1 - vec2);
		return sqrt(tmp(0,0));
	}

	return -1.0;
}

double calcuDistance(const arma::vec vec1, const arma::vec vec2, const arma::mat cov2, distance_type dis_type) {

	if(dis_type == ECULIDEAN) {
		return arma::norm(vec1-vec2, 2);
	}else if(dis_type == MANHATTAN) {
		return arma::norm(vec1-vec2, 1);
	}else if(dis_type == MAHALANOBIS) {
		arma::mat tmp = (vec1-vec2).t() * cov2.i() * (vec1 - vec2);
		return sqrt(tmp(0,0));
	}

	return -1.0;
}


#pragma once

#include "util/random.h"

#include "util/point_util.h"

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

namespace metternich {

double random::generate_radian_angle()
{
	return random::generate_in_range(0., 1.) * 2. * boost::math::constants::pi<double>();
}

QPointF random::generate_circle_position()
{
	const double angle = random::generate_radian_angle();
	return point::get_radian_angle_direction(angle);
}

}


//
// 	Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
// 	Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <iostream>

int main()
{
  namespace ublas = boost::numeric::ublas;
  using value   = float;
  using layout  = ublas::layout::first_order; // storage format
  using tensor  = ublas::tensor_dynamic<value,layout>;
  using shape   = typename tensor::extents_type;
  using matrix  = ublas::matrix<value,layout>;

  constexpr auto ones = ublas::ones<value,layout>{};

  // NOLINTNEXTLINE(google-build-using-namespace)
  using namespace boost::numeric::ublas::index;

  using namespace boost::numeric::ublas::index;
  using tensor  = boost::numeric::ublas::tensor_dynamic<float>;
  auto fones    = boost::numeric::ublas::ones<float>{};

  tensor X = fones(3,4,5);
  tensor Y = fones(4,6,3,2);

  tensor Z = 2*ones(5,6,2) + X(_i,_j,_k)*Y(_j,_l,_i,_m) + 5;

  // Matlab Compatible Formatted Output
  std::cout << "C=" << Z << ";" << std::endl;

  // Tensor-Vector-Multiplications - Including Transposition
  try {

    auto n  = shape{3,4,2};

    tensor A  = ones(n);
    matrix B1 = 2*matrix(n[1],n[2]);
    tensor v1 = 2*ones(n[0],1);
    tensor v2 = 2*ones(n[1],1);
    //      auto v3 = tensor(shape{n[2],1},2);

    // C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);
    // tensor C1 = B1 + prod(A,vector_t(n[0],1),1);
    tensor C1 = B1 + A(_i,_,_) * v1(_i,_);

    // C2(i,k) = A(i,j,k)*v2(j) + 4;
    //tensor C2 = prod(A,vector_t(n[1],1),2) + 4;
    tensor C2 = A(_,_i,_) * v2(_i,_) + 4;

    // not yet implemented!
    // C3() = A(i,j,k)*T1(i)*T2(j)*T2(k);
        // tensor C3 = prod(prod(prod(A,v1,1),v2,1),v3,1);
    // tensor C3 = A(_i,_j,_k) * v1(_i,_) * v2(_j,_) * v3(_k,_);

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);" << std::endl << std::endl;
    std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C2(i,k) = A(i,j,k)*v2(j) + 4;" << std::endl << std::endl;
    std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of multiply-tensor-einstein-notation when doing tensor-vector multiplication." << std::endl;
  }

  // Tensor-Matrix-Multiplications - Including Transposition
  try {
    auto n  = shape{3,4,2};
    auto m  = 5u;
    tensor A  = 2*ones(n);
    tensor B  = 2*ones(n[1],n[2],m);
    tensor B1 =   ones(m,n[0]);
    tensor B2 =   ones(m,n[1]);

    // C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);
    // tensor C1 = B + prod(A,B1,1);
    tensor C1 = B + A(_i,_,_) * B1(_,_i);

    // C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;
    // tensor C2 = prod(A,B2) + 4;
    tensor C2 =  A(_,_j,_) * B2(_,_j) + 4;

    // C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);
    // not yet implemented.

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);" << std::endl << std::endl;
    std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;" << std::endl << std::endl;
    std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

    // formatted output
    //        std::cout << "% --------------------------- " << std::endl;
    //        std::cout << "% --------------------------- " << std::endl << std::endl;
    //        std::cout << "% C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);" << std::endl << std::endl;
    //        std::cout << "C3=" << C3 << ";" << std::endl << std::endl;
  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of multiply-tensor-einstein-notation when doing tensor-matrix multiplication." << std::endl;
  }

  // Tensor-Tensor-Multiplications Including Transposition
  try {
    auto na = shape{3,4,5};
    auto nb = shape{4,6,3,2};
    tensor A  = 2*ones(na);
    tensor B  = 3*ones(nb);
    tensor T1 = 2*ones(na[2],na[2]);
    tensor T2 = 2*ones(na[2],nb[1],nb[3]);

    // C1(j,l) = T1(j,l) + A(i,j,k)*A(i,j,l) + 5;
    // tensor C1 = T1 + prod(A,A,perm_t{1,2}) + 5;
    tensor C1 = T1 + A(_i,_j,_m)*A(_i,_j,_l) + 5;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "% C1(k,l) = T1(k,l) + A(i,j,k)*A(i,j,l) + 5;" << std::endl << std::endl;
    std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

    // C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;
    //tensor C2 = T2 + prod(A,B,perm_t{1,2},perm_t{3,1}) + 5;
    tensor C2 = T2 + A(_i,_j,_k)*B(_j,_l,_i,_m) + 5;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "%  C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;" << std::endl << std::endl;
    std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of multiply-tensor-einstein-notation when doing transpose." << std::endl;
  }
}


#include <state_estimation/utilities/plotting_utilities.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace state_estimation {

std::vector<Eigen::Vector2d> getEllipsePoints(double a, double b, const Eigen::Vector2d& offset,
                                              double angle, uint32_t num_pts) {
    std::vector<Eigen::Vector2d> pts(num_pts);

    // Sample the points evenly with respect to angle around the ellipse
    const Eigen::Rotation2D<double> R(angle);
    for (int i = 0; i < num_pts; ++i) {
        const double theta = i * 2.0 * M_PI / num_pts;

        const Eigen::Vector2d pos(a * cos(theta), b * sin(theta));
        pts[i] = R * pos + offset;
    }

    return pts;
}

std::vector<Eigen::Vector2d> get2DCovarianceEllipsePoints(const Eigen::Matrix2d& cov,
                                                          const Eigen::Vector2d& offset,
                                                          uint32_t num_pts) {
    // Get the eigen vectors
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 2, 2>> solver(cov);
    Eigen::Matrix<double, 2, 2> vectors = solver.eigenvectors();
    Eigen::Matrix<double, 2, 1> values = solver.eigenvalues();

    // The major axis corresponds to the first eigen vector, and the orientation of the ellipse is
    // the directory of the first eigen vector
    const double a = values(0);
    const double b = values(1);
    const double angle = atan2(vectors(1, 0), vectors(0, 0));
    return getEllipsePoints(a, b, offset, angle, num_pts);
}

}  // namespace state_estimation


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(4,4);
MatrixXd A = X * X.transpose();
cout << "Here is a random positive-definite matrix, A:" << endl << A << endl << endl;

SelfAdjointEigenSolver<MatrixXd> es(A);
cout << "The inverse square root of A is: " << endl;
cout << es.operatorInverseSqrt() << endl;
cout << "We can also compute it with operatorSqrt() and inverse(). That yields: " << endl;
cout << es.operatorSqrt().inverse() << endl;

  return 0;
}


#include "main.hpp"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <limits>
#include <mimkl/data_structures.hpp>
#include <mimkl/definitions.hpp>
#include <mimkl/io.hpp>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/easy_mkl.hpp>
#include <mimkl/solvers/komd.hpp>
#include <mimkl/utilities.hpp>
#include <numeric>
#include <spdlog/fmt/ostr.h>
#include <spdlog/spdlog.h>

using dlib::mat;
using mimkl::data_structures::DataFrame;
using mimkl::data_structures::indexing_from_vector;
using mimkl::data_structures::range;
using mimkl::definitions::Indexing;
using mimkl::utilities::check_invocable;
using mimkl::utilities::print_type;

//#define SPDLOG_DEBUG_ON
//#define SPDLOG_TRACE_ON

// Compile time log levels
// define SPDLOG_DEBUG_ON or SPDLOG_TRACE_ON
// SPDLOG_TRACE(console, "Enabled only #ifdef SPDLOG_TRACE_ON..{} ,{}",
// 1, 3.23);  SPDLOG_DEBUG(console, "Enabled only #ifdef SPDLOG_DEBUG_ON.. {}
// ,{}", 1, 3.23);

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

    // Runtime log levels
    spdlog::set_level(spdlog::level::trace); // Set global log level to info
    auto console = spdlog::stdout_color_mt("console");

    const Index rows = 6; // 3 to reproduce single member in class  error
    const Index dims = 2;

    MATRIX(double) X(rows, 2);
    //	X << 1., 1., 3., 1., 1., 2.;
    X << 1., 1., 3., 1., 1., 4., 3., 2., 1., -1., 3., -1.;
    console->info("X\n{}", X);

    std::vector<std::string> labels;
    labels.reserve(rows);
    labels.push_back("a");
    labels.push_back("b");
    labels.push_back("a");
    labels.push_back("b");
    labels.push_back("c");
    labels.push_back("c");

    Eigen::SparseMatrix<double> L(2, 2);
    mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);

    Eigen::SparseMatrix<double> L1(dims, dims);
    typedef Eigen::Triplet<double> TripletDouble; // (row,col,coef)
    std::vector<TripletDouble> triplet_list;
    triplet_list.reserve(4);
    triplet_list.push_back(TripletDouble(0, 1, 1.));
    //  triplet_list.push_back(TripletDouble(1, 2, 1.));
    triplet_list.push_back(TripletDouble(1, 0, 1.));
    //  triplet_list.push_back(TripletDouble(2, 1, 1.));
    L1.setFromTriplets(triplet_list.begin(), triplet_list.end());

    std::vector<Eigen::SparseMatrix<double>> inducer_vec;
    inducer_vec.reserve(2);
    inducer_vec.push_back(L);
    inducer_vec.push_back(L1);

    typedef std::function<MATRIX(double)(const MATRIX(double) &, const MATRIX(double) &,
                                         const Eigen::SparseMatrix<double>)>
    InducerFunction;
    typedef std::function<MATRIX(double)(const MATRIX(double) &,
                                         const MATRIX(double) &)>
    InducedFunction;

    const double degree = 1.;
    const double offset = 0.;

    auto lambda_logger = spdlog::stdout_color_mt("lambda");
    InducerFunction k_poly =
    [degree, offset](const MATRIX(double) & lhs, const MATRIX(double) & rhs,
                     const Eigen::SparseMatrix<double> inducer) {
        //	 spdlog::get("lambda")->debug("before actual inducer function
        // call" 			 "lhs rows {}  cols {} \n"
        //			 "rhs rows {}  cols {} \n"
        //			 "kernel rows {}  cols {} \n"
        //			 "inducer rows {}  cols {} \n",
        //			 lhs.rows(), lhs.cols(),
        //			 rhs.rows(), rhs.cols(),
        //			 kernel_matrix.rows(), kernel_matrix.cols(),
        //			 inducer.rows(), inducer.cols()
        //			 );
        return mimkl::induction::induce_polynomial_kernel<MATRIX(double)>(
        lhs, rhs, inducer, degree, offset);
    };

    MATRIX(double) K(rows, rows);
    console->info("\e[1;32mtype: \e[0m \n{}",
                  print_type<decltype(k_poly(X, X, L))>());
    console->info("\e[1;32mtype: \e[0m \n{}", print_type(k_poly));
    console->info("check_invocable? {}", check_invocable(k_poly));

    std::vector<InducedFunction> function_vec =
    mimkl::induction::inducer_combination(k_poly, inducer_vec);
    //// 	==inducer_combination:
    //	  for (Eigen::SparseMatrix<double> inducer : inducer_vec) {
    //		  function_vec.push_back(
    //			[&,inducer](const MATRIX(double) &lhs,
    //					  const MATRIX(double)&rhs,
    //					  const MATRIX(double) &kernel_matrix) {
    //			  k_poly(lhs, rhs, kernel_matrix, inducer);
    //			});
    //
    //	  }
    std::vector<MATRIX(double)> kernel_vec;
    kernel_vec.reserve(2);

    MATRIX(double) K2 = function_vec[0](X, X);
    console->info("a kernel from function (linear kernel with identity):\n{}",
                  K2);
    kernel_vec.push_back(K2);

    MATRIX(double) K_norm = mimkl::linear_algebra::normalize_kernel(K2);
    console->info("normalized above kernel:\n{}", K_norm);

    MATRIX(double) X_norm = X;
    X_norm.rowwise().normalize();
    console->info("normalized above kernel by means of centralizing the "
                  "original "
                  "data (works for linear kernel as feature space is original "
                  "space):\n{}",
                  function_vec[0](X_norm, X_norm));
    console->trace("where the normalized data is:\n{}", X_norm);

    assert(((K_norm - function_vec[0](X_norm, X_norm)).norm() <= 0.0000001) &&
           "normalize_kernels not equal to normalization in original space for "
           "linear kernel");

    assert(
    ((K_norm - mimkl::linear_algebra::normalize_kernel_prediction(K2, K2, K2))
     .norm() == 0.0) &&
    "normalize_kernels not equal to normalize_kernels_prediction");

    MATRIX(double) K_center = mimkl::linear_algebra::centralize_kernel(K2);
    console->info("centralized above kernel:\n{}", K_center);

    MATRIX(double) X_center = X;
    X_center = X.rowwise() - X.colwise().mean(); // in each dimension remove mean
    console->info("centralized above kernel by means of centralizing the "
                  "original data (works for linear kernel as feature space is "
                  "original space):\n{}",
                  function_vec[0](X_center, X_center));
    console->trace("where the centered data is:\n{}", X_center);

    assert(
    ((K_center - function_vec[0](X_center, X_center)).norm() == 0.0) &&
    "centralize_kernels not equal to centralization in original space for "
    "linear kernel");

    console->info("centralized normalized_kernel:\n{}",
                  mimkl::linear_algebra::centralize_kernel(K_norm));
    console->info("normalized centralized_kernel:\n{}",
                  mimkl::linear_algebra::normalize_kernel(K_center));

    MATRIX(double) K3 = function_vec[1](X, X);
    console->info("another kernel from function:\n{}", K3);
    kernel_vec.push_back(K3);

    MATRIX(double) kernel_sum_reference(rows, rows);
    kernel_sum_reference << 4., 8., 10., 10., 0., 4., 8., 16., 20., 20., 0., 8.,
    10., 20., 25., 25., 0., 10., 10., 20., 25., 25., 0., 10., 0., 0., 0., 0.,
    0., 0., 4., 8., 10., 10., 0., 4.;
    // TODO normalize samples/Kernel/kernel_trace/...

    spdlog::stdout_color_mt("lin_alg");

    COLUMN(double) c = COLUMN(double)::Constant(function_vec.size(), 1);
    MATRIX(double)
    kernel_weighted_sum =
    mimkl::linear_algebra::aggregate_weighted_kernels(X, X, function_vec, c);
    MATRIX(double)
    kernel_sum = mimkl::linear_algebra::aggregate_kernels(X, X, function_vec);
    console->trace("sum of kernels:\n{}", kernel_sum);
    console->trace("sum of (same) weighted kernels:\n{}", kernel_weighted_sum);

    assert(((kernel_sum_reference - kernel_sum).norm() == 0.0) &&
           "aggregate_kernels");
    assert(((kernel_sum_reference - kernel_weighted_sum).norm() == 0.0) &&
           "aggregate_weighted_kernels");

    //  test mapping

    double pi = 3.14159265358979323846; // std::atan(1.)*4. ;
    console->info(
    "testing/debugging the mapping between eigen and dlib, pi: {}", pi);
    COLUMN(double) to_map = COLUMN(double)::Constant(4, pi);
    console->info("initial (to_map):\n{}\n sum: {}", to_map, to_map.sum());
    dlib::matrix<double, 0, 1> mapped = dlib::mat(to_map);
    change_dlib_mat(mapped);
    console->info("mapped and assigned to dlib (mapped):\n{}\n sum: {}", mapped,
                  dlib::sum(mapped));
    console->info("change in original? (to_map):\n{}\n sum: {}", to_map,
                  to_map.sum());
    //  change_dlib_mat(dlib::mat(to_map));
    //  console->info("change in original when mapping changed without
    //  assignment?
    //  (to_map):\n{}\n sum: {}", to_map, to_map.sum());

    COLUMN(double) back_mapped = mimkl::linear_algebra::dlib_to_eigen(mapped);
    console->info("backmapped with assignment (back_mapped):\n{}\n sum: {}",
                  back_mapped, back_mapped.sum());

    MATRIX(double) M;
    mimkl::linear_algebra::squared_euclidean_distances(X, M);
    M = M.array().sqrt();
    console->info("euclidean distances:\n{}", M);

    return EXIT_SUCCESS;
}


#include <boost/test/unit_test.hpp>
#include <ayla/thread_pool.hpp>

#include <iostream>

BOOST_AUTO_TEST_SUITE(ayla)
BOOST_AUTO_TEST_SUITE(thread_pool)

namespace {
	SizeType _factorial(SizeType n) {
		if (n == 0u) {
			return 1u;
		}

		return n*_factorial(n-1);
	}
}

class FactorialCalculator final : public ThreadPool::Task {
public:
	FactorialCalculator(SizeType n)
		: _n(n), _result(0u)
	{ }

	virtual void execute() override {
		_result = _factorial(_n);
	}

	inline SizeType getResult() { return _result; }
	inline SizeType getN() const { return _n; }

private:
	SizeType _n;
	SizeType _result;
};

BOOST_AUTO_TEST_CASE( factorialTest ) {
	BOOST_CHECK(_factorial(0) == 1);
	BOOST_CHECK(_factorial(5) == 120);
	BOOST_CHECK(_factorial(6) == 720);

	const SizeType numOfTasks = 12u;

	ThreadPool::TasksGroup group;
	ThreadPool pool;

	// create tasks group
	for (SizeType i = 0u; i < numOfTasks; ++i) {
		group.push_back(std::make_unique<FactorialCalculator>(rand() % numOfTasks));
	}

	// force the same execution some times
	for (SizeType i = 0u; i < 7u; ++i) {
		pool.execute(group);
	}

	// check results
	for (auto it = group.begin(); it != group.end(); ++it) {
		const auto task = static_cast<FactorialCalculator*>(it->get());
		BOOST_CHECK(_factorial(task->getN()) == task->getResult());
	}
}

////////////////////////////////////////////////////////////////////

#define MATRIX_ORDER 100
#define MAX_MATRIX_VALUE 100
#define MIN_MATRIX_VALUE -100

typedef std::vector<std::vector<Float> > TestMatrix;

void printM(const TestMatrix& m) {
	for (SizeType i = 0u; i < MATRIX_ORDER; ++i) {
		for (SizeType j = 0u; j < MATRIX_ORDER; ++j) {
			std::cout << m[i][j] << " ";
		}

		std::cout << std::endl;
	}

	std::cout << std::endl;
}

Float multiplyRowColumn(const TestMatrix& mA, SizeType aRow, const TestMatrix& mB, SizeType bCol) {
	Float sum = 0.0f;

	for (SizeType k = 0u; k < MATRIX_ORDER; ++k) {
		sum += mA[aRow][k] * mB[k][bCol];
	}

	return sum;
}

void multiply(const TestMatrix& mA, const TestMatrix& mB, TestMatrix& result) {
	for (SizeType i = 0u; i < MATRIX_ORDER; ++i) {
		for (SizeType j = 0u; j < MATRIX_ORDER; ++j) {
			result[i][j] = multiplyRowColumn(mA, i, mB, j);
		}
	}
}

class ParallelMatrixMult : public ThreadPool::Task {
public:
	ParallelMatrixMult(const TestMatrix& mA, SizeType aRow, const TestMatrix& mB, SizeType bCol, TestMatrix& result)
		: mA(mA), mB(mB), result(result), aRow(aRow), bCol(bCol)
	{ }

	virtual void execute() {
		result[aRow][bCol] = multiplyRowColumn(mA, aRow, mB, bCol);
	}

private:
	const TestMatrix& mA;
	const TestMatrix& mB;
	TestMatrix& result;

	SizeType aRow, bCol;
};

BOOST_AUTO_TEST_CASE( matrixMultiplicationTest ) {
	// initialize matrices and other data:

	TestMatrix m0( MATRIX_ORDER, std::vector<Float>(MATRIX_ORDER, 0.0f) );
	TestMatrix m1( MATRIX_ORDER, std::vector<Float>(MATRIX_ORDER, 0.0f) );
	TestMatrix r( MATRIX_ORDER, std::vector<Float>(MATRIX_ORDER, 0.0f) ); // multiplication result
	TestMatrix mt_r( MATRIX_ORDER, std::vector<Float>(MATRIX_ORDER, 0.0f) ); // multithread multiplication result

	for (SizeType i = 0u; i < MATRIX_ORDER; ++i) { // initialize matrices
		for (SizeType j = 0u; j < MATRIX_ORDER; ++j) {
			m0[i][j] = Float(MIN_MATRIX_VALUE + ( rand()%(MAX_MATRIX_VALUE-MIN_MATRIX_VALUE) ));
			m1[i][j] = Float(MIN_MATRIX_VALUE + ( rand()%(MAX_MATRIX_VALUE-MIN_MATRIX_VALUE) ));
		}
	}

	// initialize thread pool:

	ThreadPool pool(5u);
	BOOST_CHECK_EQUAL(pool.getNumberOfThreads(), 5u);

	ThreadPool::TasksGroup group;

	for (SizeType i = 0u; i < MATRIX_ORDER; ++i) { // initialize matrices
		for (SizeType j = 0u; j < MATRIX_ORDER; ++j) {
			group.push_back(std::make_unique<ParallelMatrixMult>(m0, i, m1, j, mt_r));
		}
	}

	for (SizeType i = 0u; i < 3u; ++i) { // force same execution some times
		// run serial:
		multiply(m0, m1, r);

		// run parallel:
		pool.execute(group);

		// compare serial and multithread results:
		for (SizeType i = 0u; i < MATRIX_ORDER; ++i) { // initialize matrices
			for (SizeType j = 0u; j < MATRIX_ORDER; ++j) {
				BOOST_CHECK( mt_r[i][j] == r[i][j] );
			}
		}
	}

	// printM(m0);
	// printM(m1);
	// printM(r);
	// printM(mt_r);
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


/**
 * @file burgersequation.cc
 * @brief NPDE homework BurgersEquation code
 * @author Oliver Rietmann
 * @date 15.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "burgersequation.h"

#include <Eigen/Core>
#include <cmath>

namespace BurgersEquation {
/* SAM_LISTING_BEGIN_1 */
constexpr double PI = 3.14159265358979323846;

double Square(double x) { return x * x; }

double w0(double x) {
  return 0.0 <= x && x <= 1.0 ? Square(std::sin(PI * x)) : 0.0;
}

double f(double x) { return 2.0 / 3.0 * std::sqrt(x * x * x); }

Eigen::VectorXd solveBurgersGodunov(double T, unsigned int N) {
  double h = 5.0 / N;           // meshwidth
  double tau = h;               // timestep = meshwidth by CFL condition
  int m = std::round(T / tau);  // no. of timesteps

  // initialize vector with initial nodal values
  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N + 1, -1.0, 4.0);
  Eigen::VectorXd mu = x.unaryExpr(&w0);

#if SOLUTION
  for (int i = 0; i < m; ++i) {
    for (int j = N; 0 < j; --j) {
      // Standard fully discrete evolution based on explicit Euler timestepping
      mu(j) = mu(j) - tau / h * (f(mu(j)) - f(mu(j - 1)));
    }
    // truncation to a finite vector. Only required on one side, because all
    // information flows from left to right.
    mu(0) = 0.0;  // Value of u0 to the left of x=0
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return mu;
}
/* SAM_LISTING_END_1 */

/**
 * @brief Converts a large vector on  a grid to a smaller vector correponding to
 * a sub-grid.
 *
 * @param mu vector of function values on a spacial grid of size N_large
 * @param N divides the size N_large of mu
 * @return a vector mu_sub of size N, that represents mu on a sub-grid of size N
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd reduce(const Eigen::VectorXd &mu, unsigned int N) {
  Eigen::VectorXd mu_sub(N + 1);
  int fraction = mu.size() / N;
  for (int j = 0; j < N + 1; ++j) {
    mu_sub(j) = mu(j * fraction);
  }
  return mu_sub;
}

Eigen::Matrix<double, 3, 4> numexpBurgersGodunov() {
  const unsigned int N_large = 3200;
  Eigen::Vector2d T{0.3, 3.0};
  Eigen::Vector4i N{5 * 10, 5 * 20, 5 * 40, 5 * 80};
  Eigen::Vector4d h;
  for (int i = 0; i < 4; ++i) h(i) = 5.0 / N(i);

  Eigen::Matrix<double, 3, 4> result;
  result.row(0) = h.transpose();

#if SOLUTION
  for (int k = 0; k < 2; ++k) {
    Eigen::VectorXd mu_ref = solveBurgersGodunov(T(k), N_large);
    Eigen::Vector4d error;
    for (int i = 0; i < 4; ++i) {
      Eigen::VectorXd mu = solveBurgersGodunov(T(k), N(i));
      Eigen::VectorXd mu_ref_sub = reduce(mu_ref, N(i));
      error(i) = h(i) * (mu - mu_ref_sub).lpNorm<1>();
    }
    result.row(k + 1) = error.transpose();
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return result;
}
/* SAM_LISTING_END_2 */

}  // namespace BurgersEquation


//
// Created by philipp on 22.12.19.
//

#ifndef FUNNELS_CPP_LYAPUNOV_HH
#define FUNNELS_CPP_LYAPUNOV_HH

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <cmath>
#include <memory>

namespace lyapunov{

  template <class DERIVED>
  double projected_max_radius(const Eigen::MatrixBase<DERIVED> & C0,
                                     const Eigen::MatrixBase<DERIVED> & C1){
    // todo optimize for usage of triangular shape
    auto Cinv = C0.inverse();
    auto P1_prime = (Cinv.transpose()*(C1.transpose()*C1)*Cinv);
    double max_rad = 1./P1_prime.eigenvalues().real().array().minCoeff();
//    Eigen::MatrixXd Cinv = C0.inverse();
//    Eigen::MatrixXd P1_prime = (Cinv.transpose()*(C1.transpose()*C1)*Cinv);
//    double max_rad = 1./P1_prime.eigenvalues().real().array().minCoeff();
    return std::sqrt(max_rad);
  }

  struct lyap_zone_t{};

  class lyapunov_t{
  public:
    virtual bool intersect(const lyap_zone_t & other) const;
    virtual bool covers(const lyap_zone_t & other) const;
    virtual bool is_covered(const lyap_zone_t & other) const;
    virtual double get_alpha() const;
  };

  struct fixed_ellipsoidal_zone_t{
    virtual const Eigen::VectorXd & x0() const {
      std::cerr << __FILE__ << ": " << __LINE__ << std::endl;
      throw std::runtime_error("Virtual");
      return Eigen::Vector2d::Zero();
    };
    virtual const Eigen::MatrixXd & C() const {
      std::cerr << __FILE__ << ": " << __LINE__ << std::endl;
      throw std::runtime_error("Virtual");
      return Eigen::Matrix2d::Zero();
    };
  };

  struct fixed_ellipsoidal_zone_copied_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_copied_t( const Eigen::VectorXd & x0,
        const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    Eigen::VectorXd _x0;
    Eigen::MatrixXd _C;
  };

  struct fixed_ellipsoidal_zone_ref_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_ref_t( const Eigen::VectorXd & x0,
                                     const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    const Eigen::VectorXd &_x0;
    const Eigen::MatrixXd &_C;
  };

  // todo
//  // Does zone0 cover zone1
//  template<class DIST>
//  bool covers_helper(const fixed_ellipsoidal_zone_t &zone0,
//                     const fixed_ellipsoidal_zone_t &zone1,
//                     DIST &dist){
//
//
//    // First compute the projected distance
//    auto dy = zone0.C()*(dist.cp_vv(zone1.x0(), zone0.x0()));
//    double dy_norm = dy.norm(); //l2 norm
//    // center out of bounds
//    if (dy_norm>=1.){
//      return false;
//    }
//
//    // Compute radius
//    return dy_norm+projected_max_radius(zone0.C(), zone1.C())<=1.;
//  }

  template<class TRAJ, class DIST>
  class fixed_ellipsoidal_lyap_t: public TRAJ{
  public:
    using dist_t = DIST;
    using traj_t = TRAJ;
    using dyn_t = typename traj_t::dyn_t;

    using matrix_t = typename traj_t::matrix_t;
    using vector_x_t = typename traj_t::vector_x_t;
    using vector_u_t = typename traj_t::vector_u_t;
    using vector_t_t = typename traj_t::vector_t_t;
    using matrix_ptr_t = typename traj_t::matrix_ptr_t;
    using vector_x_ptr_t = typename traj_t::vector_x_ptr_t;
    using vector_u_ptr_t = typename traj_t::vector_u_ptr_t;
    using vector_t_ptr_t = typename traj_t::vector_t_ptr_t;

    using traj_t::dimx;
    using traj_t::dimp;
    using traj_t::dimv;
    using traj_t::dimu;

    using s_mat_t = Eigen::Matrix<double,dimx,dimx>;
    using s_vec_t = Eigen::Matrix<double,dimx,1>;

    using args = std::tuple<std::shared_ptr<s_mat_t>, double, DIST*>;

    template <class DERIVED>
    fixed_ellipsoidal_lyap_t(const Eigen::MatrixBase<DERIVED> &P,
        double gamma, DIST &dist):
        _gamma(gamma), _dist(dist){
      set_P(P);
    }

    template <class ...CARGS>
    void compute(CARGS &&...cargs){
      // Just forwarding
      traj_t::compute(cargs...);
    }

    template <class DERIVED>
    void set_P(const Eigen::MatrixBase<DERIVED>& P) {
      assert(P.rows() == dimx);
      assert(P.rows() == P.cols());

      // Compute cholesky
      Eigen::LLT<Eigen::Matrix<double, dimx, dimx>> llt_pre_comp;
      llt_pre_comp.compute(P);
      _C = llt_pre_comp.matrixU();

      // Compute the bounding box corner
      std::cout << P << std::endl;
      std::cout << _C << std::endl;
      for(size_t i=0; i<dimx; i++){
        _box_corner(i) = 1./_C.row(i).norm();
      }
      std::cout << _box_corner << std::endl;
    }
    s_mat_t get_P(){
      return _C.transpose()*_C;
    }

    const s_mat_t &C() const {
      return _C;
    }
    double gamma() const {
      return _gamma;
    }
    s_vec_t min_corner(){
      return -_box_corner;
    }
    const s_vec_t &max_corner(){
      return _box_corner;
    }
// todo
//    // Conservative intersect
//    // If true, the zone may intersect with this
//    // If false, the zone does definitively not intersect
//    template<class DERIVED>
//    bool intersect(const Eigen::MatrixBase<DERIVED> &x0,
//        const fixed_ellipsoidal_zone_t &zone) const {
//      // First compute the projected distance
//      auto dy = _C*(_dist.cp_vv(zone.x0(), x0));
//      double dy_norm = dy.norm(); //l2 norm
//
//      if (dy_norm <= 1.){
//        return true;
//      }
//
//      return dy_norm>=1.+projected_max_radius(_C, zone.C());
//    }
//
//    // Conservative cover
//    // If true, zone is definitively covered by this
//    // If false, it may not be covered
//    template<class DERIVED>
//    bool covers(const Eigen::MatrixBase<DERIVED> &x0,
//        const fixed_ellipsoidal_zone_t &zone) const {
//      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
//      return covers_helper(this_zone, zone, _dist);
//    }
//
//    // Conservative cover
//    // If true, this is definitively covered by zone
//    // If false, it may not be covered
//    template<class DERIVED>
//    bool is_covered(const Eigen::MatrixBase<DERIVED> &x0,
//        const fixed_ellipsoidal_zone_t &zone) const {
//      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
//      return covers_helper(zone, this_zone, _dist);
//    }

    double get_alpha(){
      return (double) 1./((_C.transpose()*_C)(0,0));
    }

    double get_gamma()const{
      return _gamma;
    }
    void set_gamma(double gamma){
      _gamma = gamma;
    }

    dist_t & dist() const {
      return _dist;
    }

    args get_args()const{
//      Eigen::MatrixXd P_scaled = _C.transpose()*_C;
      std::shared_ptr<s_mat_t> P = std::make_shared<s_mat_t>();
      *P = _C.transpose()*_C;
      return std::make_tuple(P, _gamma, &_dist);
    }

  protected:
    s_mat_t _C; // todo: Optimize to use triangular shape
    s_vec_t _box_corner;
    double _gamma;
    DIST &_dist;
  };

}

#endif //FUNNELS_CPP_LYAPUNOV_HH


#ifndef QUATERNION_AVERAGING_H
#define QUATERNION_AVERAGING_H

//
#include <vector>
#include <stdexcept>      // std::length_error
//
//#include <Eigen/Core>
//#include <Eigen/Eigenvalues>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Eigenvalues>
//
#include <tf/transform_datatypes.h> // tf quaternion
#include <tf2/LinearMath/Quaternion.h> // tf2 quaternion

namespace math_quat {

    static tf2::Quaternion getAverageQuaternion(
            const std::vector<tf2::Quaternion> &quaternions,
            const std::vector<double> &weights)
    {

        if (quaternions.size() != weights.size())
            throw std::length_error("Weights and quaternions needs to be same"
                                    " length");

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < quaternions.size(); ++i) {
            // Weigh the quaternions according to their associated weight
            tf2::Quaternion quat = quaternions[i] * weights[i];
            // Append the weighted Quaternion to a matrix Q.
            Q(0, i) = quat.x();
            Q(1, i) = quat.y();
            Q(2, i) = quat.z();
            Q(3, i) = quat.w();
        }

        // Creat a solver for finding the eigenvectors and eigenvalues
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());

        // Find index of maximum (real) Eigenvalue.
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i) {
            double real = eigenvalues[i].real();
            if (real > max_value) {
                max_value = real;
                max_idx = i;
            }
        }

        // Get corresponding Eigenvector, normalize it and return it as the average quat
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();

        tf2::Quaternion mean_orientation(
                eigenvector[0].real(),
                eigenvector[1].real(),
                eigenvector[2].real(),
                eigenvector[3].real()
        );

        return mean_orientation.normalized();
    }

    static tf::Quaternion getAverageQuaternion(
            const std::vector<tf::Quaternion> &quaternions,
            const std::vector<double> &weights)
    {
        if (quaternions.size() != weights.size())
            throw std::length_error("Weights and quaternions needs to be same"
                                    " length");

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < quaternions.size(); ++i) {
            // Weigh the quaternions according to their associated weight
            tf::Quaternion quat = quaternions[i] * weights[i];
            // Append the weighted Quaternion to a matrix Q.
            Q(0, i) = quat.x();
            Q(1, i) = quat.y();
            Q(2, i) = quat.z();
            Q(3, i) = quat.w();
        }

        // Creat a solver for finding the eigenvectors and eigenvalues
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());

        // Find index of maximum (real) Eigenvalue.
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i) {
            double real = eigenvalues[i].real();
            if (real > max_value) {
                max_value = real;
                max_idx = i;
            }
        }

        // Get corresponding Eigenvector, normalize it and return it as the average quat
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();

        tf::Quaternion mean_orientation(
                eigenvector[0].real(),
                eigenvector[1].real(),
                eigenvector[2].real(),
                eigenvector[3].real()
        );

        return mean_orientation.normalized();
    }

} //ns
#endif //QUATERNION_AVERAGING_H


#include <cassert>
#include <cmath>

#include <type_traits>
#include <limits>
#include <climits>

#include <boost/integer/integer_mask.hpp>

namespace hlf {
    namespace math {

        constexpr long double pi = 3.141592653589793238462643383279502884;
        constexpr long double pi_2 = pi / 2.0;
        constexpr long double pi_4 = pi / 4.0;
        constexpr long double twicePi = 2.0 * pi;

        constexpr long double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
        constexpr long double RAD_TO_DEGREE = 1.0 / DEGREE_TO_RAD;

        template<typename TFloat>
        inline TFloat degToRad(const TFloat degree) {
            return static_cast<TFloat>(degree * DEGREE_TO_RAD);
        }

        template<typename TFloat>
        inline TFloat radToDeg(const TFloat radian) {
            return static_cast<TFloat>(radian * RAD_TO_DEGREE);
        }

/// Convert [0, 360] degrees bearing into [-pi, pi] azimuth.
        template<typename TFloat>
        inline TFloat Bearing2Azimuth(const TFloat degree) {
            TFloat radians = degToRad(degree);
            assert(radians >= 0.0);
            return (radians > pi ? radians - twicePi : radians);
        }

/// Positive angle between 2 azimuths.
        template<typename TFloat>
        inline TFloat AngleBetween(TFloat rad1, TFloat rad2) {
            TFloat res = rad1 - rad2;
            if (res < 0.0)
                res += twicePi;
            return (res > pi ? twicePi - res : res);
        }

        template<class T>
        T Log2(T x) {
            return log(x) / log(2);
        }

        template<typename T>
        inline T Abs(T x) {
            return (x < 0 ? -x : x);
        }

// Compare floats or doubles for almost equality.
// maxULPs - number of closest floating point values that are considered equal.
// Infinity is treated as almost equal to the largest possible floating point values.
// NaN produces undefined result.
// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
// for details.
        template<typename TFloat>
        bool AlmostEqualULPs(TFloat x, TFloat y, unsigned int maxULPs = 256) {
            static_assert(std::is_floating_point<TFloat>::value, "");
            static_assert(std::numeric_limits<TFloat>::is_iec559, "");

            // Make sure maxUlps is non-negative and small enough that the
            // default NaN won't compare as equal to anything.
            assert(maxULPs < 4 * 1024 * 1024);

            int const bits = CHAR_BIT * sizeof(TFloat);
            typedef typename boost::int_t<bits>::exact IntType;
            typedef typename boost::uint_t<bits>::exact UIntType;

            IntType xInt = *reinterpret_cast<IntType const *>(&x);
            IntType yInt = *reinterpret_cast<IntType const *>(&y);

            // Make xInt and yInt lexicographically ordered as a twos-complement int
            IntType const highestBit = IntType(1) << (bits - 1);
            if (xInt < 0)
                xInt = highestBit - xInt;
            if (yInt < 0)
                yInt = highestBit - yInt;

            UIntType const diff = Abs(xInt - yInt);

            return diff <= maxULPs;
        }

// Returns true if x and y are equal up to the absolute difference eps.
// Does not produce a sensible result if any of the arguments is NaN or infinity.
// The default value for eps is deliberately not provided: the intended usage
// is for the client to choose the precision according to the problem domain,
// explicitly define the precision constant and call this function.
        template<typename TFloat>
        inline bool AlmostEqualAbs(TFloat x, TFloat y, TFloat eps) {
            return fabs(x - y) < eps;
        }

// Returns true if x and y are equal up to the relative difference eps.
// Does not produce a sensible result if any of the arguments is NaN, infinity or zero.
// The same considerations as in AlmostEqualAbs apply.
        template<typename TFloat>
        inline bool AlmostEqualRel(TFloat x, TFloat y, TFloat eps) {
            return fabs(x - y) < eps * max(fabs(x), fabs(y));
        }

        template<typename T>
        inline T id(T const &x) {
            return x;
        }

        template<typename T>
        inline T sq(T const &x) {
            return x * x;
        }

        template<typename T, typename TMin, typename TMax>
        inline T clamp(T x, TMin xmin, TMax xmax) {
            if (x > xmax)
                return xmax;
            if (x < xmin)
                return xmin;
            return x;
        }

        template<typename T>
        inline T cyclicClamp(T x, T xmin, T xmax) {
            if (x > xmax)
                return xmin;
            if (x < xmin)
                return xmax;
            return x;
        }

        template<typename T>
        inline bool between_s(T a, T b, T x) {
            return (a <= x && x <= b);
        }

        template<typename T>
        inline bool between_i(T a, T b, T x) {
            return (a < x && x < b);
        }

        inline int rounds(double x) {
            return (x > 0.0 ? int(x + 0.5) : int(x - 0.5));
        }

        inline size_t SizeAligned(size_t size, size_t align) {
            // static_cast    .
            return size + (static_cast<size_t>(-static_cast<ptrdiff_t>(size)) & (align - 1));
        }

        template<typename T>
        bool IsIntersect(T const &x0, T const &x1, T const &x2, T const &x3) {
            return !((x1 < x2) || (x3 < x0));
        }

// Computes x^n.
        template<typename T>
        inline T PowUint(T x, uint64_t n) {
            T res = 1;
            for (T t = x; n > 0; n >>= 1, t *= t)
                if (n & 1)
                    res *= t;
            return res;
        }

        template<typename T>
        inline T NextModN(T x, T n) {
            return x + 1 == n ? 0 : x + 1;
        }

        template<typename T>
        inline T PrevModN(T x, T n) {
            return x == 0 ? n - 1 : x - 1;
        }

        inline uint32_t NextPowOf2(uint32_t v) {
            v = v - 1;
            v |= (v >> 1);
            v |= (v >> 2);
            v |= (v >> 4);
            v |= (v >> 8);
            v |= (v >> 16);

            return v + 1;
        }

// Greatest Common Divisor
        template<typename T>
        T GCD(T a, T b) {
            T multiplier = 1;
            T gcd = 1;
            while (true) {
                if (a == 0 || b == 0) {
                    gcd = max(a, b);
                    break;
                }

                if (a == 1 || b == 1) {
                    gcd = 1;
                    break;
                }

                if ((a & 0x1) == 0 && (b & 0x1) == 0) {
                    multiplier <<= 1;
                    a >>= 1;
                    b >>= 1;
                    continue;
                }

                if ((a & 0x1) != 0 && (b & 0x1) != 0) {
                    T const minV = min(a, b);
                    T const maxV = max(a, b);
                    a = (maxV - minV) >> 1;
                    b = minV;
                    continue;
                }

                if ((a & 0x1) != 0)
                    std::swap(a, b);
                a >>= 1;
            }

            return multiplier * gcd;
        }

    }
}


#include <Eigen/Dense>
#include "lsq.h"

LSQ::LSQ(const Eigen::MatrixXf& A, const Eigen::VectorXf& b){
    this->A = A;
    this->b = b;
}

LSQ::~LSQ(){}

Eigen::VectorXf LSQ::solve(){
    return this->A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(this->b);
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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_MATH_FFT_REAL_ALGORITHMS_HPP
  #define BOOST_MATH_FFT_REAL_ALGORITHMS_HPP

  #include <algorithm>
  #include <numeric>
  #include <cmath>

  #include <boost/math/fft/simple_complex.hpp>

  namespace boost { namespace math {  namespace fft {

  namespace detail {

  template<class T>
  inline void real_dft_2(
    const T* in,
    T* out, int)
  {
    T o1 = in[0]+in[1], o2 = in[0]-in[1] ;
    out[0] = o1;
    out[1] = o2;
  }

  template<class RealType>
  RealType complex_root_of_unity_real(long n,long p=1)
  /*
    Computes cos(-2 pi p/n)
  */
  {
    p = modulo(p,n);

    if(p==0)
      return RealType(1);

    long g = gcd(p,n);
    n/=g;
    p/=g;
    switch(n)
    {
      case 1:
        return RealType(1);
      case 2:
        return p==0 ? RealType(1) : RealType(-1);
      case 4:
        return p==0 ? RealType(1) :
               p==1 ? RealType(0) :
               p==2 ? RealType(-1) :
                      RealType(0) ;
    }
    using std::cos;
    RealType phase = -2*p*boost::math::constants::pi<RealType>()/n;
    return RealType(cos(phase));
  }
  template<class RealType>
  RealType complex_root_of_unity_imag(long n,long p=1)
  /*
    Computes sin(-2 pi p/n)
  */
  {
    p = modulo(p,n);

    if(p==0)
      return RealType(0);

    long g = gcd(p,n);
    n/=g;
    p/=g;
    switch(n)
    {
      case 1:
        return RealType(0);
      case 2:
        return p==0 ? RealType(0) : RealType(0);
      case 4:
        return p==0 ? RealType(0) :
               p==1 ? RealType(-1) :
               p==2 ? RealType(0) :
                      RealType(1) ;
    }
    using std::sin;
    RealType phase = -2*p*boost::math::constants::pi<RealType>()/n;
    return RealType(sin(phase));
  }

  template <class T>
  void real_dft_power2(const T *in_first, const T *in_last, T* out, int/*sign*/)
  {
    /*
      Cooley-Tukey mapping, in-place Decimation in Time
    */
    const long ptrdiff = static_cast<long>(std::distance(in_first,in_last));
    if(ptrdiff <=0 )
      return;
    const long n = lower_bound_power2(ptrdiff);

    if(in_first!=out)
      std::copy(in_first,in_last,out);

    if (n == 1)
        return;

    // Gold-Rader bit-reversal algorithm.
    for(int i=0,j=0;i<n-1;++i)
    {
      if(i<j)
        std::swap(out[i],out[j]);
      for(int k=n>>1;!( (j^=k)&k );k>>=1);
    }

    for (int len = 2, prev_len = 1; len <= n; len <<= 1,prev_len<<=1)
    {
      for (int i = 0; i < n; i += len)
      {
        {
          // j=0;
          T* u = out + i, *v = out + i + prev_len;
          T Bu = *u, Bv = *v;
          *u = Bu + Bv;
          *v = Bu - Bv;
        }
        for(int j=1;j < prev_len/2;++j)
        {
          T cos{ complex_root_of_unity_real<T>(len,j) },
            sin{ complex_root_of_unity_imag<T>(len,j) };

          T *ux = out + i + j,
            *uy = out + i + len - j;

          T *vx = out + i + prev_len - j,
            *vy = out + i + prev_len + j;

          T prev_ux = *ux,
            prev_uy = *uy;

          T prev_vx = *vx,
            prev_vy = *vy;

          *ux = prev_ux + cos * prev_vy + sin * prev_uy;
          *uy = prev_vx + cos * prev_uy - sin * prev_vy;
          *vx = prev_ux - cos * prev_vy - sin * prev_uy;
          *vy =-prev_vx + cos * prev_uy - sin * prev_vy;
        }
        //if(prev_len>=2)
        //{
        //  const int j = prev_len/2;
        //  T* u = out + i + j, *v = out + i + j + prev_len;
        //  T Bu = *u, Bv = *v;
        //  *u = Bu;
        //  *v = Bv;
        //}
      }
    }
  }
  template <class T>
  void real_inverse_dft_power2(const T *in_first, const T *in_last, T* out, int /*sign*/)
  {
    /*
      Cooley-Tukey mapping, in-place Decimation in Time
      Reverse flow graph of the real_dft_power2
    */
    const long ptrdiff = static_cast<long>(std::distance(in_first,in_last));
    if(ptrdiff <=0 )
      return;
    const long n = lower_bound_power2(ptrdiff);

    if(in_first!=out)
      std::copy(in_first,in_last,out);

    if (n == 1)
        return;

    for (int len = n, prev_len = len/2; len >= 2; len >>= 1,prev_len>>=1)
    {
      for (int i = 0; i < n; i += len)
      {
        {
          // j=0;
          T* u = out + i, *v = out + i + prev_len;
          T Bu = *u, Bv = *v;
          *u = Bu + Bv;
          *v = Bu - Bv;
        }
        for(int j=1;j < prev_len/2;++j)
        {
          T cos{ complex_root_of_unity_real<T>(len,-j) },
            sin{ complex_root_of_unity_imag<T>(len,-j) };

          T *ux = out + i + j,
            *uy = out + i + len - j;

          T *vx = out + i + prev_len - j,
            *vy = out + i + prev_len + j;

          T prev_ux = *ux,
            prev_uy = *uy;

          T prev_vx = *vx,
            prev_vy = *vy;

          T sum_x = prev_ux + prev_vx,
            dif_x = prev_ux - prev_vx,
            sum_y = prev_uy + prev_vy,
            dif_y = prev_uy - prev_vy;

          *ux = sum_x;
          *vx = dif_y;
          *uy = cos * sum_y - sin*dif_x;
          *vy = cos * dif_x + sin*sum_y;
        }
        if(prev_len>=2)
        {
          const int j = prev_len/2;
          T* u = out + i + j, *v = out + i + j + prev_len;
          T Bu = *u, Bv = *v;
          *u = Bu + Bu;
          *v = Bv + Bv;
        }
      }
    }

    // Gold-Rader bit-reversal algorithm.
    for(int i=0,j=0;i<n-1;++i)
    {
      if(i<j)
        std::swap(out[i],out[j]);
      for(int k=n>>1;!( (j^=k)&k );k>>=1);
    }
  }

  template<class T>
  void real_dft_prime_bruteForce_outofplace(
    const T* in_first,
    const T* in_last,
    T* out, int sign)
  /*
    assumptions:
    - allocated memory in out is enough to hold distance(in_first,in_last) element,
    - out!=in
  */
  {
    const long N = static_cast<long>(std::distance(in_first,in_last));
    if(N<=0)
      return;

    out[0] = std::accumulate(in_first+1,in_last,in_first[0]);

    for(long i=1,j=N-1;i<j;++i,--j)
    {
      T sum_x{in_first[0]},sum_y = 0;
      for(long l=1;l<N; ++l)
      {
        sum_x += in_first[l] * complex_root_of_unity_real<T>(N,i*l*sign);
        sum_y += in_first[l] * complex_root_of_unity_imag<T>(N,i*l*sign);
      }
      // if(i<j) // i==j never happens for odd sizes
      out[j] = -sum_y;
      out[i] = sum_x;
    }
  }

  template<class T,class Allocator_t>
  void real_dft_prime_bruteForce_inplace(
    T* in_first,
    T* in_last,
    int sign,
    const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_dft_prime_bruteForce_outofplace(in_first,in_last,work_space.data(),sign);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_dft_prime_bruteForce(
    const T* in_first,
    const T* in_last,
    T* out,
    int sign,
    const Allocator_t& alloc)
  {
    if(in_first==out)
      real_dft_prime_bruteForce_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_dft_prime_bruteForce_outofplace(in_first,in_last,out,sign);
  }

  template<class T>
  void real_inverse_dft_prime_bruteForce_outofplace(
    const T* in_first,
    const T* in_last,
    T* out, int sign)
  /*
    assumptions:
    - allocated memory in out is enough to hold distance(in_first,in_last) element,
    - out!=in
  */
  {
    const long N = static_cast<long>(std::distance(in_first,in_last));
    if(N<=0)
      return;

    {
      T sum_x{0.};
      for(long i=1,j=N-1;i<j;++i,--j)
      {
        sum_x += in_first[i];
      }
      // if(i<j) // i==j never happens for odd sizes
      out[0] = ( 2*sum_x + in_first[0]);
    }
    for(long l=1;l<N; ++l)
    {
      T sum_x{0.},sum_y{0.};
      for(long i=1,j=N-1;i<j;++i,--j)
      {
        sum_x += in_first[i] * complex_root_of_unity_real<T>(N,i*l*sign);
        sum_y += in_first[j] * complex_root_of_unity_imag<T>(N,i*l*sign);
      }
      // if(i<j) // i==j never happens for odd sizes
      out[l] = (2*sum_x - 2*sum_y + in_first[0]);
    }
  }

  template<class T,class Allocator_t>
  void real_inverse_dft_prime_bruteForce_inplace(
    T* in_first,
    T* in_last,
    int sign,
    const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_inverse_dft_prime_bruteForce_outofplace(in_first,in_last,work_space.data(),sign);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_inverse_dft_prime_bruteForce(
    const T* in_first,
    const T* in_last,
    T* out,
    int sign,
    const Allocator_t& alloc)
  {
    if(in_first==out)
      real_inverse_dft_prime_bruteForce_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_inverse_dft_prime_bruteForce_outofplace(in_first,in_last,out,sign);
  }

  template <class T, class allocator_t>
  void real_dft_composite_outofplace(
            const T *in_first,
            const T *in_last,
            T* out,
            int /*sign*/,
            const allocator_t& alloc)
  {
    /*
      Cooley-Tukey mapping, intrinsically out-of-place, Decimation in Time
      composite sizes.
    */
    using allocator_type = allocator_t;
    using ComplexType = simple_complex<T>;
    using ComplexAllocator = typename std::allocator_traits<allocator_type>::template rebind_alloc<ComplexType>;

    const long n = static_cast<long>(std::distance(in_first,in_last));
    if(n <=0 )
      return;

    if (n == 1)
    {
        out[0]=in_first[0];
        return;
    }
    std::array<int,32> prime_factors;
    const int nfactors = prime_factorization(n,prime_factors.begin());

    // reorder input
    for (long i = 0; i < n; ++i)
    {
        long j = 0, k = i;
        for (int ip=0;ip<nfactors;++ip)
        {
            int p = prime_factors[ip];
            j = j * p + k % p;
            k /= p;
        }
        out[j] = in_first[i];
    }

    std::reverse(prime_factors.begin(), prime_factors.begin()+nfactors);

    //auto show = [&] (const T* beg, const T* end)
    //{
    //  for(;beg!=end;++beg)
    //  {
    //    std::cout << *beg << ", ";
    //  }
    //  std::cout << "\n";
    //};

    // butterfly pattern
    long len = 1;
    for (int ip=0;ip<nfactors;++ip)
    {
      int p = prime_factors[ip];
      long len_old = len;
      len *= p;

      std::vector<ComplexType,ComplexAllocator> tmp(p,ComplexType(),alloc);
      //std::cout << "pass " << ip << "\n";
      for (long i = 0; i < n; i += len)
      {
        //std::cout << "    i = " << i << "\n";
        for(long k=0;2*k<=len_old;++k)
        {
          if(k==0)
          {
            tmp[0] = ComplexType{out[i],0.};
            for(long j=1;j<p;++j)
              tmp[j] = ComplexType{out[i + j*len_old],0.};
          }else if(2*k == len_old)
          {
            tmp[0] = ComplexType{out[i + k ],0.};
            for(long j=1;j<p;++j)
              tmp[j] = ComplexType{out[i + j*len_old +k ],0.}
                * complex_root_of_unity<ComplexType>(len,k*j);
          }else
          {
            tmp[0] = ComplexType{out[i + k ],-out[i+len_old-k]};
            for(long j=1;j<p;++j)
              tmp[j] = ComplexType{out[i + j*len_old +k ],-out[i+j*len_old + len_old-k]}
                * complex_root_of_unity<ComplexType>(len,k*j);
          }
          if(p==2)
          {
            complex_dft_2(tmp.data(),tmp.data(),1);
          }
          else
          {
            complex_dft_prime_rader<ComplexType,ComplexAllocator>(tmp.data(),tmp.data()+p,tmp.data(),1,alloc);
          }
          for(long j=0;j<p;++j)
          {
            int posx = j*len_old + k, posy = len - k - j*len_old;

            if(posx==0)
            {
              out[i+posx] = tmp[j].real();
            }else if(posx>posy)
            {
              out[i+posx] = tmp[j].imag();
              out[i+posy] = tmp[j].real();
            }else
            {
              out[i+posy] = -tmp[j].imag();
              out[i+posx] = tmp[j].real();
            }
          }
        }
        //show(out+i,out+i+len);
      }
    }
  }

  template<class T,class Allocator_t>
  void real_dft_composite_inplace(
          T* in_first,
          T* in_last,
          int sign,
          const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_dft_composite_outofplace(in_first,in_last,work_space.data(),sign,alloc);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_dft_composite(
          const T* in_first,
          const T* in_last,
          T* out,
          int sign,
          const Allocator_t& alloc)
  {
    if(in_first==out)
      real_dft_composite_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_dft_composite_outofplace(in_first,in_last,out,sign,alloc);
  }

  template <class T, class allocator_t>
  void real_inverse_dft_composite_outofplace(
            const T *in_first,
            const T *in_last,
            T* out,
            int /*sign*/,
            const allocator_t& alloc)
  {
    /*
      Cooley-Tukey mapping, intrinsically out-of-place, Decimation in Time
      composite sizes.
      Reverse graph.
    */
    using allocator_type = allocator_t;
    using ComplexType = simple_complex<T>;
    using ComplexAllocator = typename std::allocator_traits<allocator_type>::template rebind_alloc<ComplexType>;

    const long n = static_cast<long>(std::distance(in_first,in_last));
    if(n <=0 )
      return;

    std::copy(in_first,in_last,out);
    if (n == 1)
        return;

    std::array<int,32> prime_factors;
    const int nfactors = prime_factorization(n,prime_factors.begin());

    // butterfly pattern
    for (long ip=0,len=n,prev_len = len;ip<nfactors;++ip,len = prev_len)
    {
      int p = prime_factors[ip];
      prev_len = len/p;

      std::vector<ComplexType,ComplexAllocator> tmp(p,ComplexType(),alloc);
      for (long i = 0; i < n; i += len)
      {
        for(long k=0;2*k<=prev_len;++k)
        {
          for(long j=0;j<p;++j)
          {
            int posx = j*prev_len + k, posy = len - k - j*prev_len;

            if(posx==0 || posx==posy)
            {
              tmp[j] = ComplexType {out[i+posx],0.};
            }else if(posx>posy)
            {
              tmp[j] = ComplexType{out[i+posy],out[i+posx]};
            }else // if(posx<posy)
            {
              tmp[j] = ComplexType{out[i+posx],-out[i+posy]};
            }
          }
          if(p==2)
          {
            complex_dft_2(tmp.data(),tmp.data(),-1);
          }
          else
          {
            complex_dft_prime_rader<ComplexType,ComplexAllocator>(tmp.data(),tmp.data()+p,tmp.data(),-1,alloc);
          }

          if(k==0)
          {
            out[i] = tmp[0].real();
            for(long j=1;j<p;++j)
              out[i + j*prev_len] = tmp[j].real();
          }else if(2*k == prev_len)
          {
            out[i + k ] = tmp[0].real();
            for(long j=1;j<p;++j)
              out[i + j*prev_len +k ] = (tmp[j]* complex_root_of_unity<ComplexType>(len,-k*j) ). real();
          }else
          {
            out[i+k]          =  tmp[0].real();
            out[i+prev_len-k] = -tmp[0].imag();
            for(long j=1;j<p;++j)
            {
              ComplexType cplx = tmp[j] * complex_root_of_unity<ComplexType>(len,-k*j);
              out[i + j*prev_len +k ]        = cplx.real();
              out[i+j*prev_len + prev_len-k] = -cplx.imag();
            }
          }
        }
      }
    }

    std::vector<T,allocator_type> tmp(out,out+n);
    // reorder
    for (long i = 0; i < n; ++i)
    {
        long j = 0, k = i;
        for (int ip=0;ip<nfactors;++ip)
        {
            int p = prime_factors[ip];
            j = j * p + k % p;
            k /= p;
        }
        out[i] = tmp[j];
    }
  }

  template<class T,class Allocator_t>
  void real_inverse_dft_composite_inplace(
          T* in_first,
          T* in_last,
          int sign,
          const Allocator_t& alloc)
  {
    std::vector<T,Allocator_t> work_space(in_first,in_last,alloc);
    real_inverse_dft_composite_outofplace(in_first,in_last,work_space.data(),sign,alloc);
    std::copy(work_space.begin(),work_space.end(),in_first);
  }
  template<class T, class Allocator_t>
  void real_inverse_dft_composite(
          const T* in_first,
          const T* in_last,
          T* out,
          int sign,
          const Allocator_t& alloc)
  {
    if(in_first==out)
      real_inverse_dft_composite_inplace(out,out+std::distance(in_first,in_last),sign,alloc);
    else
      real_inverse_dft_composite_outofplace(in_first,in_last,out,sign,alloc);
  }

  } // namespace detail

  } } } // namespace boost::math::fft

#endif // BOOST_MATH_FFT_REAL_ALGORITHMS_HPP


#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;
using Eigen::MatrixXd;

using namespace std;
using std::setw;
using std::setprecision;

int main(int argc, char** argv)
{
	MatrixXd J_pseudoInv(2,2);     //  NULL space projection
	MatrixXd A = MatrixXd::Random(2,2);

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU |Eigen::ComputeThinV);
	J_pseudoInv = svd.matrixV()*svd.singularValues().inverse()*svd.matrixU().transpose();

	cout << "Hello World! "<<J_pseudoInv<<endl;
	cout << "J_pseudoInv*A "<<J_pseudoInv*A<<endl;

	return 0;
}


#include <iostream>
#include <cmath>

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

int main(int argc, const char **argv)
{
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));

    std::cout .precision(3);
    std::cout << "rotation matrix =\n" << rotation_vector.matrix() << std::endl;

    rotation_matrix = rotation_vector.toRotationMatrix();

    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl;

    v_rotated = rotation_matrix * v;
    std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl;

    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
    std::cout << "yaw pitch roll = " << euler_angles.transpose() << std::endl;

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    std::cout << "Transform matrix = \n" << T.matrix() << std::endl;
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1, 3, 4));
    std::cout << "Transform matrix = \n" << T.matrix() << std::endl;

    Eigen::Vector3d v_transformed = T * v;
    std::cout << "v transformed = " << v_transformed.transpose() << std::endl;

    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    std::cout << "quaternion = \n" << q.coeffs() << std::endl;

    q = Eigen::Quaterniond(rotation_matrix);
    std::cout << "quaternion = \n" << q.coeffs() << std::endl;

    v_rotated = q * v;
    std::cout << "(1, 0, 0) after rotation = " << v_rotated.transpose() << std::endl;

    return 0;
}