/***************************************************************************
 *                                                                          *
 *  Copyright (C) Golaem S.A.  All Rights Reserved.                         *
 *                                                                          *
 ***************************************************************************/

#pragma once

#include "glmTypes.h"
#include "glmVector3.h"
#include "glmQuaternion.h"
#include "glmRTTI.h"

#include "glmSet.h"

#include <glmGolaemCharacterDeclarations.h> // GolaemCharacter declarations

namespace glm
{
    class HierarchicalSkeletonDescriptionV1;

    //*************************************************************************************
    /**
	* \ingroup visualization
	* \brief Define a hierarchical bone. A bone contains hierarchical informations (with father bone and children bones), as well as local position/orientation of the bone relative to its father.
	*
	* \author dev@golaem.com
	*
	*
	* \note The height of a node is the length of the longest downward path to a leaf from that node. The height of the root is the height of the tree. The depth of a node is the length of the path to its root (i.e., its root path).
	* \note The world orientation of a bone is defined as follow: WorldOri = _father->getWorldOri() * getLocalOri()
	*
	*/
    //**********************************************************************************
    class GLM_CORE_API HierarchicalBoneV1
    {
        GLM_RTTI("6672f096-b5ec-4aa2-9aa0-d22c546913f3", glm::HierarchicalBoneV1, addAttributes)

    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation)

        friend class HierarchicalSkeletonDescriptionV1;

        //************************************************************
        //! @name Constructors and destructor
        //************************************************************
        //@{
        /**
		* \brief Default Constructor
		*/
        HierarchicalBoneV1(const char* name = "DefaultHierarchicalBoneName");

        /**
		* \brief Constructor
		* \param name bone name
		* \param father bone's father
		* \param relativePos position of the bone in the father coordinate system
		* \param relativeOri orientation of the bone in the father coordinate system
		* \param tabIndex the index of the bone in the hierachical skeleton description it belong to
		* \warning two different bones in a skeleton should never have the same tabIndex
		* \warning the tabIndex should never be more than the total number of bones in a skeleton description
		*/
        HierarchicalBoneV1(const char* name, HierarchicalBoneV1* father, const Vector3& relativePos, const Quaternion& relativeOri, int tabIndex);

        /**
		* \brief Constructor without Pos/Ori initialization
		* \param name bone name
		* \param father bone's father
		* \param tabIndex the index of the bone in the hierachical skeleton description it belong to
		* \warning two different bones in a skeleton should never have the same tabIndex
		* \warning the tabIndex should never be more than the total number of bones in a skeleton description
		*/
        HierarchicalBoneV1(const char* name, HierarchicalBoneV1* father, int tabIndex);

        /**
		* \brief Copy constructor
		* \param source 
		* \note this constructor also create new bones corresponding to each child of the source bone
		*/
        HierarchicalBoneV1(const HierarchicalBoneV1& source);

        /**
		* \brief Copy constructor
		* \param source 
		* \param father the father of this bone (can be NULL)
		* \note this constructor also create new bones corresponding to each child of the source bone
		*/
        HierarchicalBoneV1(const HierarchicalBoneV1& source, HierarchicalBoneV1* father);

        /**
		* \brief Destructor
		* \return 
		*/
        ~HierarchicalBoneV1();
        //@}

        /**
		* \brief Tell if bones are similar
		* \param otherSkeleton 
		* \return true if the name, the parent's name, local positions, local orientation are identical, and chidlren are similar
		* \note if this method is called to comapre roots of skeletons, then it's the same as comparing the skeleton
		*/
        bool similar(const HierarchicalBoneV1& otherBone) const;

        //************************************************************
        //! @name const Accessors
        //************************************************************
        //@{
        /**
		* \brief 
		* \return 
		*/
        inline const glm::GlmString& getName() const;

        inline void setName(const glm::GlmString& name);

        /**
		* \brief 
		* \return 
		*/
        inline const HierarchicalBoneV1* getFather() const;

        /**
		* \brief test if a Bone is the direct or indirect parent (recursively) of another bone.
		* \return 
		*/
        inline bool isParentOf(const HierarchicalBoneV1* bone) const;

