// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause

#ifndef IDYNTREE_TRANSFORM_H
#define IDYNTREE_TRANSFORM_H

#include <iDynTree/MatrixFixSize.h>
#include <iDynTree/Position.h>
#include <iDynTree/Rotation.h>

#include <string>

namespace iDynTree
{
class Position;
class Rotation;
class Wrench;
class Twist;
class SpatialMomentum;
class SpatialAcc;
class SpatialMotionVector;
class SpatialForceVector;
class SpatialInertia;
class ArticulatedBodyInertia;

/**
 * Class representation the relative displacement between two different frames.
 *
 * \ingroup iDynTreeCore
 *
 * \image html transform.svg
 *
 *
 * This class is designed to be an easy to use proxy to perform change of frame of
 * expression for iDynTree::Position, iDynTree::Twist, iDynTree::Wrench and other data
 * structure in \ref iDynTreeCore.  For this reason the class is called "Transform", because it will
 * be mainly used to transform quantities between frames.
 *
 * Given that this class it may used to represent homogeneous transform matrix as well as adjoint
 * matrix, no raw access to the underline storage ( data() method ) is provided, because it does not
 * have a canonical representation. Instead, access is given to the two elements of a transform:
 *  The position vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$ and
 * the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}}\f$.
 *
 * We will indicate this tranform as \f$( {}^{\texttt{refOrient}}
 * p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} )\f$
 *
 * The frame whose origin is the reference point and whose orientation the reference orientation is
 * often indicated as \f$ \texttt{refFrame} = (\texttt{refPoint},\texttt{refOrient}) \f$.
 *
 * Similarly the frame whose origin is the point and whose orientation is the orientation is
 * indicated as \f$ \texttt{frame} = (\texttt{point},\texttt{orient}) \f$.
 *
 * This transform object can be obtained as the \f${}^{\texttt{refFrame}} H_{\texttt{frame}}\f$  4x4
 * homogeneous matrix using the asHomogeneousTransform method, or as the \f${}^{\texttt{refFrame}}
 * X_{\texttt{frame}}\f$ 6x6 adjoint matrix using the asAdjointTransform .
 *
 *
 *
 */
class Transform
{
protected:
    /**
     * \brief  The position part of the transform.
     *
     * The 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$,
     * that is  the vector between the point and the
     * reference point, pointing toward the point and expressed
     * in the reference orientation frame.
     */
    Position pos;

    /**
     * \brief The rotation part of the transform
     *
     * Set the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3
     * \times 3}\f$, that is the rotation matrix that takes a 3d vector expressed in the
     * orientationFrame and transform it in a 3d vector expressed in the reference orientation
     * frame.
     */
    Rotation rot;

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

    /**
     * Constructor from a rotation and a point
     */
    Transform(const Rotation& _rot, const Position& origin);

    /**
     * Constructor from a Matrix4x4 object. It is equivalent of calling fromHomogeneousTransform()
     * @param transform The input homogeneous matrix
     */
    Transform(const Matrix4x4& transform);

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

    /**
     * Set rotation and translation from a iDynTree::Matrix4x4 object
     * @param transform The input homogeneous matrix
     */
    void fromHomogeneousTransform(const Matrix4x4& transform);

    /**
     * Assigment operator
     *
     * @param other the rhs
     *
     * @return *this equal to the other object
     */
    Transform& operator=(const Transform& other);

    /**
     * Get the rotation part of the transform
     *
     * Get the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3
     * \times 3}\f$, that is the rotation matrix that takes a 3d vector expressed in the
     * orientationFrame and transform it in a 3d vector expressed in the reference orientation
     * frame.
     */
    const Rotation& getRotation() const;

    /**
     * \brief Get the translation part of the transform
     *
     * Get 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$,
     * that is  the vector between the point and the
     * reference point, pointing toward the point and expressed
     * in the reference orientation frame.
     */
    const Position& getPosition() const;

