#include "../Fusion/RuFusion_Trekker.h"

#include "../Core/DataStore/RuDataStore_Disk.h"

#pragma unmanaged

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

ruRTTI_DEFINE_SUBCLASS(CRuFusion_Trekker, IRuFusion_Trekker_Interface)
ruHEAP_DEFINE_SUBCLASS(CRuFusion_Trekker, "CRuFusion_Trekker", "IRuFusion_Trekker_Interface")
ruCLASSGUID_DEFINE(CRuFusion_Trekker, 0x4A4DA205, 0x3232483a, 0xA974E971, 0x9333854B)

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

CRuFusion_Trekker::CRuFusion_Trekker(const char *terrainPath)
:	m_defaultNodeSearchRadius(80.0f),
	m_defaultLocationSearchRange(20.0f),
	m_pathMaximumExpansion(1024),
	m_runTime_PathFinder(NULL)
{
	// Setup internal path finder
	m_runTime_PathFinder = ruNEW CRuPathFinder_AStar();
	m_runTime_PathFinder->SetMaximumExpansions(m_pathMaximumExpansion);

	// Open terrain data store
	CRuDataStore_Disk *mapFileDataStore = ruNEW CRuDataStore_Disk();
	mapFileDataStore->Open(terrainPath, TRUE);

	// Create terrain data source
	CRuOptimalTerrain_DataSource_DS *terrainDS = ruNEW CRuOptimalTerrain_DataSource_DS();

	// Enable collision-only mode
	terrainDS->SetCollisionOnlyMode(TRUE);

	if(terrainDS->Open(mapFileDataStore))
	{
		// Create new terrain using new data source
		m_terrain = ruNEW CRuOptimalTerrain();
		m_terrain->Initialize();
		m_terrain->AttachDataSource(terrainDS, TRUE);
		m_terrain->SetChunkQueueSize(1024);

		// Get GAIA object from the terrain
		m_gaiaObject = m_terrain->GetGaiaObject();

		// Get path hierarchy from GAIA
		m_pathHierarchy = m_gaiaObject->GetPathHierarchy();

		// Set path finder for the path hierarchy
		m_pathHierarchy->SetPathFinder(m_runTime_PathFinder);

		if(m_pathHierarchy->GetType() == CRuPathHierarchy_DS::Type)
		{
			static_cast<CRuPathHierarchy_DS *>(m_pathHierarchy)->SetPathGraphQueueSize(1024);
		}
	}
}

CRuFusion_Trekker::CRuFusion_Trekker(CRuOptimalTerrain *terrain)
{
	// Setup internal path finder
	m_runTime_PathFinder = ruNEW CRuPathFinder_AStar();
	m_runTime_PathFinder->SetMaximumExpansions(m_pathMaximumExpansion);

	// Store reference to terrain
	m_terrain = terrain;
	m_terrain->AddRef();

	// Get GAIA object from the terrain
	m_gaiaObject = m_terrain->GetGaiaObject();

	// Get path hierarchy from GAIA
	m_pathHierarchy = m_gaiaObject->GetPathHierarchy();

	// Set path finder for the path hierarchy
	m_pathHierarchy->SetPathFinder(m_runTime_PathFinder);
}

CRuFusion_Trekker::~CRuFusion_Trekker()
{
	// Release terrain object
	ruSAFE_RELEASE(m_terrain);

	// Release path finder
	ruSAFE_RELEASE(m_runTime_PathFinder);
}

BOOL CRuFusion_Trekker::TestPathValidity(const CRuVector3 &origin, const CRuVector3 &destination)
{
	// Setup collision query
	m_runtime_SegQuery.m_colSegment = CRuSegment3(origin, destination);		// Setup collision segment
	m_runtime_SegQuery.m_exclusionMask = 0x00A00000;						// Exclude water and foliage surfaces
	m_runtime_SegQuery.m_positiveHit = FALSE;								// Reset result

	// Query collision
	m_terrain->QueryCollision(&m_runtime_SegQuery);

	return (m_runtime_SegQuery.m_positiveHit == FALSE);
}

BOOL CRuFusion_Trekker::TestRestingValidity(const CRuVector3 &origin)
{
	return TRUE;
}

BOOL CRuFusion_Trekker::TestRestingHeight(const CRuVector3 &origin, REAL testDistance, REAL &restingHeightOut)
{
	// Setup collision query
	m_runtime_NearestSegQuery.m_colSegment = CRuSegment3(origin, CRuVector3(0.0f, -testDistance, 0.0f));		// Setup collision segment
	m_runtime_NearestSegQuery.m_exclusionMask = 0x00A00000;														// Exclude water and foliage surfaces
	m_runtime_NearestSegQuery.m_positiveHit = FALSE;															// Reset result
	m_runtime_NearestSegQuery.m_collisionT = FLT_MAX;

	// Query collision
	m_terrain->QueryCollision(&m_runtime_NearestSegQuery);

	// Output collision y
	restingHeightOut = m_runtime_NearestSegQuery.m_collisionPoint.m_y;

	return (m_runtime_NearestSegQuery.m_positiveHit == TRUE);
}

