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

#ifndef IDYNTREE_TRAVERSAL_H
#define IDYNTREE_TRAVERSAL_H

#include <iDynTree/Indices.h>

#include <vector>

namespace iDynTree
{
class IJoint;
class Link;
class Model;

/**
 * Class that represents a traversal of a set of links of a Model.
 * The traversal is represented by an ordered vector of links.
 * For a given model, the traversal is always built in such a way
 * that there exist a spanning tree that has as a root the first link
 * (with traversal index 0) of the traversal, and such that every link comes
 * after its spanning tree parent.
 *
 * For every link in the traversal are provided:
 *  * a pointer to the link itself
 *  * a pointer to the parent link in the spanning tree
 *  * a pointer to the joint connecting the link to the parent
 *
 * For the first link of the traversal (i.e. base of the traversal)
 * there is not spanning tree parent, so the point to the parent link/joint are NULL.
 *
 *
 * \ingroup iDynTreeModel
 */
class Traversal
{
private:
    std::vector<const Link*> links;
    std::vector<const Link*> parents;
    std::vector<const IJoint*> toParentJoints;
    std::vector<int> linkIndexToTraversalIndex;

    /**
     * Copy constructor is forbidden
     */
    Traversal(const Traversal& other);

    /**
     * Copy operator is forbidden
     */
    Traversal& operator=(const Traversal& other);

public:
    Traversal();

    ~Traversal();

    /**
     * Get the number of visited links.
     *
     * @return the number of links in the Traversal
     */
    unsigned int getNrOfVisitedLinks() const;

    /**
     * Get the traversalIndex-th link of the traversal.
     *
     * @return a pointer to the traversalIndex-th link of the traversal.
     */
    const Link* getLink(const TraversalIndex traversalIndex) const;

    /**
     * Get the base link of the traversal.
     *
     * @note this is equivalent to getLink(0), as the base link
     *       is by definition the first link of the traversal.
     * @return a pointer to the base link of the traversal.
     */
    const Link* getBaseLink() const;

    /**
     * Get the parent link of the traversalIndex-th link of the traversal.
     *
     * @return a pointer to the parent link of the traversalIndex-th link of the traversal.
     */
    const Link* getParentLink(const TraversalIndex traversalIndex) const;

    /**
     * Get the joint connecting the traversalIndex-th link of the traversal
     * to its parent.
     *
     * @return a pointer to the joint connecting the  link traversalIndex-th link of the traversal.
     */
    const IJoint* getParentJoint(const TraversalIndex traversalIndex) const;

    /**
     * Get the parent link of the link with index linkIndex of the traversal.
     *
     * @return a pointer to the parent link of the traversalIndex-th link of the traversal.
     */
    const Link* getParentLinkFromLinkIndex(const LinkIndex linkIndex) const;

    /**
     * Get the joint connecting the link with index linkIndex
     * to its parent.
     *
     * @return a pointer to the joint connecting the  link traversalIndex-th link of the traversal.
     */
    const IJoint* getParentJointFromLinkIndex(const LinkIndex linkIndex) const;

    /**
     * Get the traversal index of the specified link
     *
     * @return the traversalIndex of the specified link, or TRAVERSAL_INDEX_INVALID if the link does
     * not belong to the traversal.
     */
    TraversalIndex getTraversalIndexFromLinkIndex(const LinkIndex linkIndex) const;

    /**
     * Reset the Traversal.
     *
     * After a call to reset, the Traversal will contain
     * no visited links, so it needs to be approprately populated
     * with the addTraversalBase and addTraversalElement methods.
     *
     * @param[in] nrOfLinksInModel total number of links in the model,
     *                             not the number of visited links in the Traversal.
     * @return true if all went well, false otherwise
     */
    bool reset(const unsigned int nrOfLinksInModel);

    /**
     * Reset the Traversal.
     *
     * After a call to reset, all the pointers in the Traversal are set
     * to 0, and the Traversal should be approprialy populated with the
     * setters before use.
     *
     * @param[in] model Model on which this traversal will be used,
     *                  used to initialize the internal data structure
     *                  using the getNrOfLinks() method.
     *
     * @return true if all went well, false otherwise
     */
    bool reset(const Model& model);

    /**
     * Add a base to traversal.
     *
     * Equivalent to addTraversalElement(link,NULL,NULL), but
     * will print an error and return false if it is called with a
     * traversal that already has a base, i.e. getNrOfVisitedLinks() > 0
     *
     * @param[in] link a pointer to the base link
     * @return true if all went well, false otherwise
     */
    bool addTraversalBase(const Link* link);

    /**
     * Add an element to the traversal.
     *
     * After a call to this method, the getNrOfVisitedLinks()
     * will be increased of 1 unit.
     *
     * @param[in] link a pointer to the link to add to the traversal
     * @param[in] jointToParent a pointer to the joint that connects
     *                          the added link to its parent in this traversal
     * @param[in] parentLink a pointer to the parent on this link in this traversal.
     *                       It should be a link already contained in this traversal.
     * @return true if all went well, false otherwise.
     */
    bool addTraversalElement(const Link* link, const IJoint* jointToParent, const Link* parentLink);

    /**
     * \brief Check if a link is the parent of another link for this traversal.
     *
     *
     * @param[in] parentCandidate the link candidate to be the parent of childCandidate.
     * @param[in] childCandidate  the link candidate to be a child of parentCandidate.
     * @return true if parentCandidate is actually the parent of childCandidate, false otherwise.
     */
    bool isParentOf(const LinkIndex parentCandidate, const LinkIndex childCandidate) const;

    /**
     * \brief Get the child link (according to the traversal) of a Joint.
     *
     * @param[in] m_model the considered model.
     * @param[in] jntIdx the index of the joint of which we want to get the child link.
     * @return the index of the child link if all went well, LINK_INVALID_INDEX otherwise .
     */
    LinkIndex getChildLinkIndexFromJointIndex(const Model& model, const JointIndex jntIdx) const;

    /**
     * \brief Get the parent link (according to the traversal) of a Joint.
     *
     * @param[in] m_model the considered model.
     * @param[in] jntIdx the index of the joint of which we want to get the parent link.
     * @return the index of the parent link if all went well, LINK_INVALID_INDEX otherwise .
     */
    LinkIndex getParentLinkIndexFromJointIndex(const Model& model, const JointIndex jntIdx) const;

    /**
     * Return a human-readable representation of the traversal.
     */
    std::string toString(const Model& model) const;
};

} // namespace iDynTree

#endif /* IDYNTREE_TRAVERSAL_H */