    /**
     * Set the rotation part of the transform
     *
     * Set the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3
     * \times 3}\f$, that is the rotation matrix that takes a 3d vector expressed in the
     * orientationFrame and transform it in a 3d vector expressed in the reference orientation
     * frame.
     */
    void setRotation(const Rotation& rotation);

    /**
     * \brief Set the translation part of the transform
     *
     * Set 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$,
     * that is  the vector between the point and the
     * reference point, pointing toward the point and expressed
     * in the reference orientation frame.
     */
    void setPosition(const Position& position);

    // geometric operations on 3x1 vectors (positions and rotations and homogemeous tranform)
    static Transform compose(const Transform& op1, const Transform& op2);
    static Transform inverse2(const Transform& trans);

    /**
     * \name Overloaded operators.
     *
     * This operators are used to change the frame in which a quantity is
     * expressed from the \f$\texttt{frame}\f$ to the \f$\texttt{refFrame}\f$ of
     * the transform.
     */
    ///@{

    /**
     * \brief Combine two transforms.
     *
     * If this object is
     * \f[
     * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} ,
     * {}^{\texttt{refOrient}} R_{\texttt{orient}} )
     * \f]
     * and the argument is
     * \f[
     * (p',R') = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}} , {}^{\texttt{orient}}
     * R_{\texttt{newOrient}} )
     * \f].
     *
     * The resulting transform will be:
     * \f[
     * (p+Rp', RR') = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}} ,
     * {}^{\texttt{refOrient}} R_{\texttt{newOrient}} )
     * \f].
     *
     * Notice that this is equivalent to multiply the associated homogemeous matrices:
     * \f[
     * {}^{\texttt{refFrame}} H_{\texttt{newFrame}} = {}^{\texttt{refFrame}} H_{\texttt{frame}}
     * {}^{\texttt{frame}} H_{\texttt{newFrame}}
     * \f]
     * or the associated adjoint matrices :
     * \f[
     * {}^{\texttt{refFrame}} X_{\texttt{newFrame}} = {}^{\texttt{refFrame}} X_{\texttt{frame}}
     * {}^{\texttt{frame}} X_{\texttt{newFrame}}
     * \f]
     */
    Transform operator*(const Transform& other) const;

    /**
     * Return the inverse of this Transform.
     *
     *  If this object is
     * \f[
     * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} ,
     * {}^{\texttt{refOrient}} R_{\texttt{orient}} )
     * \f]
     * this function will return:
     * \f[
     * (-R^\top p , R^\top) = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{refPoint}} ,
     * {}^{\texttt{orient}} R_{\texttt{refOrient}} )
     * \f]
     *
     */
    Transform inverse() const;

    /**
     * Change reference frame of a point.
     *
     *  If this object is
     * \f[
     * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} ,
     * {}^{\texttt{refOrient}} R_{\texttt{orient}} )
     * \f]
     *
     *  And the Position argument represent a point:
     * \f[
     *  p' = {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}}
     * \f]
     *
     * The result of the operation is:
     * \f[
     *  Rp' + p =  {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}}
     * \f]
     *
     */
    Position operator*(const Position& other) const;

    /**
     * Change frame in which a Wrench is expressed.
     *
     *  If this object is
     * \f[
     * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} ,
     {}^{\texttt{refOrient}} R_{\texttt{orient}} )
     * \f]
     *
     * And the argument is a wrench:
     * \f[
     * {}_{\texttt{frame}} F =
     * \begin{bmatrix}
     * f \\ \tau
     * \end{bmatrix}
     * \f]
     *
     * The result of this operation is :
     * \f[
     * {}_{\texttt{refFrame}} F
     * =
     * {}_{\texttt{refFrame}}X^{\texttt{frame}}
     * {}_{\texttt{frame}} F
     * =
     * \begin{bmatrix}
     *    R &
     *    0_{3\times3} \\
     *    p \times R &
     *    R
     * \end{bmatrix}
     *  \begin{bmatrix}
     * f \\ \tau
     * \end{bmatrix}
     * =
      \begin{bmatrix}
     * Rf \\ p \times R f + R\tau
     * \end{bmatrix}
     * \f]
     */
    Wrench operator*(const Wrench& other) const;

