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

#pragma once

#include "glmSDKAPI.h"
#include "glmMotionDefinitions.h"
#include "glmMotionEntityComponent.h"
#include "glmAbstractMotionBehavior.h"
#include "glmDelaunayLocomotionInterface.h"
#include "glmAnimationPlayBN.h"
#include "glmSynchronizedBlendBN.h"
#include "glmMotionContext.h"
#include <glmPathDefinitions.h>
#include <glmGolaemMotionDeclarations.h> // GolaemMotion declarations

namespace glm
{
    class Entity;
    class AbstractBlendNode;
    class SimpleBlendBN;
    class SynchronizedBlendBN;
    class AnimationReplayMapping;
    class GenericSpecificConverter;

    /**
	 * \brief Behavior providing locomotion to a character
	 *
	 * The locomotion behavior requires as much locomotion animations as possible (walk forward/backward, run, sterring left/right ...) to generate
	 * a resuting animation according to the entity position/orientation and velocity. It can be controlled according to diffferent modes:
	 * - FOLLOW_MODE: the locomotion behavior tries to follow the entity position/orientation (with a resulting error on position/orientation);
	 * - DIRECT_MODE: the locomotion behavior directly applies the required velocity (without modifying the resulting position/orientation).
	 *
	 * A status is used to properly control the follow mode:
	 * - STANDING: entity velocity is null, blending configuration results in a standing animation;
	 * - STARTING: entity velocity changed from null to anything, a starting animation is played (the one that has a delta orientaiton fitting the best
	 * the delta orientation to the moving direction), and then the status change to MOVING
	 * - MOVING: entity velocity is not null and changing, blending configuration is regularily updated to result in a moving animation that matches
	 * the entity velocity;
	 * - STOPPING: entity velocity is null, last blending animation is kept applied in order to fill the gap between entity position and character
	 * skeleton position.
	 *
	 * Indeed, when using the follow mode, the position/orientation of the character skeleton is slightly different from the position/orientation of
	 * the entity. To adapt to the current entity velocity and to correct this gap, a new command evaluation is triggered according to specific
	 * parameters:
	 * - a minimum time that must elapsed since last command evaluation;
	 * - a maximum time that must elapsed since last command evaluation;
	 * - a maximum gap allowed between entity position and blend tree applied position;
	 * - a maximum gap allowed between current entity velocity and previous entity velocity used for last command evaluation.
	 *
	 * The reactivity parameter provides control on how quickly the position/orientation gap between character skeleton and entity. The higher its
	 * value is, the faster the position/orientation gap is corrected. Notice that its value is bounded to interval [0..1].
	 */
    class GLM_SDK_API LocomotionBehavior : public AbstractMotionBehavior, public DelaunayLocomotionInterface
    {
    public:
        static const int BLENDNODE_LOCOSYNCHRO_SUBNODE = 0;
        static const int BLENDNODE_STARTING_SUBNODE = 1;

        struct ChannelMask
        {
            int _channelIndex;
            bool _limbMask;
            bool _globalEffectorMask;
            glm::Array<bool> _effectorsMask;

            inline ChannelMask(int channelIndex, bool limbMask, bool effectorMask);
            inline ChannelMask(int channelIndex, bool limbMask, const glm::Array<bool>& effectorsMask);
        };

        class LocomotionBehaviorData
        {
            friend class LocomotionBehavior;

        public:
            //************************************************************
            //! @name Follow Mode Specific Data
            //************************************************************
            //@{
            int _followModeStatus;
            float _followModeTriggerTimeElapsed;
            float _followModePreviousPositionError;
            glm::Vector2 _followModePreviousEntityVelocity;
            float _followModeCurrentLinearVelocity;
            float _followModeCurrentAngularVelocity;
            float _followModeDesiredLinearVelocity;
            float _followModeDesiredAngularVelocity;
            glm::Vector2 _desiredBlendTreeVelocity;
            //@}

            //************************************************************
            //! @name Runtime Data
            //************************************************************
            //@{
            float _desiredStartAnimationWeight;
            int _startingAnimationIndex;
            Time _startMovingTime;

            Angle _entityAngularVelocityMeanValue;

            float* _activeBones;
            float* _activeBlindData;
            //@}

            //************************************************************
            //! @name Runtime Data for the collision check
            //************************************************************
            //@{
            //Vector3 _previousEntityPosition;
            //Vector3 _previousNavigationPosition;
            //NavigationMeshCellId _previousCellId;
            //@}

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