        /**
		* \brief 
		* \return 
		*/
        inline unsigned int getNbChildren() const;

        /**
		* \brief 
		* \return 
		*/
        inline const PODArray<HierarchicalBoneV1*>& getChildren() const;

        /**
		* \brief 
		* \return 
		*/
        inline const HierarchicalBoneV1* getChild(unsigned int indice) const;

        /**
		* \brief Get the local position of a bone (ie. relative to its father)
		*/
        inline const Vector3& getLocalPosUnscaled() const;

        /**
		* \brief Get the world position of a bone
		*/
        const Vector3& getWorldPosUnscaled() const;

        /**
		* \brief Get the world position of a bone with scales applied on the local offset on all the hierarchy
		*/
		Vector3 computeWorldPosWithScales(const Array<Vector3>& boneScales) const;
        Vector3 computeWorldPosWithScalesAndOriOffsets(const Array<Vector3>& boneScales, const Array<Quaternion>& boneOriOverrides) const;
        Vector3 computeWorldPosWithLocalOverrides(const Array<Vector3>& localPosOverrides, const Array<Quaternion>& localOriOverrides) const;
        Quaternion computeWorldOriWithLocalOverrides(const Array<Quaternion>& localOriOverrides) const;

        /** 
		* \brief Get the local orientation of a bone (ie. relative to its father)
		*/
        inline const Quaternion& getLocalOri() const;

        /** 
		* \brief Get the world orientation of a bone
		* \note The world orientation of a bone is defined as follow: WorldOri = _father->getWorldOri() * getLocalOri()
		*/
        const Quaternion& getWorldOri() const;

        //@}

        //************************************************************
        //! @name Misc
        //************************************************************
        //@{
        /** 
		* \brief Return the depth of the node
		* \return 
		* \note The depth of a node is the length of the path to its root (i.e., its root path).
		*/
        inline unsigned int depth() const;

        /** \brief Return the height of the node
		*
		* \return 
		* \note The height of a node is the length of the longest downward path to a leaf from that node. The height of the root is the height of the tree.
		*/
        inline unsigned int height() const;

        /**
		* \brief Return the nodes at each depth of the tree
		* \param leaves[out] vector of set with the nodes. leaves.at(0) is the input bone
		*/
        void getSubNodes(glm::Array<GlmSet<const HierarchicalBoneV1*>>& allSubNodes) const;

        /** 
		* \brief Get the leaves (nodes with a null height)
		* \param[in/out] leaves Input: List of nodes - Output: Their computed leaves
		* \note input nodes can also be leaves. In such case, they remain in the output set
		*/
        static void getLeaves(GlmSet<const HierarchicalBoneV1*>& leaves);

        /** 
		* \brief Get the morphology signature of a node, which is composed of the number of children of the node, and the number of children of each child
		* \return a multiset containing the number of chidlren of each child. The number of chidlren of the node itself is the size of the multiset
		*/
        inline std::multiset<unsigned int> getMorphSignature() const;

        /** 
		* \brief Tells if two trees are isomorphic
		* \param treeA root node for the first tree
		* \param treeB root node for the second tree
		* \return true if A and B are isomorphic
		* \note algorithm from: "McGill University, SOCS 308-250B, Assignment 5: Tree Isomorphism"
		*/
        static bool isomorphic(const HierarchicalBoneV1* treeA, const HierarchicalBoneV1* treeB);

        /** 
		* \brief Get the isomorphic sub-branches within the given node
		* \param tree root-node under which finding the isomorphic sub-trees
		* \return true if any isomorphic branches were found
		*/
        static bool isomorphicSubBranches(const HierarchicalBoneV1* tree, glm::Array<GlmSet<const HierarchicalBoneV1*>>& result);

        /** 
		* \brief Find the lowest common ancester of bones
		* \param tree root-node under which finding the common ancester
		* \param bones bone to find a common ancester
		* \return the lowest common ancester if any, or NULL of the bones share no common ancester within the tree
		*/
        static const HierarchicalBoneV1* findLowestCommonAncester(const HierarchicalBoneV1* tree, const glm::PODArray<const HierarchicalBoneV1*>& bones);
        //@}

