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

#pragma once

#include "glmSDKAPI.h"

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

namespace glm
{
    class NavigationPipeline;
    class AbstractReachTargetNavigator;
    class FaceNavigator;
    class PerceptionContext;
    class PerceptionEntityTypeData;

    // Navigation behavior, provides a navigation pipeline to an entity.
    class GLM_SDK_API NavigationBehavior : public AbstractUserBehavior
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

        class GLM_SDK_API NavigationBehaviorData
        {
        public:
            // Navigation Behavior Data, forwarded to NavData when behavior starts (for data not used only in NavigationBehavior)
            float _comfortSpeed;    /// Comfort speed
            float _maxAcceleration; /// Maximum accelelration
            float _boldness;
            float _maxAngularSpeed;
            float _groupLaxness;
            Time _timeSinceLastUpdate;

            // pipeline runtime data
            VelocitySampling* _velocitySampling;
            //Quaternion			_lookMeanOrientation;
            Vector3 _lookMeanOrientationVector;
            Quaternion _upToZRotationCached;
            Quaternion _zToUpRotationCached;

            Vector2 _velocity2D;
            Vector2 _desiredVelocity;
            bool _hasDesiredVelocity;

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

            NavigationBehaviorData();
            ~NavigationBehaviorData();
        };

        // Runtime abstraction layer, used to store entity or group data, to forward to update without testing entity/group each time
        // these data is only valid for one frame, don't need to keep it between frames, compared to NavigationData
        struct EntityOrGroupData
        {
            Quaternion _bodyOrientation;
            Quaternion _moveOrientation;
            RandomNumberGenerator* _behaviorRNG;
            Entity* _entity = NULL;
            Group* _group = NULL;
            KnowledgeBase* _kb = NULL;
            float _personalSpace = 0.1f;
            SimuEntityHandle _handle; // can switch if needed on isGroup

            // those arguments are former PathComponentData
            Vector2 _orientationCosSinCached;

            int _groundPinMode = 0;
        };

        class AvoidanceType
        {
        public:
            enum Value
            {
                EXTRAPOL = 0,
                SOCIAL_FORCE,
                RVO,
                END
            };
        };

        AvoidanceType::Value _avoidanceType;

        // config
        bool _collisionDetectionActivated;

        // former configuration of pipeline & navigators :
        Time _updateInterval; // NavigationPipeline

        bool _automaticOrientation;
        unsigned int _nbLowPassFilterSamples; // lookWhereYourGoingNavigator
        float _speedThreshold;                // lookWhereYourGoingNavigator

        float _extrapolMinimumExtrapolationTime;
        float _extrapolMaximumExtrapolationTime;
        bool _extrapolConsideringReciprocalAvoidance;
        bool _extrapolExtrapolatingObstacles;
        float _extrapolDistanceToObstaclesPenalty;
        float _extrapolMaximumDistanceToEntitiesPenalty;

        float _rvoTimeToCollisionPenalty;

        VelocitySamplingConfiguration _velocitySamplingConfiguration; // shared for rvo and extrapol

        float _socialForceRepulsionForceLinearFactor;
        float _socialForceRepulsionForceExponentialFactorInverse;
        float _socialForceBodyForceLinearFactor;
        float _socialForceFrictionForceLinearFactor;
        Angle _socialForceAwarenessField;
        float _socialForceRearForcesLinearFactor;

        glm::PODArray<int> _sensorIds;

    private:
        PathContext* _pathContext;

        glm::Array<NavigationBehaviorData> _navigationBehaviorData;

        ICallBack::SP _updateCallback;

        static const char TARGET_VALID = 0x01;
        static const char TARGET_JUST_CLEARED = 0x02;

        // Perception parameters to apply when the behavior starts
        PerceptionContext* _perceptionContext;
        PerceptionEntityTypeData* _perceptionEntityTypeData;

    public:
        NavigationBehavior();
        virtual ~NavigationBehavior();

        //void overrideLocomotionData(NavigationData& pathComponent, int64_t index);

        void setComfortSpeed(float value, int64_t index);
        void setMaxAcceleration(float value, int64_t index);

        void setBoldness(float value, int64_t index);

        void setMaxAngularSpeed(float value, int64_t index);
        void setGroupLaxness(float value, int index);

        void setUpdateCallback(ICallBack* callback);

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

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

        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;

    private:
        bool updateGroupMembers(NavigationData& navigationData, EntityOrGroupData& entityData, NavigationBehaviorData& navigationBehaviorData, float time);
        bool updateExtrapol(NavigationData& navigationData, EntityOrGroupData& entityData, NavigationBehaviorData& behaviorData, float time);
        bool updateSocialForce(NavigationData& navigationData, EntityOrGroupData& entityData, NavigationBehaviorData& behaviorData, float time);
        bool updateRVO(NavigationData& navigationData, EntityOrGroupData& entityData, NavigationBehaviorData& behaviorData, float time);

        void displayDebugPerceived(NavigationData& thisNavigationData, NavigationBehaviorData& navBeData, EntityOrGroupData& entityData, glm::PODArray<NavPerceivedObject>& perceivedEntities, glm::PODArray<NavPerceivedObject>& perceivedObstacles, int spatialEnvIndex);

        //bool createNavigationPipeline(const NavigationPipelineConfiguration& config);
        void clearNavigationData(int64_t index, BehaviorGraph& graph);
    };

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

    //-------------------------------------------------------------------------
    inline void NavigationBehavior::setMaxAcceleration(float value, int64_t index)
    {
        _navigationBehaviorData[index]._maxAcceleration = value;
    }

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

    //-------------------------------------------------------------------------
    inline void NavigationBehavior::setMaxAngularSpeed(float value, int64_t index)
    {
        _navigationBehaviorData[index]._maxAngularSpeed = value;
    }

    //-------------------------------------------------------------------------
    inline void NavigationBehavior::setGroupLaxness(float value, int index)
    {
        _navigationBehaviorData[index]._groupLaxness = value;
    }

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