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

#pragma once

#include "glmSDKAPI.h"
#include "glmMemory.h"
#include "glmArray.h"
#include "glmVector3.h"
#include "glmQuaternion.h"
#include "glmGenericSpecificConverterMapping.h"

namespace glm
{
    class SpecificPosture;
    class GenericSpecificConverter;

    /**\brief Class to set the posture of a skeleton in a the blend tree
    * \author Golaem <dev@golaem.com>
    */
    class GLM_SDK_API BlendTreePosture
    {
    public: //methods
        GLM_CLASS_STAT_ID(glm::MemoryStatIds::Simulation);

        BlendTreePosture();
        ~BlendTreePosture();

        void init(const GenericSpecificConverterMapping* genericSpecificConverterMapping, const HierarchicalSkeletonDescription::Override* skeletonOverride);
        void finish();
        void resetPosture(float scale, bool computeWorlds);
        void fromSpecificPosture(const SpecificPosture& specificPosture, float srcEntityScale, const HierarchicalSkeletonDescription* specificPostureSkeletonDescription=NULL);
        void toSpecificPosture(SpecificPosture& specificPosture) const;
        void blindDataToSpecificPosture(SpecificPosture& dstSpecificPosture, const GenericSpecificConverter* converter) const;

        void computeWorldFromLocals(const Array<int>& orderedBoneIndices) const;
        void computeWorldFromLocals(int firstBoneIndex, int lastBoneIndex) const;
        void computeWorldFromLocalsForBoneAndChildrenOf(int boneIndex) const;

        void computeSelfDifferenceWith(const BlendTreePosture& otherPosture, int lastBoneIndex, int lastBlindDataIndex);

        Quaternion computeZupPelvisWorldOrientation() const;     //standardized pelvis orientation (Z-up, X-front, whatever the real bone axis are)
        Vector3 computeZupPelvisWorldPosition() const;           //standardized pelvis position (position of the hierarchical bone mapped as pelvis in the mapping)
        Vector3 computeZupMovingDirection() const;               //standardized moving direction in the pelvis coordinate system, (1,0,0) being the front of the character

        void setTransformsFromZupPelvis(const Vector3& ZupPelvisWorldPos, const Quaternion& ZupPelvisWorldOri, float scale);
        void setTransformsFromBlendTreePelvis(const Vector3& pelvisWorldPos, const Quaternion& pelvisWorldOri, float scale);

        inline Quaternion computeRestRelativeFromDesiredWorld(int boneIndex, const Quaternion& desiredWorldOrientation) const;
        inline Vector3 computeRestRelativeFromDesiredWorld(int boneIndex, const Vector3& desiredWorldPosition) const;

        void boneBlend(const BlendTreePosture& otherPosture, const float interpolationWeights[], int firstBoneIndex, int lastBoneIndex);
        void boneBlend(const BlendTreePosture& otherPosture, const float thisPostureWeights[], const float otherPostureWeights[], int firstBoneIndex, int lastBoneIndex);

        void boneAdd(const BlendTreePosture& otherPosture, const float positionOrientationInterpolationWeight, float interpolationWeight, int firstBoneIndex, int lastBoneIndex);
        void boneAdd(const BlendTreePosture& otherPosture, const float movingDirectionInterpolationWeight, const float interpolationWeights[], int firstBoneIndex, int lastBoneIndex);

        void blindDataAdd(const BlendTreePosture& otherPosture, const float interpolationWeight[]);
        void blindDataBlend(const BlendTreePosture& otherPosture, const float interpolationWeights[]);

        void computeChainLength(const Array<int>& boneIndices, Array<float>& outLengths) const;
        float computeChainLength(const Array<int>& boneIndices) const;

    protected:  //methods
        inline void computeWorldFromLocals(int currentBoneIndex, const HierarchicalSkeletonDescription::Override* skeletonOverride) const;
        inline void computeWorldFromLocals(int currentBoneIndex) const;

    public: //data
        const GenericSpecificConverterMapping* _genericSpecificConverterMapping;
        const HierarchicalSkeletonDescription::Override* _skeletonOverride;

        float _entityScale = 1.f;

        Array<Vector3> _restRelativePositions;
        Array<Quaternion> _restRelativeOrientations;
        Array<Vector3> _restRelativeScales;
        glm::PODArray<float> _blindData;

        Vector3 _movingDirection;               //moving direction in the pelvis coordinate system (use getPelvisWorldOrientation()*getMovingDirection() to obtain the moving direction in world coordinate system )

        mutable Array<Vector3> _worldPositions;         //_worldPositions[i] = _worldPositions[father(i)] + _worldOrientations[father(i)] * _worldScales[father(i)] * (bone[i]->getLocalPos() + _restRelativePositions[i])
        mutable Array<Quaternion> _worldOrientations;   //_worldOrientations[i] = _worldOrientations[father(i)] * bone[i]->getLocalOri() * _restRelativeOrientations[i]
        mutable Array<Vector3> _worldScales;            //_worldScales[i] = _worldScales[father(i)] * _restRelativeScales[i]
    };

    //------------------------------------------------------------
    inline Quaternion BlendTreePosture::computeRestRelativeFromDesiredWorld(int boneIndex, const Quaternion& desiredWorldOrientation) const
    {
        HierarchicalBone* currentBone = _genericSpecificConverterMapping->_skeletonDescription->getBones()[boneIndex];
        Quaternion parentBoneWorldOri(0,0,0,1);

        const HierarchicalBone* parentBone(currentBone->getFather());
        if (parentBone != NULL)
        {
            int fatherBoneIndex = currentBone->getFather()->getSpecificBoneIndex();
            parentBoneWorldOri = _worldOrientations[fatherBoneIndex];
        }

        if (_skeletonOverride)
            return (parentBoneWorldOri * _skeletonOverride->_boneslocalOri[boneIndex]).computeInverse() * desiredWorldOrientation;
        else
            return (parentBoneWorldOri * currentBone->getLocalOri()).computeInverse() * desiredWorldOrientation;
    }

