/*
* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
* its licensors.
*
* For complete copyright and license terms please see the LICENSE at the root of this
* distribution (the "License"). All use of this software is governed by the License,
* or, if provided, by the license below or the license accompanying this file. Do not
* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*
*/
// Original file Copyright Crytek GMBH or its affiliates, used under license.

// Description : Implementation of the CGraph class.


#include "CryLegacy_precompiled.h"

#include <limits>
#include <algorithm>

#include <ISystem.h>
#include <IRenderAuxGeom.h>
#include <ILog.h>
#include <I3DEngine.h>
#include <ITimer.h>
#include <IConsole.h>
#include <IPhysics.h>
#include <CryFile.h>

#include "Graph.h"
#include "CAISystem.h"
#include "AILog.h"
#include "Cry_Math.h"
#include "AIObject.h"
#include "VertexList.h"
#include "NavRegion.h"

#include "GraphLinkManager.h"
#include "GraphNodeManager.h"


#define BAI_TRI_FILE_VERSION 54

// identifier so links can be marked as impassable, then restored
//static const float RADIUS_FOR_BROKEN_LINKS = -121314.0f;

const float AStarSearchNode::fInvalidCost = 999999.0f;

Vec3 CObstacleRef::GetPos() const
{
    CCCPOINT(CObstacleRef_GetPos);

    if (m_refAnchor.IsValid())
    {
        return m_refAnchor.GetAIObject()->GetPos();
    }
    else if (m_pNode)
    {
        return m_pNode->GetPos();
    }
    else if (m_pSmartObject && m_pRule)
    {
        return m_pRule->pObjectHelper ? m_pSmartObject->GetHelperPos(m_pRule->pObjectHelper) : m_pSmartObject->GetPos();
    }
    else
    {
        assert(false);
        return Vec3(ZERO);
    }
}

float CObstacleRef::GetApproxRadius() const
{
    assert(false);
    return 0.0f;
}

//====================================================================
// ValidateNode
//====================================================================
inline bool CGraph::ValidateNode(unsigned nodeIndex, bool fullCheck) const
{
    const GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);

    if (!nodeIndex)
    {
        return false;
    }
    if (!m_allNodes.DoesNodeExist(nodeIndex))
    {
        return false;
    }
    if (!fullCheck)
    {
        return true;
    }
    else
    {
        return ValidateNodeFullCheck(pNode);
    }
}

//====================================================================
// ValidateNodeFullCheck
//====================================================================
bool CGraph::ValidateNodeFullCheck(const GraphNode* pNode) const
{
    bool result = true;
    AIAssert(pNode);
    int nNonRoadLinks = 0;
    unsigned nTriLinks = 0;
    for (unsigned linkId = pNode->firstLinkIndex; linkId; linkId = GetLinkManager().GetNextLink(linkId))
    {
        unsigned nextNodeIndex = GetLinkManager().GetNextNode(linkId);
        const GraphNode* next = GetNodeManager().GetNode(nextNodeIndex);
        if (!CGraph::ValidateNode(nextNodeIndex, false))
        {
            result = false;
        }
        if (next->navType != IAISystem::NAV_ROAD)
        {
            ++nNonRoadLinks;
        }
        if (next->navType == IAISystem::NAV_TRIANGULAR)
        {
            ++nTriLinks;
        }
    }
    if (nNonRoadLinks > 50)
    {
        AIWarning("Too many non-road links (%d) from node %p type %d at (%5.2f, %5.2f, %5.2f)", nNonRoadLinks, pNode, pNode->navType,
            pNode->GetPos().x, pNode->GetPos().y, pNode->GetPos().z);
    }
    if (pNode->navType == IAISystem::NAV_TRIANGULAR && nTriLinks != 3)
    {
        // NAV_TRIANGULAR is replaced by MNM
        assert(false);
        result = false;
    }
    return result;
}

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

//====================================================================
// CGraph
//====================================================================
CGraph::CGraph()
    :   m_pGraphLinkManager(new CGraphLinkManager)
    , m_pGraphNodeManager(new CGraphNodeManager)
    , m_allNodes(*m_pGraphNodeManager)
    , m_triangularBBox(AABB::RESET)
{
    m_safeFirstIndex = CreateNewNode(IAISystem::NAV_UNSET, Vec3(ZERO), 0);
    m_firstIndex = m_safeFirstIndex;
    m_pFirst = GetNodeManager().GetNode(m_safeFirstIndex);
    m_pSafeFirst = m_pFirst;
    m_pCurrent = m_pFirst;
    m_currentIndex = m_firstIndex;
    m_pCurrent->firstLinkIndex = 0;
}

//====================================================================
// ~CGraph
//====================================================================
CGraph::~CGraph()
{
    //Takes a long time and shouldn't be needed
    //Clear(IAISystem::NAVMASK_ALL);

    delete m_pGraphNodeManager;
    delete m_pGraphLinkManager;

    GraphNode::ClearFreeIDs();
}


//====================================================================
// Clear
//====================================================================
void CGraph::Clear(IAISystem::tNavCapMask navTypeMask)
{
    ClearMarks();
    DeleteGraph(navTypeMask);
    if (navTypeMask & (IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_WAYPOINT_3DSURFACE))
    {
        EntranceMap::iterator next;
        for (EntranceMap::iterator it = m_mapEntrances.begin(); it != m_mapEntrances.end(); it = next)
        {
            next = it;
            ++next;
            GraphNode* pNode = GetNodeManager().GetNode(it->second);
            if (pNode->navType & navTypeMask)
            {
                m_mapEntrances.erase(it);
            }
        }
        for (EntranceMap::iterator it = m_mapExits.begin(); it != m_mapExits.end(); it = next)
        {
            next = it;
            ++next;
            GraphNode* pNode = GetNodeManager().GetNode(it->second);
            if (pNode->navType & navTypeMask)
            {
                m_mapExits.erase(it);
            }
        }
    }

    GetNodeManager().Clear(navTypeMask);

    if (m_pSafeFirst)
    {
        Disconnect(m_safeFirstIndex, false);
    }
    else
    {
        m_safeFirstIndex = CreateNewNode(IAISystem::NAV_UNSET, Vec3(ZERO), 0);
        m_pSafeFirst = GetNodeManager().GetNode(m_safeFirstIndex);
    }
    m_pFirst = m_pSafeFirst;
    m_firstIndex = m_safeFirstIndex;
    m_pCurrent = m_pFirst;
    m_currentIndex = m_firstIndex;
}

