/*!
	@project Rune
	@file RuPathHierarchy_DS.cpp

	Copyright (c) 2004-2005 All rights reserved

	@author John Tang
	@date 2005/12/27
*/

#include "../PathEngine/RuPathHierarchy_DS.h"

#pragma unmanaged

// ************************************************************************************************************************************************************

ruRTTI_DEFINE_SUBCLASS(CRuPathHierarchy_DS, IRuPathHierarchy_Base)
ruHEAP_DEFINE_SUBCLASS(CRuPathHierarchy_DS, "CRuPathHierarchy_DS", "IRuPathHierarchy_Base")
ruCLASSGUID_DEFINE(CRuPathHierarchy_DS, 0x843AE4AA, 0x72864d80, 0x8A0DC1BB, 0xAEB8FAAA)

// ************************************************************************************************************************************************************

static const char *pathDSInternal_TableName_Header = "pathds_header";
static const char *pathDSInternal_TableName_GraphKey = "pathds_graphkeys";
static const char *pathDSInternal_TableName_Graph = "pathds_graphs";

// ************************************************************************************************************************************************************

CRuPathHierarchy_DS::CRuPathHierarchy_DS()
:	m_dataStore(NULL),
	m_graphKeyPartition(NULL),
	m_graphQueue(NULL),
	m_pathFinder(NULL),
	m_pathRover(NULL)
{
	// Initialize utility objects
	m_streamCompressor = ruNEW CRuStreamCompressor();
	m_compressedStream = ruNEW CRuMemoryStream();
	m_decompressedStream = ruNEW CRuMemoryStream();
}

CRuPathHierarchy_DS::~CRuPathHierarchy_DS()
{
	// Clear graph queue
	ClearGraphQueue();

	// Clear uncommitted graphlist
	for(INT32 i = 0; i < m_uncommittedGraphs.Count(); ++i)
	{
		ruSAFE_RELEASE(m_uncommittedGraphs[i]);
	}
	m_uncommittedGraphs.Clear();

	// Release graph descriptors
	for(INT32 i = 0; i < m_graphDescriptors.Count(); ++i)
	{
		delete m_graphDescriptors[i];
	}

	// Release graph nodes
	for(INT32 i = 0; i < m_graphNodes.Count(); ++i)
	{
		delete m_graphNodes[i];
	}

	// Release data store
	ruSAFE_RELEASE(m_dataStore);

	// Delete key partition
	delete m_graphKeyPartition;

	// Delete graph map
	delete m_graphDescriptorHash;

	// Delete graph queue
	delete m_graphQueue;

	// Delete utility objects
	delete m_streamCompressor;
	delete m_compressedStream;
	delete m_decompressedStream;

	ruSAFE_RELEASE(m_pathRover);
	ruSAFE_RELEASE(m_pathFinder);
}