    //------------------------------------------------------------
    inline Vector3 BlendTreePosture::computeRestRelativeFromDesiredWorld(int boneIndex, const Vector3& desiredWorldPosition) const
    {
        HierarchicalBone* currentBone = _genericSpecificConverterMapping->_skeletonDescription->getBones()[boneIndex];
        const HierarchicalBone* parentBone(currentBone->getFather());
        if (parentBone == NULL)
        {
            if (_skeletonOverride)
                return ((desiredWorldPosition / _entityScale) - _skeletonOverride->_boneslocalPos[boneIndex]);
            else
                return ((desiredWorldPosition / _entityScale) - currentBone->getLocalPosUnscaled());
        }
        else
        {
            int fatherBoneIndex = currentBone->getFather()->getSpecificBoneIndex();
            const Quaternion& parentBoneWorldOri = _worldOrientations[fatherBoneIndex];
            const Vector3& parentBoneWorldPos = _worldPositions[fatherBoneIndex];
            const Vector3& parentBoneWorldScale = _worldScales[fatherBoneIndex];

            Vector3 unscaledLocalPos = memberToMemberProduct(parentBoneWorldOri.computeInverse() * (desiredWorldPosition - parentBoneWorldPos), 1.f / parentBoneWorldScale);
            if (_skeletonOverride)
                return (unscaledLocalPos - _skeletonOverride->_boneslocalPos[boneIndex]);
            else
                return (unscaledLocalPos - currentBone->getLocalPosUnscaled());
        }
    }

    //-------------------------------------------------------------------------
    inline void BlendTreePosture::computeWorldFromLocals(int currentBoneIndex, const HierarchicalSkeletonDescription::Override* skeletonOverride) const
    {
        const HierarchicalBone* bone = _genericSpecificConverterMapping->_skeletonDescription->getBones()[currentBoneIndex];
        const HierarchicalBone* father = bone->getFather();

        if (father == NULL)
        {
            _worldOrientations[currentBoneIndex] = skeletonOverride->_boneslocalOri[currentBoneIndex] * _restRelativeOrientations[currentBoneIndex];
            _worldPositions[currentBoneIndex] = (_entityScale * (skeletonOverride->_boneslocalPos[currentBoneIndex] + _restRelativePositions[currentBoneIndex]));
            _worldScales[currentBoneIndex] = _entityScale * skeletonOverride->_bonesSkinningScale[currentBoneIndex];
        }
        else
        {
            int fatherId(father->getSpecificBoneIndex());

            Quaternion::mulmul(_worldOrientations[currentBoneIndex], _worldOrientations[fatherId], skeletonOverride->_boneslocalOri[currentBoneIndex], _restRelativeOrientations[currentBoneIndex]);
            Vector3 fatherWorldScaleWithoutSkeletonOverride = memberToMemberProduct(_worldScales[fatherId], 1.f / skeletonOverride->_bonesSkinningScale[fatherId]);    //skinning scale override is not inherited
            Quaternion::mulAdd(_worldPositions[currentBoneIndex], _worldPositions[fatherId], _worldOrientations[fatherId], memberToMemberProduct(fatherWorldScaleWithoutSkeletonOverride, skeletonOverride->_boneslocalPos[currentBoneIndex] + _restRelativePositions[currentBoneIndex]));
            _worldScales[currentBoneIndex] = memberToMemberProduct(fatherWorldScaleWithoutSkeletonOverride, _restRelativeScales[currentBoneIndex]);
            _worldScales[currentBoneIndex] = memberToMemberProduct(_worldScales[currentBoneIndex], skeletonOverride->_bonesSkinningScale[currentBoneIndex]);   //local skinning scale override
        }
    }

    //-------------------------------------------------------------------------
    inline void BlendTreePosture::computeWorldFromLocals(int currentBoneIndex) const
    {
        const HierarchicalBone* bone = _genericSpecificConverterMapping->_skeletonDescription->getBones()[currentBoneIndex];
        const HierarchicalBone* father = bone->getFather();

        if (father == NULL)
        {
            _worldOrientations[currentBoneIndex] = bone->getLocalOri() * _restRelativeOrientations[currentBoneIndex];
            _worldPositions[currentBoneIndex] = (_entityScale * (bone->getLocalPosUnscaled() + _restRelativePositions[currentBoneIndex]));
            _worldScales[currentBoneIndex] = _entityScale * _restRelativeScales[currentBoneIndex];
        }
        else
        {
            int fatherId(father->getSpecificBoneIndex());

            Quaternion::mulmul(_worldOrientations[currentBoneIndex], _worldOrientations[fatherId], bone->getLocalOri(), _restRelativeOrientations[currentBoneIndex]);
            Quaternion::mulAdd(_worldPositions[currentBoneIndex], _worldPositions[fatherId], _worldOrientations[fatherId], memberToMemberProduct(_worldScales[fatherId], bone->getLocalPosUnscaled() + _restRelativePositions[currentBoneIndex]));
            _worldScales[currentBoneIndex] = memberToMemberProduct(_worldScales[fatherId], _restRelativeScales[currentBoneIndex]);
        }
    }

} // namespace glm