    /**
     * Change the frame in which a Twist is expressed.
     *
     *  If this object is
     * \f[
     * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} ,
     {}^{\texttt{refOrient}} R_{\texttt{orient}} )
     * \f]
     *
     * And the argument is a twist:
     * \f[
     * {}^{\texttt{frame}} V =
     * \begin{bmatrix}
     * v \\ \omega
     * \end{bmatrix}
     * \f]
     *
     * The result of this operation is :
     * \f[
     * {}^{\texttt{refFrame}} V
     * =
     * {}^{\texttt{refFrame}}X_{\texttt{frame}}
     * {}^{\texttt{frame}} V
     * =
     * \begin{bmatrix}
     *    R &
     *    p \times R\\
     *     0_{3\times3}  &
     *    R
     * \end{bmatrix}
     *  \begin{bmatrix}
     * v \\ \omega
     * \end{bmatrix}
     * =
      \begin{bmatrix}
     * R v + p \times R \omega \\ R\omega
     * \end{bmatrix}
     * \f]
     */
    Twist operator*(const Twist& other) const;

    /**
     * Change the frame in which a SpatialMomentum is expressed.
     *
     * Check the operator*(const Wrench & other) documentation
     * for the mathematical details.
     */
    SpatialMomentum operator*(const SpatialMomentum& other) const;

    /**
     * Change the frame in which a SpatialAcc is expressed.
     *
     * Check the operator*(const Twist & other) documentation
     * for the mathematical details.
     */
    SpatialAcc operator*(const SpatialAcc& other) const;

    /**
     * Change the frame in which a SpatialMotionVector is expressed.
     *
     * Check the operator*(const Twist & other) documentation
     * for the mathematical details.
     */
    SpatialMotionVector operator*(const SpatialMotionVector& other) const;

    /**
     * Change the frame in which a SpatialForceVector is expressed.
     *
     * Check the operator*(const Wrench & other) documentation
     * for the mathematical details.
     */
    SpatialForceVector operator*(const SpatialForceVector& other) const;

    /**
     * Change the frame in which a SpatialInertia is expressed.
     *
     */
    SpatialInertia operator*(const SpatialInertia& other) const;

    /**
     * Change the frame in which a ArticulatedBodyInertia is expressed.
     *
     */
    ArticulatedBodyInertia operator*(const ArticulatedBodyInertia& other) const;

    /**
     * Change the frame in which a Direction is expressed.
     *
     *  If this object is
     * \f[
     * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} ,
     * {}^{\texttt{refOrient}} R_{\texttt{orient}} )
     * \f]
     *
     * And the argument is a direction represented by a unit norm 3d vector :
     * \f[
     *   {}^{\texttt{orient}} d
     * \f]
     *
     * The result of this operation is:
     * \f[
     *   {}^{\texttt{refOrient}} d = R  {}^{\texttt{orient}} d
     * \f]
     *
     */
    Direction operator*(const Direction& other) const;

    /**
     * Change the frame in which a Axis is expressed.
     *
     *  If this object is
     * \f[
     * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} ,
     * {}^{\texttt{refOrient}} R_{\texttt{orient}} )
     * \f]
     *
     * And the argument is an axis, specified by the axis origin and the axis direction:
     * \f[
     *  {}^{\texttt{frame}} A = ({}^{\texttt{orient}} p_{\texttt{point},\texttt{axisOrigin}}  ,
     * {}^{\texttt{orient}} d) = (p',d)
     * \f]
     *
     * The result of this operation is:
     * \f[
     *  {}^{\texttt{refFrame}} A =  ({}^{\texttt{refOrient}}
     * p_{\texttt{refPoint},\texttt{axisOrigin}}  , {}^{\texttt{refOrient}} d)  =  ( Rp' + p , Rd)
     * \f]
     *
     */
    Axis operator*(const Axis& other) const;
    ///@}

