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

#include <glmSDKAPI.h>
#include "glmMotionDefinitions.h"
#include "glmString.h"

#include "glmPlayAnimationBehavior.h"
#include "glmSyncActionBehavior.h"
#include "glmLocomotionBehavior.h"
#include "glmNavAndLocoBehavior.h"
#include "glmPhysicalizeBehavior.h"
#include "glmForceBehavior.h"

#include <glmGolaemCharacterDeclarations.h> // GolaemCharacter declarations

namespace glm
{
    class AbstractSpecificAnimation;
    class AnimationReplayMapping;

    namespace motionUtils
    {
        /**
		* \brief returns the limbId of a limb name
		*/
        int GLM_SDK_API getSpecificLimbId(const GenericSpecificConverterMapping* converterMapping, const glm::GlmString& limbName);

        /**
		* \brief returns the boneId of a bone name
		*/
        int GLM_SDK_API getSpecificBoneId(const GenericSpecificConverterMapping* converterMapping, const glm::GlmString& boneName);
        glm::GlmString GLM_SDK_API getSpecificBoneName(const GenericSpecificConverterMapping* converterMapping, int boneId);

        /**
		* \brief returns the blendshapes ids of a blendshape
		* \param blendShapeName can be of form BSGroup.BSName or BSName
		* \return the blendshape id in the Generic Posture
		*/
        int GLM_SDK_API getBlindDataId(const GenericSpecificConverterMapping* converterMapping, const glm::GlmString& blendShapeCompleteName, const glm::GlmString& separator);
        glm::GlmString GLM_SDK_API getBlindDataName(const GenericSpecificConverterMapping* converterMapping, int blindDataId);

        /**
		* \brief check if a given specific bone id is valid
		*/
        bool GLM_SDK_API isSpecificBoneIdValid(const GenericSpecificConverterMapping* converterMapping, int specificBoneId);

        /**
		* \brief Convert a specific bone id into a generic bone id
		* \return -1 if not valid input
		*/
        int GLM_SDK_API getGenericBoneId(const GenericSpecificConverterMapping* converterMapping, int specificBoneId);

        /**
		* \brief for a specific bone id, return true if it correspond to a End Of generic bone
		*/
        bool GLM_SDK_API correspondToEndOfGenericBone(const GenericSpecificConverterMapping* converterMapping, int specificBoneId);

        /**
        * \brief Extract the linear and angular speeds from a generic animation when replayed on a skeleton with a given root height
        */
        template<class AnimationType>
        void extractAnimationNormalizedSpeeds(float& linearVelocityRet, float& angularVelocityRet, const AnimationType* animation, bool mirror, float replayRatio);

        //body masks
        void GLM_SDK_API cleanBodyMaskString(glm::GlmString& inoutValue, bool lower = true);
        bool GLM_SDK_API isInBodyMask(const glm::GlmString& inputString, const glm::Array<glm::GlmString>& maskPossibleNames);
        void GLM_SDK_API computeBodyMask(Array<bool>& outMask, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);
        template <class BehaviorType>
        void applyBodyMaskOnBehavior(BehaviorType* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);
        void GLM_SDK_API applyBlindDataMask(AnimationReplayMapping* mapping, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBlindDataGroups, const glm::Array<glm::GlmString>& maskBlindDatas);

        //legacy function
        template <class BehaviorType>
        void applyLegacyBodyMaskOnBehavior(BehaviorType* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors);















        //-------------------------------------------------------------------------
        //definitions

        template<class AnimationType>
        void extractAnimationNormalizedSpeeds(float& linearVelocityRet, float& angularVelocityRet, const AnimationType* animation, bool mirror, float replayRatio)
        {
            // Extract required information from animations
            float duration(static_cast<float>((animation->getPosturesCount() - 1) * animation->getDeltaTime() / replayRatio));

            //param 1 is linear velocity in m/s
            Vector3 animationDeltaPosition;
            animation->getTotalNormalizedDeltaPosition(animationDeltaPosition);
            linearVelocityRet = (float)(animationDeltaPosition.norm() / duration);
            //param 2 is angular velocity in rad/s
            float heading = animation->computeTotalDeltaHeading(); // mirror is not handled internally by GenericAnimation
            angularVelocityRet = (float)(heading / duration);

            if (mirror)
                angularVelocityRet = -angularVelocityRet;
        }