BOOL CRuPathHierarchy_DS::Bind(CRuDataStore_Base *dataStore, const CRuAABB &bounds)
{
	// Construct the graph key map
	m_graphDescriptorHash = ruNEW CRuHashMap<INT32, PathGraphDesc *>(8192);

	// Create LRU queue for path graphs
	m_graphQueue = ruNEW CRuLRUQueue<INT32, IRuPathGraph_Base *>(256, 1024);

	// Save reference to data store
	m_dataStore = dataStore;
	m_dataStore->AddRef();

	// Get data table handles
	m_tableHandle_Header = m_dataStore->GetTableHandle(pathDSInternal_TableName_Header);
	m_tableHandle_PathGraphKey = m_dataStore->GetTableHandle(pathDSInternal_TableName_GraphKey);
	m_tableHandle_PathGraph = m_dataStore->GetTableHandle(pathDSInternal_TableName_Graph);

	// Create data tables if they do not exist
	if(m_tableHandle_Header < 0)
	{
		m_tableHandle_Header = m_dataStore->CreateTable(pathDSInternal_TableName_Header, 4);
	}
	if(m_tableHandle_PathGraphKey < 0)
	{
		m_tableHandle_PathGraphKey = m_dataStore->CreateTable(pathDSInternal_TableName_GraphKey, 4);
	}
	if(m_tableHandle_PathGraph < 0)
	{
		m_tableHandle_PathGraph = m_dataStore->CreateTable(pathDSInternal_TableName_Graph, 4);
	}

	// Construct the graph key partition
	m_graphKeyPartition = ruNEW CRuPathGraphKeyPartition(bounds);

	// Read data source header
	if(m_tableHandle_Header >= 0)
	{
		INT32 dsHeaderKey = 0;
		IRuStream *stream = m_dataStore->Retrieve(m_tableHandle_Header, (BYTE *) &dsHeaderKey);

		if(stream)
		{
			// Decompress header data block
			m_streamCompressor->Decompress(stream, 0, stream->GetStreamSize(), m_decompressedStream);
			delete stream;

			// Read DS version
			INT32 dsVersion = 1;
			m_decompressedStream->Read(&dsVersion, sizeof(INT32));

			// Load graph descriptors
			INT32 numGraphDescriptors = 0;
			m_decompressedStream->Read(&numGraphDescriptors, sizeof(INT32));
			for(INT32 i = 0; i < numGraphDescriptors; ++i)
			{
				// Read key
				INT32 graphKey;
				m_decompressedStream->Read(&graphKey, sizeof(INT32));
				DWORD keyHash = RuGlobalHashGen().GetHashCode(graphKey);

				// Create path graph descriptor
				PathGraphDesc *pathGraphDesc = ruNEW PathGraphDesc();
				pathGraphDesc->m_graphKey = graphKey;

				// Read number of neighbors
				INT32 numNeighbors = 0;
				m_decompressedStream->Read(&numNeighbors, sizeof(INT32));
				pathGraphDesc->m_neighbors.SetArraySize(numNeighbors);
				pathGraphDesc->m_neighbors.SetNumEntries(numNeighbors);

				// Read neighbors
				for(INT32 j = 0; j < numNeighbors; ++j)
				{
					m_decompressedStream->Read(&pathGraphDesc->m_neighbors[j], sizeof(INT32));
				}

				// Insert path graph descriptor into the map & list
				m_graphDescriptors.Add(pathGraphDesc);
				m_graphDescriptorHash->Add(graphKey, keyHash, pathGraphDesc);
			}

			// Load graph nodes
			INT32 numGraphNodes = 0;
			m_decompressedStream->Read(&numGraphNodes, sizeof(INT32));
			m_graphNodes.SetArraySize(numGraphNodes);
			m_graphNodes.SetNumEntries(numGraphNodes);
			for(INT32 i = 0; i < numGraphNodes; ++i)
			{
				m_graphNodes[i] = ruNEW CRuPathNode();
				m_graphNodes[i]->SerializeFrom(m_decompressedStream);
			}
		}
	}

	// Read path graph key descriptors
	if(m_tableHandle_PathGraphKey >= 0)
	{
		// Read graph keys into graph key partition
		INT32 numGraphs = m_dataStore->GetNumRecords(m_tableHandle_PathGraphKey);
		for(INT32 i = 0; i < numGraphs; ++i)
		{
			// Retrieve graph key record
			IRuStream *stream = m_dataStore->RetrieveByIndex(m_tableHandle_PathGraphKey, i);

			if(stream == NULL)
				continue;

			// Read graph key from stream
			INT32 graphKey;
			stream->Read(&graphKey, sizeof(INT32));

			// Read graph bounds from stream
			CRuAABB graphBounds;
			stream->Read(&graphBounds.Minima().x, sizeof(REAL));
			stream->Read(&graphBounds.Minima().y, sizeof(REAL));
			stream->Read(&graphBounds.Minima().z, sizeof(REAL));
			stream->Read(&graphBounds.Maxima().x, sizeof(REAL));
			stream->Read(&graphBounds.Maxima().y, sizeof(REAL));
			stream->Read(&graphBounds.Maxima().z, sizeof(REAL));

			// Release stream
			delete stream;

			// Insert graph key into graph key partition
			m_graphKeyPartition->InsertPathGraphKey(graphKey, graphBounds);
		}
	}

	return TRUE;
}

