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

#pragma once

#include "glmSDKAPI.h"
#include "glmAnimationInterface.h"

#include "glmMotionDefinitions.h"
#include "glmHierarchicalSkeletonDescription.h"
#include "glmSpecificPosture.h"
#include "glmGroundSupportSequence.h"
#include "glmInterpolation.h"
#include "glmBlendTreePosture.h"

namespace glm
{
	/**
	 * \brief Interface for a specific animation
	 *
	 * \author dev@golaem.com
	 *
	 */
	class GLM_SDK_API AbstractSpecificAnimation : public AnimationInterface
	{
	public: //static methods
		GLM_ABSTRACTCLASS_STAT_ID( glm::MemoryStatIds::GolaemShared)

		static void computeNextLoopOrigin(Vector3& loopOriginNormalizedPosition, Quaternion& loopOriginOrientation, const AbstractSpecificAnimation& animation);
		static void computeLastLoopOrigin(Vector3& loopOriginNormalizedPosition, Quaternion& loopOriginOrientation, const AbstractSpecificAnimation& animation);

	public:	//methods
		AbstractSpecificAnimation()
			: AnimationInterface(AnimationType::SPECIFIC)
			, _skeletonMapping(NULL)
			, _genericConverter(NULL)
		{ }
		virtual ~AbstractSpecificAnimation();

		//************************************************************
		//! @name Animation accessors
		//************************************************************
		//@{
		/**
		 * \brief Get the animation delta time between each posture
		 */
		virtual Time getDeltaTime() const = 0;

		/**
		 * \brief Get the skeleton description from the animation
		*/
		virtual const HierarchicalSkeletonDescription* getSkeletonDescription() const = 0;
        int remapOnNewSkeletonMapping(const GenericSpecificConverterMapping& newSkeletonMapping);
        int remapPosturesByName(const HierarchicalSkeletonDescription& skeletonDescription);

		/**
		 * \brief Get the size of the specific postures array
		 */
		virtual int getPosturesCount() const = 0;

		/**
		 * \brief Get the array of the specific postures representing the animation
		 */
		virtual const SpecificPosture* getPostures() const = 0;

		virtual void getTotalNormalizedDeltaPosition(Vector3& result) const;
		virtual void getTotalDeltaPosition(Vector3& result) const;
		virtual void getTotalDeltaOrientation(Quaternion& result) const;
		virtual glm::Angle computeTotalDeltaHeading() const;
        virtual void computePosturesMovingDirection();
        virtual Vector3 computeAverageMovingDirection() const;
        virtual void computeAverageMovingDirection(Vector3& result) const;
        virtual inline Vector3 getMovingDirectionAtFrame(unsigned int frameIndex) const;
		//@}

		GenericSpecificConverter* getGenericConverter() const;
        GenericSpecificConverterMapping* getSkeletonMapping() const;

		virtual bool cropMotion(int newFirstFrame, int newLastFrame) = 0;
		void cropSupportSequence(int newFirstFrame, int newLastFrame);

        void computeLimbsSupportSequencesFromBonesSupportSequences();
		void computeFootPrintsFullAutomatic(const glm::PODArray<bool>& limbs);
        void computeFootPrints(const glm::PODArray<bool>& limbs, float heightThreshold, float speedTheshold);

        inline const Array<GroundSupportSequence>& getConstraintSequences() const;
        inline const GlmMap<int, GroundSupportSequence>& getBonesSupportSequences() const;

        virtual const Array<BlindData>& getBlindDataInfo() const = 0;
		virtual const PODArray<float>& getBlindData(int frameIndex) const = 0;

    protected: //methods
        virtual SpecificPosture* touchPostures() = 0;
        virtual void setSkeletonDescription(const HierarchicalSkeletonDescription& newSkeletonDescription) = 0;

	public:	//data
		mutable GenericSpecificConverterMapping* _skeletonMapping = NULL;
		mutable GenericSpecificConverter* _genericConverter = NULL;

		Array<GroundSupportSequence> _groundSupportSequences;       //per limb in the mapping representation
        GlmMap<int, GroundSupportSequence>  _bonesSupportSequences; //per bone in the skeleton

        Array<Vector3> _postureMovingDirections;            //in animation root coordinate system
	};

