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

#pragma once

#include "glmSDKAPI.h"

#include "glmAbstractMotionBehavior.h"
#include "glmDelaunayLocomotionInterface.h"
#include <glmPerceptionContext.h>
#include <glmMotionEntityComponent.h>
#include "glmLocomotionBehavior.h"
#include <glmBlendBNTrajectorySampler.h>

//golaem core includes
#include "glmCachePerThread.h"
#include <glmAbstractUserBehavior.h>
#include <glmNavigationData.h>

namespace glm
{
    class PerceptionContext;

    class GLM_SDK_API NavAndLocoBehavior : public AbstractMotionBehavior, public DelaunayLocomotionInterface
    {
    public:
        //structures
        struct TargetProvider
        {
            enum Value
            {
                GOTO_BEHAVIOR,
                GOLAEM_KB,
                END
            };
        };

        struct CollisionStatus
        {
            enum Value
            {
                OK,
                DISMISSED_OBSTACLE_COLLISION,
                DISMISSED_ENTITY_COLLISION,
                END
            };
        };

        struct TrajectoryStatus
        {
            CollisionStatus::Value _collisionStatus = NavAndLocoBehavior::CollisionStatus::OK;
            float _distanceToObstacles = FLT_MAX;
            float _distanceToEntities = FLT_MAX;
        };

        class GLM_SDK_API NavAndLocoBehaviorData
        {
        public:
            //************************************************************
            //! @name Navigation Data
            //************************************************************
            //@{
            float _comfortSpeed; /// Comfort speed
            float _boldness;
            Time _timeSinceLastUpdate;
            //@}

            //************************************************************
            //! @name Locomotion data
            //************************************************************
            //@{
            float _currentTrajectoryLinearVelocity;
            float _currentTrajectoryAngularVelocity;
            float _desiredTrajectoryLinearVelocity;
            float _desiredTrajectoryAngularVelocity;
            //@}

            //************************************************************
            //! @name AbstractMotionBehavior data
            //************************************************************
            //@{
            float* _activeBones;
            float* _activeBlindData;
            //@}
        public:
            GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

            NavAndLocoBehaviorData();
            ~NavAndLocoBehaviorData();
        };

        class NavAndLocoEntityInitBTCB : public IEntityInitBlendTreeCB
        {
        private:
            NavAndLocoBehavior* _behavior;

        public:
            inline NavAndLocoEntityInitBTCB(NavAndLocoBehavior* behavior);

            inline virtual void doCall(int64_t index);
        };

    public: //data
        //************************************************************
        //! @name Navigation Configuration Data
        //************************************************************
        //@{
        // Perception parameters to apply when the behavior starts
        glm::PODArray<int> _sensorIds;
        PerceptionContext* _perceptionContext;
        PathContext* _pathContext;

        float _minimumExtrapolationTime;
        float _maximumExtrapolationTime;
        float _distanceToTrajectoryPenaltyCoefficient;
        float _distanceToConfortSpeedPenaltyCoefficient;
        float _distanceToObstaclesPenaltyCoefficient;
        float _distanceToEntitiesPenaltyCoefficient;
        float _updateInterval;

        TargetProvider::Value _targetProvider;
        GlmString _targetKBName;
        int64_t _targetKBHashKey;

        //navigation data parameter(s)
        bool _collisionDetectionActivated;
        //@}

        //************************************************************
        //! @name Locomotion Configuration Data
        //************************************************************
        //@{
        bool _createNeutralAnimationIfNeeded;
        float _delaunayDataBaseEpsilon;
        MotionEntityComponent::EntityAndSkeletonTransformSynchronization::Value _entityAndSkeletonTransformSynchronizationMode;
        MotionEntityComponent::EntityAndSkeletonTransformBaseOri::Value _entityAndSkeletonTransformBaseOri;

        SynchronizedBlendBN::SynchronizedMode::Value _syncMode;
        AnimationPlayBN::IKPlaneOrientationMode _ikPlaneOrientationMode;
        bool _enableTimeWarping;

        float _commandMaxLinearAcceleration;
        float _commandMaxAngularAccelerationForBlending;

        // sampler config and data
        bool _matchingTrajectoryOrientation;

        float _trajMatchMode_AngularDistanceWeight; // how to weight angular distance when comparing the extrapolated trajectories: 0 to give angular and linear distances the same weight, positive values to give more weight to angular distance, negative values to give more weight to linear distance
        float _trajMatchMode_FutureSamplesWeight;   // how to weight samples when comparing the extrapolated trajectories: 0 to give same weight to future and present samples, negatives values to give more weight to near future values, positive values to give more weight to far future values