//====================================================================
// ConnectInCm
//====================================================================
void CGraph::ConnectInCm(unsigned oneNodeIndex, unsigned twoNodeIndex,
    int16 radiusOneToTwoCm, int16 radiusTwoToOneCm,
    unsigned* pLinkOneTwo, unsigned* pLinkTwoOne)
{
    GraphNode* one = GetNodeManager().GetNode(oneNodeIndex);
    GraphNode* two = GetNodeManager().GetNode(twoNodeIndex);

    if (pLinkOneTwo)
    {
        *pLinkOneTwo = 0;
    }
    if (pLinkTwoOne)
    {
        *pLinkTwoOne = 0;
    }

    if (one == two)
    {
        return;
    }

    if (!one || !two)
    {
        return;
    }

    if (!CGraph::ValidateNode(oneNodeIndex, false) || !CGraph::ValidateNode(twoNodeIndex, false))
    {
        AIError("CGraph::Connect Attempt to connect nodes that aren't created [Code bug]");
        return;
    }

    if ((one == m_pSafeFirst || two == m_pSafeFirst) && m_pSafeFirst->firstLinkIndex)
    {
        AIWarning("Second link being made to safe/first node");
        return;
    }

#ifdef CRYAISYSTEM_DEBUG
    extern std::vector<const GraphNode*> g_DebugGraphNodesToDraw;
    g_DebugGraphNodesToDraw.clear();
#endif //CRYAISYSTEM_DEBUG

    // handle case where they're already connected
    unsigned linkIndexOne = one->GetLinkTo(GetNodeManager(), GetLinkManager(), two);
    unsigned linkIndexTwo = two->GetLinkTo(GetNodeManager(), GetLinkManager(), one);

    if ((linkIndexOne == 0) != (linkIndexTwo == 0))
    {
        AIWarning("Trying to connect links but one is already connected, other isn't");
        return;
    }

    // Check that if both links have bidirectional data, then it is the same.
    assert(linkIndexOne == 0 || linkIndexTwo == 0 || (linkIndexOne & ~1) == (linkIndexTwo & ~1));

    // Create new bidirectional data if necessary.
    unsigned linkIndex = linkIndexOne;
    if (!linkIndex && linkIndexTwo)
    {
        linkIndex = linkIndexTwo ^ 1;
    }
    if (!linkIndex)
    {
        linkIndex = m_pGraphLinkManager->CreateLink();
    }

    Vec3 midPos = 0.5f * (one->GetPos() + two->GetPos());

    if (!linkIndexOne)
    {
        // [1/3/2007 MichaelS] Should be possible to push link on front, but I don't want to run the risk
        // of breaking code that relies on the link order being preserved, so we add it to the end.
        //one->links.push_back(linkIndex);
        if (!one->firstLinkIndex)
        {
            one->firstLinkIndex = linkIndex;
        }
        else
        {
            unsigned lastLink, nextLink;
            for (lastLink = one->firstLinkIndex; nextLink = m_pGraphLinkManager->GetNextLink(lastLink); lastLink = nextLink)
            {
                ;
            }
            m_pGraphLinkManager->SetNextLink(lastLink, linkIndex);
        }

        linkIndexOne = linkIndex;
        GetLinkManager().SetNextNode(linkIndex, twoNodeIndex);
        GetLinkManager().SetRadiusInCm(linkIndex, two == m_pSafeFirst ? -100 : radiusOneToTwoCm);
        GetLinkManager().GetEdgeCenter(linkIndex) = midPos;
        two->AddRef();
    }
    else
    {
        if (radiusOneToTwoCm != 0)
        {
            GetLinkManager().ModifyRadiusInCm(linkIndexOne, radiusOneToTwoCm);
        }
    }
    if (!linkIndexTwo)
    {
        // [1/3/2007 MichaelS] Should be possible to push link on front, but I don't want to run the risk
        // of breaking code that relies on the link order being preserved, so we add it to the end.
        //one->links.push_back(linkIndex);
        if (!two->firstLinkIndex)
        {
            two->firstLinkIndex = linkIndex ^ 1;
        }
        else
        {
            unsigned lastLink, nextLink;
            for (lastLink = two->firstLinkIndex; nextLink = m_pGraphLinkManager->GetNextLink(lastLink); lastLink = nextLink)
            {
                ;
            }
            m_pGraphLinkManager->SetNextLink(lastLink, linkIndex ^ 1);
        }

        linkIndexTwo = linkIndex ^ 1;
        GetLinkManager().SetNextNode(linkIndex ^ 1, oneNodeIndex);
        GetLinkManager().SetRadiusInCm(linkIndex ^ 1, one == m_pSafeFirst ? -100 : radiusTwoToOneCm);
        GetLinkManager().GetEdgeCenter(linkIndex ^ 1) = midPos;
        one->AddRef();
    }
    else
    {
        if (radiusTwoToOneCm != 0)
        {
            GetLinkManager().ModifyRadiusInCm(linkIndexTwo, radiusTwoToOneCm);
        }
    }

    if (pLinkOneTwo && linkIndexOne)
    {
        *pLinkOneTwo = linkIndexOne;
    }
    if (pLinkTwoOne && linkIndexTwo)
    {
        *pLinkTwoOne = linkIndexTwo;
    }

    if (m_pSafeFirst->firstLinkIndex)
    {
        return;
    }

    if (one->navType == IAISystem::NAV_TRIANGULAR)
    {
        ConnectInCm(m_safeFirstIndex, oneNodeIndex, 10000, -100);
        m_pFirst = one;
        m_firstIndex = oneNodeIndex;
    }
    else if (two->navType == IAISystem::NAV_TRIANGULAR)
    {
        ConnectInCm(m_safeFirstIndex, twoNodeIndex, 10000, -100);
        m_pFirst = two;
        m_firstIndex = twoNodeIndex;
    }
    // may have incurred a reallocation
    // [1/3/2007 MichaelS] Should no longer be necessary since we use indices instead of pointers, but leaving it here for now.
    if (pLinkOneTwo && linkIndexOne)
    {
        *pLinkOneTwo = linkIndexOne;
    }
    if (pLinkTwoOne && linkIndexTwo)
    {
        *pLinkTwoOne = linkIndexTwo;
    }
}

/// Connects (two-way) two nodes, optionally returning pointers to the new links
void CGraph::Connect(unsigned oneIndex, unsigned twoIndex, float radiusOneToTwo, float radiusTwoToOne,
    unsigned* pLinkOneTwo, unsigned* pLinkTwoOne)
{
    int16 radiusOneToTwoInCm = NavGraphUtils::InCentimeters(radiusOneToTwo);
    int16 radiusTwoToOneInCm = NavGraphUtils::InCentimeters(radiusTwoToOne);

    ConnectInCm(oneIndex, twoIndex, radiusOneToTwoInCm, radiusTwoToOneInCm, pLinkOneTwo, pLinkTwoOne);
}