            inline LocomotionBehaviorData();
            inline ~LocomotionBehaviorData();
        };

        /*
		 * \brief Enumerated type defining the control mode to use
		 *
		 * The locomotion behavior can be controlled according to diffferent modes:
		 * - FOLLOW_MODE: the locomotion behavior tries to follow the entity position/orientation (with a resulting error on position/orientation)
		 * - DIRECT_MODE: the locomotion behavior directly applies the required velocity (without modifying the resulting position/orientation)
		 */
        struct ControlMode
        {
        public:
            enum Value
            {
                FOLLOW_MODE = 0,
                DIRECT_MODE = 1,
                DIRECT_MODE_CONTROLLED_BY_MOTION = 2,
                END
            };
        };

        /*
		 * \brief Enumerated type defining the current status when using follow control mode
		 *
		 * When using follow trajectory mode, the locomotion behavior is in one those status:
		 * - STANDING: character and entity is not moving (null velocity);
		 * - MOVING: charcter is moving and trying to apply entity position/orienation and velocity
		 * - STOPPING: entity has stopped (null velocity) but character is still moving to reduce the gap between skeelton position/orientation and entity position/orientation
		 */
        struct FollowModeStatus
        {
        public:
            enum Value
            {
                STANDING = 0,
                STARTING,
                MOVING,
                STOPPING,
                END
            };
        };

        /*
		 * \brief Structure containing parameters to trigger locomotion command re-evaluation (when using follow trajectory mode)
		 *
		 * The following parameters are used to configure when locomotion command re-evaluation must be triggered:
		 * - _minDeltaTime: minimum time that must elapsed since last command evaluation;
		 * - _maxDeltaTime: maximum time that must elapsed since last command evaluation;
		 * - _maxDeltaPosition: maximum gap allowed between entity position and blend tree applied position;
		 * - _maxDeltaVelocity: maximum gap allowed between current entity velocity and previous entity velocity used for last command evaluation.
		 */
        struct FollowModeCommandTriggerParameters
        {
            float _minDeltaTime;
            float _maxDeltaTime;
            float _maxDeltaPosition;
            float _maxDeltaVelocity;

            FollowModeCommandTriggerParameters();
        };

        class LocoEntityInitBTCB : public IEntityInitBlendTreeCB
        {
        private:
            LocomotionBehavior* _behavior;

        public:
            inline LocoEntityInitBTCB(LocomotionBehavior* behavior);

            inline virtual void doCall(int64_t index);
        };

    public:
        //************************************************************
        //! @name Parameter public data
        //************************************************************
        //@{
        float _desiredLinearVelocityOnBehaviorStart = 0;
        float _desiredAngularVelocityOnBehaviorStart = 0;

        float _commandMaxLinearAcceleration;
        float _commandMaxAngularAccelerationForBlending;
        float _maxAngularVelocityForDirectCorrection;

        float _startingAnimationReplayRatio;
        float _startingAnimationStartDuration;
        float _startingAnimationStopDuration;

        bool _createNeutralAnimationIfNeeded;
        float _delaunayDataBaseEpsilon;

        float _movingNormalizedVelocityThreshold; //!< (normalized) velocity under which the character is considered not moving anymore (and the stand animation will be played)

        unsigned int _nbFramesToMeanAngularVelocity;

        bool _checkCollisionWithEnv;
        SynchronizedBlendBN::SynchronizedMode::Value _syncMode;
        MotionEntityComponent::EntityAndSkeletonTransformBaseOri::Value _entityAndSkeletonTransformBaseOri;
        //@}

        AnimationPlayBN::IKPlaneOrientationMode _ikPlaneOrientationMode;
        float _timeWarpingToPelvisWeight;


    protected:
        Array<bool> _mask;

    private:
        //************************************************************
        //! @name Generic Data
        //************************************************************
        //@{
        glm::Array<DelaunayLocomotionInterface::LocomotionDataPerAnimation> _animations; // -> size = former _animationsCount, capacity = former maxCount
        SimpleBlendBN* _simpleBlendNode;
        SynchronizedBlendBN* _syncBlendNode;
        AnimationPlayBN* _startAnimationBlendNode;
        glm::AbstractBlendNode* _rootBlendNode;

        AnimationReplayMapping _identityReplayMapping;              // to be used by the neutral animation
        glm::GlmMap<int, glm::PODArray<int>> _animationsInChannels; // to be used with animation channels (animation with the same channel will be added to the same AnimationPlayBN)
        //@}