        //************************************************************
        //! @name Skeleton construction
        //************************************************************
        //@{
        /**
		* \brief 
		* \param father 
		*/
        void setFather(HierarchicalBoneV1* father);

    private:
        /**
		* \brief 
		* \param children 
		*/
        void setChildren(const PODArray<HierarchicalBoneV1*>& children);
        //@}

    public:
        //************************************************************
        //! @name Skeleton construction
        //************************************************************
        //@{
        /**
		* \brief 
		* \param child 
		*/
        void removeChild(HierarchicalBoneV1* child);
        //@}

        //************************************************************
        //! @name not const Accessors
        //************************************************************
        //@{
        /**
		* \brief 
		* \return 
		*/
        inline HierarchicalBoneV1* touchFather() const;

        /**
		* \brief 
		* \return 
		*/
        inline PODArray<HierarchicalBoneV1*>& touchChildren();

        /**
		* \brief 
		* \return 
		*/
        inline HierarchicalBoneV1* touchChild(const unsigned int indice);

        /**
		* \brief Set the local position of a bone (ie. relative to its father)
		* \return 
		*/
        inline void setLocalPos(const Vector3& localPos);

        /**
		* \brief Set the local orientation of a bone (ie. relative to its father)
		* \return 
		*/
        inline void setLocalOri(const Quaternion& localOri);

        /** 
		* \brief Set the world orientation of a bone
		* \note will get the current world orientation of the father bone, and set the local orientation according to the desirted world orientation: the world orientation is not kept in the bone
		* \note The world orientation of a bone is defined as follow: WorldOri = _father->getWorldOri() * getLocalOri()
		*/
        void setWorldOri(const Quaternion& worldOri);

        /** 
		* \brief Set the world position of a bone
		* \note will get the current world orientation/position of the father bone, and set the local position according to the desirted world position: the world position is not kept in the bone
		*/
        void setWorldPos(const Vector3& worldPos);

        /** 
		* \brief Manually set the dirty flag to 'true'
		* \note This method should usuallt not be called, as it is already caleld by all the set methods for orientations and positions, but in case the hierarchy is changed and the bone has no way to know abvout it, then it should be manually called.
		*/
        inline void setCacheDirty();
        //@}

        //************************************************************
        //! @name Low level accessors. Should not be used in normal usage of Motion, but only when cosntructing a hierarchicalSkeleton.
        //************************************************************
        //@{
        /**
		* \brief Get the tab index of the bone
		* \return 
		*/
        inline int getSpecificBoneIndex() const;

        inline void setSpecificBoneIndex(int index);
        //@}

    protected:
        /**
		* \brief Compute the cache of bones
		* \return 
		*/
        void computeCache() const;

        //************************************************************
        //! @name Serialization
        //************************************************************
        //@{
        /**
		* \brief register the inspectable attributes to the class RTTI
		*/
        static void addAttributes();
        //@}

    private:
        //************************************************************
        //! @name private datas
        //************************************************************
        //@{
        glm::GlmString _name; //!< bone name
        Vector3 _localPos;    //!< position of the bone relative to its father
        Quaternion _localOri; //!< orientation of the bone relative to its father

        HierarchicalBoneV1* _father;             //!< pointer to the father bone
        PODArray<HierarchicalBoneV1*> _children; //!< pointers to children bones

        int _specificBoneIndex;
        //@}