BOOL CRuPathHierarchy_DS::CopyTo(CRuDataStore_Base *targetDataStore)
{
	INT32 srcTableList[3];
	srcTableList[0] = m_tableHandle_Header;
	srcTableList[1] = m_tableHandle_PathGraphKey;
	srcTableList[2] = m_tableHandle_PathGraph;

	// Create tables
	INT32 targetTableList[3];
	targetTableList[0] = targetDataStore->CreateTable(pathDSInternal_TableName_Header, 4);
	targetTableList[1] = targetDataStore->CreateTable(pathDSInternal_TableName_GraphKey, 4);
	targetTableList[2] = targetDataStore->CreateTable(pathDSInternal_TableName_Graph, 4);

	// Copy data in each table
	for(INT32 i = 0; i < 3; ++i)
	{
		INT32 numEntries = m_dataStore->GetNumRecords(srcTableList[i]);
		INT32 keyLength = m_dataStore->GetTableKeyLength(srcTableList[i]);
		BYTE *key = ruNEW BYTE [keyLength];

		for(INT32 j = 0; j < numEntries; ++j)
		{
			// Retrieve key and data
			IRuStream *stream = m_dataStore->RetrieveByIndex(srcTableList[i], j);

			if(stream)
			{
				m_dataStore->RetrieveKeyByIndex(srcTableList[i], j, key);
				targetDataStore->Store(targetTableList[i], key, stream);
				delete stream;
			}
		}

		delete[] key;
	}

	return TRUE;
}

BOOL CRuPathHierarchy_DS::Save()
{
	// Stream DS header
	m_decompressedStream->SetStreamSize(0);

	// Write DS version
	INT32 dsVersion = 1;
	m_decompressedStream->Write(&dsVersion, sizeof(INT32));

	// Save graph descriptors
	INT32 numGraphDescriptors = m_graphDescriptors.Count();
	m_decompressedStream->Write(&numGraphDescriptors, sizeof(INT32));
	for(INT32 i = 0; i < numGraphDescriptors; ++i)
	{
		PathGraphDesc *pathGraphDesc = m_graphDescriptors[i];

		// Calculate key and key hash
		INT32 graphKey = pathGraphDesc->m_graphKey;
		DWORD keyHash = RuGlobalHashGen().GetHashCode(graphKey);

		// Write key
		m_decompressedStream->Write(&graphKey, sizeof(INT32));

		// Write number of neighbors
		INT32 numNeighbors = pathGraphDesc->m_neighbors.Count();
		m_decompressedStream->Write(&numNeighbors, sizeof(INT32));

		// Write neighbors
		for(INT32 j = 0; j < numNeighbors; ++j)
		{
			m_decompressedStream->Write(&pathGraphDesc->m_neighbors[j], sizeof(INT32));
		}
	}

	// Save graph nodes
	INT32 numGraphNodes = m_graphNodes.Count();
	m_decompressedStream->Write(&numGraphNodes, sizeof(INT32));
	for(INT32 i = 0; i < numGraphNodes; ++i)
	{
		m_graphNodes[i]->SerializeTo(m_decompressedStream);
	}

	// Compress
	m_compressedStream->SetStreamSize(0);
	m_streamCompressor->Compress(m_decompressedStream, m_compressedStream);

	// Store to data store
	INT32 dsHeaderKey = 0;
	m_dataStore->Store(m_tableHandle_Header, (BYTE *) &dsHeaderKey, m_compressedStream);

	// Store uncommitted graphs to data store
	for(INT32 i = 0; i < m_uncommittedGraphs.Count(); ++i)
	{
		INT32 graphKey = m_uncommittedGraphs[i]->GetGraphKey();
		DWORD keyHash = RuGlobalHashGen().GetHashCode(graphKey);

		// Stream graph header
		m_decompressedStream->SetStreamSize(0);

		// Write graph key
		m_decompressedStream->Write(&graphKey, sizeof(INT32));

		// Write graph bounds
		m_decompressedStream->Write(&m_uncommittedGraphs[i]->Bounds().Minima().x, sizeof(REAL));
		m_decompressedStream->Write(&m_uncommittedGraphs[i]->Bounds().Minima().y, sizeof(REAL));
		m_decompressedStream->Write(&m_uncommittedGraphs[i]->Bounds().Minima().z, sizeof(REAL));
		m_decompressedStream->Write(&m_uncommittedGraphs[i]->Bounds().Maxima().x, sizeof(REAL));
		m_decompressedStream->Write(&m_uncommittedGraphs[i]->Bounds().Maxima().y, sizeof(REAL));
		m_decompressedStream->Write(&m_uncommittedGraphs[i]->Bounds().Maxima().z, sizeof(REAL));

		// Store to data store
		m_dataStore->Store(m_tableHandle_PathGraphKey, (BYTE *) &graphKey, m_decompressedStream);

		// Stream graph
		m_decompressedStream->SetStreamSize(0);
		m_uncommittedGraphs[i]->SerializeTo(m_decompressedStream);

		// Compress
		m_compressedStream->SetStreamSize(0);
		m_streamCompressor->Compress(m_decompressedStream, m_compressedStream);

		// Store to data store
		m_dataStore->Store(m_tableHandle_PathGraph, (BYTE *) &graphKey, m_compressedStream);

		// Release reference
		ruSAFE_RELEASE(m_uncommittedGraphs[i]);
	}

	// Clear list of uncommitted graphs
	m_uncommittedGraphs.Clear();

	// Remove deleted graphs

	return TRUE;
}

