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

#pragma once

#include "glmIContext.h"
#include "glmSensor.h"
#include <glmPODArray.h>

#include <glmPerceptionZone.h>
#include <glmVoxelField.h>

namespace glm
{
    struct PerceptionZoneEntity;
    class PerceptionZone;
    class FastBoundingBox;
    class RandomNumberGenerator;

    struct GLM_SDK_API NavPerceivedObject
    {
        int _id;
        float _distance;
        int _closestVoxelId;
        int _closestPerceivedVoxelId;

        bool operator==(const NavPerceivedObject& other) const;
    };

    struct GLM_SDK_API SensorHashkeys
    {
        int64_t _objectIndexHashKey = -1;
        glm::GlmString _objectIndexName;
        int64_t _objectIdHashKey = -1;
        glm::GlmString _objectIdName;
        int64_t _objectDistHashKey = -1;
        glm::GlmString _objectDistName;
        int64_t _closestVoxelToObjectHashKey = -1;
        glm::GlmString _closestVoxelToObjectName;
        int64_t _closestObjectVoxelHashKey = -1;
        glm::GlmString _closestObjectVoxelName;
    };

    class PerceptionEntityTypeData;

    class GLM_SDK_API PerceptionEntityComponent
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

    public:
        PerceptionEntityTypeData* _entityTypeData = NULL;
        PerceptionZoneEntity* _perceptionEntity = NULL; // optimization data

        Vector3 _position;
        Vector3 _lastSeenPosition; // only for computing velocity
        Vector3 _computedVelocity;
        float _computedVelocityNorm = 0;
        Quaternion _bodyOrientation;
        Quaternion _bodyOrientationInverse;
        Quaternion _moveOrientation;
        Quaternion _desiredMoveOrientation;
        float _speed;
        float _desiredSpeed;
        Vector3 _worldUpVector;
        float _scale;

        Array<Vector3> _voxelCenters;
        Array<Vector3> _navVoxelCenters;
        float _voxelSize = 1.f; // depends on entity scale

        FastBoundingBox _boundingBox;

    protected:
        glm::Array<Quaternion> _sensorsOrientationCache;

    public:
        inline const glm::Quaternion& getSensorOrientation(size_t sensorIdx) const;

        friend class PerceptionContext;

