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

#ifndef IDYNTREE_MODEL_H
#define IDYNTREE_MODEL_H

#include <iDynTree/IJoint.h>
#include <iDynTree/Link.h>
#include <iDynTree/Transform.h>

#include <iDynTree/Indices.h>
#include <iDynTree/Sensors.h>
#include <iDynTree/SolidShapes.h>

#include <cstdlib>
#include <string>
#include <vector>

namespace iDynTree
{
class Traversal;

struct Neighbor
{
    LinkIndex neighborLink;
    JointIndex neighborJoint;
};

/**
 * Class that represents a generic multibody model.
 *
 * A model is composed by rigid bodies (i.e. links) connected
 * by joints. Each joint can have from 0 to 6 degrees of freedom.
 *
 * Each link has a "link frame" rigidly attached to it.
 * Additionally, other rigidly attachable frames can be defined for each link.
 *
 * The model contains also a serialization for the different elements,
 * i.e. a function between:
 *  * joint names and the integers 0..getNrOfJoints()-1
 *  * dof   names and the integers 0..getNrOfDOFs()-1
 *  * link  names and the integers 0..getNrOfLinks()-1
 *  * frame names and the integers 0..getNrOfFrames()-1
 *
 * For simplicity, these mappings are build when building the model.
 * In particular the link and joint indices are assigned when the links
 * and joint are added to the model using the `addLink` and `addJoint`
 * methods.
 *
 * The DOF indices are also assigned when the joint is added to the model
 * with the addJoint method. For example if a model is composed only of
 * 0 or 1 DOF joints and the 1 DOFs joints are added before the 0 DOFs then
 * the joint index and dof index for 1 DOF joints will be coincident (this is
 * how the URDF parser is actually implemented). For this reason the current
 * implementation does not have a concept of DOF explicit identifier,
 * i.e. a getDOFName(DOFIndex dofIndex) method does not exist.
 *
 * The frame indices between 0 and getNrOfLinks()-1 are always assigned to the
 * "main" link frame of the link with the same index. The frame indices
 * between getNrOfLinks() and getNrOfFrames()-1 are assigned when the additional
 * frame is added to the model with the addAdditionalFrameToLink call. All the additional
 * frame indices are incremented by 1 whenever a new link is added, to ensure that
 * its "link frame" has a frame index in the 0...getNrOfLinks()-1 range.
 *
 *
 *
 * \ingroup iDynTreeModel
 */
class Model
{
private:
    /** Vector of links. For each link its index indicates its location in this vector */
    std::vector<Link> links;

    /** Vector of joints. For each joint its index indicates its location in this vector */
    std::vector<IJointPtr> joints;

    /** Vector of additional frames.
     *  The element additionalFrames[frameOffset] will be the link_H_frame transform  of the frame
     * with FrameIndex getNrOfLinks() + frameOffset .
     */
    std::vector<Transform> additionalFrames;

    /**
     *  Vector of link indices corresponding to an additional frame.
     *  The element additionalFrameNames[frameOffset] will be the link_H_frame transform  of the
     * frame with FrameIndex getNrOfLinks() + frameOffset .
     */
    std::vector<LinkIndex> additionalFramesLinks;

    /** Vector of link names, matches the index of each link to its name. */
    std::vector<std::string> linkNames;

    /** Vector of joint names, matches the index of each joint to its name. */
    std::vector<std::string> jointNames;

    /**
     *  Vector of additional frame names.
     *  The element frameNames[frameOffset] will be the name of the frame with
     *  FrameIndex getNrOfLinks() + frameOffset .
     */
    std::vector<std::string> frameNames;

    /** Adjacency lists: match each link index to a list of its neighbors,
        and the joint connecting to them. */
    std::vector<std::vector<Neighbor>> neighbors;

    /** Vector containing the package directories associated to the model. */
    std::vector<std::string> packageDirs;

