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

#pragma once

#include "glmSDKAPI.h"
#include "glmNavigationMeshLocation.h"
#include "glmPathEntityComponentLocation.h"
#include <glmAngle.h>
#include <glmVector3.h>
#include <glmEntity.h>
#include <glmVelocitySampling.h>
#include <glmPerceptionContext.h>
#include <glmVelocityModel.h>

namespace glm
{
    class AbstractPathFollower;
    class NavigationBehavior;
    class VelocityModel;

    class NavigationData
    {
        friend class VelocityModel;
        friend class NavigationBehavior;

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

        // configuration
        VelocityModel _velocityModel;
        AbstractPathFollower* _pathFollower = NULL; // channel, debugDisplay, targetrovidersVectorField, gotoBe, navBe
        float _boldness = 0.5f;
        float _comfortSpeed = 1.2f;              // used in TargetProviderVectorFields
        float _maxAngularSpeed = GLM_PI * 0.66f; // NavBe & setFormationBe
        float _maxClearance = 0.f;
        float _comfortClearance = 0.f;        // _maxClearance + personal space
        int _collisionDetectionActivated = 0; // When the collision detection is activated, init fails if the initial state leads to a collision and update sets the velocity to 0,0 before a collision occurs.

        // runtime
        PathEntityComponentLocation _currentLocation; // this holds a 3D world position (includes worldMatrix)
        Vector3 _velocity = Vector3(0.f);             // NavBe, PathEntityComp, PathGroupComp
        int _navigationRunning = 0;                   // NavBe, pathEntityComponent & channelManager
        const Vector3* _worldUpVector = NULL;
        bool _groupMemberLost = false;     // NavBe & PathEntityComp
        bool _resetDesiredVelocity = true; // NavBe, PathEntityComp, PathGroupComp
        int _spatialEnvironmentIndex = 0;  // index of the SpatialEnvironment in PathContext, default to 0 = "main" SpatialEnvironment which was the default registered one

#ifndef NDEBUG
        glm::Array<Segment3> _perceivedEntitiesLocalTrajectories; //for debug only !
        glm::Array<Segment3> _perceivedObstaclesLocal;            //for debug only !
        mutable Triangle3 _localTargetPlatterLiftTriangle;        //for debug only !
#endif
    private:
        bool _hasDesiredVelocity = false;
        Vector3 _desiredVelocity = Vector3(0.f); // for external use, set by NavigationBehavior

        bool _hasDesiredAngularVelocity = false;
        Angle _desiredAngularVelocity = 0;

    public:
        bool isCollisionDetected(float radius, float precision = GLM_EPSILON) const;

        // used by NavBe, gotoBe & components
        bool hasDesiredVelocity() const; // can we get the 2D/3D desired velocity ?
        void clearDesiredVelocity();
        const Vector3& getDesiredVelocity() const;
        Vector3& touchDesiredVelocity();
        void setDesiredVelocity(const Vector3& velocity);

        // used by NavBe, gotoBe & components
        bool hasDesiredAngularVelocity() const;
        void clearDesiredAngularVelocity();
        Angle getDesiredAngularVelocity() const;
        Angle& touchDesiredAngularVelocity();
        void setDesiredAngularVelocity(Angle angularVelocity);
    };

    //-------------------------------------------------------------------------
    inline bool NavigationData::hasDesiredVelocity() const
    {
        return _hasDesiredVelocity;
    }

    //-------------------------------------------------------------------------
    inline void NavigationData::clearDesiredVelocity()
    {
        _hasDesiredVelocity = false;
        _resetDesiredVelocity = true;
    }

    //-------------------------------------------------------------------------
    inline const Vector3& NavigationData::getDesiredVelocity() const
    {
        GLM_DEBUG_ASSERT_MSG(_hasDesiredVelocity, "No desired velocity is set.");
        return _desiredVelocity;
    }

    //-------------------------------------------------------------------------
    inline Vector3& NavigationData::touchDesiredVelocity()
    {
        _hasDesiredVelocity = true;
        return _desiredVelocity;
    }

    //-------------------------------------------------------------------------
    inline void NavigationData::setDesiredVelocity(const Vector3& velocity)
    {
        _desiredVelocity = velocity;
        _hasDesiredVelocity = true;
        _resetDesiredVelocity = false;
    }

    //-------------------------------------------------------------------------
    inline bool NavigationData::hasDesiredAngularVelocity() const
    {
        return _hasDesiredAngularVelocity;
    }

    //-------------------------------------------------------------------------
    inline void NavigationData::clearDesiredAngularVelocity()
    {
        _hasDesiredAngularVelocity = false;
    }

    //-------------------------------------------------------------------------
    inline Angle NavigationData::getDesiredAngularVelocity() const
    {
        GLM_DEBUG_ASSERT_MSG(_hasDesiredAngularVelocity, "No desired angular velocity is set.");
        return _desiredAngularVelocity;
    }

    //-------------------------------------------------------------------------
    inline Angle& NavigationData::touchDesiredAngularVelocity()
    {
        _hasDesiredAngularVelocity = true;
        return _desiredAngularVelocity;
    }

    //-------------------------------------------------------------------------
    inline void NavigationData::setDesiredAngularVelocity(Angle angularVelocity)
    {
        _desiredAngularVelocity = angularVelocity;
        _hasDesiredAngularVelocity = true;
    }

    ////-------------------------------------------------------------------------
    //inline bool NavigationData::hasDesiredAngularCorrection() const
    //{
    //	return _hasDesiredAngularCorrection;
    //}

    ////-------------------------------------------------------------------------
    //inline void NavigationData::clearDesiredAngularCorrection()
    //{
    //	_hasDesiredAngularCorrection = false;
    //}

    ////-------------------------------------------------------------------------
    //inline const Quaternion& NavigationData::getDesiredAngularCorrection() const
    //{
    //	GLM_DEBUG_ASSERT_MSG(_hasDesiredAngularCorrection, "No desired angular Correction is set.");
    //	return _desiredAngularCorrection;
    //}

    ////-------------------------------------------------------------------------
    //inline Quaternion& NavigationData::touchDesiredAngularCorrection()
    //{
    //	_hasDesiredAngularCorrection = true;
    //	return _desiredAngularCorrection;
    //}

    ////-------------------------------------------------------------------------
    //inline void NavigationData::setDesiredAngularCorrection(const Quaternion& angularCorrection)
    //{
    //	_desiredAngularCorrection = angularCorrection;
    //	_hasDesiredAngularCorrection = true;
    //}
} // namespace glm