//====================================================================
// DeleteGraph
//====================================================================
void CGraph::DeleteGraph(IAISystem::tNavCapMask navTypeMask)
{
    std::vector<unsigned> nodesToDelete;
    CAllNodesContainer::Iterator it(m_allNodes, navTypeMask);
    while (unsigned nodeIndex = it.Increment())
    {
        GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
        AIAssert(pNode->navType & navTypeMask);
        nodesToDelete.push_back(nodeIndex);
    }

    for (unsigned i = 0; i < nodesToDelete.size(); ++i)
    {
        GraphNode* pNode = GetNodeManager().GetNode(nodesToDelete[i]);
        Disconnect(nodesToDelete[i]);
        if (pNode == m_pSafeFirst)
        {
            m_pSafeFirst = 0;
            m_safeFirstIndex = 0;
        }
    }

    m_allNodes.Compact();
}

//====================================================================
// Disconnect
//====================================================================
void CGraph::Disconnect(unsigned nodeIndex, unsigned linkId)
{
    GraphNode* pNode = m_pGraphNodeManager->GetNode(nodeIndex);

    if (!CGraph::ValidateNode(nodeIndex, false))
    {
        AIError("CGraph::Disconnect Attempt to disconnect link from node that isn't created [Code bug]");
        return;
    }
    AIAssert(linkId);
    unsigned otherNodeIndex = GetLinkManager().GetNextNode(linkId);
    GraphNode* pOtherNode = GetNodeManager().GetNode(otherNodeIndex);
    if (!CGraph::ValidateNode(otherNodeIndex, false))
    {
        AIError("CGraph::Disconnect Attempt to disconnect link from other node that isn't created [Code bug]");
        return;
    }

    pNode->RemoveLinkTo(GetLinkManager(), otherNodeIndex);
    pNode->Release();

    pOtherNode->RemoveLinkTo(GetLinkManager(), nodeIndex);
    pOtherNode->Release();

    GetLinkManager().DestroyLink(linkId);
}

//====================================================================
// Disconnect
//====================================================================
void CGraph::Disconnect(unsigned nodeIndex, bool bDelete)
{
    GraphNode* pDisconnected = m_pGraphNodeManager->GetNode(nodeIndex);

    if (!CGraph::ValidateNode(nodeIndex, false))
    {
        AIError("CGraph::Disconnect Attempt to disconnect node that isn't created [Code bug]");
        return;
    }

#ifdef CRYAISYSTEM_DEBUG
    extern std::vector<const GraphNode*> g_DebugGraphNodesToDraw;
    g_DebugGraphNodesToDraw.clear();
#endif //CRYAISYSTEM_DEBUG

    // if the node we are disconnecting is the current node, move the current
    // to one of his links, or the root if it has no links
    if (pDisconnected == m_pCurrent)
    {
        if (pDisconnected->firstLinkIndex)
        {
            m_currentIndex = GetLinkManager().GetNextNode(pDisconnected->firstLinkIndex);
            m_pCurrent = GetNodeManager().GetNode(m_currentIndex);
        }
        else
        {
            m_currentIndex = m_safeFirstIndex;
            m_pCurrent = m_pSafeFirst;
        }
    }

    // if its the root that is being disconnected, move it
    if (m_pFirst == pDisconnected)
    {
        if (pDisconnected->firstLinkIndex)
        {
            m_firstIndex = GetLinkManager().GetNextNode(pDisconnected->firstLinkIndex);
            m_pFirst = GetNodeManager().GetNode(m_firstIndex);
        }
        else
        {
            if (m_pFirst != m_pSafeFirst)
            {
                m_pFirst = m_pSafeFirst;
                m_firstIndex = m_safeFirstIndex;
            }
            else
            {
                m_pFirst = 0;
                m_firstIndex = 0;
            }
        }
    }

    // now disconnect this node from its links
    for (unsigned link = pDisconnected->firstLinkIndex, nextLink; link; link = nextLink)
    {
        nextLink = GetLinkManager().GetNextLink(link);

        unsigned nextNodeIndex = GetLinkManager().GetNextNode(link);
        GraphNode* pNextNode = GetNodeManager().GetNode(nextNodeIndex);
        pNextNode->RemoveLinkTo(GetLinkManager(), nodeIndex);
        pNextNode->Release();
        pDisconnected->Release();
        GetLinkManager().DestroyLink(link);
    }

    pDisconnected->firstLinkIndex = 0;

    if (pDisconnected->nRefCount != 1)
    {
        AIWarning("Node reference count is not 1 after disconnecting");
    }

    if (bDelete)
    {
        DeleteNode(nodeIndex);
    }

    if (!m_pSafeFirst)
    {
        return;
    }

    if (pDisconnected != m_pSafeFirst && !m_pSafeFirst->firstLinkIndex)
    {
        unsigned firstIndex = m_firstIndex;
        // we have disconnected the link to the dummy safe node - relink it to any outdoor node of the graph
        if (firstIndex == m_safeFirstIndex)
        {
            if (m_currentIndex == m_safeFirstIndex)
            {
                // try any entrance
                if (!m_mapEntrances.empty())
                {
                    firstIndex = (m_mapEntrances.begin()->second);
                }
                else
                {
                    return; // m_pSafeFirst links will stay empty
                }
            }
            else
            {
                firstIndex = m_currentIndex;
            }
        }

        if (firstIndex)
        {
            GraphNode* pFirst = GetNodeManager().GetNode(firstIndex);
            if (pFirst->navType == IAISystem::NAV_TRIANGULAR)
            {
                ConnectInCm(m_safeFirstIndex, firstIndex, 10000, -100);
            }
            else if (pFirst->navType == IAISystem::NAV_WAYPOINT_HUMAN)
            {
                GraphNode* pEntrance = GetEntrance(pFirst->GetWaypointNavData()->nBuildingID, Vec3(0, 0, 0));
                if (pEntrance)
                {
                    for (unsigned link = pEntrance->firstLinkIndex; link; link = GetLinkManager().GetNextLink(link))
                    {
                        unsigned nextNodeIndex = GetLinkManager().GetNextNode(link);
                        GraphNode* pNextNode = GetNodeManager().GetNode(nextNodeIndex);
                        if (pNextNode->navType == IAISystem::NAV_WAYPOINT_HUMAN)
                        {
                            ConnectInCm(m_safeFirstIndex, nextNodeIndex, 10000, -100);
                            break;
                        }
                    }
                }
            }
        }
    }
}