    /**
     * Most data structures are not undirected, so we store the original
     * root of the tree, to provide a default root for Traversal generation.
     */
    LinkIndex defaultBaseLink;

    /**
     * Cache number of position coordinaties of the model.
     * If all joints are 0 or 1 dofs, this is equal to nrOfDOFs.
     */
    unsigned int nrOfPosCoords;

    /**
     * Cached number of (internal) DOFs of the model.
     *  This is just the sum of all the getNrOfDOFs of the joint in the model.
     */
    unsigned int nrOfDOFs;

    /**
     * Solid shapes used for visualization.
     */
    ModelSolidShapes m_visualSolidShapes;

    /**
     * Solid shapes used for collision checking.
     */
    ModelSolidShapes m_collisionSolidShapes;

    /**
     * Sensors associated to the model.
     */
    SensorsList m_sensors;

    /**
     * Copy the structure of the model from another instance of a model.
     */
    void copy(const Model& model);

    /**
     * Destroy the object, properly deallocating memory.
     */
    void destroy();

public:
    /**
     * Costructor
     */
    Model();

    /**
     * Copy costructor
     */
    Model(const Model& other);

    /**
     * Copy operator
     */
    Model& operator=(const Model& other);

    /**
     * Copy the model.
     *
     * Get a copy of the model.
     */
    Model copy() const;

    /**
     * Destructor
     *
     */
    virtual ~Model();

    /**
     * Get the number of links in the model.
     */
    size_t getNrOfLinks() const;

    /**
     * @return a vector containing all the directories of the meshes
     */
    const std::vector<std::string>& getPackageDirs() const;

    void setPackageDirs(const std::vector<std::string>& packageDirs);

    /**
     * Get the name of a link given its index, or
     * an LINK_INVALID_NAME string if linkIndex < 0 or >= getNrOfLinks()
     */
    std::string getLinkName(const LinkIndex linkIndex) const;

    LinkIndex getLinkIndex(const std::string& linkName) const;

    /**
     * \brief Check if a given LinkIndex is valid.
     *
     * A link index is valid if is different from
     * LINK_INVALID_INDEX and 0 =< index < getNrOfLinks()-1
     *
     * @return true if the index is valid, false otherwise.
     */
    bool isValidLinkIndex(const LinkIndex index) const;

    LinkPtr getLink(const LinkIndex linkIndex);
    LinkConstPtr getLink(const LinkIndex linkIndex) const;

    LinkIndex addLink(const std::string& name, const Link& link);

    /**
     * Get number of joints in the model.
     */
    size_t getNrOfJoints() const;

    /**
     * Get the name of a joint given its index, or
     * an JOINT_INVALID_NAME if linkIndex < 0 or >= getNrOfLinks()
     */
    std::string getJointName(const JointIndex index) const;

    /**
     * Get the total mass of the robot
     */
    double getTotalMass() const;

    /**
     * Get the index of a joint, given a jointName.
     * If the jointName is not found in the model,
     * return JOINT_INVALID_INDEX .
     *
     */
    JointIndex getJointIndex(const std::string& jointName) const;

    IJointPtr getJoint(const JointIndex index);

    IJointConstPtr getJoint(const JointIndex index) const;

    /**
     * \brief Check if a given JointIndex is valid.
     *
     * A joint index is valid if is different from
     * JOINT_INVALID_INDEX and 0 =< index < getNrOfJoints()-1
     *
     * @return true if the index is valid, false otherwise.
     */
    bool isValidJointIndex(const JointIndex index) const;

    /**
     * Check if a name is already used for a link in the model.
     *
     * @return true if a name is used by a link in a model, false otherwise.
     */
    bool isLinkNameUsed(const std::string linkName) const;

    /**
     * Check if a name is already used for a joint in the model.
     *
     * @return true if a name is used by a joint in a model, false otherwise.
     */
    bool isJointNameUsed(const std::string jointName) const;