        int _trajMatchMode_useDualCommands;
        int _trajMatchMode_ExtrapolationPointsCount;

        int _trajSampler_linearSpeedBlendingSamples;
        int _trajSampler_angularSpeedBlendingSamples;
        int _trajSampler_linearSpeedBlendingSamplesOnNextCommands;
        int _trajSampler_angularSpeedBlendingSamplesOnNextCommands;

        bool _trajSampler_useHyperbolicLinearSpeedDistribution;
        bool _trajSampler_useHyperbolicAngularSpeedDistribution;
        float _trajSampler_samplingSideOffset;
        //@}

        bool _debugMode;

    private: //data
        //************************************************************
        //! @name Navigation Data
        //************************************************************
        //@{
        KnowledgeBase* _targetKB;
        glm::Array<GlmString> _KBFieldNames;
        //@}

        //************************************************************
        //! @name Locomotion Data
        //************************************************************
        //@{
        Array<bool> _mask;

        glm::Array<DelaunayLocomotionInterface::LocomotionDataPerAnimation> _animations;
        SynchronizedBlendBN* _syncBlendNode;
        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;

        BlendBNTrajectorySampler _trajectorySampler;
        //@}

        //************************************************************
        //! @name Per thread trajectories data
        //************************************************************
        //@{
        CachePerThread<Array<int>> _allTrajectoriesIndices_cache;
        CachePerThread<Array<TrajectoryStatus>> _allTrajectoriesStatus_cache;
        CachePerThread<Array<Vector3>> _allTrajectoriesPositions_cache;
        CachePerThread<Array<Quaternion>> _allTrajectoriesOrientations_cache;
        //@}

        glm::Array<NavAndLocoBehaviorData> _navAndLocoBehaviorData;

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

        ICallBack::SP _updateCallback;

        NavAndLocoEntityInitBTCB _entityBTInitCallback;

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

        NavAndLocoBehavior(const size_t animationsMaxCountconst = GLMLOCOMOTIONBEHAVIOR_DEFAULT_ANIMATIONS_SIZE);
        virtual ~NavAndLocoBehavior();

        inline const NavAndLocoBehaviorData& getNavAndLocoBehaviorData(int64_t index) const;

        //************************************************************
        //! @name Behavior blend-tree structure
        //************************************************************
        //@{
        inline SynchronizedBlendBN* touchSynchroBlendNode() const;
        inline SynchronizedBlendBN* getSynchroBlendNode() const;
        inline const SimpleBlendBN* getSimpleBlendNode() const;
        //@}

        //************************************************************
        //! @name Animations informations
        //************************************************************
        //@{
        inline int getAnimationsCount() const;
        inline int getStartingAnimationIndex(int64_t /*indexInEntityType*/) const; //necessary for the locomotion widget, but as no starting animatione exists, always return an invalid index
        inline const char* getAnimationPath(int animationIndex) const;
        inline const DelaunayLocomotionInterface::LocomotionDataPerAnimation& getAnimationData(int animationIndex) const;
        bool isAnimationInModel(int animationIndex);
        int getAnimationIndexInChannel(int64_t entityIndex, int animationIndexWithChannel);
        //@}

        //************************************************************
        //! @name Current state information
        //************************************************************
        //@{
        inline float getCurrentLinearVelocity(int64_t index) const;
        inline float getMinLinearVelocity() const;
        inline float getMaxLinearVelocity() const;

        inline float getCurrentAngularVelocity(int64_t index) const;
        inline float getMinAngularVelocity() const;
        inline float getMaxAngularVelocity() const;
        //@}

        //************************************************************
        //! @name Navigation
        //************************************************************
        //@{
        float getComfortSpeed(int64_t index) const;
        void setComfortSpeed(float value, int64_t index);

        float getBoldness(int64_t index) const;
        void setBoldness(float value, int64_t index);

        void setUpdateCallback(ICallBack* callback);
        //@}

        //************************************************************
        //! @name Locomotion
        //************************************************************
        //@{
        int addAnimation(AnimationCache& animationCache, const char* animationPath, bool mirror = false, float replaySpeedRatio = 1.f, bool interpolateBetweenFrames = true, bool forceConstantGroundHeightInAnimation = false, int animationChannel = -1);
        bool isMirrorAvailable(int animationIndex, const GenericSpecificConverter* converter, glm::GlmString* returnWarningMsg) const;
        void clearAnimations();

        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;   }

        inline const glm::Array<DelaunayLocomotionInterface::LocomotionDataPerAnimation>& getAnimations() const;
        const AnimationPlayBN* getAnimationNode(int animationIndex) const;