bool operator<(const Vec3r& v1, const Vec3r& v2)
{
    if (v1.x < v2.x)
    {
        return true;
    }
    else if (v1.x > v2.y)
    {
        return false;
    }
    if (v1.y < v2.y)
    {
        return true;
    }
    else if (v1.y > v2.y)
    {
        return false;
    }
    if (v1.z < v2.z)
    {
        return true;
    }
    else if (v1.z > v2.z)
    {
        return false;
    }
    return false;
}

//====================================================================
// PointInTriangle
// Check whether a position is within a node's triangle
//====================================================================
bool PointInTriangle(const Vec3& pos, GraphNode* pNode)
{
    // NAV_TRIANGULAR is replaced by MNM
    assert(false);
    return false;
}

//#define VALIDATEINMARK

//====================================================================
// MarkNode
// uses mark for internal graph operation without disturbing the pathfinder
//====================================================================
void CGraph::MarkNode(unsigned nodeIndex) const
{
#ifdef VALIDATEINMARK
    if (!CGraph::ValidateNode(nodeIndex))
    {
        AIError("CGraph::MarkNode Unable to validate/mark node %p [Code bug]", pNode);
        return;
    }
#endif
    GetNodeManager().GetNode(nodeIndex)->mark = 1;
    m_markedNodes.push_back(nodeIndex);
}

//====================================================================
// ClearMarks
// clears the marked nodes
//====================================================================
void CGraph::ClearMarks() const
{
    while (!m_markedNodes.empty())
    {
#ifdef VALIDATEINMARK
        if (!CGraph::ValidateNode(m_markedNodes.back()))
        {
            AIError("CGraph::ClearMarks Unable to validate/clear mark node %p [Code bug]", m_markedNodes.back());
        }
        else
#endif
        GetNodeManager().GetNode(m_markedNodes.back())->mark = 0;
        m_markedNodes.pop_back();
    }
}

//====================================================================
// ReadFromFile
// Reads the AI graph from a specified file
//====================================================================
bool CGraph::ReadFromFile(const char* szName)
{
    CCryFile file;
    ;
    if (file.Open(szName, "rb"))
    {
        return ReadNodes(file);
    }
    else
    {
        return false;
    }
}