    //------------------------------------------------------------
    inline Vector3 AbstractSpecificAnimation::getMovingDirectionAtFrame(unsigned int frameIndex) const
    {
        if (frameIndex < _postureMovingDirections.size())
        {
            return _postureMovingDirections[frameIndex];
        }
        GLM_SDK_TRACE_WARNING("No moving direction at frame " << frameIndex << ". _postureMovingDirections.size() is " << _postureMovingDirections.size());
        return Vector3::getNullVector();
    }

    //------------------------------------------------------------
    inline const Array<GroundSupportSequence>& AbstractSpecificAnimation::getConstraintSequences() const
    {
        return _groundSupportSequences;
    }

    //------------------------------------------------------------
    inline const GlmMap<int, GroundSupportSequence>& AbstractSpecificAnimation::getBonesSupportSequences() const
    {
        return _bonesSupportSequences;
    }

    //------------------------------------------------------------
    template<class AnimationClass>
    bool templateCropMotion(AnimationClass* editedAnimation, int newFirstFrame, int newLastFrame)
    {
        if (newFirstFrame >= editedAnimation->_animationPostures.sizeInt())
        {
            newFirstFrame = editedAnimation->_animationPostures.sizeInt() - 1;
        }
        if (newLastFrame >= editedAnimation->_animationPostures.sizeInt())
        {
            newLastFrame = editedAnimation->_animationPostures.sizeInt() - 1;
        }
        if (newFirstFrame < newLastFrame)
        {
            Array<SpecificPosture> oldSpecificPostures(editedAnimation->_animationPostures);

            int croppedPosturesCount = newLastFrame - newFirstFrame + 1;
            editedAnimation->_animationPostures.resize(croppedPosturesCount);
            editedAnimation->_animationPostures.trim();
            //copy postures
            for (int iPosture = newFirstFrame; iPosture <= newLastFrame; ++iPosture)
            {
                int newIndex = iPosture - newFirstFrame;
                editedAnimation->_animationPostures[newIndex] = oldSpecificPostures[iPosture];
            }

            //crop support sequence
            editedAnimation->cropSupportSequence(newFirstFrame, newLastFrame);

            //delete olds
            oldSpecificPostures.clear();
            oldSpecificPostures.trim();
            return true;
        }
        return false;
    }

    //------------------------------------------------------------
    template<class AnimationClass>
    void applyTransformOffset(const AnimationClass& srcMotion, AnimationClass& dstMotion, const Vector3& normalizedTranslationOffset, const Quaternion& orientationOffset, int offsetBeginOfMotion, int firstOffsetPosture, int lastOffsetPosture)
    {
        int postureCount = srcMotion._animationPostures.sizeInt();
        int boneCount = srcMotion._skeletonDescription.getBones().sizeInt();
        GLM_DEBUG_ASSERT(dstMotion._animationPostures.sizeInt() == postureCount);
        GLM_DEBUG_ASSERT(dstMotion._skeletonDescription.getBones().sizeInt() == boneCount);

        int offsetPosturesCount(lastOffsetPosture - firstOffsetPosture);
        for (int iPosture = 0; iPosture < postureCount; ++iPosture)
        {
            float errorCorrectionFactor(0.f);
            switch (offsetBeginOfMotion)
            {
            default:
            case 0: //normal way: full offset at the end of the animation (increase from the beggining)
            {
                errorCorrectionFactor = glm::clamp((float)(iPosture - firstOffsetPosture) / (float)(offsetPosturesCount - 1), 0.f, 1.f);
            }
            break;
            case 1: //reverse way: full offset at the beginning of the animation and decrease till the end
            {
                errorCorrectionFactor = glm::clamp(1.f - ((float)(iPosture - firstOffsetPosture) / (float)(offsetPosturesCount - 1)), 0.f, 1.f);
            }
            break;
            }

            const Array<Vector3>& srcPositions = srcMotion._animationPostures[iPosture].getWorldPositions();
            const Array<Quaternion>& srcOrientations = srcMotion._animationPostures[iPosture].getWorldOrientations();
            Array<Vector3>& dstPositions = dstMotion._animationPostures[iPosture].touchWorldPositions();
            Array<Quaternion>& dstOrientations = dstMotion._animationPostures[iPosture].touchWorldOrientations();

            for (int iBone = 0; iBone < boneCount; ++iBone)
            {
                dstPositions[iBone] = srcPositions[iBone] + (errorCorrectionFactor * normalizedTranslationOffset);
                interpolate(dstOrientations[iBone], srcOrientations[iBone], srcOrientations[iBone] * orientationOffset, errorCorrectionFactor);
            }
        }
    }