    /**
     * Check if a name is already used for a frame in the model.
     *
     * \note this function will check the name of the links and the names of the additional frames.
     * @return true if a name is used by a frame in a model, false otherwise.
     */
    bool isFrameNameUsed(const std::string frameName) const;

    /**
     *
     * Add a joint to the model. The two links to which the joint is connected
     * are specified in the joint itself, throught the appropriate methods.
     * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if
     *         there was an error in adding the joint.
     */
    JointIndex addJoint(const std::string& jointName, IJointConstPtr joint);

    /**
     * Add a joint to the model, and specify the two links that are connected
     * by the specified joints. The setAttachedLinks of the passed joint
     * is called appropriately, to ensure consistency in the model.
     * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if
     *         there was an error in adding the joint.
     */
    JointIndex addJoint(const std::string& link1,
                        const std::string& link2,
                        const std::string& jointName,
                        IJointConstPtr joint);

    /**
     * Add a joint to the model, and add also a link.
     * The added joints connects an existing link of the model to the newly
     * added link.
     * The setAttachedLinks of the passed joint
     * is called appropriately, to ensure consistency in the model.
     * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if
     *         there was an error in adding the joint.
     */
    JointIndex addJointAndLink(const std::string& existingLink,
                               const std::string& jointName,
                               IJointConstPtr joint,
                               const std::string& newLinkName,
                               Link& newLink);

    /**
     * \brief Displace a link by inserting a new link and a new joint.
     * Displace a link by replacing it with a new link in the existing joint and insert new joint
     * between the new link and the displaced link. Inputs:
     * @param[in] existingJoint is where the new link will be inserted.
     * @param[in] unmovableLink is the link that was previously connected to the displaced link by
     * the existingJoint.
     * @param[in] _unmovable_X_newLink is the transformation matrix from the unmovableLink to the
     * new Link.
     * @param[in] jointName is the name of the new joint.
     * @param[in] joint is the new joint connecting the new link and the link that was displaced.
     * @param[in] newLinkName is the name of the new link.
     * @param[in] newLink is the new link that will be attached to the unmovable link using the
     * existingJoint. The setAttachedLinks of the new joint and the existing joint are edited
     * appropriately, to ensure consistency in the model.
     * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if
     *         there was an error in adding the joint.
     */
    JointIndex
    insertLinkToExistingJointAndAddJointForDisplacedLink(const std::string& existingJoint,
                                                         const std::string& unmovableLink,
                                                         const Transform& _unmovableLink_X_newLink,
                                                         const std::string& jointName,
                                                         IJointConstPtr joint,
                                                         const std::string& newLinkName,
                                                         Link& newLink);

    /**
     * Get the dimension of the vector used to parametrize the positions of the joints of the robot.
     * This number can be obtained by summing the getNrOfPosCoords of all the joints of the model.
     *
     * \warning This is *not* including the 6 degrees of freedom of the base.
     */
    size_t getNrOfPosCoords() const;

    /**
     * Get the number of degrees of freedom of the joint of the robot.
     * This number can be obtained by summing the getNrOfDOFs of all the joints of the model.
     *
     * \warning This is *not* including the 6 degrees of freedom of the base.
     */
    size_t getNrOfDOFs() const;

    /**
     * Get the number of frames in the model.
     *
     * \note this will return the sum of the number of link
     *       (as each link has an attached frame) and the total number
     *       of additional frames.
     *
     * @return the number of frames in the model.
     */
    size_t getNrOfFrames() const;

    /**
     * Add an additional frame to a link.
     *
     * \note This function has signature different from
     *       addJoint/addLink because the FrameIndex of the
     *       additional frame are invalidated at each call to
     *       the addLink. So we don't return the FrameIndex in this
     *       function, as the model construction should be completed
     *       before that FrameIndex are stored.
     *
     * @param[in] linkName the link to which attach the additional frame.
     * @param[in] frameName the name of the frame added to the model.
     * @param[in] link_H_frame the pose of added frame with respect to the link main frame,
     *                         expressed with a transform with:
     *                              refFrame: the main link frame
     *                              frame: the added frame
     * @return true if all went well, false if an error occured.
     *
     */
    bool addAdditionalFrameToLink(const std::string& linkName,
                                  const std::string& frameName,
                                  iDynTree::Transform link_H_frame);