//====================================================================
// ReadNodes
// reads all the nodes in a map
//====================================================================
bool CGraph::ReadNodes(CCryFile& file)
{
    NodeDescBuffer nodeDescs;

    //AIAssert(_heapchk()==_HEAPOK);
    int iNumber;
    Vec3 mins, maxs;

    AILogLoading("Verifying BAI file version");
    file.ReadType(&iNumber);
    if (iNumber != BAI_TRI_FILE_VERSION)
    {
        AIError("CGraph::ReadNodes Wrong triangulation BAI file version - found %d expected %d: Regenerate triangulation in the editor [Design bug]", iNumber, BAI_TRI_FILE_VERSION);
        return false;
    }

    AILogLoading("Reading BBOX");
    file.ReadType(&mins);
    file.ReadType(&maxs);

    SetBBox(mins, maxs);

    AILogLoading("Reading node descriptors");

    file.ReadType(&iNumber);

    if (iNumber > 0)
    {
        nodeDescs.resize(iNumber);
        file.ReadType(&nodeDescs[0], iNumber);
    }

    AILogLoading("Verifying graph nodes");
    bool bFoundFirstNode = false;

    for (NodeDescBuffer::const_iterator ni = nodeDescs.begin(), end = nodeDescs.end(); ni != end; ++ni)
    {
        if (ni->ID == 1)
        {
            bFoundFirstNode = true;
            break;
        }
    }

    if (bFoundFirstNode)
    {
        // m_pSafeFirstIndex overlaps an ID we've read in. We need to disconnect m_pSafeFirst
        // before we create a node with the same ID to avoid accidentally reusing the ID.
        if (m_pSafeFirst)
        {
            Disconnect(m_safeFirstIndex);
        }
        m_pSafeFirst = NULL;
        m_safeFirstIndex = 0;
    }
    else
    {
        AIError("CGraph::ReadNodes First node not found - navigation loading failed [code bug]");
        gEnv->pLog->UpdateLoadingScreen(" ");
        return false;
    }

    AILogLoading("Creating graph nodes");

    typedef std::map< unsigned, unsigned > ReadNodesMap;
    ReadNodesMap mapReadNodes;

    I3DEngine* pEngine = gEnv->p3DEngine;
    NodeDescBuffer::iterator ni;
    int index = 0;

    if (/*!gEnv->IsEditor() && */ iNumber)
    {
        //int nodeCounts[IAISystem::NAV_TYPE_COUNT] = {0};

        NodeDescBuffer::iterator end = nodeDescs.end();
        //for (ni=nodeDescs.begin();ni != end;++ni,++index)
        //{
        //  NodeDescriptor desc = (*ni);
        //  IAISystem::ENavigationType type = (IAISystem::ENavigationType) desc.navType;

        //  int typeIndex = TypeIndexFromType(type);

        //  // in editor waypoint nodes get added by the editor
        //  // MichaelS - removed check to improve stats consistency.
        //  if (typeIndex < 0 ||
        //      (/*type != IAISystem::NAV_TRIANGULAR || */type != IAISystem::NAV_UNSET && gEnv->IsEditor()))
        //      continue;

        //  ++nodeCounts[typeIndex];
        //}

        for (ni = nodeDescs.begin(); ni != end; ++ni, ++index)
        {
            NodeDescriptor desc = (*ni);
            IAISystem::ENavigationType type = (IAISystem::ENavigationType) desc.navType;

            unsigned int nodeIndex = 0;
            GraphNode* pNode = 0;
            nodeIndex = CreateNewNode(type, desc.pos, desc.ID);
            pNode = GetNodeManager().GetNode(nodeIndex);

            switch (type)
            {
            case IAISystem::NAV_UNSET:
                break;
            case IAISystem::NAV_TRIANGULAR:
            {
                // NAV_TRIANGULAR is replaced by MNM
                assert(false);
            }
            break;
            case IAISystem::NAV_WAYPOINT_3DSURFACE:
            case IAISystem::NAV_WAYPOINT_HUMAN:
            {
                pNode->GetWaypointNavData()->type = (EWaypointNodeType) desc.type;

                // building id isn't preserved
                pNode->GetWaypointNavData()->nBuildingID = -1;
                pNode->GetWaypointNavData()->pArea = 0;
                SpecialArea::EType areaType = pNode->navType == IAISystem::NAV_WAYPOINT_HUMAN ? SpecialArea::TYPE_WAYPOINT_HUMAN : SpecialArea::TYPE_WAYPOINT_3DSURFACE;
                const SpecialArea* sa = gAIEnv.pNavigation->GetSpecialArea(pNode->GetPos(), areaType);
                if (sa)
                {
                    pNode->GetWaypointNavData()->nBuildingID = sa->nBuildingID;
                    I3DEngine* p3dEngine = gEnv->p3DEngine;
                    pNode->GetWaypointNavData()->pArea = p3dEngine->GetVisAreaFromPos(pNode->GetPos());
                }

                if (desc.type == WNT_ENTRYEXIT)
                {
                    m_mapEntrances.insert(EntranceMap::iterator::value_type(pNode->GetWaypointNavData()->nBuildingID, nodeIndex));
                }

                if (desc.type == WNT_ENTRYEXIT || desc.type == WNT_EXITONLY)
                {
                    m_mapExits.insert(EntranceMap::iterator::value_type(pNode->GetWaypointNavData()->nBuildingID, nodeIndex));
                }

                // NOTE Oct 15, 2009: <pvl> removable nodes have been unused for
                // a very long time now, removing support altogether
                assert (!desc.bRemovable);

                pNode->GetWaypointNavData()->dir = desc.dir;
                pNode->GetWaypointNavData()->up = desc.up;
            }
            break;
            case IAISystem::NAV_FLIGHT:
                AIAssert(!"flight nav should be loaded separately");
                pNode->GetFlightNavData()->nSpanIdx = desc.index;
                break;
            case IAISystem::NAV_VOLUME:
                AIAssert(!"volume nav should be loaded separately");
                pNode->GetVolumeNavData()->nVolimeIdx = desc.index;
                break;
            case IAISystem::NAV_ROAD:
                AIAssert(!"road nav should be loaded separately");
                pNode->GetRoadNavData()->fRoadWidth = desc.dir.x;
                /*pNode->GetRoadNavData()->fRoadOffset = desc.dir.y;*/
                break;
            case IAISystem::NAV_SMARTOBJECT:
                AIAssert(!"smart object nav should be loaded separately");
                break;
            default:
                AIError("CGraph::ReadNodes Unhandled nav type %d", pNode->navType);
            }
            mapReadNodes.insert(ReadNodesMap::value_type(desc.ID, nodeIndex));
        }
    }

    AILogLoading("Reading links");

    LinkDescBuffer linkDescs;

    file.ReadType(&iNumber);
    if (iNumber > 0)
    {
        linkDescs.resize(iNumber);
        file.ReadType(&linkDescs[0], iNumber);
    }

    ReadNodesMap::iterator ei, link;

    ei = mapReadNodes.find(1);
    AIAssert(ei != mapReadNodes.end());
    m_safeFirstIndex = (ei->second);
    m_pSafeFirst = GetNodeManager().GetNode(m_safeFirstIndex);
    m_pFirst = m_pSafeFirst;
    m_firstIndex = m_safeFirstIndex;
    m_pCurrent = m_pSafeFirst;
    m_currentIndex = m_safeFirstIndex;

    float fStartTime = gEnv->pTimer->GetAsyncCurTime();
    AILogLoading("Reconnecting links");

    if (!linkDescs.empty())
    {
        LinkDescBuffer::iterator iend = linkDescs.end();
        for (LinkDescBuffer::iterator ldbi = linkDescs.begin(); ldbi != iend; ++ldbi)
        {
            LinkDescriptor& ldb = (*ldbi);

            ei = mapReadNodes.find((unsigned)ldb.nSourceNode);
            if (ei == mapReadNodes.end())
            {
#if defined __GNUC__
                AIWarning("NodeId %lld not found in node map", (long long)ldb.nSourceNode);
#else
                AIWarning("NodeId %I64d not found in node map", (int64)ldb.nSourceNode);
#endif
            }
            else
            {
                unsigned nodeIndex = ei->second;
                GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
                link = mapReadNodes.find((unsigned int)ldb.nTargetNode);
                if (link == mapReadNodes.end() || ei == mapReadNodes.end())
                {
                    AIError("CGraph::ReadNodes Read a link to a node which could not be found [Code bug]");
                }
                else
                {
                    if (pNode != m_pSafeFirst && link->second != m_safeFirstIndex)
                    {
                        unsigned linkOneTwo = 0;
                        unsigned linkTwoOne = 0;
                        // incoming link gets set when the other way is read
                        Connect(nodeIndex, link->second, ldb.fMaxPassRadius, 0.0f, &linkOneTwo, &linkTwoOne);

                        if (linkOneTwo)
                        {
                            unsigned nextNodeIndex = link->second;
                            GraphNode* pNextNode = GetNodeManager().GetNode(nextNodeIndex);
                            AIAssert(GetLinkManager().GetNextNode(linkOneTwo) == nextNodeIndex);

                            float radius = ldb.fMaxPassRadius;

                            // Marcio: Hax for Crysis 2 Critters
                            // ... but only if the link is passable - hence condition (radius > 0.f) [2/1/2011 evgeny]
                            if ((radius > 0.f) && (pNode->navType == IAISystem::NAV_WAYPOINT_HUMAN) && (pNextNode->navType == IAISystem::NAV_WAYPOINT_HUMAN))
                            {
                                const SpecialArea* sa1 = gAIEnv.pNavigation->GetSpecialArea(pNode->GetWaypointNavData()->nBuildingID);
                                if (sa1->bCritterOnly)
                                {
                                    radius = 0.150001f;
                                }
                                else
                                {
                                    const SpecialArea* sa2 = gAIEnv.pNavigation->GetSpecialArea(pNextNode->GetWaypointNavData()->nBuildingID);
                                    if (sa2->bCritterOnly)
                                    {
                                        radius = 0.150001f;
                                    }
                                }
                            }

                            if (GetLinkManager().GetNextNode(linkOneTwo) == link->second)
                            {
                                GetLinkManager().SetRadius(linkOneTwo, radius);
                                GetLinkManager().SetStartIndex(linkOneTwo, ldb.nStartIndex);
                                GetLinkManager().SetEndIndex(linkOneTwo, ldb.nEndIndex);
                                GetLinkManager().GetEdgeCenter(linkOneTwo) = ldb.vEdgeCenter; // TODO: Don't read shared value twice
                                GetLinkManager().SetExposure(linkOneTwo, ldb.fExposure); // TODO: Don't read shared value twice
                                GetLinkManager().SetMaxWaterDepth(linkOneTwo, ldb.fMaxWaterDepth);
                                GetLinkManager().SetMinWaterDepth(linkOneTwo, ldb.fMinWaterDepth);
                                GetLinkManager().SetSimple(linkOneTwo, ldb.bSimplePassabilityCheck);
                            }
                            if (linkTwoOne)
                            {
                                GetLinkManager().RestoreLink(linkTwoOne);
                            }
                        }
                    }
                }
            }
        }
    }

    AILogLoading("Finished Reconnecting links in %6.3f sec",
        gEnv->pTimer->GetAsyncCurTime() - fStartTime);

    mapReadNodes.clear();
    //AIAssert(_heapchk()==_HEAPOK);

    return true;
}