    //------------------------------------------------------------
    template<class AnimationClass>
    void templateEditMotion(AnimationClass* editedAnimation, const EditMotionParameters& parameters)
    {
        //needs a copy of the original animation while editing it
        AnimationClass originalMotion(*editedAnimation);

        int postureCount = editedAnimation->_animationPostures.sizeInt();
        int firstBlendPosture = clamp(parameters._firstBlendingFrame, 0, postureCount);
        int lastBlendPosture = clamp(parameters._lastBlendingFrame, firstBlendPosture, postureCount);
        int blendPosturesCount(lastBlendPosture - firstBlendPosture);

        //frequency
        editedAnimation->_animationDeltaTime = parameters._deltaTime;
        Time newLoopDuration = editedAnimation->_animationDeltaTime * (postureCount - 1);
        for (size_t iSupport = 0; iSupport < editedAnimation->_groundSupportSequences.size(); ++iSupport)
        {
            GroundSupportSequence& currentSupportSequence= editedAnimation->_groundSupportSequences[iSupport];
            currentSupportSequence.retime(newLoopDuration);
        }

        //compute additional delta pos/ori to add
        Vector3 originalNormalizedDeltaPos;
        originalMotion.getTotalDeltaPosition(originalNormalizedDeltaPos);
        Vector3 totalNormalizedDeltaPosError(parameters._deltaNormalizedPosition - originalNormalizedDeltaPos);
        //compute additional delta ori to add
        Angle originalDeltaHeading = originalMotion.computeTotalDeltaHeading();
        Angle totalDeltaHeadingError = parameters._deltaHeading - originalDeltaHeading;
        if (parameters._correctFromBeginOrEnd == 1)
        {
            totalNormalizedDeltaPosError = -totalNormalizedDeltaPosError;
            totalDeltaHeadingError = -totalDeltaHeadingError;
        }
        Quaternion totalDeltaOriError(totalDeltaHeadingError, Vector3(0, 0, 1));
        //apply
        Quaternion tmpOrientation;
        int firstOffsetPosture(0);
        int lastOffsetPosture(postureCount);
        if (parameters._applyPosOriDeltaOnBlendingFramesOnly && blendPosturesCount > 0)
        {
            firstOffsetPosture = firstBlendPosture;
            lastOffsetPosture = lastBlendPosture;
        }
        applyTransformOffset(originalMotion, *editedAnimation, totalNormalizedDeltaPosError, totalDeltaOriError, parameters._correctFromBeginOrEnd, firstOffsetPosture, lastOffsetPosture);

        //2nd loop to fix the looping part (attitude/bank)
        if (parameters._fixLoopingOffset)
        {
            size_t lastPostureIndex(editedAnimation->_animationPostures.size() - 1);
            //loop 0, posture 0
            Quaternion loop0Posture0WOri = editedAnimation->_animationPostures[0].getWorldOrientations()[0];
            //loop 0, posture X (last posture)
            Quaternion loop0PostureXWOri = editedAnimation->_animationPostures[lastPostureIndex].getWorldOrientations()[0];
            //loop 1, posture 0
            Vector3 loopOriginNormalizedPosition(0, 0, 0);
            Quaternion loopOriginOrientation(0, 0, 0, 1);
            AnimationClass::computeNextLoopOrigin(loopOriginNormalizedPosition, loopOriginOrientation, *editedAnimation);
            Quaternion loop1Posture0WOri = loopOriginOrientation * loop0Posture0WOri;
            //diff
            Quaternion loopWOriDiff = loop0PostureXWOri.computeInverse() * loop1Posture0WOri;
            if (parameters._correctFromBeginOrEnd == 1)
                loopWOriDiff.inverseSelf();
            applyTransformOffset(*editedAnimation, *editedAnimation, Vector3(0, 0, 0), loopWOriDiff, parameters._correctFromBeginOrEnd, firstOffsetPosture, lastOffsetPosture);
        }

        // moving direction
        for (int iPosture = 0; iPosture < postureCount; ++iPosture)
        {
            editedAnimation->_postureMovingDirections[iPosture] = editedAnimation->_animationPostures[iPosture].getWorldOrientations()[0].computeInverse() * parameters._movingDirection;
        }

        //apply blending
        if (blendPosturesCount > 0)
        {
            int boneCount = originalMotion._skeletonDescription.getBones().sizeInt();

            switch (parameters._correctFromBeginOrEnd)
            {
            default:
            case 0:
            {
                //compute the global error to correct from the beginning to the end
                BlendTreePosture lastPosture;
                lastPosture.fromSpecificPosture(originalMotion._animationPostures[postureCount - 1], 1.f, originalMotion.getSkeletonDescription());

                BlendTreePosture differencePosture;
                differencePosture.fromSpecificPosture(originalMotion._animationPostures[0], 1.f, originalMotion.getSkeletonDescription());

                differencePosture.computeSelfDifferenceWith(lastPosture, boneCount, 0);

                BlendTreePosture currentPostureAsBlendTreePosture;
                //add a few of the error to correct at each frame
                for (int iPosture = firstBlendPosture; iPosture < lastBlendPosture; ++iPosture)
                {
                    currentPostureAsBlendTreePosture.fromSpecificPosture(editedAnimation->_animationPostures[iPosture], 1.f, editedAnimation->getSkeletonDescription());

                    float blendingCoeficient((float)(iPosture - firstBlendPosture + 1) / (float)(lastBlendPosture - firstBlendPosture + 1));
                    currentPostureAsBlendTreePosture.boneAdd(differencePosture, 0., blendingCoeficient, 0, boneCount);
                    currentPostureAsBlendTreePosture.computeWorldFromLocals(0, boneCount);

                    currentPostureAsBlendTreePosture.toSpecificPosture(editedAnimation->_animationPostures[iPosture]);
                }
                //add the full error to all postures after thelast frame of correction
                for (int iPosture = lastBlendPosture; iPosture < postureCount; ++iPosture)
                {
                    currentPostureAsBlendTreePosture.fromSpecificPosture(editedAnimation->_animationPostures[iPosture], 1.f, editedAnimation->getSkeletonDescription());

                    currentPostureAsBlendTreePosture.boneAdd(differencePosture, 0., 1.f, 0, boneCount);
                    currentPostureAsBlendTreePosture.computeWorldFromLocals(0, boneCount);

                    currentPostureAsBlendTreePosture.toSpecificPosture(editedAnimation->_animationPostures[iPosture]);

                }
            }
            break;
            case 1:
            {
                //compute the global error to correct from the beginning to the end
                BlendTreePosture firstPosture;
                firstPosture.fromSpecificPosture(originalMotion._animationPostures[0], 1.f, originalMotion.getSkeletonDescription());

                BlendTreePosture differencePosture;
                differencePosture.fromSpecificPosture(originalMotion._animationPostures[postureCount - 1], 1.f, originalMotion.getSkeletonDescription());
                differencePosture.computeSelfDifferenceWith(firstPosture, boneCount, 0);

                BlendTreePosture currentPostureAsBlendTreePosture;
                //add the full error to all postures before the first frame of correction
                for (int iPosture = 0; iPosture < firstBlendPosture; ++iPosture)
                {
                    currentPostureAsBlendTreePosture.fromSpecificPosture(editedAnimation->_animationPostures[iPosture], 1.f, editedAnimation->getSkeletonDescription());

                    currentPostureAsBlendTreePosture.boneAdd(differencePosture, 0., 1.f, 0, boneCount);
                    currentPostureAsBlendTreePosture.computeWorldFromLocals(0, boneCount);

                    currentPostureAsBlendTreePosture.toSpecificPosture(editedAnimation->_animationPostures[iPosture]);
                }
                //add a few of the error to correct at each frame
                for (int iPosture = firstBlendPosture; iPosture < lastBlendPosture; ++iPosture)
                {
                    currentPostureAsBlendTreePosture.fromSpecificPosture(editedAnimation->_animationPostures[iPosture], 1.f, editedAnimation->getSkeletonDescription());

                    float blendingCoeficient(1.f - (float)(iPosture - firstBlendPosture + 1) / (float)(lastBlendPosture - firstBlendPosture + 1));
                    currentPostureAsBlendTreePosture.boneAdd(differencePosture, 0., blendingCoeficient, 0, boneCount);
                    currentPostureAsBlendTreePosture.computeWorldFromLocals(0, boneCount);

                    currentPostureAsBlendTreePosture.toSpecificPosture(editedAnimation->_animationPostures[iPosture]);
                }
            }
            }
        }

        //before returning, we need to set the mapping and converter from the originalMotion to NULL, because they are shared with the real animation and we do'nt want to delete them
        originalMotion._genericConverter = NULL;
        originalMotion._skeletonMapping = NULL;
    }
}
