/*
 * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
 *
 * Licensed under either the GNU Lesser General Public License v3.0 :
 * https://www.gnu.org/licenses/lgpl-3.0.html
 * or the GNU Lesser General Public License v2.1 :
 * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 * at your option.
 */

#ifndef IDYNTREE_ARTICULATED_BODY_INERTIA_H
#define IDYNTREE_ARTICULATED_BODY_INERTIA_H

#include <iDynTree/Core/MatrixFixSize.h>
#include <string>

namespace iDynTree
{
    class SpatialInertia;
    class SpatialForceVector;
    class SpatialMotionVector;
    class Wrench;
    class SpatialAcc;

    /**
     * Class representing an Articulated Body Inertia.
     *
     * For more information on Articulated Body Inertia, please
     * check Featherstone 2008, Chapter 7 .
     *
     * Storage:
     * The symmetric articulated body inertia is stored as 3
     * 3x3 matrices :
     *  * the linearLinear is the top left submatrix
     *  * the linearAngular is the top right submatrix
     *  * the angularAngular is the bottom right submatrix
     *
     * The bottom left submatrix can be obtained as the
     * transpose of the linearAngular matrix.
     *
     *
     *
     * \warning This class is exposing for convenience the IMatrix interface.
     *          Notice that using this methods you can damage the underlyng articulated body inertia.
     *          In doubt, don't use them and rely on more high level functions.
     *
     * \ingroup iDynTreeCore
     */
    class ArticulatedBodyInertia
    {
    private:
        Matrix3x3 linearLinear;
        Matrix3x3 linearAngular;
        Matrix3x3 angularAngular;

    public:
        /**
         * The data is not reset to zeo for perfomance reason.
         * Please initialize the data in the class before any use.
         */
        ArticulatedBodyInertia();

        /**
         * Constructor from a buffer of 36 doubles,
         * stored as a C-style array (i.e. row major).
         *
         */
        ArticulatedBodyInertia(const double* in_data,
                               const unsigned int in_rows,
                               const unsigned int in_cols);

        /**
         * Constructor from a Rigid Body Inertia.
         */
        ArticulatedBodyInertia(const iDynTree::SpatialInertia& rigidBodyInertia);

        /**
         * Copy constructor: create a ArticulatedBodyInertia from another ArticulatedBodyInertia.
         */
        ArticulatedBodyInertia(const ArticulatedBodyInertia & other);

        /**
         * Low level data getters.
         */
        Matrix3x3 & getLinearLinearSubmatrix();
        Matrix3x3 & getLinearAngularSubmatrix();
        Matrix3x3 & getAngularAngularSubmatrix();
        const Matrix3x3 & getLinearLinearSubmatrix() const;
        const Matrix3x3 & getLinearAngularSubmatrix() const;
        const Matrix3x3 & getAngularAngularSubmatrix() const;

        // Operations on SpatialInertia
        static ArticulatedBodyInertia combine(const ArticulatedBodyInertia & op1,
                                              const ArticulatedBodyInertia & op2);

        /**
         * Return the inverse of this ArticulatedBodyInertia
         * applied to the passed wrench.
         */
        SpatialAcc applyInverse(const Wrench & wrench) const;

        /**
         * Get the ArticulatedBodyInertia as a 6x6 matrix
         */
        Matrix6x6 asMatrix() const;

        /**
         * Get the inverse of the ArticulatedBodyInertia matrix.
         */
        Matrix6x6 getInverse() const;

        // overloaded operators
        ArticulatedBodyInertia  operator+(const ArticulatedBodyInertia& other) const;
        ArticulatedBodyInertia  operator-(const ArticulatedBodyInertia& other) const;
        ArticulatedBodyInertia& operator+=(const ArticulatedBodyInertia& other);
        SpatialForceVector operator*(const SpatialMotionVector &other) const;
        Wrench operator*(const SpatialAcc &other) const;


        /** reset to zero (i.e. the inertia of body with zero mass) the ArticulatedBodyInertia */
        void zero();

        // Static helpers

        /**
         * Build the ArticulatedInertia \f$U d^{-1} U^\top\f$ .
         * Used in the articulated body algorithm .
         */
        static ArticulatedBodyInertia ABADyadHelper(const SpatialForceVector & U, const double d);

        /**
         * Build the ArticulatedInertia dU inv_d U^\top + U d_inv_d U^\top + U inv_d dU^\top
         * Used in the linearization of the articulated body algorithm.
         */
        static ArticulatedBodyInertia ABADyadHelperLin(const SpatialForceVector &  U, const double   inv_d,
                                                       const SpatialForceVector & dU, const double d_inv_d);

    };
}

#endif