BOOL CRuPathHierarchy_DS::SetPathGraphQueueSize(INT32 pathGraphQueueSize)
{
	// Flush all path graphs
	ClearGraphQueue();

	// Destroy and re-create LRU queue for path graphs
	delete m_graphQueue;
	m_graphQueue = ruNEW CRuLRUQueue<INT32, IRuPathGraph_Base *>(pathGraphQueueSize, pathGraphQueueSize * 4);

	return TRUE;
}

BOOL CRuPathHierarchy_DS::SetPathFinder(IRuPathFinder_Base *pathFinder)
{
	if(pathFinder)
	{
		// Increment reference on the new path finder
		pathFinder->AddRef();

		// Release old path finder and store reference to new path finder
		ruSAFE_RELEASE(m_pathFinder);
		m_pathFinder = pathFinder;

		return TRUE;
	}

	return FALSE;
}

BOOL CRuPathHierarchy_DS::SetPathRover(IRuPathRover_Base *pathRover)
{
	pathRover->AddRef();
	ruSAFE_RELEASE(m_pathRover);
	m_pathRover = pathRover;

	return TRUE;
}

BOOL CRuPathHierarchy_DS::InsertPathGraph(IRuPathGraph_Base *pathGraph)
{
	// Remove path graph from graph key partition
	m_graphKeyPartition->DeletePathGraphKey(pathGraph->GetGraphKey());

	// Remove path graph from graph queue
	DWORD keyHash = RuGlobalHashGen().GetHashCode(pathGraph->GetGraphKey());
	m_graphQueue->Remove(pathGraph->GetGraphKey(), keyHash);

	// Insert path graph into graph key partition
	m_graphKeyPartition->InsertPathGraphKey(pathGraph->GetGraphKey(), pathGraph->Bounds());

	// Override path finder setting
	pathGraph->SetPathFinder(m_pathFinder);

	// If the uncommitted graph list already has a path graph with the same key, overwrite it
	BOOL graphInserted = FALSE;
	for(INT32 i = 0; i < m_uncommittedGraphs.Count(); ++i)
	{
		if(pathGraph->GetGraphKey() == m_uncommittedGraphs[i]->GetGraphKey())
		{
			// Replace path graph
			ruSAFE_RELEASE(m_uncommittedGraphs[i]);
			m_uncommittedGraphs[i] = pathGraph;
			m_uncommittedGraphs[i]->AddRef();

			// Break
			graphInserted = TRUE;
			break;
		}
	}

	// Otherwise append the path graph to the uncommitted graph list
	if(graphInserted == FALSE)
	{
		pathGraph->AddRef();
		m_uncommittedGraphs.Add(pathGraph);
	}

	// Get path graph descriptor, or create one if it does not already exist
	PathGraphDesc *pathGraphDesc = NULL;
	if(m_graphDescriptorHash->Get(pathGraph->GetGraphKey(), keyHash, pathGraphDesc) == FALSE)
	{
		// Create path graph descriptor
		pathGraphDesc = ruNEW PathGraphDesc();
		pathGraphDesc->m_graphKey = pathGraph->GetGraphKey();

		// Insert path graph descriptor into the map & list
		m_graphDescriptors.Add(pathGraphDesc);
		m_graphDescriptorHash->Add(pathGraph->GetGraphKey(), keyHash, pathGraphDesc);

		// Create matching path node
		CRuVector3 pathGraphCenter = (pathGraph->Bounds().Maxima() + pathGraph->Bounds().Minima()) / 2.0f;
		CRuPathNode *pathGraphNode = ruNEW CRuPathNode(pathGraphCenter);
		m_graphNodes.Add(pathGraphNode);
	}

	// Re-link dirty path graphs (at this point, all graphs linked to the inserted graph are considered dirty)
	m_dirtyGraphKeys.CloneFrom(pathGraphDesc->m_neighbors);
	m_dirtyGraphKeys.Add(pathGraph->GetGraphKey());

	for(INT32 i = 0; i < m_dirtyGraphKeys.Count(); ++i)
	{
		PathGraphDesc *dirtyPathGraphDesc = NULL;
		if(m_graphDescriptorHash->Get(m_dirtyGraphKeys[i], RuGlobalHashGen().GetHashCode(m_dirtyGraphKeys[i]), dirtyPathGraphDesc))
		{
			// Clear old neighbors
			dirtyPathGraphDesc->m_neighbors.Clear();

			// Rebuild neighbors list
			BuildNeighborsList(m_dirtyGraphKeys[i]);
		}
	}

	return TRUE;
}