    private:
        // sensors orientation is a mix between entity orientation and move orientaiton that depends on the entity speed and configuration
        void computeSensorOrientation(size_t sensorIdx, bool sensorOrientationIn2d, float sensorAlignedWithMoveOrientationSpeedThreshold, bool useDesiredMoveOriInsteadOfMoveOri);
    };

    class GLM_SDK_API PerceptionEntityTypeData
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

        glm::PODArray<Sensor*> _sensors;

        float _updatePeriod = 0.04f;

    public:
        PerceptionEntityTypeData();
        ~PerceptionEntityTypeData();
        friend class PerceptionEntityComponent;
    };

    class GLM_SDK_API PrePerceptionContext : public IContext
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

    private:
        PerceptionContext* _perceptionContext = NULL;

    public:
        // time since last update kb entry (float per entity)
        static const char* _timeSinceLastUpdateEntryName;
        static int64_t _timeSinceLastUpdateHashKey;

    public:
        PrePerceptionContext();
        virtual ~PrePerceptionContext();

    protected:
        virtual void doInit() override;

        inline virtual bool registeredOnInitEntityType() override;
        virtual void onInitEntityType(int entityTypeIndex) override;

        inline virtual bool registeredOnUpdateEntityComponent() override;
        virtual void onUpdateEntityComponent(Time dt, int entityTypeIndex, int64_t entityIndex) override;

        inline virtual bool registeredOnUpdateGroupComponent() override;
        virtual void onUpdateGroupComponent(Time dt, int groupTypeIndex, int64_t groupIndex) override;

        virtual void setContextType() override;

        inline static ContextType::Value getStaticContextType();
    };

    class GLM_SDK_API PerceptionContext : public IContext
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

        PerceptionZone* _perceptionZone = NULL;
        bool _ownPerceptionZone = false;

        SensorDetectionType::Value _defaultDetectionType = SensorDetectionType::BOTH;
        float _defaultScaleFactor = 1.f;
        float _defaultDetectionPeriod = 0.249f;
        glm::GlmString _defaultFiltersStr;

    private:
        glm::PODArray<PerceptionEntityTypeData*> _entityTypeData;      // stored as pointers to avoid data movement when resizing (POD)Array. Anyway it can only be allocated instance per instance at callback
        glm::PODArray<PerceptionEntityTypeData*> _groupEntityTypeData; // stored as pointers to avoid data movement when resizing (POD)Array. Anyway it can only be allocated instance per instance at callback
        glm::Array<SensorHashkeys> _sensorHashkeys;
        glm::SpinLock _sensorHashkeysLock;

        ComponentsArray<PerceptionEntityComponent> _entityComponents;

    public:
        PerceptionContext();
        virtual ~PerceptionContext();

        inline PerceptionEntityTypeData& getEntityTypeData(int64_t index) const;
        inline PerceptionEntityTypeData& getGroupTypeData(int64_t index) const;

        inline PerceptionEntityComponent& getEntityComponent(SimuEntityHandle entityHandle);
        inline PerceptionEntityComponent& getEntityComponent(int typeIndex, int64_t index);

        // spatialEnv index per entity
        static const char* _spatialEnvEntryName;
        static int64_t _spatialEnvEntryHashKey;

        inline static ContextType::Value getStaticContextType();

        static void getPerceivedObjectsVision(
            glm::PODArray<float>& retVision,
            const Entity& entity,
            EntityType* entityType,
            const Simulation* simulation,
            int sensorIdx,
            int angularSectorsCount,
            int distanceSectorsCounts,
            float minDistance,
            float maxDistance,
            float minAngle,
            float maxAngle,
            bool keepOnlyLowestDistance,
            const float& distanceWeight,
            const float& relativeVelocityWeight);

        static void getPerceivedObjectsVisionSphere(
            glm::PODArray<float>& retVision,
            const Entity& entity,
            EntityType* entityType,
            const Simulation* simulation,
            int sensorIdx,
            int sectorsCount,
            const Vector2& distances, // x = minDistance, y = maxDistance
            const Vector4& angles,    // x = minPhi, y = maxPhi, z = minTheta, w = maxTheta
            const float& visionRadius,
            const float& distanceWeight,
            const float& relativeVelocityWeight,
            bool projectOnPlane // for max clearance
        );

        static float fuzz(const glm::PODArray<float>& inputVision, int inputCount, float rangeBegin, float rangeEnd, float leftZeroBound, float leftOneBound, float rightOneBound, float rightZeroBound);
        static float fuzz(const glm::PODArray<float>& inputVision, int inputAngularSectorsCount, int inputDistanceSectorsCount, float angularRangeBegin, float angularRangeEnd, float angularLeftMinBound, float angularLeftMaxBound, float angularRightMaxBound, float angularRightMinBound, float distanceRangeBegin, float distanceRangeEnd, float distanceLowerBound, float distanceUpperBound);
        static float fuzzSphere(const glm::PODArray<float>& inputVision, int sectorsCount, const Vector4& angles);

        static void computeDistanceMap(
            glm::PODArray<float>& inputVisionOutputDistanceMap,
            int& clearanceX,
            int& clearanceY,
            const int defaultClearanceX,
            const int defaultClearanceY,
            const int sectorsCount,
            const glm::Array<glm::Array<std::pair<int, glm::Array<std::pair<int, float>>>>>& distanceMapNeighbors,
            bool inverse = false,
            float tolerance = 0.05f);

        void computeSensorsPerceivedObjects(
            glm::PODArray<NavPerceivedObject>& outPerceivedEntities, glm::PODArray<NavPerceivedObject>& outPerceivedObstacles,
            KnowledgeBase& kb, const glm::PODArray<int>& sensorIds, int64_t indexInEntityType, const PerceptionEntityTypeData& perceptionEntityTypeData, int& outSpatialEnvIndex);

        void addEntitiesFromSimulation(Simulation* simulation);
        bool addObstaclesFromNavmesh(int spatialEnv, NavigationMesh3* navmesh);

        const SensorHashkeys computeSensorHashkeys(size_t sensorIdx);

        void createEntityTypeDefaultSensors(EntityType* entityType);
        void createGroupTypeDefaultSensors(GroupType* groupType);

        void setSensorActive(KnowledgeBase& kb, RandomNumberGenerator& rng, int sensorIdx, const PerceptionEntityTypeData& perceptionEntityTypeData, int64_t index, int typeIndex);
        void setSensorInactive(KnowledgeBase& kb, int sensorIdx, int64_t index);

        static PODArray<int>& getEntityActiveSensors(const KnowledgeBase& kb, int64_t index);

    protected:
        virtual void doInit() override;

        virtual void doUpdate(Time dt) override;

        virtual void doFinish() override;

        /**
        * \brief called by init, derived classes must implement it to set their component type
        */
        virtual void setContextType() override;

        inline virtual bool registeredOnCreateEntityType() override;
        virtual void onCreateEntityType(int64_t maxCount) override;

        inline virtual bool registeredOnCreateGroupType() override;
        virtual void onCreateGroupType(int64_t maxCount) override;

        inline virtual bool registeredOnInitEntityType() override;
        virtual void onInitEntityType(int entityTypeIndex) override;

        inline virtual bool registeredOnInitGroupType() override;
        virtual void onInitGroupType(int groupTypeIndex) override;

        inline virtual bool registeredOnCreateEntityComponent() override;
        virtual void onCreateEntityComponent(int entityTypeIndex, int64_t entityIndex) override;

        inline virtual bool registeredOnInitEntityComponent() override;
        virtual void onInitEntityComponent(int entityTypeIndex, int64_t entityIndex) override;

        inline virtual bool registeredOnInitGroupComponent() override;
        virtual void onInitGroupComponent(int groupTypeIndex, int64_t entityIndex) override;

        inline virtual bool registeredOnUpdateEntityComponent() override;
        virtual void onUpdateEntityComponent(Time dt, int entityTypeIndex, int64_t entityIndex) override;

        inline virtual bool registeredOnUpdateGroupComponent() override;
        virtual void onUpdateGroupComponent(Time dt, int groupTypeIndex, int64_t groupIndex) override;

        inline virtual bool registeredOnDestroyEntityComponent() override;
        virtual void onDestroyEntityComponent(int entityTypeIndex, int64_t entityIndex) override;

        inline virtual bool registeredOnFinishEntityComponent() override;
        virtual void onFinishEntityComponent(int entityTypeIndex, int64_t entityIndex) override;

        void clearSensorInfo(KnowledgeBase& kb, int sensorIdx, int64_t index);

    private:
        GLM_DISALLOW_COPY_AND_ASSIGN(PerceptionContext);

        void createCylinderGeometry(glm::Array<glm::Triangle3>& geometry, const Vector3& origin, float radius, float height, int nbDivisions);

        friend class PrePerceptionContext;
    };

    //-------------------------------------------------------------------------
    inline IContext::ContextType::Value PrePerceptionContext::getStaticContextType()
    {
        return IContext::ContextType::PRE_PERCEPTION;
    }

    //-------------------------------------------------------------------------
    inline bool PrePerceptionContext::registeredOnInitEntityType()
    {
        return true;
    }

    //-------------------------------------------------------------------------
    inline bool PrePerceptionContext::registeredOnUpdateEntityComponent()
    {
        return _perceptionContext->registeredOnUpdateEntityComponent();
    }

    //-------------------------------------------------------------------------
    inline bool PrePerceptionContext::registeredOnUpdateGroupComponent()
    {
        return _perceptionContext->registeredOnUpdateGroupComponent();
    }

    //-------------------------------------------------------------------------
    inline PerceptionEntityTypeData& PerceptionContext::getEntityTypeData(int64_t index) const
    {
        return *_entityTypeData[index];
    }

    //-------------------------------------------------------------------------
    inline PerceptionEntityTypeData& PerceptionContext::getGroupTypeData(int64_t index) const
    {
        return *_groupEntityTypeData[index];
    }

    //-------------------------------------------------------------------------
    inline IContext::ContextType::Value PerceptionContext::getStaticContextType()
    {
        return IContext::ContextType::PERCEPTION;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnCreateEntityType()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnCreateGroupType()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnInitEntityType()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnInitGroupType()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnInitEntityComponent()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnInitGroupComponent()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnUpdateEntityComponent()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnUpdateGroupComponent()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnDestroyEntityComponent()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnFinishEntityComponent()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline bool PerceptionContext::registeredOnCreateEntityComponent()
    {
        return _perceptionZone != NULL;
    }

    //-------------------------------------------------------------------------
    inline PerceptionEntityComponent& PerceptionContext::getEntityComponent(SimuEntityHandle entityHandle)
    {
        GLM_DEBUG_ASSERT(entityHandle._isGroup == 0);
        return _entityComponents.getComponent(entityHandle._typeIndex, entityHandle._index);
    }

    //-------------------------------------------------------------------------
    inline PerceptionEntityComponent& PerceptionContext::getEntityComponent(int typeIndex, int64_t index)
    {
        return _entityComponents.getComponent(typeIndex, index);
    }

    //-------------------------------------------------------------------------
    inline const glm::Quaternion& PerceptionEntityComponent::getSensorOrientation(size_t sensorIdx) const
    {
        return _sensorsOrientationCache[sensorIdx];
    }
} // namespace glm