        float _minLinearVelocityInAnimations;
        float _maxLinearVelocityInAnimations;
        float _minAngularVelocityInAnimations;
        float _maxAngularVelocityInAnimations;

        /// \name Generic Data
        //@{
        int _controlMode;
        //@}

        /// \name Follow Mode Specific Data
        //@{
        float _followModeReactivity;
        //float _followModeMaximumAngularSpeed;
        float _followModeStopDistanceThreshold;
        FollowModeCommandTriggerParameters _followModeTriggerParameters;
        //glm::Vector2 _followModeCurrentBlendTreeVelocity;
        //@}

        bool _enableTimeWarping;

        ICallBack::SP _startBehaviorCallBack;
        ICallBack::SP _stopBehaviorCallBack;

        glm::Array<LocomotionBehaviorData> _locomotionBehaviorData;

        LocoEntityInitBTCB _entityBTInitCallback;

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

#define GLMLOCOMOTIONBEHAVIOR_DEFAULT_ANIMATIONS_SIZE 4

        LocomotionBehavior(int controlMode = ControlMode::END, const size_t animationsMaxCount = GLMLOCOMOTIONBEHAVIOR_DEFAULT_ANIMATIONS_SIZE);
        virtual ~LocomotionBehavior();

        //************************************************************
        //! @name Animations
        //************************************************************
        //@{
        inline int getAnimationsCount() const;
        const DelaunayLocomotionInterface::LocomotionDataPerAnimation& getAnimationData(int animationIndex) const;

        int getStartingAnimationIndex(int64_t indexInEntityType) const;

        //************************************************************
        //! @name Configuration
        //************************************************************
        //@{
        /*
		 * \warning Behavior must not be initilalized
		 */
        void setControlMode(ControlMode::Value controlMode);
        /**
		 * \brief configure the play mode for the whole skeleton
		 */
        void setAnimationReplayMapping(int animationIndex, const AnimationReplayMapping& replayMapping);
		AnimationReplayMapping& getAnimationReplayMapping(const unsigned int animationIndex);

        inline const Array<bool>& getMask() const { return _mask; }
        inline void setMask(const Array<bool>& mask) { _mask = mask; }
        //@}

        const AnimationPlayBN* getAnimationNode(int animationIndex) const;
        const char* getAnimationPath(int animationIndex) const;
        bool isAnimationInModel(int animationIndex);
        int getAnimationIndexInChannel(int64_t entityIndex, int animationIndexWithChannel);
        /*
		 * \return Index of animation within behavior (-1 if animation can not be added because the blend tree is already initialized, -2 if animation can not be added because it has the same velocities than an animation already present)
		 */
        int addAnimation(AnimationCache& animationCache, const char* animationPath, const bool allowBoneToBoneIdentityMappingIfAvailable = true, bool mirror = false, float replaySpeedRatio = 1.f, bool interpolateBetweenFrames = true, bool forceConstantGroundHeightInAnimation = false, int animationChannel = -1);
        void clearAnimations();
        bool isMirrorAvailable(int animationIndex, const GenericSpecificConverter* converter, glm::GlmString* returnWarningMsg) const;
        //@}

        //************************************************************
        //! @name Parameters
        //************************************************************
        //@{
        /*
		 * \warning Reactivity value must be positive, and ideally bound inside interval [ 0.0f, 1.0f ]
		 */
        void setFollowModeReactivity(float followModeReactivity);
        float getFollowModeReactivity() const;
        void setFollowModeStopDistanceThreshold(float followModeStopDistanceThreshold);
        float getFollowModeStopDistanceThreshold() const;
        const FollowModeCommandTriggerParameters& getFollowModeCommandTriggerParameters() const;
        FollowModeCommandTriggerParameters& touchFollowModeCommandTriggerParameters();
        //@}

        //************************************************************
        //! @name Current state information
        //************************************************************
        //@{
        inline float getCurrentLinearVelocity(int64_t index) const;
        //Vector2 getCurrentLinearVelocityVector(int64_t index) const { return _locomotionBehaviorData[index]._desiredBlendTreeVelocity; }
        inline float getCurrentAngularVelocity(int64_t index) const;

        inline float getMinLinearVelocity() const;
        inline float getMaxLinearVelocity() const;
        inline float getMinAngularVelocity() const;
        inline float getMaxAngularVelocity() const;
        //@}

        inline void enableTimeWarping(bool enable);
        inline bool isTimeWarpingEnabled() const;