BOOL CRuPathHierarchy_DS::DeletePathGraph(IRuPathGraph_Base *pathGraph)
{
	return TRUE;
}

IRuPathGraph_Base *CRuPathHierarchy_DS::GetPathGraph(INT32 graphKey)
{
	IRuPathGraph_Base *pathGraph = NULL;
	DWORD graphKeyHash = RuGlobalHashGen().GetHashCode(graphKey);

	// Attempt to locate the specified graph in the graph LRU queue
	if(m_graphQueue->Get(graphKey, graphKeyHash, pathGraph))
	{
		return pathGraph;
	}

	// Attempt to locate the specified graph in the uncommitted graph list
	for(INT32 i = 0; i < m_uncommittedGraphs.Count(); ++i)
	{
		if(m_uncommittedGraphs[i]->GetGraphKey() == graphKey)
		{
			pathGraph = m_uncommittedGraphs[i];
			pathGraph->AddRef();
		}
	}

	// Attempt to locate the specified graph in the data store
	if(pathGraph == NULL)
	{
		IRuStream *stream = m_dataStore->Retrieve(m_tableHandle_PathGraph, (BYTE *) &graphKey);

		if(stream)
		{
			// Decompress chunk data block
			m_streamCompressor->Decompress(stream, 0, stream->GetStreamSize(), m_decompressedStream);
			delete stream;

			IRuObject *pathGraphObject = g_ruObjectFactory.CreateObject(m_decompressedStream);

			if(pathGraphObject && pathGraphObject->GetType().IsSubClassOf(IRuPathGraph_Base::Type))
			{
				pathGraph = static_cast<IRuPathGraph_Base *>(pathGraphObject);
				pathGraph->SetPathFinder(m_pathFinder);
			}
		}
	}

	if(pathGraph)
	{
		// Free up space in the graph queue
		IRuPathGraph_Base *reclaimedPathGraph;
		while(m_graphQueue->Reclaim(graphKey, graphKeyHash, 1, reclaimedPathGraph))
		{
			// Release instanced path graph
			ruSAFE_RELEASE(reclaimedPathGraph);
		}

		// Insert graph into graph queue
		m_graphQueue->Add(graphKey, graphKeyHash, pathGraph, 1);
	}

	return pathGraph;
}

BOOL CRuPathHierarchy_DS::EnumeratePathGraphKeys(const CRuSphere &bounds, CRuArrayList<INT32> &pathGraphKeysOut)
{
	m_graphKeyPartition->Enumerate(bounds, pathGraphKeysOut);

	return TRUE;
}