        //************************************************************
        //! @name Cache values
        //************************************************************
        //@{
        mutable bool _cacheIsDirty;
        mutable Vector3 _worldPos;    //!< world position of the bone (cache)
        mutable Quaternion _worldOri; //!< world orientation of the bone (cache)
                                      //@}
    };

    //************************************************************
    // Definition of inline methods
    //************************************************************

    //------------------------------------------------------------
    inline const glm::GlmString&
    HierarchicalBoneV1::getName() const
    {
        return _name;
    }

    //------------------------------------------------------------
    inline void HierarchicalBoneV1::setName(const glm::GlmString& name)
    {
        _name = name;
    }

    //------------------------------------------------------------
    inline const HierarchicalBoneV1*
    HierarchicalBoneV1::getFather() const
    {
        return _father;
    }

    //------------------------------------------------------------
    inline bool HierarchicalBoneV1::isParentOf(const HierarchicalBoneV1* bone) const
    {
        if (bone->getFather() != NULL)
        {
            if (bone->getFather() == this)
            {
                return true;
            }
            else
            {
                return isParentOf(bone->getFather());
            }
        }
        else
        {
            return false;
        }
    }

    //------------------------------------------------------------
    inline HierarchicalBoneV1*
    HierarchicalBoneV1::touchFather() const
    {
        return _father;
    }

    //------------------------------------------------------------
    inline unsigned int
    HierarchicalBoneV1::getNbChildren() const
    {
        return (unsigned int)_children.size();
    }

    //------------------------------------------------------------
    inline const PODArray<HierarchicalBoneV1*>&
    HierarchicalBoneV1::getChildren() const
    {
        return _children;
    }

    //------------------------------------------------------------
    inline PODArray<HierarchicalBoneV1*>&
    HierarchicalBoneV1::touchChildren()
    {
        return _children;
    }

    //------------------------------------------------------------
    inline const HierarchicalBoneV1*
    HierarchicalBoneV1::getChild(unsigned int indice) const
    {
        return _children[indice];
    }

    //------------------------------------------------------------
    inline HierarchicalBoneV1*
    HierarchicalBoneV1::touchChild(unsigned int indice)
    {
        return _children[indice];
    }

    //------------------------------------------------------------
    inline const Vector3&
    HierarchicalBoneV1::getLocalPosUnscaled() const
    {
        return _localPos;
    }

    //------------------------------------------------------------
    inline void
    HierarchicalBoneV1::setLocalPos(const Vector3& localPos)
    {
        _localPos = localPos;
        setCacheDirty();
    }

    //------------------------------------------------------------
    inline const Quaternion&
    HierarchicalBoneV1::getLocalOri() const
    {
        return _localOri;
    }

    //------------------------------------------------------------
    inline void
    HierarchicalBoneV1::setLocalOri(const Quaternion& localOri)
    {
        _localOri = localOri;
        _localOri.normalizeSelf();
        setCacheDirty();
    }

    //------------------------------------------------------------
    inline unsigned int
    HierarchicalBoneV1::depth() const
    {
        //The depth of a node is the length of the path to its root (i.e., its root path).
        unsigned int depth = 0;
        if (_father != NULL)
            depth = _father->depth() + 1;

        return depth;
    }

    //------------------------------------------------------------
    inline unsigned int
    HierarchicalBoneV1::height() const
    {
        //The height of a node is the length of the longest downward path to a leaf from that node. The height of the root is the height of the tree.
        unsigned int height = 0;
        for (PODArray<HierarchicalBoneV1*>::const_iterator it = _children.begin(); it != _children.end(); ++it)
            height = max(height, (*it)->height() + 1);

        return height;
    }

    //------------------------------------------------------------
    inline std::multiset<unsigned int>
    HierarchicalBoneV1::getMorphSignature() const
    {
        std::multiset<unsigned int> morphSignature;
        for (PODArray<HierarchicalBoneV1*>::const_iterator it = _children.begin(); it != _children.end(); ++it)
        {
            morphSignature.insert((*it)->getNbChildren());
        }

        return morphSignature;
    }

    //------------------------------------------------------------
    //setCacheDirty
    //------------------------------------------------------------
    inline void
    HierarchicalBoneV1::setCacheDirty()
    {
        _cacheIsDirty = true;
        for (PODArray<HierarchicalBoneV1*>::const_iterator it = _children.begin(); it != _children.end(); ++it)
        {
            (*it)->setCacheDirty();
        }
    }

    //------------------------------------------------------------
    //getTabIndex
    //------------------------------------------------------------
    inline int
    HierarchicalBoneV1::getSpecificBoneIndex() const
    {
        return _specificBoneIndex;
    }

    //------------------------------------------------------------
    //setSpecificBoneIndex
    //------------------------------------------------------------
    inline void HierarchicalBoneV1::setSpecificBoneIndex(int index)
    {
        _specificBoneIndex = index;
    }

} // namespace glm