BOOL CRuFusion_Trekker::SetPathMaximumExpansion(INT32 pathMaximumExpansion)
{
	m_pathMaximumExpansion = pathMaximumExpansion;
	m_runTime_PathFinder->SetMaximumExpansions(m_pathMaximumExpansion);

	return TRUE;
}

BOOL CRuFusion_Trekker::FindNearestNodeKey(const CRuVector3 &position, REAL radius, INT32 &nodeKeyOut)
{
	CRuSphere boundingSphere = CRuSphere(position, radius);

	// Enumerate keys for path graphs that intersect the specified sphere
	m_runTime_PathGraphKeys.Clear();
	m_pathHierarchy->EnumeratePathGraphKeys(boundingSphere, m_runTime_PathGraphKeys);

	REAL nearestPathNodeSqrDistance = FLT_MAX;

	for(INT32 i = 0; i < m_runTime_PathGraphKeys.Count(); ++i)
	{
		// Get the path graph from the path graph hierarchy by its graph key
		IRuPathGraph_Base *pathGraph = m_pathHierarchy->GetPathGraph(m_runTime_PathGraphKeys[i]);

		// Enumerate keys for path nodes that intersect the specified sphere
		m_runTime_PathNodeKeys.Clear();
		pathGraph->EnumeratePathNodes(boundingSphere, m_runTime_PathNodeKeys);

		// Find nearest unobstructed path node
		for(INT32 j = 0; j < m_runTime_PathNodeKeys.Count(); ++j)
		{
			// Get path node from the path graph
			INT32 nodeID = RuPathEngine_NodeIDFromNodeKey(m_runTime_PathNodeKeys[j]);
			const CRuPathNode *pathNode = pathGraph->GetPathNode(nodeID);

			// TODO: Test for obstruction

			// If the path node is closest at the moment, store it
			REAL sqrDistance = (pathNode->Position() - position).SquaredMagnitude();
			if(sqrDistance <= nearestPathNodeSqrDistance)
			{
				nearestPathNodeSqrDistance = sqrDistance;
				nodeKeyOut = RuPathEngine_MakeNodeKey(pathGraph->GetGraphKey(), nodeID);
			}
		}
	}

	if(nearestPathNodeSqrDistance < FLT_MAX)
		return TRUE;

	return FALSE;
}

BOOL CRuFusion_Trekker::FindPath(INT32 originNodeKey, INT32 destinationNodeKey, CRuArrayList<CRuVector3> *pathPositionOut, CRuArrayList<INT32> *pathNodeKeysOut)
{
	if(originNodeKey != 0xFFFFFFFF && destinationNodeKey != 0xFFFFFFFF)
	{
		// Clear output arrays
		pathPositionOut->Clear();
		pathNodeKeysOut->Clear();

		// Perform path query
		if(m_pathHierarchy->FindPath(originNodeKey, destinationNodeKey, *pathNodeKeysOut))
		{
			for(INT32 i = 0; i < pathNodeKeysOut->Count(); ++i)
			{
				INT32 graphKey = RuPathEngine_GraphKeyFromNodeKey((*pathNodeKeysOut)[i]);
				INT32 nodeID = RuPathEngine_NodeIDFromNodeKey((*pathNodeKeysOut)[i]);

				// Retrieve the path graph
				IRuPathGraph_Base *pathGraph = m_pathHierarchy->GetPathGraph(graphKey);
				if(pathGraph == NULL)
					return FALSE;

				// Retrieve the path node
				const CRuPathNode *pathNode = pathGraph->GetPathNode(nodeID);
				if(pathNode == NULL)
					return FALSE;

				// Append path node position to the output path position array
				pathPositionOut->Add(pathNode->Position());
			}

			return TRUE;
		}
	}

	return FALSE;
}

BOOL CRuFusion_Trekker::FindPath(const CRuVector3 &origin, const CRuVector3 &destination, CRuArrayList<CRuVector3> *pathPositionOut, CRuArrayList<INT32> *pathNodeKeysOut)
{
	INT32 originNodeKey = 0xFFFFFFFF;
	INT32 destinationNodeKey = 0xFFFFFFFF;

	if(FindNearestNodeKey(origin, m_defaultNodeSearchRadius, originNodeKey) == FALSE)
		return FALSE;

	if(FindNearestNodeKey(destination, m_defaultNodeSearchRadius, destinationNodeKey) == FALSE)
		return FALSE;

	return FindPath(originNodeKey, destinationNodeKey, pathPositionOut, pathNodeKeysOut);
}

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

#pragma managed