BOOL CRuPathHierarchy_DS::FindPath(INT32 startNodeKey, INT32 endNodeKey, CRuArrayList<INT32> &pathOut)
{
	INT32 startGraphKey = RuPathEngine_GraphKeyFromNodeKey(startNodeKey);
	INT32 endGraphKey = RuPathEngine_GraphKeyFromNodeKey(endNodeKey);

	if(m_pathFinder == NULL)
	{
		m_pathFinder = ruNEW CRuPathFinder_AStar();
	}

	// temp
	if(startGraphKey == endGraphKey)
	{
		INT32 startNodeID = RuPathEngine_NodeIDFromNodeKey(startNodeKey);
		INT32 endNodeID = RuPathEngine_NodeIDFromNodeKey(endNodeKey);

		IRuPathGraph_Base *pathGraph = GetPathGraph(startGraphKey);

		// Sanity check
		if(pathGraph == NULL)
			return FALSE;

		if(pathGraph->FindPath(startNodeID, endNodeID, pathOut))
		{
			// Append path graph key to the path list
			for(INT32 i = 0; i < pathOut.Count(); ++i)
			{
				pathOut[i] = RuPathEngine_MakeNodeKey(startGraphKey, pathOut[i]);
			}

			return TRUE;
		}
	}
	else
	{
		// Find start and end graph indices
		INT32 startGraphID = -1;
		INT32 endGraphID = -1;

		for(INT32 i = 0; i < m_graphDescriptors.Count(); ++i)
		{
			if(m_graphDescriptors[i]->m_graphKey == startGraphKey)
			{
				startGraphID = i;
			}
			else if(m_graphDescriptors[i]->m_graphKey == endGraphKey)
			{
				endGraphID = i;
			}
		}

		if(startGraphID >= 0 && endGraphID >= 0)
		{
			// Top-level path finding - find path between blocks
			CRuArrayList<INT32> graphPath;
			CRuArrayList<INT32> subPath;
			if(m_pathFinder->FindPath(m_graphNodes, startGraphID, endGraphID, graphPath))
			{
				// Iterate through each graph in the path
				for(INT32 i = 0; i < graphPath.Count(); ++i)
				{
					IRuPathGraph_Base *graph0 = GetPathGraph(m_graphDescriptors[graphPath[i]]->m_graphKey);
					IRuPathGraph_Base *graph1 = (i + 1 >= graphPath.Count()) ? NULL : GetPathGraph(m_graphDescriptors[graphPath[i + 1]]->m_graphKey);

					INT32 startNodeID = 0;
					INT32 endNodeID0 = 0;
					INT32 endNodeID1 = 0;

					if(i == 0)
					{
						// Sanity check
						if(graph0 == NULL || graph1 == NULL)
							return FALSE;

						// Start node ID is the specified node
						startNodeID = RuPathEngine_NodeIDFromNodeKey(startNodeKey);

						// End node is the closest linked node to the next graph
						INT32 srcNodeKey, dstNodeKey;
						if(FindClosestLinkNode(graph0, graph1, startNodeID, srcNodeKey, dstNodeKey) == FALSE)
						{
						}

						endNodeID0 = RuPathEngine_NodeIDFromNodeKey(srcNodeKey);
						endNodeID1 = RuPathEngine_NodeIDFromNodeKey(dstNodeKey);
					}
					else if(i == graphPath.Count() - 1)
					{
						// Start node ID is the last node on the current path
						startNodeID = RuPathEngine_NodeIDFromNodeKey(pathOut[pathOut.Count() - 1]);

						endNodeID0 = RuPathEngine_NodeIDFromNodeKey(endNodeKey);
						endNodeID1 = endNodeID0;
					}
					else
					{
						// Sanity check
						if(graph0 == NULL || graph1 == NULL)
							return FALSE;

						// Start node ID is the last node on the current path
						startNodeID = RuPathEngine_NodeIDFromNodeKey(pathOut[pathOut.Count() - 1]);

						// End node is the closest linked node to the next graph
						INT32 srcNodeKey, dstNodeKey;
						if(FindClosestLinkNode(graph0, graph1, startNodeID, srcNodeKey, dstNodeKey) == FALSE)
						{
						}

						endNodeID0 = RuPathEngine_NodeIDFromNodeKey(srcNodeKey);
						endNodeID1 = RuPathEngine_NodeIDFromNodeKey(dstNodeKey);
					}

					// Find sub path for the current graph
					subPath.Clear();
					if(graph0->FindPath(startNodeID, endNodeID0, subPath) == FALSE)
					{
						return FALSE;
					}

					// Convert node ID to node key
					for(INT32 j = 0; j < subPath.Count(); ++j)
					{
						subPath[j] = RuPathEngine_MakeNodeKey(graph0->GetGraphKey(), subPath[j]);
					}

					// Append the last end node (the beginning of the next graph) to the output path
					if(graph1)
					{
						subPath.Add(RuPathEngine_MakeNodeKey(graph1->GetGraphKey(), endNodeID1));
					}

					// Append path graph key to the path list
					for(INT32 j = 0; j < subPath.Count(); ++j)
					{
						if(pathOut.Count() > 0 && subPath[j] == pathOut[pathOut.Count() - 1])
						{
							continue;
						}

						pathOut.Add(subPath[j]);
					}
				}

				return TRUE;
			}
		}
	}

	return FALSE;
}

