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

#pragma once

#include "glmSDKAPI.h"
#include "glmNavigationMeshLocation.h"
#include "glmRoadmap.h"
#include "glmInterpolation.h"

namespace glm
{
/**
	* \brief Information associated to a node of a planned path
	*/
class GLM_SDK_API NavigationPathVertex
{

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

    NavigationPathVertex();
    explicit NavigationPathVertex(NavigationMesh3& navMesh, const NavigationMeshLocation& location);
    NavigationPathVertex(const Roadmap& roadMap, const RoadmapNodeId& roadmapNodeId);
    NavigationPathVertex(const NavigationPathVertex& vertex);
    ~NavigationPathVertex();

    NavigationPathVertex& operator=(const NavigationPathVertex& vertex);

    //const NavigationMeshLocation& getNavigationMeshLocation() const
    //{
    //    return _location;
    //}
    //NavigationMeshLocation& getNavigationMeshLocation()
    //{
    //    return _location;
    //}

    Vector3 getNavigationPositionWorld(const NavigationMesh3& navMesh) const;
    Vector3 getNavigationPositionLocal() const;
	void setNavigationPositionLocal(const NavigationMesh3& navMesh, const Vector3& position, NavigationMeshCellId cellId);
	NavigationMeshLocation getNavigationMeshLocationBis() const;

    const NavigationMeshCellId& getCellId() const;

    const RoadmapNodeId& getRoadmapNodeId() const;

    bool operator==(const NavigationPathVertex& vertex) const;
    bool operator!=(const NavigationPathVertex& vertex) const;

    static float distance(const NavigationPathVertex& from, const NavigationPathVertex& to);

    inline void setMain(bool value);
    inline bool isMain() const;

	inline bool isOnCellEdge() const;
	inline bool isOnCellVertex() const;
	inline int getCellEdgeIndex() const;
	inline int getCellVertexIndex() const;

  private:

    // Underlying location.
    //NavigationMeshLocation _location;
	// -> replaced by barycentric position, to enable dynamic deformable navMesh 
#ifndef NDEBUG
	Vector3 _debugPeekLocalPosition;
#endif
	  
	Vector3 _barycentricCoods;
	NavigationMeshCellId _navMeshCellId;
	const NavigationMesh3* _navMesh;

    // Waypoint id in the roadmap of the spatial environment
    RoadmapNodeId _roadmapNodeId;

    // if true, reuse this waypoint
    bool _isMain;
};

//-----------------------------------------------------------------------------
void GLM_SDK_API interpolate(const NavigationMesh3& navMesh, NavigationPathVertex& result, const NavigationPathVertex& predecessor, const NavigationPathVertex& successor, float factor);

//-----------------------------------------------------------------------------
NavigationPathVertex GLM_SDK_API interpolate(const NavigationMesh3& navMesh, const NavigationPathVertex& predecessor, const NavigationPathVertex& successor, float factor);

//-----------------------------------------------------------------------------
inline Vector3 NavigationPathVertex::getNavigationPositionWorld(const NavigationMesh3& navMesh) const
{
	Vector3 navPositionLocal = navMesh.getPosFromBary(_barycentricCoods, _navMeshCellId);
    return transformPoint(navPositionLocal, navMesh._worldTransformMatrix);
}

//-----------------------------------------------------------------------------
inline Vector3 NavigationPathVertex::getNavigationPositionLocal() const
{
    return _navMesh->getPosFromBary(_barycentricCoods, _navMeshCellId);;
}

//-----------------------------------------------------------------------------
inline const NavigationMeshCellId& NavigationPathVertex::getCellId() const
{
    return _navMeshCellId;
}

//-----------------------------------------------------------------------------
inline const RoadmapNodeId& NavigationPathVertex::getRoadmapNodeId() const
{
    return _roadmapNodeId;
}

//-----------------------------------------------------------------------------
inline void NavigationPathVertex::setMain(bool value)
{
    _isMain = value;
}

//-----------------------------------------------------------------------------
inline bool NavigationPathVertex::isMain() const
{
    return _isMain;
}

//-----------------------------------------------------------------------------
inline bool NavigationPathVertex::isOnCellEdge() const
{
	return _barycentricCoods.x == 0 || _barycentricCoods.y == 0 || _barycentricCoods.z == 0;
}

//-----------------------------------------------------------------------------
inline bool NavigationPathVertex::isOnCellVertex() const
{
	return (_barycentricCoods.y == 0 && _barycentricCoods.z == 0)
		|| (_barycentricCoods.x == 0 && _barycentricCoods.z == 0)
		|| (_barycentricCoods.x == 0 && _barycentricCoods.y == 0);
}

//-----------------------------------------------------------------------------
inline int NavigationPathVertex::getCellVertexIndex() const
{
	// better test for 0 with normalization that could produce 0.99999994 0 0 for example
	if (_barycentricCoods.y == 0 && _barycentricCoods.z == 0)
		return 0;
	else if (_barycentricCoods.x == 0 && _barycentricCoods.z == 0)
		return 1;
	else if (_barycentricCoods.x == 0 && _barycentricCoods.y == 0)
		return 2;

	return -1;
}

//-----------------------------------------------------------------------------
inline int NavigationPathVertex::getCellEdgeIndex() const
{
	// is on segment BC
	if (_barycentricCoods.x == 0)
		return 1;

	// is on segment CA
	if (_barycentricCoods.y == 0)
		return 2;

	// is on segment AB
	if (_barycentricCoods.z == 0)
		return 0;
	
	return -1;
}
} // namespace glm