    /**
     * Get the name of a frame given its index.
     *
     * @param[in] frameIndex the index of the frame whose name is requested.
     * @return the name of a frame given its index, or
     *         a FRAME_INVALID_NAME string if frameIndex < 0 or >= getNrOfFrames()
     */
    std::string getFrameName(const FrameIndex frameIndex) const;

    /**
     * Get the index of a frame given its name.
     *
     * @param[in] frameName the name of the frame for which the index is requested.
     * @return    the index of the frame, of FRAME_INVALID_INDEX if frameName is not
     *            not a frame name.
     */
    FrameIndex getFrameIndex(const std::string& frameName) const;

    /**
     * \brief Check if a given FrameIndex is valid.
     *
     * A frame index is valid if is different from
     * FRAME_INVALID_INDEX and 0 =< index < getNrOfFrames()-1
     *
     * @return true if the index is valid, false otherwise.
     */
    bool isValidFrameIndex(const FrameIndex index) const;

    /**
     * Get the tranform of the frame with respect to the main
     * frame of the link, returned as link_H_frame tranform
     * with refFrame : the link main frame and frame : the
     *  frame with index frameIndex .
     *
     * @param[in] frameIndex the index of the frame for which transform is requested.
     * @return the link_H_frame transform, or an identity tranform
     *         if frameIndex < 0 or frameIndex >= getNrOfFrames .
     */
    Transform getFrameTransform(const FrameIndex frameIndex) const;

    /**
     * Get the link at which the frame with index frameIndex
     * is attached.
     *
     */
    LinkIndex getFrameLink(const FrameIndex frameIndex) const;

    /**
     * Get the additional frames of a specified link.
     *
     * @note The vector of returned frame index is ordered according to the frame index.
     * @warning This method searches linearly over all the frames.
     *
     * @param[in] link a LinkIndex of the specified link,
     * @param[out] frames a vector of FrameIndex of the frame indeces attached to the specified link
     * index,
     * @return true if the specified link is a valid link, false otherwise.
     */
    bool
    getLinkAdditionalFrames(const LinkIndex lnkIndex, std::vector<FrameIndex>& frameIndeces) const;

    /**
     * Get the nr of neighbors of a given link.
     */
    unsigned int getNrOfNeighbors(const LinkIndex link) const;

    /**
     * Get the neighbor of a link. neighborIndex should be
     * >= 0 and <= getNrOfNeighbors(link)
     */
    Neighbor getNeighbor(const LinkIndex link, unsigned int neighborIndex) const;

    /**
     * Set the default base link, used for generation of the default traversal.
     */
    bool setDefaultBaseLink(const LinkIndex linkIndex);

    /**
     * Get the default base link, used for generation of the default traversal.
     *
     * If no link are present in model, returns LINK_INVALID_INDEX.
     * If setDefaultBaseLink was never called but at least a link has been added
     * to the model, returns 0 (i.e. the index of the first link added to the model.
     *
     * @return index of the default base link, if valid
     */
    LinkIndex getDefaultBaseLink() const;

    /**
     * Compute a Traversal of all the links in the Model, doing a Depth First Search starting
     * at the default base.
     *
     * \warning The traversal computed with this function contains pointers to the joints
     *          and links present in this model. Whenever these pointers are invalidated,
     *          for example because a link or a joint is added or the model is copied,
     *          the traversal need to be recomputed.
     *
     * \warning this function works only on Models without cycles.
     * @param[out] traversal traversal of all links in the model
     * @return true if all went well, false otherwise.
     */
    bool computeFullTreeTraversal(Traversal& traversal) const;