BOOL CRuPathHierarchy_DS::BuildNeighborsList(INT32 graphKey)
{
	// Instantiate graph
	IRuPathGraph_Base *pathGraph = GetPathGraph(graphKey);
	if(pathGraph == NULL)
		return FALSE;

	// Retrieve the graph descriptor
	PathGraphDesc *pathGraphDesc = NULL;
	DWORD keyHash = RuGlobalHashGen().GetHashCode(pathGraph->GetGraphKey());
	if(m_graphDescriptorHash->Get(pathGraph->GetGraphKey(), keyHash, pathGraphDesc) == FALSE)
		return FALSE;

	// Calculate graph centroid based on its bounding box
	CRuVector3 graphCenter = (pathGraph->Bounds().Maxima() + pathGraph->Bounds().Maxima()) / 2.0f;

	// Add reference so that the path graph does not get deleted accidentally
	pathGraph->AddRef();

	// Iterate through all path node links in the graph
	for(INT32 i = 0; i < pathGraph->GetNumPathNodeLinks(); ++i)
	{
		INT32 nodeID;
		RuPathNodeLinkType linkType;
		REAL linkDistance;
		INT32 linkData;

		if(pathGraph->GetPathNodeLink(i, nodeID, linkType, linkDistance, linkData))
		{
			switch(linkType)
			{
				case ruPATHNODELINKTYPE_FIXED:
					{
						// Determine graph key at neighbor from the link data
						INT32 neighborGraphKey = RuPathEngine_GraphKeyFromNodeKey(linkData);

						// Determine whether the neighbor is already in the neighbors list
						BOOL duplicate = FALSE;
						for(INT32 j = 0; j < pathGraphDesc->m_neighbors.Count(); ++j)
						{
							if(pathGraphDesc->m_neighbors[j] == neighborGraphKey)
							{
								duplicate = TRUE;
								break;
							}
						}

						// If the neighbor is not in the neighbors list, append it to the list
						if(!duplicate)
						{
							pathGraphDesc->m_neighbors.Add(neighborGraphKey);
						}

						// TODO: Must set up the neighbor's connection too, so add it to the dirty list.. or something
						BOOL alreadyProcessedAsDirty = FALSE;
						for(INT32 j = 0; j < m_dirtyGraphKeys.Count(); ++j)
						{
							if(m_dirtyGraphKeys[j] == neighborGraphKey)
							{
								alreadyProcessedAsDirty = TRUE;
								break;
							}
						}

						if(alreadyProcessedAsDirty == FALSE)
						{
							m_dirtyGraphKeys.Add(neighborGraphKey);
						}
					}

					break;
			}
		}
	}

	// TODO: NOT DONE!
	// Enumerate nearby graphs
	CRuSphere neighborhoodBounds((pathGraph->Bounds().Maxima() + pathGraph->Bounds().Minima()) / 2.0f, max(max(pathGraph->Bounds().GetXSize(), pathGraph->Bounds().GetYSize()), pathGraph->Bounds().GetZSize()));
	CRuArrayList<INT32> potentialNeighbors;
	EnumeratePathGraphKeys(neighborhoodBounds, potentialNeighbors);

	// Scan through nearby graphs and hook up relocatable links
	for(INT32 i = 0; i < potentialNeighbors.Count(); ++i)
	{
	}

	// Find the path graph's index
	INT32 pathGraphIndex = -1;
	for(INT32 i = 0; i < m_graphDescriptors.Count(); ++i)
	{
		if(m_graphDescriptors[i]->m_graphKey == graphKey)
		{
			pathGraphIndex = i;
			break;
		}
	}

	// Clear all neighbors
	m_graphNodes[pathGraphIndex]->ClearAllNeighbors();

	// Rebuild neighbor list
	for(INT32 i = 0; i < pathGraphDesc->m_neighbors.Count(); ++i)
	{
		// Find index for the neighbor graph
		INT32 neighborGraphIndex = -1;
		for(INT32 j = 0; j < m_graphDescriptors.Count(); ++j)
		{
			if(m_graphDescriptors[j]->m_graphKey == pathGraphDesc->m_neighbors[i])
			{
				neighborGraphIndex = j;
				break;
			}
		}

		if(neighborGraphIndex >= 0)
		{
			IRuPathGraph_Base *neighborGraph = GetPathGraph(pathGraphDesc->m_neighbors[i]);
			CRuVector3 neighborCenter = (neighborGraph->Bounds().Maxima() + neighborGraph->Bounds().Maxima()) / 2.0f;
			REAL cost = (graphCenter - neighborCenter).Magnitude();

			m_graphNodes[pathGraphIndex]->InsertNeighbor(neighborGraphIndex, cost);
		}
	}

	return TRUE;
}