        //-----------------------------------------------------------------------------
        template <class BehaviorType>
        void applyBodyMaskOnBehavior(BehaviorType* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones)
        {
            Array<bool> mask = behavior->getMask(); //get previous values
            computeBodyMask(mask, converter, maskBones);

            // --- effective configuration
            behavior->setMask(mask);
        }

        //-----------------------------------------------------------------------------
        inline void addRollBonesMask(const GenericSpecificConverterBone& converterBone, int auxExt, Array<bool>& mask)
        {
            for (size_t rollAttributesIdx = 0, nbRollAttributes = converterBone._rollAttributes[auxExt].size(); rollAttributesIdx < nbRollAttributes; ++rollAttributesIdx)
            {
                const RollAttributes& rollAttributes = converterBone._rollAttributes[auxExt][rollAttributesIdx];
                for (size_t rollExtractionIdx = 0, nbRollExtractions = rollAttributes._rollBonesIdx.size(); rollExtractionIdx < nbRollExtractions; ++rollExtractionIdx)
                {
                    int rollExtractionBoneIdx = rollAttributes._rollBonesIdx[rollExtractionIdx];
                    float weight = rollAttributes._rollWeights[rollExtractionIdx];
                    if (fabs(weight) >= GLM_NUMERICAL_PRECISION)
                        mask[rollExtractionBoneIdx] = true;
                }
            }
        }