        inline SynchronizedBlendBN* touchSynchroBlendNode() const;
        inline SynchronizedBlendBN* getSynchroBlendNode() const;
        inline const SimpleBlendBN* getSimpleBlendNode() const;
        inline const BlendBNTrajectorySampler* getTrajectorySampler() const;

        // AbstractMotionBehavior implementation
        virtual const AbstractBlendNode* getSubBlendTreeRootNode() const;
        virtual void connectToBlendTree(BlendTree& blendTree, AbstractBlendNode* connectionNode, BehaviorGraph& graph);
        virtual void disconnectFromBlendTree(BlendTree& blendTree);
        inline virtual const float* getActiveBones(int64_t entityIndex) const;
        inline virtual const float* getActiveBlindData(int64_t entityIndex) const;

        // to solve RNG issue not available yet when initing blendtree
        void initEntityBlendTreeCB(int64_t index);

        // type checking - value derived from this type
        virtual bool isThisType(const AbstractBehavior* value) const override;

    protected:
        // cloning
        friend class CloneFactory;
        inline virtual LocomotionBehavior* create() const override;

        // AbstractMotionBehavior implementation
        virtual void registerToMotionBehaviorsHandler() override;

        // AbstractBehavior implementation
        virtual void doAllocate(BehaviorGraph& graph) override;
        virtual void doDeallocate() override;
        virtual void doResize(BehaviorGraph& graph) override;
        virtual void doInit(BehaviorGraph& graph) override;
        virtual void doFinish(BehaviorGraph& graph) override;
        virtual void doInit(int64_t index, BehaviorGraph& graph) override;
        virtual void doFinish(int64_t index, BehaviorGraph& graph) override;
        virtual void doStart(int64_t index, BehaviorGraph& graph) override;
        virtual void doUpdate(int64_t index, BehaviorGraph& graph, Time dt) override;
        virtual AbstractBehavior::ResultStatus::Value doStop(int64_t index, BehaviorGraph& graph, Time dt) override;

        void incrementAnimationsMaxCount(const size_t animationCountMaxIncrement);

    private:
        void deallocateAnimationBlendNodes();

        void onBehaviorStart(int64_t index);
        void onBehaviorStop(int64_t index);

    private:
        bool hasNullAnimation(AnimationCache& animationCache) const;
        void addNullAnimationToCache(AnimationCache& animationCache, const GlmString& nullAnimationName, const GenericSkeleton& animationSkeleton, const Time animationDeltaTime, const int animationPostureCount);
        void addNullAnimation(const GenericSkeleton& blendTreeSkeleton, const GlmString& nullAnimationName);
        
        void createAndInitBlendNodes(AnimationCache& animationCache, BlendTreePostureCache& blendTreePostureCacheForAnimInit, BehaviorGraph& graph, BlendTree& blendTree, AbstractBlendNode* connectionNode);

        void setStartingWeight(int64_t index, float weight);
    };

    //-------------------------------------------------------------------------
    inline LocomotionBehavior::ChannelMask::ChannelMask(int channelIndex, bool limbMask, bool effectorMask)
        : _channelIndex(channelIndex)
        , _limbMask(limbMask)
        , _globalEffectorMask(effectorMask)
    {
    }

    //-------------------------------------------------------------------------
    inline LocomotionBehavior::ChannelMask::ChannelMask(int channelIndex, bool limbMask, const glm::Array<bool>& effectorsMask)
        : _channelIndex(channelIndex)
        , _limbMask(limbMask)
        , _globalEffectorMask(false)
        , _effectorsMask(effectorsMask)
    {
    }

    //-------------------------------------------------------------------------
    inline LocomotionBehavior::FollowModeCommandTriggerParameters::FollowModeCommandTriggerParameters()
        : _minDeltaTime(0.2f)
        , _maxDeltaTime(2.0f)
        , _maxDeltaPosition(0.3f)
        , _maxDeltaVelocity(0.1f)
    {
    }

    //-------------------------------------------------------------------------
    inline LocomotionBehavior::LocoEntityInitBTCB::LocoEntityInitBTCB(LocomotionBehavior* behavior)
        : _behavior(behavior)
    {
    }

    //-------------------------------------------------------------------------
    inline void LocomotionBehavior::LocoEntityInitBTCB::doCall(int64_t index)
    {
        _behavior->initEntityBlendTreeCB(index);
    }