    /**
     * Return an identity transfom
     *
     * Set the rotation to the identity and the translation to 0 :
     * \f[
     * (0_{3 \times 1}, I_{3 \times 3})
     * \f]
     *
     * @return an identity transform.
     */
    static Transform Identity();

    /**
     * @name Raw data accessors.
     *
     * For several applications it may be useful to access the fransform
     * contents in the raw form of homogeneous matrix or an adjoint matrix.
     */
    ///@{

    /**
     * Return the 4x4 homogeneous matrix representing the transform.
     *
     * The returned matrix is the one with this structure:
     *
     * \f[
     *  {}^{\texttt{refFrame}} H_{\texttt{frame}} =
     * \begin{bmatrix}
     *    {}^{\texttt{refOrient}} R_{\texttt{orient}} & {}^{\texttt{refOrient}}
     * p_{\texttt{refPoint},\texttt{point}} \\ 0_{1\times3} & 1
     * \end{bmatrix}
     * \f]
     * Where \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$ is the
     * rotation matrix that takes a 3d vector expressed in the orientationFrame and transform it in
     * a 3d vector expressed in the reference orientation frame.
     *
     * \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$
     * is the vector between the point and the
     * reference point, pointing toward the point and expressed
     * in the reference orientation frame.
     *
     */
    Matrix4x4 asHomogeneousTransform() const;

    /**
     * Return the 6x6 adjoint matrix representing this transform.
     *
     * The returned matrix is the one with this structure:
     *
     * \f[
     *  {}^{\texttt{refFrame}} X_{\texttt{frame}} =
     * \begin{bmatrix}
     *    R &
     *    p \times R \\
     *    0_{3\times3} &
     *    R
     * \end{bmatrix}
     * \f]
     *
     * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$
     * is the rotation matrix that takes
     * a 3d vector expressed in the orientationFrame and transform it
     * in a 3d vector expressed in the reference orientation frame.
     *
     * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$
     * is the vector between the point and the
     * reference point, pointing toward the point and expressed
     * in the reference orientation frame \f$p \times\f$ is the skew simmetric
     * matrix such that \f$(p \times) v = p \times v\f$ .
     *
     * \warning Note that in iDynTree the matrix are stored
     *          in row major order, and the 6d quantites are
     *          serialized in linear/angular order.
     *
     */
    Matrix6x6 asAdjointTransform() const;

    /**
     * Return the 6x6 adjoint matrix (for wrench) representing this transform.
     *
     * The returned matrix is the one with this structure:
     *
     * \f[
     *  {}_{\texttt{refFrame}} X^{\texttt{frame}} =
     * \begin{bmatrix}
     *    R &
     *      0_{3\times3} \\
     *    p \times R &
     *    R
     * \end{bmatrix}
     * \f]
     *
     * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$
     * is the rotation matrix that takes
     * a 3d vector expressed in the orientationFrame and transform it
     * in a 3d vector expressed in the reference orientation frame.
     *
     * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$
     * is the vector between the point and the
     * reference point, pointing toward the point and expressed
     * in the reference orientation frame \f$p \times\f$ is the skew simmetric
     * matrix such that \f$(p \times) v = p \times v\f$ .
     *
     * \warning Note that in iDynTree the matrix are stored
     *          in row major order, and the 6d quantites are
     *          serialized in linear/angular order.
     *
     */
    Matrix6x6 asAdjointTransformWrench() const;

    /*
     * Exp mapping between a  generic element of se(3) (iDynTree::SpatialMotionVector)
     * to the corresponding element of SE(3) (iDynTree::Transform).
     */
    SpatialMotionVector log() const;

    ///@}

    /**
     * @name Output helpers.
     *  Function to print out the Transform.
     */
    ///@{
    std::string toString() const;

    std::string reservedToString() const;
    ///@}
};
} // namespace iDynTree

#endif