        //-------------------------------------------------------------------------
        inline BlendBNTrajectorySampler* getTrajectorySampler();
        inline const BlendBNTrajectorySampler* getTrajectorySampler() const;
        //@}

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

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

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

        // AbstractMotionBehavior implementation
        virtual void registerToMotionBehaviorsHandler() override;

        // AbstractUserBehavior 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 ResultStatus::Value doStop(int64_t index, BehaviorGraph& graph, Time dt) override;

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

    private: //methods
        //int updateNavigation(Time time, PathComponentData& pathEntityComponent, PathContext* pathContext, NavigationEntityData& entityData);
        int updateNavigation_v2(int64_t index, BehaviorGraph& graph, float minExtraplationDuration);

        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 initEntityBlendTreeCB(int64_t index);
        void deallocateAnimationBlendNodes();

        void computeBlendingWeights(float& outLinearSpeed, float& outAngularSpeed, int64_t entityIndex, float entityScale, float currentLinearVelocity, float currentAngularVelocity, float desiredLinearVelocity, float desiredAngularVelocity, Time dt);
    };

    //-------------------------------------------------------------------------
    inline NavAndLocoBehavior::NavAndLocoEntityInitBTCB::NavAndLocoEntityInitBTCB(NavAndLocoBehavior* behavior)
        : _behavior(behavior)
    {
    }

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

    //-------------------------------------------------------------------------
    inline const NavAndLocoBehavior::NavAndLocoBehaviorData& NavAndLocoBehavior::getNavAndLocoBehaviorData(int64_t index) const
    {
        return _navAndLocoBehaviorData[index];
    }

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

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

    //-------------------------------------------------------------------------
    inline const SimpleBlendBN* NavAndLocoBehavior::getSimpleBlendNode() const
    {
        return NULL;
    }

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

    //-------------------------------------------------------------------------
    inline int NavAndLocoBehavior::getStartingAnimationIndex(int64_t /*indexInEntityType*/) const
    {
        return -1;
    }
    //-------------------------------------------------------------------------
    inline const DelaunayLocomotionInterface::LocomotionDataPerAnimation& NavAndLocoBehavior::getAnimationData(int animationIndex) const
    {
        return _animations[animationIndex];
    }

    //-------------------------------------------------------------------------
    inline float NavAndLocoBehavior::getCurrentLinearVelocity(int64_t index) const
    {
        return _navAndLocoBehaviorData[index]._currentTrajectoryLinearVelocity;
    }

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

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

    //-------------------------------------------------------------------------
    inline float NavAndLocoBehavior::getCurrentAngularVelocity(int64_t index) const
    {
        return _navAndLocoBehaviorData[index]._currentTrajectoryAngularVelocity;
    }

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

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

    //-------------------------------------------------------------------------
    inline const glm::Array<DelaunayLocomotionInterface::LocomotionDataPerAnimation>& NavAndLocoBehavior::getAnimations() const
    {
        return _animations;
    }

    //-------------------------------------------------------------------------
    inline float NavAndLocoBehavior::getComfortSpeed(int64_t index) const
    {
        return _navAndLocoBehaviorData[index]._comfortSpeed;
    }

    //-------------------------------------------------------------------------
    inline void NavAndLocoBehavior::setComfortSpeed(float value, int64_t index)
    {
        _navAndLocoBehaviorData[index]._comfortSpeed = value;
    }

    //-------------------------------------------------------------------------
    inline float NavAndLocoBehavior::getBoldness(int64_t index) const
    {
        return _navAndLocoBehaviorData[index]._boldness;
    }

    //-------------------------------------------------------------------------
    inline void NavAndLocoBehavior::setBoldness(float value, int64_t index)
    {
        _navAndLocoBehaviorData[index]._boldness = value;
    }

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

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

    //-------------------------------------------------------------------------
    inline BlendBNTrajectorySampler* NavAndLocoBehavior::getTrajectorySampler()
    {
        return &_trajectorySampler;
    }

    //-------------------------------------------------------------------------
    inline const BlendBNTrajectorySampler* NavAndLocoBehavior::getTrajectorySampler() const
    {
        return &_trajectorySampler;
    }

    //-------------------------------------------------------------------------
    inline const AbstractBlendNode* NavAndLocoBehavior::getSubBlendTreeRootNode() const
    {
        return _syncBlendNode;
    }

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

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

    //-------------------------------------------------------------------------
    inline NavAndLocoBehavior* NavAndLocoBehavior::create() const
    {
        NavAndLocoBehavior* myNewNavAndLocoBehavior = GLM_NEW(NavAndLocoBehavior, myNewNavAndLocoBehavior, getClassMemoryStatId());
        return myNewNavAndLocoBehavior;
    }
} // namespace glm