BOOL CRuPathHierarchy_DS::BuildGraphNodes()
{
	return TRUE;
}

BOOL CRuPathHierarchy_DS::FindClosestLinkNode(IRuPathGraph_Base *graph0, IRuPathGraph_Base *graph1, INT32 startNodeID, INT32 &srcNodeKeyOut, INT32 &dstNodeKeyOut)
{
	REAL closestDistance = FLT_MAX;
	CRuVector3 startNodePosition = graph0->GetPathNode(startNodeID)->Position();

	// Iterate through all path node links in graph0
	for(INT32 i = 0; i < graph0->GetNumPathNodeLinks(); ++i)
	{
		INT32 nodeID;
		RuPathNodeLinkType linkType;
		REAL linkDistance;
		INT32 linkData;

		if(graph0->GetPathNodeLink(i, nodeID, linkType, linkDistance, linkData))
		{
			switch(linkType)
			{
				case ruPATHNODELINKTYPE_FIXED:
					{
						// Determine graph key and node ID at neighbor from the link data
						INT32 neighborGraphKey = RuPathEngine_GraphKeyFromNodeKey(linkData);
						INT32 neighborNodeID = RuPathEngine_NodeIDFromNodeKey(linkData);

						// If the destination of the link is graph1, compare against existing closest link
						if(neighborGraphKey == graph1->GetGraphKey())
						{
							// Calculate distance to node
							REAL distance = (startNodePosition - graph1->GetPathNode(neighborNodeID)->Position()).Magnitude();

							// If the distance to node is shorter than the existing link, use this node instead
							if(distance < closestDistance)
							{
								closestDistance = distance;
								srcNodeKeyOut = RuPathEngine_MakeNodeKey(graph0->GetGraphKey(), nodeID);
								dstNodeKeyOut = linkData;
							}
						}
					}

					break;
			}
		}
	}

	// Return true if a link node was found
	return (closestDistance < FLT_MAX) ? TRUE : FALSE;
}

BOOL CRuPathHierarchy_DS::ClearGraphQueue()
{
	if(m_graphQueue)
	{
		m_graphQueue->ForAllElements(ClearGraphQueueCallback, this);
		m_graphQueue->Clear();
	}

	return TRUE;
}

BOOL CRuPathHierarchy_DS::ClearGraphQueueCallback(const void *key, const void *data, void *userData)
{
	IRuPathGraph_Base *graph = *((IRuPathGraph_Base **) data);
	ruSAFE_RELEASE(graph);

	return TRUE;
}

// ************************************************************************************************************************************************************

#pragma managed