        //-----------------------------------------------------------------------------
        template <class BehaviorType>
        void applyLegacyBodyMaskOnBehavior(BehaviorType* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors)
        {
            GenericSpecificConverterMapping* converterMapping = converter->_converterMapping;
            const PODArray<HierarchicalBone*> bones = converterMapping->_skeletonDescription->getBones();

            Array<bool> mask = behavior->getMask(); //get previous values
            mask.resize(bones.size(), false);

            //////////////////
            //pelvis
            if (maskPelvis)
            {
                const HierarchicalBone* currentSpecificBone = converterMapping->getPelvisBone();
                while (currentSpecificBone)
                {
                    mask[currentSpecificBone->getSpecificBoneIndex()] = true;
                    currentSpecificBone = currentSpecificBone->getFather();
                }
            }

            //////////////////
            //spine mask
            int nbSkeletonSpines = converterMapping->getSpineBones().sizeInt();
            if (!maskSpines.empty() && maskSpines[0] == "*") // all channels
            {
                for (int iSpine = 0; iSpine < nbSkeletonSpines; ++iSpine)
                {
                    for (int auxExt = 0; auxExt < 2; ++auxExt)
                    {
                        const GenericSpecificConverterBone& converterBone = converterMapping->getSpineBones()[iSpine];
                        int lastBoneOfChainIndex = converterBone._lastBoneOfChain[auxExt];
                        if (lastBoneOfChainIndex >= 0)
                        {
                            const HierarchicalBone* currentSpecificBone = bones[lastBoneOfChainIndex];
                            while (currentSpecificBone)
                            {
                                mask[currentSpecificBone->getSpecificBoneIndex()] = true;
                                if (currentSpecificBone->getSpecificBoneIndex() == converterBone._firstBoneOfChain[auxExt])
                                    break;
                                currentSpecificBone = currentSpecificBone->getFather();
                            }
                            addRollBonesMask(converterBone, auxExt, mask);
                        }
                    }
                }
            }
            else
            {
                GlmString masterSpineName;
                for (int iSpine = 0; iSpine < nbSkeletonSpines; ++iSpine)
                {
                    converterMapping->getSpineBoneName(masterSpineName, iSpine);
                    cleanBodyMaskString(masterSpineName);
                    if (isInBodyMask(masterSpineName, maskSpines))
                    {
                        for (int auxExt = 0; auxExt < 2; ++auxExt)
                        {
                            const GenericSpecificConverterBone& converterBone = converterMapping->getSpineBones()[iSpine];
                            int lastBoneOfChainIndex = converterBone._lastBoneOfChain[auxExt];
                            if (lastBoneOfChainIndex >= 0)
                            {
                                const HierarchicalBone* currentSpecificBone = bones[lastBoneOfChainIndex];
                                while (currentSpecificBone)
                                {
                                    mask[currentSpecificBone->getSpecificBoneIndex()] = true;
                                    if (currentSpecificBone->getSpecificBoneIndex() == converterBone._firstBoneOfChain[auxExt])
                                        break;
                                    currentSpecificBone = currentSpecificBone->getFather();
                                }
                                addRollBonesMask(converterBone, auxExt, mask);
                            }
                        }
                    }
                }
            }

            //////////////////
            //limbs mask
            int nbSkeletonLimbs = converterMapping->getLimbBones().sizeInt();
            if (!maskChannelLimbs.empty() && maskChannelLimbs[0] == "*") // all channels
            {
                for (int iChannel = 0; iChannel < nbSkeletonLimbs; ++iChannel)
                {
                    for (int auxExt = 0; auxExt < 2; ++auxExt)
                    {
                        const GenericSpecificConverterBone& converterBone = converterMapping->getLimbBones()[iChannel];
                        int lastBoneOfChainIndex = converterBone._lastBoneOfChain[auxExt];
                        if (lastBoneOfChainIndex >= 0)
                        {
                            const HierarchicalBone* currentSpecificBone = bones[lastBoneOfChainIndex];
                            while (currentSpecificBone)
                            {
                                mask[currentSpecificBone->getSpecificBoneIndex()] = true;
                                if (currentSpecificBone->getSpecificBoneIndex() == converterBone._firstBoneOfChain[auxExt])
                                    break;
                                currentSpecificBone = currentSpecificBone->getFather();
                            }
                            addRollBonesMask(converterBone, auxExt, mask);
                        }
                    }
                }
            }
            else
            {
                glm::GlmString masterLimbName;
                for (size_t iMasterLimb = 0, iMasterLimbEnd = converterMapping->getLimbBonesSize(); iMasterLimb < iMasterLimbEnd; ++iMasterLimb)
                {
                    converterMapping->getLimbBoneName(masterLimbName, iMasterLimb);
                    cleanBodyMaskString(masterLimbName);
                    if (isInBodyMask(masterLimbName, maskChannelLimbs))
                    {
                        for (int auxExt = 0; auxExt < 2; ++auxExt)
                        {
                            const GenericSpecificConverterBone& converterBone = converterMapping->getLimbBones()[iMasterLimb];
                            int lastBoneOfChainIndex = converterBone._lastBoneOfChain[auxExt];
                            if (lastBoneOfChainIndex >= 0)
                            {
                                const HierarchicalBone* currentSpecificBone = bones[lastBoneOfChainIndex];
                                while (currentSpecificBone)
                                {
                                    mask[currentSpecificBone->getSpecificBoneIndex()] = true;
                                    if (currentSpecificBone->getSpecificBoneIndex() == converterBone._firstBoneOfChain[auxExt])
                                        break;
                                    currentSpecificBone = currentSpecificBone->getFather();
                                }
                                addRollBonesMask(converterBone, auxExt, mask);
                            }
                        }
                    }
                }
            }

            //////////////////
            //effectors mask
            ChannelInfo* channelInfos(NULL);
            int channelsCount = 0;
            converter->getGenericSkeleton().computeChannelsInfos(&channelsCount, &channelInfos);

            bool effectorsWildCard = !maskChannelEffectors.empty() && maskChannelEffectors[0] == "*";
            for (int iChannel = 0; iChannel < channelsCount; ++iChannel)
            {
                for (int iEffectorInChannel = 0; iEffectorInChannel < channelInfos[iChannel].getNbEffectors(); iEffectorInChannel++)
                {
                    int effectorGenericBoneIndex = channelInfos[iChannel]._genericBoneIndices[2 + (iEffectorInChannel * 2)];
                    int effectorMappingBoneIndex = (effectorGenericBoneIndex - 1 - nbSkeletonSpines - (2 * nbSkeletonLimbs)) / 2;
                    if (effectorsWildCard)
                    {
                        for (int auxExt = 0; auxExt < 2; ++auxExt)
                        {
                            const GenericSpecificConverterBone& converterBone = converterMapping->getEffectorBones()[effectorMappingBoneIndex];
                            int lastBoneOfChainIndex = converterBone._lastBoneOfChain[auxExt];
                            if (lastBoneOfChainIndex >= 0)
                            {
                                const HierarchicalBone* currentSpecificBone = bones[lastBoneOfChainIndex];
                                while (currentSpecificBone)
                                {
                                    mask[currentSpecificBone->getSpecificBoneIndex()] = true;
                                    if (currentSpecificBone->getSpecificBoneIndex() == converterBone._firstBoneOfChain[auxExt])
                                        break;
                                    currentSpecificBone = currentSpecificBone->getFather();
                                }
                                addRollBonesMask(converterBone, auxExt, mask);
                            }
                        }
                    }
                    else
                    {
                        glm::GlmString masterEffectorName;
                        converter->getEffectorBoneName(masterEffectorName, iChannel, iEffectorInChannel);

                        if (isInBodyMask(masterEffectorName, maskChannelEffectors))
                        {
                            for (int auxExt = 0; auxExt < 2; ++auxExt)
                            {
                                const GenericSpecificConverterBone& converterBone = converterMapping->getEffectorBones()[effectorMappingBoneIndex];
                                int lastBoneOfChainIndex = converterBone._lastBoneOfChain[auxExt];
                                if (lastBoneOfChainIndex >= 0)
                                {
                                    const HierarchicalBone* currentSpecificBone = bones[lastBoneOfChainIndex];
                                    while (currentSpecificBone)
                                    {
                                        mask[currentSpecificBone->getSpecificBoneIndex()] = true;
                                        if (currentSpecificBone->getSpecificBoneIndex() == converterBone._firstBoneOfChain[auxExt])
                                            break;
                                        currentSpecificBone = currentSpecificBone->getFather();
                                    }
                                    addRollBonesMask(converterBone, auxExt, mask);
                                }
                            }
                        }
                    }
                }
            }

            GLM_SAFE_DELETE_ARRAY(channelInfos, ChannelInfo::getClassMemoryStatId());

            // --- effective configuration
            behavior->setMask(mask);
        }