    /**
     * Compute a Traversal of all the links in the Model, doing a Depth First Search starting
     * at the given traversalBase.
     *
     * \warning The traversal computed with this function contains pointers to the joints
     *          and links present in this model. Whenever these pointers are invalidated,
     *          for example because a link or a joint is added or the model is copied,
     *          the traversal need to be recomputed.
     *.
     * \warning this function works only on Models without cycles.
     * @param[out] traversal traversal of all links in the model
     * @param[in]  traversalBase base (root) link in the traversal
     * @return true if all went well, false otherwise
     */
    bool computeFullTreeTraversal(Traversal& traversal, const LinkIndex traversalBase) const;

    /**
     * Get the inertial parameters of the links of the model in vector forms.
     *
     * This methods gets the inertial parameters (mass, center
     * of mass, 3D inertia matrix) of the links of the robot.
     *
     * The output vector of inertial parameters must have 10*getNrOfLinks() elements,
     * each 10 elements subvector corresponds to the inertial parameters of one link,
     * following the serialization induced by the link indices
     * (link 0 corresponds to elements 0-9, link 1 to 10-19, etc).
     *
     * The mapping between the SpatialInertia class and the Vector10 elements is the one
     * defined in SpatialInertia::asVector() method.
     *
     * @param[out] modelInertialParams vector of inertial parameters
     * @return true if all went well, false otherwise.
     *
     */
    bool getInertialParameters(VectorDynSize& modelInertialParams) const;

    /**
     * Update the inertial parameters of the links of the model.
     *
     * This methods modifies the inertial parameters (mass, center
     * of mass, 3D inertia matrix) of the links of the robot.
     *
     * The input vector of inertial parameters must have 10*getNrOfLinks() elements,
     * each 10 elements subvector corresponds to the inertial parameters of one link,
     * following the serialization induced by the link indices
     * (link 0 corresponds to elements 0-9, link 1 to 10-19, etc).
     *
     * The mapping between the SpatialInertia class and the Vector10 elements is the one
     * defined in SpatialInertia::asVector() method.
     *
     * @note For efficency reason, inertial parameters are not checked for full physical
     *       consistency before being update.
     *
     * @param[in] modelInertialParams vector of inertial parameters
     * @return true if all went well, false otherwise.
     *
     */
    bool updateInertialParameters(const VectorDynSize& modelInertialParams);

    /**
     * Get the ModelSolidShapes meant for visualization.
     *
     * @return a reference to ModelSolidShapes meant for visualization.
     */
    ModelSolidShapes& visualSolidShapes();

    /**
     * Get the ModelSolidShapes meant for visualization (const version)
     *
     * @return a reference to ModelSolidShapes meant for visualization.
     */
    const ModelSolidShapes& visualSolidShapes() const;

    /**
     * Get the ModelSolidShapes meant for collision checking.
     *
     * @return a reference to ModelSolidShapes meant for visualization.
     */
    ModelSolidShapes& collisionSolidShapes();

    /**
     * Get the ModelSolidShapes meant for collision checking (const version)
     *
     * @return a reference to ModelSolidShapes meant for visualization.
     */
    const ModelSolidShapes& collisionSolidShapes() const;

    /**
     * Get the (mutable) sensors associated with the model.
     *
     * @return a (mutable) reference to SensorsList associated with the model.
     */
    SensorsList& sensors();

    /**
     * Get the (const) sensors associated with the model.
     *
     * @return a (const) reference to SensorsList associated with the model.
     */
    const SensorsList& sensors() const;

    /**
     * \brief Get a printable representation of the Model.
     *
     * Useful for debugging.
     */
    std::string toString() const;

    /**
     * \brief Check if the model is valid.
     *
     * Useful for debugging.
     */
    bool isValid() const;
};

} // namespace iDynTree

#endif /* IDYNTREE_LINK_H */
