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

#pragma once

#include "glmSDKAPI.h"
#include "glmPathDefinitions.h"
#include <glmPODArrayOfArray.h>
#include <glmIndex.h>
#include <glmMap.h>
#include <glmTriangle2.h>
#include <glmMatrix4.h>
#include <glmCachePerThread.h>

namespace glm
{
    class NavigationMeshCellsAABBTree;
    class NavigationMeshLocation;

    class NavMeshCacheData
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

        NavMeshCacheData()
            : _bitmask(32, getClassMemoryStatId())
            , _exploreList(32, getClassMemoryStatId())
            , _nextExploreList(32, getClassMemoryStatId())
            , _exploreQueue(getClassMemoryStatId())
            , _toExplore(getClassMemoryStatId())
        {
        }

        ~NavMeshCacheData()
        {
        }

        glm::PODArray<unsigned char> _bitmask;
        glm::PODArray<IDDistancePairPOD> _exploreList;
        glm::PODArray<IDDistancePairPOD> _nextExploreList;
        glm::PODArray<IDDistancePairPOD> _exploreQueue;
        glm::PODArray<NavigationMeshCellId> _toExplore;
    };

    /**
	* \defgroup NavMesh3 3D Navigation Mesh
	*
	* \ingroup NavMesh
	*
	* \author Golaem <dev@golaem.com>
	*/
    /**
	* \brief Implementation of the INavigationMesh interface
	*
	* \ingroup NavMesh3
	*
	* \author Golaem <dev@golaem.com>
	*/
    class GLM_SDK_API NavigationMesh3
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

        /**
		* \brief Destructor
		*/
        ~NavigationMesh3();

        /**
		* \name Caches
		*/
        //@{
        void prefetchCaches();
        void clearCaches();
        //@}

        /**
		* \see INavigationMesh::locate
		*/
        bool locate(NavigationMeshLocation& navLocation, const Vector3& worldUpVector) const;
        bool locateValidCell(NavigationMeshLocation& navLocation, const Vector3& worldUpVector) const;

        /**
		* \brief Locate the given 2D position in the navigation mesh.
		*
		* It proceed as follows :
		* \li The given cellId is used as a starting point for the location search,
		* \li From neighbor to neighbor a cell which projection contains the position2 is found,
		* \li The returned position3 is the projection of the given position2 along the vertical axis on the cell
		*   ,ie the ground,
		* \li The returned cellId is the identifier of the cell on which the returned position lies
		*	(-1 if the given position doesn't exist in the prismatic subdivision).
		*
		* \param[in] position2 The 2D position.
		* \param[out] position3 The 3D position.
		* \param[in,out] cellId The cell identifier.
		* \return True if the given position can be located.
		* \pre cellId must be valid.
		*/
        bool locate(const Vector3& position, const Vector3& upVector, Vector3& position3, NavigationMeshCellId& cellId) const;

        /**
		* \name Cells
		* \see INavigationMesh
		*/
        //@{
        unsigned int getNbCellIds() const;

        NavigationMeshCellId getFreeCellId() const;

        GLM_FORCE_INLINE bool isCellIdValid(const NavigationMeshCellId& cellId) const;

        const Triangle3& getCellTriangle3(const NavigationMeshCellId& cellId) const;

        //const Triangle2& getCellTriangle2(const NavigationMeshCellId& cellId) const;

        NavigationMeshEdgeId getCellEdge(const NavigationMeshCellId& cellId, unsigned int idxEdge) const;

        unsigned int getCellNbNeighbors(const NavigationMeshCellId& cellId) const;

        NavigationMeshCellId getCellNeighbor(const NavigationMeshCellId& cellId, unsigned int idxNeighbor) const;

        unsigned int getCellNbStackedCells(const NavigationMeshCellId& cellId) const;

        NavigationMeshCellId getCellStackedCell(const NavigationMeshCellId& cellId, unsigned int idxStackCell) const;

        bool areStacked(const NavigationMeshCellId& cellId1, const NavigationMeshCellId& cellId2) const;

        bool areNeighbors(const NavigationMeshCellId& cellId1, const NavigationMeshCellId& cellId2) const;

        SemanticObjectTypeValue getCellSemanticObject(const NavigationMeshCellId& cellId) const;
        //@}

        /**
		* \name Edges
		* \see INavigationMesh
		*/
        //@{
        unsigned int getNbEdgeIds() const;

        unsigned int countEdges() const;

        NavigationMeshEdgeId getFreeEdgeId() const;

        GLM_FORCE_INLINE bool isEdgeIdValid(const NavigationMeshEdgeId& edgeId) const;

        const Segment3& getEdgeSegment3(const NavigationMeshEdgeId& edgeId) const;

        //const Segment2& getEdgeSegment2(const NavigationMeshEdgeId& edgeId) const;

        unsigned int getEdgeNbCells(const NavigationMeshEdgeId& edgeId) const;

        const NavigationMeshCellId& getEdgeCell(const NavigationMeshEdgeId& edgeId, unsigned int idxCell) const;

        SemanticObjectTypeValue getEdgeSemanticObject(const NavigationMeshEdgeId& edgeId) const;

        bool isEdgeShared(const NavigationMeshEdgeId& edgeId) const;

        /**
		* \brief Retrieve the step height of a given edge.
		*
		* \param edgeId The edge identifier.
		* \pre The edge must belong to two accessible cells.
		* \return The height of the step.
		*/
        float getEdgeStepHeight(const NavigationMeshEdgeId& edgeId) const;
        //@}

        /**
		* \brief Returns the common edge of 2 cells
		*/
        NavigationMeshEdgeId getCommonEdge(const NavigationMeshCellId& cellId1, const NavigationMeshCellId& cellId2) const;

        /**
		* \name Bounding box accessors
		*/
        //@{
        const Vector2& getBoundingBoxLowerBound() const;
        const Vector2& getBoundingBoxUpperBound() const;
        //@}

        /**
		* \brief Erases a cell from the navigation mesh
		* classifyCells must be called after erasing of all of the wanted cells
		* Cell arrays and ids are left contiguous after erase
		*/
        void eraseCell(const NavigationMeshCellId& cellId);

        /**
		* \name Spatial Data Structures
		*/
        //@{
        const NavigationMeshCellsAABBTree& getCellsAABBTree() const;
        //@}

        /**
		* \brief returns the edgeType of a given Edge (by ID)
		*/
        NavMeshEdgeType::Value getEdgeType(const NavigationMeshEdgeId& edgeId) const;

        /**
		* \brief Update Edges Types cache (resize to current id count, and refresh data)
		*/
        void updateEdgeTypes();

        /**
		* \name GraphicInfos
		*/
        //@{
        /**
		* \brief Compute a 3D multisegment representing the edges.
		* \param[out] geometry The geometry.
		*/
        void computeEdgesGeometry(glm::Array<Segment3>& geometry, const std::pair<Vector2, Vector2>& boundingBox = std::make_pair(Vector2(0.f), Vector2(0.f))) const;
        void computeSpecificEdgesGeometry(glm::Array<Segment3>& geometry, const glm::PODArray<NavigationMeshEdgeId>& edgeIds, const std::pair<Vector2, Vector2>& boundingBox = std::make_pair(Vector2(0.f), Vector2(0.f))) const;

        /**
		* \brief Compute a 3D multisegment representing the edges having the given role.
		* \param[out] geometry The geometry.
		* \param[in] semanticType The desired role RTTI infos.
		*/
        void computeEdgesGeometry(glm::Array<Segment3>& geometry, const SemanticObjectTypeValue& semanticType, const std::pair<Vector2, Vector2>& boundingBox = std::make_pair(Vector2(0.f), Vector2(0.f))) const;

        /**
		* \brief Compute a 3D multisegment representing the shared edges.
		* \param[out] geometry The geometry.
		*/
        void computeSharedEdgesGeometry(glm::Array<Segment3>& geometry, const std::pair<Vector2, Vector2>& boundingBox = std::make_pair(Vector2(0.f), Vector2(0.f))) const;

        /**
		* \brief Compute a list of triangles representing the cells.
		* \param[out] geometry The geometry.
		*/
        void computeCellsGeometry(glm::Array<Triangle3>& geometry, glm::Array<Segment3>& normals, const std::pair<Vector2, Vector2>& boundingBox = std::make_pair(Vector2(0.f), Vector2(0.f)), const glm::PODArray<int>& connexZones = glm::PODArray<int>()) const;
        void computeSpecificCellsGeometry(glm::Array<Triangle3>& geometry, const glm::PODArray<NavigationMeshCellId>& cellIds, const std::pair<Vector2, Vector2>& boundingBox = std::make_pair(Vector2(0.f), Vector2(0.f))) const;

        /**
		* \brief Compute a list of triangles representing the cells having the given role.
		* \param[out] geometry The geometry.
		* \param[in] semanticType The desired role RTTI infos.
		*/
        void computeCellsGeometry(glm::Array<Triangle3>& geometry, const SemanticObjectTypeValue& semanticType, const std::pair<Vector2, Vector2>& boundingBox = std::make_pair(Vector2(0.f), Vector2(0.f))) const;

        //@}

        inline void getNavMeshCacheData(LockedThreadData<NavMeshCacheData>& lockedNavMeshData) const
        {
            _navMeshCache.getCacheForCurrentThread(lockedNavMeshData);
        }

        inline bool areCellsConnex(NavigationMeshCellId cellId1, NavigationMeshCellId cellId2) const;

        inline int getCellConnexZone(NavigationMeshCellId cellId) const;

        void updateEdgesSemanticTypeCount();

        Vector3 getBaryFromPos(const Vector3& pos, NavigationMeshCellId cellId) const;
        Vector3 getPosFromBary(const Vector3& bary, NavigationMeshCellId cellId) const;

        glm::Array<glm::PODArray<unsigned int>> _edgeSemanticTypeCountPerZone;

        // used for dynamic navigation meshes : use inverse on input position, and applyw worlMat on output
        Matrix4 _worldTransformMatrix;
        Matrix4 _worldTransformMatrixInverse;

        // for dynamic deform updates
        glm::Array<Segment3>& touchEdgesSegments3()
        {
            return _edgesSegments3;
        }
        glm::Array<Triangle3>& touchCellsTriangles3()
        {
            return _cellsTriangles3;
        }
        void updateAABBTree();

    protected:
        /**
		* \brief Default Constructor
		*/
        NavigationMesh3();

        GLM_DISALLOW_COPY_AND_ASSIGN(NavigationMesh3);

        /**
		* \brief Deallocation of all heap memory allocated by this class for its members.
		*/
        void clear();

        /**
		* \brief Swaps 2 cells (_cellsToOverlappingCells is needs to be empty or recomputed)
		*/
        void swapCells(const NavigationMeshCellId& cellId1, const NavigationMeshCellId& cellId2);

        /**
		* \brief Swaps 2 edges
		*/
        void swapEdges(const NavigationMeshEdgeId& edgeId1, const NavigationMeshEdgeId& edgeId2);

        /// Was cell validity -> the int is now a connex zone. Cells with the same connex zone are reachable
        glm::Array<int> _cellConnexZones; // bool replaced by char for performance, slightly bigger but much faster !

        /// The edge ids validity.
        glm::Array<int> _edgeValidity; // bool replaced by char for performance, slightly bigger but much faster !

        /// The cells edges.
        glm::Array<StaticArray<NavigationMeshEdgeId, 3, false>> _cellsEdges;

        /// Edges
        glm::Array<StaticArray<NavigationMeshCellId, 2>> _edgesCells;

        /// Edges 3D segments
        glm::Array<Segment3> _edgesSegments3;

        /// Edges border info
        glm::Array<std::pair<SemanticObjectTypeValue, bool>> _edgesSemanticObjects;

        /// Cells 3D triangles
        glm::Array<Triangle3> _cellsTriangles3;

        /**
		* \name internal caches
		*/
        //@{
        /// The cells neighbors
        glm::Array<StaticArray<NavigationMeshCellId, 3>> _cellsNeighborsCache;

        /// Associates each cells with the cells its projection overlaps theirs.
        glm::PODArrayOfArray<NavigationMeshCellId> _cellsToOverlappingCells;

        /// Cells semantic objects
        glm::PODArray<SemanticObjectTypeValue> _cellsSemanticObjects;

        /**
		* \brief The navigation mesh 2d bounding box lower bound
		*/
        Vector2 _boundingBoxLowerBound;

        /**
		* \brief The navigation mesh 2d bounding box upper bound
		*/
        Vector2 _boundingBoxUpperBound;

        mutable NavigationMeshCellsAABBTree* _cellsAABBTree;

        /// keep a cache of each edge type (needs to be udpated by updateEdgeTypes method whenever required)
        // stores edgeTypes in a cache data, for fast access, as it is heavily used
        glm::Array<NavMeshEdgeType::Value> _edgeTypes;

        friend class AbstractNavigationMesh3Creator;

        CachePerThread<NavMeshCacheData> _navMeshCache;
    };

    //-----------------------------------------------------------------------------
    // isCellIdValid
    //-----------------------------------------------------------------------------
    inline bool NavigationMesh3::isCellIdValid(const NavigationMeshCellId& cellId) const
    {
        return isIndexValid(cellId, static_cast<int>(_cellConnexZones.size())) && _cellConnexZones[cellId] > 0;
    }

    //-----------------------------------------------------------------------------
    // isEdgeIdValid
    //-----------------------------------------------------------------------------
    inline bool NavigationMesh3::isEdgeIdValid(const NavigationMeshEdgeId& edgeId) const
    {
        return isIndexValid(edgeId, static_cast<int>(_edgeValidity.size())) && _edgeValidity[edgeId] == 1;
    }

    //-----------------------------------------------------------------------------
    // getNbCells
    //-----------------------------------------------------------------------------
    inline unsigned int NavigationMesh3::getNbCellIds() const
    {
        return static_cast<unsigned int>(_cellConnexZones.size());
    }

    //-----------------------------------------------------------------------------
    // getFreeCellId
    //-----------------------------------------------------------------------------
    inline NavigationMeshCellId NavigationMesh3::getFreeCellId() const
    {
        NavigationMeshCellId freeCellId(static_cast<int>(std::find(_cellConnexZones.begin(), _cellConnexZones.end(), 0) - _cellConnexZones.begin()));
        return freeCellId;
    }

    //-----------------------------------------------------------------------------
    // getCellTriangle3(const CellId&)
    //-----------------------------------------------------------------------------
    inline const Triangle3& NavigationMesh3::getCellTriangle3(const NavigationMeshCellId& cellId) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId), "Invalid cell id #%d", cellId);
        return _cellsTriangles3[cellId];
    }

    //-----------------------------------------------------------------------------
    // getCellEdge
    //-----------------------------------------------------------------------------
    inline NavigationMeshEdgeId NavigationMesh3::getCellEdge(const NavigationMeshCellId& cellId, unsigned int idxEdge) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId), "Invalid cell id #%d", cellId);
        GLM_DEBUG_ASSERT_MSG(idxEdge < 3, "Invalid edge index! It must be between 0 and 2");
        return _cellsEdges[cellId][idxEdge];
    }

    //-----------------------------------------------------------------------------
    // getCellNeighbor
    //-----------------------------------------------------------------------------
    inline NavigationMeshCellId NavigationMesh3::getCellNeighbor(const NavigationMeshCellId& cellId, unsigned int idxNeighbor) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId), "Invalid cell id #%d", cellId);
        return _cellsNeighborsCache[cellId][idxNeighbor];
    }

    //-----------------------------------------------------------------------------
    // getCellNbStackedCells
    //-----------------------------------------------------------------------------
    inline unsigned int NavigationMesh3::getCellNbStackedCells(const NavigationMeshCellId& cellId) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId), "Invalid cell id #%d", cellId);
        return static_cast<unsigned int>(_cellsToOverlappingCells.size_items(cellId));
    }

    //-----------------------------------------------------------------------------
    // getCellStackedCell
    //-----------------------------------------------------------------------------
    inline NavigationMeshCellId NavigationMesh3::getCellStackedCell(const NavigationMeshCellId& cellId, unsigned int idxStackCell) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId), "Invalid cell id #%d", cellId);
        GLM_DEBUG_ASSERT_MSG(idxStackCell < static_cast<uint32_t>(_cellsToOverlappingCells.size_items(cellId)),
                             "Invalid stacked cell index! It must be inside [0, %lu[",
                             static_cast<uint32_t>(_cellsToOverlappingCells.size_items(cellId)));

        return _cellsToOverlappingCells.at(cellId, idxStackCell);
    }

    //-----------------------------------------------------------------------------
    // areStacked
    //-----------------------------------------------------------------------------
    inline bool NavigationMesh3::areStacked(const NavigationMeshCellId& cellId1, const NavigationMeshCellId& cellId2) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId1), "Invalid cell id #%d", cellId1);
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId2), "Invalid cell id #%d", cellId2);
        const NavigationMeshCellId* cellId1_begin = _cellsToOverlappingCells.begin(cellId1);
        const NavigationMeshCellId* cellId1_end = _cellsToOverlappingCells.end(cellId1);
        return (glmFind(cellId1_begin, cellId1_end, cellId2) != cellId1_end);
    }

    //-----------------------------------------------------------------------------
    // getCellSemanticObject
    //-----------------------------------------------------------------------------
    inline SemanticObjectTypeValue NavigationMesh3::getCellSemanticObject(const NavigationMeshCellId& cellId) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId), "Invalid cell id #%d", cellId);
        return _cellsSemanticObjects[cellId];
    }

    //-----------------------------------------------------------------------------
    // getNbEdges
    //-----------------------------------------------------------------------------
    inline unsigned int NavigationMesh3::getNbEdgeIds() const
    {
        return static_cast<unsigned int>(_edgeValidity.size());
    }

    //-----------------------------------------------------------------------------
    // getFreeEdgeId
    //-----------------------------------------------------------------------------
    inline NavigationMeshEdgeId NavigationMesh3::getFreeEdgeId() const
    {
        NavigationMeshEdgeId freeEdgeId(static_cast<int>(std::find(_edgeValidity.begin(), _edgeValidity.end(), 0) - _edgeValidity.begin()));
        return freeEdgeId;
    }

    //-----------------------------------------------------------------------------
    // getEdgeSegment3
    //-----------------------------------------------------------------------------
    inline const Segment3& NavigationMesh3::getEdgeSegment3(const NavigationMeshEdgeId& edgeId) const
    {
        GLM_DEBUG_ASSERT_MSG(isEdgeIdValid(edgeId), "Invalid edge id #%d", edgeId);
        return _edgesSegments3[edgeId];
    }

    //-----------------------------------------------------------------------------
    // getEdgeNbCells
    //-----------------------------------------------------------------------------
    inline unsigned int NavigationMesh3::getEdgeNbCells(const NavigationMeshEdgeId& edgeId) const
    {
        GLM_DEBUG_ASSERT_MSG(isEdgeIdValid(edgeId), "Invalid edge id #%d", edgeId);
        return _edgesCells[edgeId].size();
    }

    //-----------------------------------------------------------------------------
    // getEdgeCell
    //-----------------------------------------------------------------------------
    inline const NavigationMeshCellId& NavigationMesh3::getEdgeCell(const NavigationMeshEdgeId& edgeId, unsigned int idxCell) const
    {
        GLM_DEBUG_ASSERT_MSG(isEdgeIdValid(edgeId), "Invalid edge id #%d", edgeId);
        GLM_DEBUG_ASSERT_MSG(idxCell < getEdgeNbCells(edgeId), "Invalid cell index! It must be inside [0, %d[", getEdgeNbCells(edgeId));

        return _edgesCells[edgeId][idxCell];
    }

    //-----------------------------------------------------------------------------
    // getEdgeSemanticObject
    //-----------------------------------------------------------------------------
    inline SemanticObjectTypeValue NavigationMesh3::getEdgeSemanticObject(const NavigationMeshEdgeId& edgeId) const
    {
        GLM_DEBUG_ASSERT_MSG(isEdgeIdValid(edgeId), "Invalid edge id #%d", edgeId);
        return _edgesSemanticObjects[edgeId].first;
    }

    //-----------------------------------------------------------------------------
    // getEdgeSemanticObject
    //-----------------------------------------------------------------------------
    inline bool NavigationMesh3::isEdgeShared(const NavigationMeshEdgeId& edgeId) const
    {
        GLM_DEBUG_ASSERT_MSG(isEdgeIdValid(edgeId), "Invalid edge id #%d", edgeId);
        return _edgesSemanticObjects[edgeId].second;
    }

    //-----------------------------------------------------------------------------
    // getBoundingBoxLowerBound
    //-----------------------------------------------------------------------------
    inline const Vector2& NavigationMesh3::getBoundingBoxLowerBound() const
    {
        return _boundingBoxLowerBound;
    }

    //-----------------------------------------------------------------------------
    // getBoundingBoxUpperBound
    //-----------------------------------------------------------------------------
    inline const Vector2& NavigationMesh3::getBoundingBoxUpperBound() const
    {
        return _boundingBoxUpperBound;
    }

    //-----------------------------------------------------------------------------
    // getEdgeType
    //-----------------------------------------------------------------------------
    inline NavMeshEdgeType::Value NavigationMesh3::getEdgeType(const NavigationMeshEdgeId& edgeId) const
    {
        GLM_DEBUG_ASSERT((unsigned int)edgeId < getNbEdgeIds());
        return _edgeTypes[edgeId];
    }

    //-----------------------------------------------------------------------------
    inline bool NavigationMesh3::areCellsConnex(NavigationMeshCellId cellId1, NavigationMeshCellId cellId2) const
    {
        return getCellConnexZone(cellId1) == getCellConnexZone(cellId2);
    }

    //-----------------------------------------------------------------------------
    inline int NavigationMesh3::getCellConnexZone(NavigationMeshCellId cellId) const
    {
        GLM_DEBUG_ASSERT_MSG(isCellIdValid(cellId), "Invalid cell id #%d", cellId);
        return _cellConnexZones[cellId];
    }
} // namespace glm