//====================================================================
// SetBBox
// defines bounding rectangle of this graph
//====================================================================
void CGraph::SetBBox(const Vec3& min, const Vec3& max)
{
    m_triangularBBox.min = min;
    m_triangularBBox.max = max;
}

//====================================================================
// InsideOfBBox
//====================================================================
bool CGraph::InsideOfBBox(const Vec3& pos) const
{
    return pos.x > m_triangularBBox.min.x && pos.x < m_triangularBBox.max.x && pos.y > m_triangularBBox.min.y && pos.y < m_triangularBBox.max.y;
}

unsigned CGraph::CreateNewNode(IAISystem::tNavCapMask type, const Vec3& pos, unsigned ID)
{
    unsigned nodeIndex = m_pGraphNodeManager->CreateNode(type, pos, ID);
    GraphNode* pNode = m_pGraphNodeManager->GetNode(nodeIndex);
    pNode->AddRef();
    m_allNodes.AddNode(nodeIndex);

    if (type != IAISystem::NAV_UNSET)
    {
        CNavRegion* pNavRegion = gAIEnv.pNavigation->GetNavRegion(pNode->navType, this);
        if (pNavRegion)
        {
            pNavRegion->NodeCreated(nodeIndex);
        }
    }

    return nodeIndex;
}

//====================================================================
// GetNode
//====================================================================
GraphNode* CGraph::GetNode(unsigned index)
{
    return GetNodeManager().GetNode(index);
}

const GraphNode* CGraph::GetNode(unsigned index) const
{
    return GetNodeManager().GetNode(index);
}


//====================================================================
// MoveNode
//====================================================================
void CGraph::MoveNode(unsigned nodeIndex, const Vec3& newPos)
{
    GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);
    if (pNode->GetPos().IsEquivalent(newPos))
    {
        return;
    }
    m_allNodes.RemoveNode(nodeIndex);
    pNode->SetPos(newPos);
    m_allNodes.AddNode(nodeIndex);

    CNavRegion* pNavRegion = gAIEnv.pNavigation->GetNavRegion(pNode->navType, this);
    if (pNavRegion)
    {
        pNavRegion->NodeMoved(nodeIndex);
    }
}

struct SNodeFinder
{
    SNodeFinder(unsigned nodeIndex)
        : nodeIndex(nodeIndex) {}
    bool operator()(const EntranceMap::value_type& val) const {return val.second == nodeIndex; }
    unsigned nodeIndex;
};

//====================================================================
// DeleteNode
//====================================================================
void CGraph::DeleteNode(unsigned nodeIndex)
{
    GraphNode* pNode = GetNodeManager().GetNode(nodeIndex);

    if (!CGraph::ValidateNode(nodeIndex, false))
    {
        AIError("CGraph::DeleteNode Attempting to delete node that doesn't exist %p [Code bug]", pNode);
        return;
    }

    if (pNode->firstLinkIndex)
    {
        AIWarning("Deleting node %p but it is still connected - disconnecting", pNode);
        Disconnect(nodeIndex, false);
    }

    if (pNode->Release())
    {
        CNavRegion* pNavRegion = gAIEnv.pNavigation ? gAIEnv.pNavigation->GetNavRegion(pNode->navType, this) : NULL;
        if (pNavRegion)
        {
            pNavRegion->NodeAboutToBeDeleted(pNode);
        }

        m_allNodes.RemoveNode(nodeIndex);

        VectorConstNodeIndices::iterator it;
        it = std::remove(m_taggedNodes.begin(), m_taggedNodes.end(), nodeIndex);
        m_taggedNodes.erase(it, m_taggedNodes.end());
        it = std::remove(m_markedNodes.begin(), m_markedNodes.end(), nodeIndex);
        m_markedNodes.erase(it, m_markedNodes.end());

        EntranceMap::iterator entranceIt;
        entranceIt = std::find_if(m_mapEntrances.begin(), m_mapEntrances.end(), SNodeFinder(nodeIndex));
        if (entranceIt != m_mapEntrances.end())
        {
            m_mapEntrances.erase(entranceIt);
        }
        entranceIt = std::find_if(m_mapExits.begin(), m_mapExits.end(), SNodeFinder(nodeIndex));
        if (entranceIt != m_mapExits.end())
        {
            m_mapExits.erase(entranceIt);
        }

        m_pGraphNodeManager->DestroyNode(nodeIndex);
    }
}


struct SVolumeHideSpotFinder
{
    SVolumeHideSpotFinder(ListObstacles& obs)
        : obs(obs) {}

    void operator()(const SVolumeHideSpot& hs, float)
    {
        obs.push_back(ObstacleData(hs.pos, hs.dir));
    }

private:
    ListObstacles& obs;
};