        //export the applyBodyMaskOnBehavior function for the class that uses it
        template void GLM_SDK_API applyBodyMaskOnBehavior<PlayAnimationBehavior>(PlayAnimationBehavior* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);
        template void GLM_SDK_API applyBodyMaskOnBehavior<SyncActionBehavior>(SyncActionBehavior* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);
        template void GLM_SDK_API applyBodyMaskOnBehavior<LocomotionBehavior>(LocomotionBehavior* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);
        template void GLM_SDK_API applyBodyMaskOnBehavior<NavAndLocoBehavior>(NavAndLocoBehavior* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);
        template void GLM_SDK_API applyBodyMaskOnBehavior<PhysicalizeBehavior>(PhysicalizeBehavior* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);
        template void GLM_SDK_API applyBodyMaskOnBehavior<ForceBehavior>(ForceBehavior* behavior, const GenericSpecificConverter* converter, const glm::Array<glm::GlmString>& maskBones);

        //export the applyLegacyBodyMaskOnBehaviorOnDynamicBehavior function for the class that uses it
        template void GLM_SDK_API applyLegacyBodyMaskOnBehavior<PlayAnimationBehavior>(PlayAnimationBehavior* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors);
        template void GLM_SDK_API applyLegacyBodyMaskOnBehavior<SyncActionBehavior>(SyncActionBehavior* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors);
        template void GLM_SDK_API applyLegacyBodyMaskOnBehavior<LocomotionBehavior>(LocomotionBehavior* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors);
        template void GLM_SDK_API applyLegacyBodyMaskOnBehavior<NavAndLocoBehavior>(NavAndLocoBehavior* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors);
        template void GLM_SDK_API applyLegacyBodyMaskOnBehavior<PhysicalizeBehavior>(PhysicalizeBehavior* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors);
        template void GLM_SDK_API applyLegacyBodyMaskOnBehavior<ForceBehavior>(ForceBehavior* behavior, const GenericSpecificConverter* converter, bool maskPelvis, const glm::Array<glm::GlmString>& maskSpines, const glm::Array<glm::GlmString>& maskChannelLimbs, const glm::Array<glm::GlmString>& maskChannelEffectors);

    } // namespace motionUtils
} //End of namespace glm