    //-------------------------------------------------------------------------
    inline int LocomotionBehavior::getAnimationsCount() const
    {
        return _animations.sizeInt();
    }

    //-------------------------------------------------------------------------
    inline const char* LocomotionBehavior::getAnimationPath(int animationIndex) const
    {
        return _animations[animationIndex]._animationPath.c_str();
    }

    ////-------------------------------------------------------------------------
    inline const AnimationPlayBN* LocomotionBehavior::getAnimationNode(int animationIndex) const
    {
        return _animations[animationIndex]._animationBlendNode;
    }

    //-------------------------------------------------------------------------
    inline void LocomotionBehavior::setControlMode(ControlMode::Value controlMode)
    {
        GLM_DEBUG_ASSERT(isInitialized() == false);

        _controlMode = controlMode;
    }

    //-------------------------------------------------------------------------
    inline void LocomotionBehavior::setFollowModeReactivity(float followModeReactivity)
    {
        _followModeReactivity = max(0.0f, followModeReactivity);
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getFollowModeReactivity() const
    {
        return _followModeReactivity;
    }

    //-------------------------------------------------------------------------
    inline void LocomotionBehavior::setFollowModeStopDistanceThreshold(float followModeStopDistanceThreshold)
    {
        _followModeStopDistanceThreshold = max(0.0f, followModeStopDistanceThreshold);
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getFollowModeStopDistanceThreshold() const
    {
        return _followModeStopDistanceThreshold;
    }

    //-------------------------------------------------------------------------
    inline const LocomotionBehavior::FollowModeCommandTriggerParameters& LocomotionBehavior::getFollowModeCommandTriggerParameters() const
    {
        return _followModeTriggerParameters;
    }

    //-------------------------------------------------------------------------
    inline LocomotionBehavior::FollowModeCommandTriggerParameters& LocomotionBehavior::touchFollowModeCommandTriggerParameters()
    {
        return _followModeTriggerParameters;
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getCurrentLinearVelocity(int64_t index) const
    {
        return _locomotionBehaviorData[index]._followModeCurrentLinearVelocity;
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getCurrentAngularVelocity(int64_t index) const
    {
        return _locomotionBehaviorData[index]._followModeCurrentAngularVelocity;
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getMinLinearVelocity() const
    {
        return _minLinearVelocityInAnimations;
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getMaxLinearVelocity() const
    {
        return _maxLinearVelocityInAnimations;
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getMinAngularVelocity() const
    {
        return _minAngularVelocityInAnimations;
    }

    //-------------------------------------------------------------------------
    inline float LocomotionBehavior::getMaxAngularVelocity() const
    {
        return _maxAngularVelocityInAnimations;
    }

    //-------------------------------------------------------------------------
    inline void LocomotionBehavior::enableTimeWarping(bool enable)
    {
        _enableTimeWarping = enable;
    }

    //-------------------------------------------------------------------------
    inline bool LocomotionBehavior::isTimeWarpingEnabled() const
    {
        return _enableTimeWarping;
    }

    //-------------------------------------------------------------------------
    inline SynchronizedBlendBN* LocomotionBehavior::touchSynchroBlendNode() const
    {
        return _syncBlendNode;
    }

    //-------------------------------------------------------------------------
    inline SynchronizedBlendBN* LocomotionBehavior::getSynchroBlendNode() const
    {
        return _syncBlendNode;
    }

    //-------------------------------------------------------------------------
    inline const SimpleBlendBN* LocomotionBehavior::getSimpleBlendNode() const
    {
        return _simpleBlendNode;
    }

    //-------------------------------------------------------------------------
    inline const BlendBNTrajectorySampler* LocomotionBehavior::getTrajectorySampler() const
    {
        return NULL;
    }

    //-------------------------------------------------------------------------
    inline const float* LocomotionBehavior::getActiveBones(int64_t entityIndex) const
    {
        return _locomotionBehaviorData[entityIndex]._activeBones;
    }

    //-------------------------------------------------------------------------
    inline const float* LocomotionBehavior::getActiveBlindData(int64_t entityIndex) const
    {
        return _locomotionBehaviorData[entityIndex]._activeBlindData;
    }

    //-------------------------------------------------------------------------
    inline LocomotionBehavior* LocomotionBehavior::create() const
    {
        LocomotionBehavior* myNewLocomotionBehavior = GLM_NEW(LocomotionBehavior, myNewLocomotionBehavior, getClassMemoryStatId());
        return myNewLocomotionBehavior;
    }

} // namespace glm