//===================================================================
// FindNodesWithinRange
//===================================================================
void CGraph::FindNodesWithinRange(MapConstNodesDistance& result, float curDistT, float maxDist,
    const GraphNode* pStartNode, float passRadius, const class CAIObject* pRequester) const
{
    FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

    bool checkWaterDepth = false;
    int16 minWaterDepthInCm = 0;
    int16 maxWaterDepthInCm = 0;

    if (pRequester && pRequester->CastToCAIActor())
    {
        const CAIActor* pActor = pRequester->CastToCAIActor();
        if (pActor)
        {
            minWaterDepthInCm = NavGraphUtils::InCentimeters(pActor->m_movementAbility.pathfindingProperties.minWaterDepth);
            maxWaterDepthInCm = NavGraphUtils::InCentimeters(pActor->m_movementAbility.pathfindingProperties.maxWaterDepth);
            checkWaterDepth = true;
        }
    }

    const CGraphLinkManager& linkManager = GetLinkManager();
    const CGraphNodeManager& nodeManager = GetNodeManager();

    typedef std::multimap<float, const GraphNode*, std::less<float> > OpenListMap;

    static OpenListMap openList;
    openList.clear();

    pStartNode->mark = 1;
    openList.insert(std::make_pair(0.0f, pStartNode));
    //  openList[0.0f] = pStartNode;

    result[pStartNode] = 0.0f;

    while (!openList.empty())
    {
        OpenListMap::iterator front = openList.begin();
        const GraphNode* pNode = front->second;
        float curDist = front->first;
        openList.erase(front);
        pNode->mark = 0;

        for (unsigned link = pNode->firstLinkIndex; link; link = linkManager.GetNextLink(link))
        {
            float fCostMultiplier = 1.0f;
            unsigned int nextNodeIndex = linkManager.GetNextNode(link);
            const GraphNode* pNext = nodeManager.GetNode(nextNodeIndex);

            if (!(pNext->navType & (IAISystem::NAV_SMARTOBJECT | IAISystem::NAV_WAYPOINT_HUMAN | IAISystem::NAV_WAYPOINT_3DSURFACE | IAISystem::NAV_TRIANGULAR | IAISystem::NAV_VOLUME)))
            {
                continue;
            }

            if (pRequester && pNode->navType == IAISystem::NAV_SMARTOBJECT && pNext->navType == IAISystem::NAV_SMARTOBJECT)
            {
                const GraphNode* nodes[2] = {pNode, pNext};
                float resFactor = gAIEnv.pSmartObjectManager->GetSmartObjectLinkCostFactor(nodes, pRequester, &fCostMultiplier);
                if (resFactor < 0.0f)
                {
                    continue;
                }
            }
            else if (linkManager.GetRadius(link) < passRadius)
            {
                continue;
            }
            else if (checkWaterDepth)
            {
                int16 wd = linkManager.GetMaxWaterDepthInCm(link);
                if (wd > maxWaterDepthInCm)
                {
                    continue;
                }

                if (wd < minWaterDepthInCm)
                {
                    continue;
                }
            }

            float linkLen = 0.01f + fCostMultiplier * Distance::Point_Point(pNode->GetPos(), pNext->GetPos()); // bias to prevent loops

            float totalDist = curDist + linkLen;
            if (totalDist <= maxDist)
            {
                // If we've already processed pNext and had a lower range value before then don't continue
                MapConstNodesDistance::iterator it = result.find(pNext);
                if (it != result.end())
                {
                    if (totalDist >= it->second)
                    {
                        continue;
                    }
                    // Smaller distance, update value.
                    it->second = totalDist;
                }
                else
                {
                    result[pNext] = totalDist;
                }

                // Update open list.
                OpenListMap::iterator found = openList.end();
                if (pNext->mark == 1)
                {
                    for (OpenListMap::iterator it2 = openList.begin(), end = openList.end(); it2 != end; ++it2)
                    {
                        if (it2->second == pNext)
                        {
                            found = it2;
                            break;
                        }
                    }
                }

                if (found != openList.end())
                {
                    // Replace value in open list.
                    if (totalDist < found->first)
                    {
                        openList.erase(found);
                        openList.insert(std::make_pair(totalDist, pNext));
                    }
                }
                else
                {
                    // Add to the open list
                    pNext->mark = 1;
                    openList.insert(std::make_pair(totalDist, pNext));
                }
            }
        }
    }
    openList.clear();
}


//====================================================================
// GetNodesInRange
//====================================================================
MapConstNodesDistance& CGraph::GetNodesInRange(MapConstNodesDistance& result, const Vec3& startPos,
    float maxDist, IAISystem::tNavCapMask navCapMask, float passRadius,
    unsigned startNodeIndex, const class CAIObject* pRequester)
{
    FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
    result.clear();

    GraphNode* pStart = GetNodeManager().GetNode(startNodeIndex);

    const CAllNodesContainer& allNodes = GetAllNodes();
    const CAllNodesContainer::Iterator it(allNodes, IAISystem::NAV_TRIANGULAR | IAISystem::NAV_VOLUME);
    if (!it.GetNode())
    {
        return result; // no navigation
    }
    unsigned nodeIndex = 0;
    if (pStart && !allNodes.DoesNodeExist(startNodeIndex))
    {
        startNodeIndex = 0;
        pStart = 0;
    }

    if (pStart)
    {
        if ((pStart->navType & navCapMask) && startPos.IsEquivalent(pStart->GetPos(), 0.01f))
        {
            nodeIndex = startNodeIndex;
        }
    }

    if (!nodeIndex)
    {
        return result;
    }

    // don't add the distance to the current node since that
    float curDist = 0.0f; //Distance::Point_Point(startPos, pNode->GetPos());
    FindNodesWithinRange(result, curDist, maxDist, GetNodeManager().GetNode(nodeIndex), passRadius, pRequester);
    return result;
}

//====================================================================
// Reset
//====================================================================
void CGraph::Reset()
{
    ClearMarks();
    RestoreAllNavigation();
}

void CGraph::ResetIDs()
{
    GraphNode::ResetIDs(GetNodeManager(), m_allNodes, m_pSafeFirst);
}

//====================================================================
// GetEntrance
//====================================================================
GraphNode* CGraph::GetEntrance(int nBuildingID, const Vec3& pos)
{
    GraphNode* pEntrance = 0;
    float mindist = 1000000;
    EntranceMap::iterator ei = m_mapEntrances.find(nBuildingID);
    if (ei != m_mapEntrances.end())
    {
        pEntrance = GetNodeManager().GetNode(ei->second);
        if (m_mapEntrances.count(nBuildingID) > 1)
        {
            mindist = (pEntrance->GetPos() - pos).GetLengthSquared();
            for (; ei != m_mapEntrances.end(); ++ei)
            {
                if (ei->first != nBuildingID)
                {
                    break;
                }
                float curr_dist = (GetNodeManager().GetNode(ei->second)->GetPos() - pos).GetLengthSquared();
                if (curr_dist <= mindist)
                {
                    pEntrance = GetNodeManager().GetNode(ei->second);
                    mindist = curr_dist;
                }
            }
        }
    }

    ei = m_mapExits.find(nBuildingID);
    if (ei != m_mapExits.end())
    {
        pEntrance = GetNodeManager().GetNode(ei->second);
        if (m_mapExits.count(nBuildingID) > 1)
        {
            mindist = (pEntrance->GetPos() - pos).GetLengthSquared();
            for (; ei != m_mapExits.end(); ++ei)
            {
                if (ei->first != nBuildingID)
                {
                    break;
                }
                float curr_dist = (GetNodeManager().GetNode(ei->second)->GetPos() - pos).GetLengthSquared();
                if (curr_dist <= mindist)
                {
                    pEntrance = GetNodeManager().GetNode(ei->second);
                    mindist = curr_dist;
                }
            }
        }
    }


    return pEntrance;
}

//====================================================================
// GetEntrances
//====================================================================
bool CGraph::GetEntrances(int nBuildingID, const Vec3& pos, std::vector<unsigned>& nodes)
{
    EntranceMap::iterator it1 = m_mapEntrances.lower_bound(nBuildingID);
    EntranceMap::iterator it2 = m_mapEntrances.upper_bound(nBuildingID);
    for (EntranceMap::iterator it = it1; it != it2; ++it)
    {
        nodes.push_back(it->second);
    }
    it1 = m_mapExits.lower_bound(nBuildingID);
    it2 = m_mapExits.upper_bound(nBuildingID);
    for (EntranceMap::iterator it = it1; it != it2; ++it)
    {
        nodes.push_back(it->second);
    }
    return !nodes.empty();
}

//====================================================================
// RestoreAllNavigation
// for all removable nodes restore links passibility etc
//====================================================================
void CGraph::RestoreAllNavigation()
{
    CAllNodesContainer::Iterator it(m_allNodes, IAISystem::NAVMASK_ALL);
    while (GraphNode* node = GetNodeManager().GetNode(it.Increment()))
    {
        for (unsigned link = node->firstLinkIndex; link; link = GetLinkManager().GetNextLink(link))
        {
            GetLinkManager().RestoreLink(link);
        }
    }
}

//====================================================================
// CheckForEmpty
//====================================================================
bool CGraph::CheckForEmpty(IAISystem::tNavCapMask navTypeMask) const
{
    unsigned count = 0;
    CAllNodesContainer::Iterator it(m_allNodes, navTypeMask);
    while (const GraphNode* node = GetNodeManager().GetNode(it.Increment()))
    {
        ++count;
        AILogEvent("Unexpected Node %p, type = %d, pos = (%5.2f %5.2f %5.2f)", node, node->navType,
            node->GetPos().x, node->GetPos().y, node->GetPos().z);
    }
    if (count)
    {
        AIWarning("Detected %d unexpected nodes whilst checking types %u", count, (unsigned )navTypeMask);
    }
    return (count == 0);
}

//====================================================================
// Validate
//====================================================================
bool CGraph::Validate(const char* msg, bool checkPassable) const
{
#ifdef _DEBUG
    return true;
#endif

#ifdef CRYAISYSTEM_VERBOSITY
    if (!AIGetWarningErrorsEnabled())
    {
        AILogEvent("CGraph::Validate Skipping: %s", msg);
        return true;
    }

    AILogProgress("CGraph::Validate Starting: %s", msg);

    // Danny todo tweak this so that if !checkPassable then we only clear
    // errors that are not to do with the passable checks...
    if (checkPassable)
    {
        mBadGraphData.clear();
    }

    if (!m_pFirst)
    {
        return false;
    }

    if (!m_pSafeFirst->firstLinkIndex)
    {
        return true;
    }

    // the first safe node is different - expect only one link
    if (GetLinkManager().GetNextLink(m_pSafeFirst->firstLinkIndex) != 0)
    {
        AIWarning("CGraph::Validate Expect only 1 link to the first safe node");
        return false;
    }

    int badOutdoorNodes = 0;
    int badIndoorNodes = 0;
    int badLinks = 0;
    int badNumOutsideLinks = 0;
    int badLinkPassability = 0;

    static int maxBadImassabilityWarnings = 10;

    // nodes containing vertices with identical indices
    int indexDegenerates = 0;
    // nodes containing vertices that have different indices, but essentially
    // the same position.
    int posDegenerates = 0;

    CAllNodesContainer::Iterator it(m_allNodes, IAISystem::NAVMASK_ALL);
    while (unsigned nodeIndex = it.Increment())
    {
        const GraphNode* node = GetNodeManager().GetNode(nodeIndex);
        if (!CGraph::ValidateNode(nodeIndex, true))
        {
            AIWarning("CGraph::Validate Node validation failed: %p", node);
        }

        if (node->navType == IAISystem::NAV_WAYPOINT_HUMAN)
        {
            if (node->GetWaypointNavData()->nBuildingID == -1)
            {
                AIWarning("Node at (%5.2f, %5.2f, %5.2f) is not in a human waypoint nav modifier [design bug]",
                    node->GetPos().x, node->GetPos().y, node->GetPos().z);
                ++badIndoorNodes;
            }
        }
        else if (node->navType == IAISystem::NAV_WAYPOINT_3DSURFACE)
        {
            if (node->GetWaypointNavData()->nBuildingID == -1)
            {
                AIWarning("Node at (%5.2f, %5.2f, %5.2f) is not in a 3d surface nav modifier [design bug]",
                    node->GetPos().x, node->GetPos().y, node->GetPos().z);
                ++badIndoorNodes;
            }
        }
        else if (node->navType == IAISystem::NAV_TRIANGULAR)
        {
            // NAV_TRIANGULAR is replaced by MNM
            assert(false);
        }
    }

    if (badOutdoorNodes + badIndoorNodes + badNumOutsideLinks + indexDegenerates + posDegenerates + badLinkPassability > 0)
    {
        AIError("CGraph::Validate AI graph contains invalid: %d outdoor, %d indoor, %d outdoor links, %d degenerate index, %d degenerate pos, %d passable: %s: Regenerate triangulation in editor [Design bug]",
            badOutdoorNodes, badIndoorNodes, badNumOutsideLinks, indexDegenerates, posDegenerates, badLinkPassability, msg);
    }

    AILogProgress("CGraph::Validate Finished graph validation: %s", msg);
    return (0 == badOutdoorNodes + badIndoorNodes);
#else
    return true;
#endif
}

//===================================================================
// ResetIDs
//===================================================================
void GraphNode::ResetIDs(CGraphNodeManager& nodeManager, class CAllNodesContainer& allNodes, GraphNode* pNodeForID1)
{
    AIAssert(pNodeForID1);

    CAllNodesContainer::Iterator itAll(allNodes, 0xffffffff);

    freeIDs.clear();
#ifdef DEBUG_GRAPHNODE_IDS
    usedIds.clear();
#endif

    maxID = 1;
    while (GraphNode* pCurrent = nodeManager.GetNode(itAll.Increment()))
    {
        if (pCurrent == pNodeForID1)
        {
            pCurrent->ID = 1;
        }
        else
        {
            pCurrent->ID = ++maxID;
        }

#ifdef DEBUG_GRAPHNODE_IDS
        usedIds.push_back(pCurrent->ID);
#endif
    }
}