/*
	Lan Wars.
	Copyright (c) 2009, Darragh Coy.
	All rights reserved.
	Original work(s), source code & author(s) details available at: http://www.darraghcoy.com

	Redistribution and use in source and binary forms, with or without modification, are permitted 
	provided that the following conditions are met:

		* 	Redistributions of source code must retain the above copyright notice, this list of 
			conditions and the following disclaimer.
		* 	Redistributions in binary form must reproduce the above copyright notice, this list of 
			conditions and the following disclaimer in the documentation.
		* 	Neither the name of the author nor the names of contributors may be used to endorse or 
			promote products derived from this software without specific prior written permission.
		* 	The use of any third party libraries, modules or data files provided with the source 
			form of the program must follow and respect the individual licensing terms of these 
			components. You do not have permission to modify or circumvent the licensing terms for 
			provided third party components unless the third party licenses specifically allow this. 
			If you do not agree to the terms of provided third party components then you do not have 
			permission to use them in your derived work(s).
		* 	The terms of this license do not apply to separate (but related) distributions if supplied 
			by the author(s), only to the contents of this source distribution, derived works, and the 
			binaries produced by either modified or unmodified versions of the source code. The terms 
			of this license do not also apply to any third party libraries or components provided with 
			the source distribution. Such third party elements are licensed under their own separate 
			agreements.

	THIS SOFTWARE IS PROVIDED BY DARRAGH COY ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 
	BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
	ARE DISCLAIMED. IN NO EVENT SHALL DARRAGH COY BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
	EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 
	OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY 
	OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
	IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

//=====================================================================================================================
/// @brief		This class builds a list of inter-connected nodes for the level that can be used for A* path
///				finding. It also maintains which areas of the map are connected to which nodes which can be used 
///				to tell the AI what part of the network it is on.
//=====================================================================================================================

#include "Precompiled.h"
#include "AI/PathNetwork.h"
#include "AI/BuildPathNetwork.h"
#include "AI/BuildPathNode.h"
#include "AI/BuildPathSubsector.h"
#include "Doom/DoomLevel.h"
#include "Doom/DoomPPSeg.h"
#include "Doom/DoomGLSubsectorV2.h"
#include "Graphics/Camera3D.h"
#include "Graphics/BufferedLineRenderer.h"
#include "Game/GameCore.h"
#include "Game/GameSession.h"

using namespace Game;

//=====================================================================================================================
/// @brief		Creates the path network
/// @param		level - level the path network is for
/// @pre		GameCore should exist
/// @pre		GameCore should have a session object
/// @pre		This object should only be created on the server
//=====================================================================================================================

PathNetwork::PathNetwork( const DoomLevel & level ) : m_level(&level)
{
	// Make sure ok

	ENSURE( GameCore::GetSingleton()							, "GameCore should exist"								)
	ENSURE( GameCore::GetSingleton()->GetSession()				, "GameCore should have a session object"				)
	ENSURE( GameCore::GetSingleton()->GetSession()->IsServer()	, "This object should only be created on the server"	)

	// Initialise these:
	
	m_num_nodes					= 0;
	m_num_subsectors			= 0;
	m_subsector_nodes			= 0;
	m_nodes						= 0;
	m_examined_nodes			= 0;
	m_num_examined_nodes		= 0;
	m_generated_path			= 0;
	m_num_generated_path_nodes	= 0;

	// Build the path network using this helper object:
	
	BuildPathNetwork * net = new BuildPathNetwork(level);
	
	// Create the path network from this helper object
	
	CreatePathNetwork(net);
	
	// Done with the helper object
	
	delete net;
}

//=====================================================================================================================
/// @brief		Destroys the path network
/// @pre		The number of examined nodes by the A* algorithm should be 0 at this stage
//=====================================================================================================================

PathNetwork::~PathNetwork()
{
	// Cleanup:
	
	for ( unsigned i = 0 ; i < m_num_nodes ; ++i )
	{
		if ( m_nodes[i].m_num_connected_nodes > 0 )
		{	
			delete[] m_nodes[i].m_connected_nodes;
			delete[] m_nodes[i].m_connected_distances;
			delete[] m_nodes[i].m_connected_costs_extra;
		}
	}

	delete[] m_nodes;
	delete[] m_subsector_nodes;
	delete[] m_examined_nodes;
	delete[] m_generated_path;
	
	// Done with the heap
	
	m_open_list->RemoveReference();
	
	// Make sure this is ok:
	
	ENSURE( m_num_examined_nodes == 0 , "The number of examined nodes by the A* algorithm should be 0 at this stage" )
}

//=====================================================================================================================
/// @brief		Populates and creates our internal node list given the network builder object
/// @param		net - helper object that built the path network
/// @pre		The pathbuilder object cannot be null
/// @pre		The number of subectors in the level must match the number of subsectors in the node builder
//=====================================================================================================================

void PathNetwork::CreatePathNetwork( BuildPathNetwork * net )
{
	// Make sure ok
	
	ENSURE( net != 0 , "The pathbuilder object cannot be null" )
	
	// Check this:
	
	ENSURE
	( 
		net->GetSubsectors().size() == m_level->GetSubsectorCount() ,
		"The number of subectors in the level must match the number of subsectors in the node builder"
	)
	
	// Error check:
	
	if ( net->GetSubsectors().size() == 0 || net->GetNodes().size() == 0 )
	{
		Errors::IssueError("The loaded level has no geometry data. Cannot build pathfinding network.");
	}
	
	// Save the number of nodes and subsectors:
	
	m_num_nodes			= net->GetNodes().size();
	m_num_subsectors	= net->GetSubsectors().size();
	
	//-----------------------------------------------------------------------------------------------------------------
	// Make our array of nodes:
	//-----------------------------------------------------------------------------------------------------------------
	
	m_nodes = new Node[ m_num_nodes ];
	
	// Populate the array of nodes:
	
	for ( unsigned i = 0 ; i < m_num_nodes ; ++i )
	{
		// Get this node:
		
		Node & node = m_nodes[i];
		
		// Set it's position:
		
		node.m_position = net->GetNodes()[i]->GetPosition();
		
		// Not examined yet by the A* algorithm
		
		node.m_examined = false;
		
		// See if the node is connected to anything:
		
		if ( net->GetNodes()[i]->GetConnectedNodes().size() == 0 )
		{
			// Node not connected to anything:
		
			node.m_num_connected_nodes  = 0;
			node.m_connected_nodes		= 0;			
		}
		else
		{
			// Node is connected: allocate an array to hold which node numbers it is connected to
			
			node.m_num_connected_nodes = net->GetNodes()[i]->GetConnectedNodes().size();
			
			// Do the alloc:
			
			node.m_connected_nodes = new Node * [node.m_num_connected_nodes];
			
			// Alloc array to hold distances to each node also:
			
			node.m_connected_distances = new float[node.m_num_connected_nodes];

			// Alloc array for extra costs to each connected node:

			node.m_connected_costs_extra = new float[node.m_num_connected_nodes];
				
			// Now we need to set the node numbers for all the nodes it is connected to
			
			for ( unsigned j = 0 ; j < node.m_num_connected_nodes ; j++ )
			{
				// Get this connected node:
				
				BuildPathNode * connected_node = net->GetNodes()[i]->GetConnectedNodes()[j];
				
				// Save it:
				
				node.m_connected_nodes[j] = m_nodes + connected_node->GetNodeNumber();
				
				// Save the distance to this node:
				
				node.m_connected_distances[j] = connected_node->GetPosition().SqDistanceTo(node.m_position);

				// Set the extra costs to zero:

				node.m_connected_costs_extra[j] = 0;
			}
		}
	}
	
	//-----------------------------------------------------------------------------------------------------------------
	// Make the array of subsectors and which node number each subsector is connected to:
	//-----------------------------------------------------------------------------------------------------------------
	
	m_subsector_nodes = new Node * [ m_level->GetSubsectorCount() ];
	
	// Set which node is closest to which subsector
	
	for ( unsigned i = 0 ; i < m_num_subsectors ; ++i )
	{
		m_subsector_nodes[i] = m_nodes + net->GetSubsectors()[i]->GetClosestNode()->GetNodeNumber();
	}
	
	//-----------------------------------------------------------------------------------------------------------------
	// Debug only: build a list of subsectors that are closest to each node
	//-----------------------------------------------------------------------------------------------------------------
	
	#ifdef USE_DEBUG_CONSOLE
	
		m_node_subsectors.resize(m_num_nodes);
		
		// Make the list for each node:
		
		for ( unsigned i = 0 ; i < m_num_subsectors ; ++i )
		{
			// Calculate the node number that this subsector is assigned to:
			
			unsigned node_num = ( unsigned( m_subsector_nodes[i] ) - unsigned(m_nodes) ) / sizeof(Node);
		
			// Add to the list of subsectors for this node:
			
			m_node_subsectors[node_num].push_back(i);
		}
	
	#endif
	
	//-----------------------------------------------------------------------------------------------------------------
	// Information used during and after the A* algorithm
	//-----------------------------------------------------------------------------------------------------------------
	
	// These are the nodes examined by the last call to the A* algorithm: we save these so we can clear their pathfinding attributes afterwords
	
	m_examined_nodes = new Node * [m_num_nodes];
	
	// This should always be zero when the A* algorithm is not running
	
	m_num_examined_nodes = 0;
	
	// Create an array to hold the last generated path by the A* algorithm
	
	m_generated_path = new Node * [m_num_nodes];
	
	// Clear this initially
	
	m_num_generated_path_nodes = 0;
	
	// Create the open list:
	
	m_open_list = new Heap<AStarRatedNode>( m_num_nodes );
}

//=====================================================================================================================
/// @brief		Adds an extra cost to going through the selected position to get to somewhere.
///				This is used by the AI to implement surrounding team based behaviour.
///	@param		position - position to add the extra cost for moving through
/// @param		cost - extra cost to associate with this position.
//=====================================================================================================================

void PathNetwork::AddPathCost( const Vec3f & position , float cost )
{
	// Get the subsector of the point is in:
	
	const DoomGLSubsectorV2 * subsec = m_level->GetSubsector(position);
	
	// Make sure there:
	
	if ( subsec )
	{
		// Ok: get the subsector number
		
		unsigned subsec_num = m_level->GetSubsectorNumber(*subsec);
		
		// Now get the closest path node for this subsector
		
		Node * node = m_subsector_nodes[subsec_num];
		
		// Should never be the case:
		
		ENSURE( node , "Subsector marked as having no path node!")
		
		// Run through all connected nodes:

		for ( unsigned i = 0 ; i < node->m_num_connected_nodes ; ++i )
		{
			// Get this connected node

			Node & connected_node = * ( node->m_connected_nodes[i] );

			// See if there is a connection back to this node:

			for ( unsigned j = 0 ; j < connected_node.m_num_connected_nodes ; ++j )
			{
				// See if this runs back to the area we are in:

				if ( connected_node.m_connected_nodes[j] == node )
				{
					// Great: add an extra cost to travelling to our current node from this node

					connected_node.m_connected_costs_extra[j] += cost;

					// Done looking for the back connection:

					break;
				}
			}
		}
	}
}

//=====================================================================================================================
/// @brief		Performs the A* algorithm, searching for the best path between the two given nodes
/// @param		start - node to start from
/// @param		end - target goal
/// @pre		The number of examined nodes by the last call to the A* algorithm should be zero
/// @pre		The open list should be empty
/// @pre		The start and end node should be within the path network
//=====================================================================================================================

void PathNetwork::Pathfind( Node * start , Node * end )
{
	// Make sure ok:
	
	ENSURE( m_num_examined_nodes == 0 , "The number of examined nodes by the last call to the A* algorithm should be zero"	)
	ENSURE( m_open_list->GetSize() == 0 , "The open list should be empty"													)
	ENSURE( start >= m_nodes && start < m_nodes + m_num_nodes , "The start and end node should be within the path network"	)
	ENSURE( end   >= m_nodes && end   < m_nodes + m_num_nodes , "The start and end node should be within the path network"	)
	
	// Debug only: clear the previous pointer for all nodes:
	
	#ifdef DEBUG
	
		for ( unsigned i = 0 ; i < m_num_nodes ; ++i )
		{
			m_nodes[i].m_previous_path_node = (Node*)(0xffffffff);
			m_nodes[i].m_f_cost				= Float::NaN();
			m_nodes[i].m_g_cost				= Float::NaN();
			m_nodes[i].m_h_cost				= Float::NaN();
			m_examined_nodes[i]				= (Node*)(0xffffffff);
			m_generated_path[i]				= (Node*)(0xffffffff);
		}
	
	#endif
	
	//-----------------------------------------------------------------------------------------------------------------
	// Add the start node into the open list
	//-----------------------------------------------------------------------------------------------------------------
	
	// F , H and G cost for the starting node:
	
	start->m_g_cost = 0;
	start->m_h_cost = end->m_position.SqDistanceTo(start->m_position);
	start->m_f_cost = start->m_h_cost;
	
	// No previous node for the starting node of the path:
	
	start->m_previous_path_node = 0;
	
	// Examined the starting node
	
	start->m_examined		= true;
	start->m_in_open_list	= true;
	
	m_examined_nodes[0] = start; 
	m_num_examined_nodes++;
	
	// Add the starting node to the open list:
	
	m_open_list->Push( AStarRatedNode(start) );
	
	//-----------------------------------------------------------------------------------------------------------------
	// This is false if we have no path found:
	//-----------------------------------------------------------------------------------------------------------------
	
	bool found_path = false;
	
	//-----------------------------------------------------------------------------------------------------------------
	// Continue until a path is found, or until the open list is empty:
	//-----------------------------------------------------------------------------------------------------------------
	
	while ( m_open_list->GetSize() > 0 )
	{
		//-------------------------------------------------------------------------------------------------------------
		// Add lowest F score node to the closed list
		//-------------------------------------------------------------------------------------------------------------
	
		// Take the lowest cost node off the open list:
		
		Node * lowest_open_node = m_open_list->GetTop().m_node; m_open_list->Pop();
		
		// Make sure ok:
		
		ENSURE( lowest_open_node != 0 , "Null node in the open list" )
		
		// This should always be true:
		
		ENSURE( lowest_open_node->m_examined , "Node in the open list that is not marked as being examined!" )
		
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		// Because of the way we deal with changes to nodes that are already in the priority queue 
		// (add duplicate into the queue), we may have a duplicate node that is already in the closed list. If this 
		// is the case then ignore the node entirely. 
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		
		if ( lowest_open_node->m_in_open_list == false ) continue;
		
		// Add it to the closed list:
		
		lowest_open_node->m_in_open_list = false;
		
		// If the node is our target then stop the algorithm: we are done
		
		if ( lowest_open_node == end ){ found_path = true; break; }
		
		//-------------------------------------------------------------------------------------------------------------
		// Go through all the connected nodes of the lowest F score open node:
		//-------------------------------------------------------------------------------------------------------------
		
		for ( unsigned i = 0 ; i < lowest_open_node->m_num_connected_nodes ; ++i )
		{
			// Get this connected node:
			
			Node * connected = lowest_open_node->m_connected_nodes[i];
			
			// Make sure ok
			
			ENSURE( connected != 0 , "Null node encountered in connected nodes list" )
			
			// See if examined yet:
			
			if ( connected->m_examined )
			{
				// Have examined node already: see if still in the open list. If not then it is in the closed list
				
				if ( connected->m_in_open_list )
				{
					//-------------------------------------------------------------------------------------------------
					// Connected node is in the open list. See if the G cost to this node is lower from the current 
					// node. If so we have found a better path to the connected node.
					//-------------------------------------------------------------------------------------------------
					
					// Compute G cost to the node:
					
					float g = lowest_open_node->m_g_cost;
					
					g += lowest_open_node->m_connected_distances[i];
					g += lowest_open_node->m_connected_costs_extra[i];
					
					// See if lower than the current g cost:
					
					if ( g < connected->m_g_cost )
					{
						//---------------------------------------------------------------------------------------------
						// Path to connected node is better from this node. Set the new g and f cost and make the 
						// previous node of the connected node the current node
						//---------------------------------------------------------------------------------------------
						
						connected->m_g_cost = g; connected->m_f_cost = g + connected->m_h_cost;
						
						// New previous node:
						
						connected->m_previous_path_node = lowest_open_node;
						
						//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
						// Now.. This seems a little strange. The problem we have now is that we have modified an object
						// that is in the priority_queue structure (a heap internally) and may now have violated the 
						// heap ordering property. We could remove every single item from the heap and reconstruct it to restore 
						// ordering, but this would be extremely inefficient. We cannot keep track of an item's position
						// in the heap either because adding another item may disturb the items position (invalidate iterators).
						// 
						// The quick and dirty way around this problem is to simply re-add the node into the heap. 
						// If we encounter a duplicate later we simply ignore it. We will still get the best node from the 
						// top in any case, and this is all we need. Considering the alternatives, this approach does 
						// not seem too bad..
						//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
						
						m_open_list->Push( AStarRatedNode(connected) );
					}
				}
			}
			else
			{
				//-----------------------------------------------------------------------------------------------------		
				// Have not examined current node: add to the open list and score etc.
				//-----------------------------------------------------------------------------------------------------
				
				// Calculate cost to get here
				
				connected->m_g_cost  =	lowest_open_node->m_g_cost;				
				connected->m_g_cost +=	lowest_open_node->m_connected_distances		[i];
				connected->m_g_cost +=	lowest_open_node->m_connected_costs_extra	[i];
				
				// Estimated cost to target
				
				connected->m_h_cost = end->m_position.SqDistanceTo( connected->m_position );
				
				// Total cost
				
				connected->m_f_cost = connected->m_g_cost + connected->m_h_cost;
				
				// Set previous node:
				
				connected->m_previous_path_node = lowest_open_node;
				
				// Add to the open list:
				
				connected->m_examined		= true;
				connected->m_in_open_list	= true;
				
				m_examined_nodes[m_num_examined_nodes] = connected;
				m_num_examined_nodes++;
				
				// In you go:
				
				m_open_list->Push( AStarRatedNode(connected) );
			}
		}
	}
	
	//-----------------------------------------------------------------------------------------------------------------
	// If we have found a path then back-trace from the end to find all the nodes along that path
	//-----------------------------------------------------------------------------------------------------------------
	
	// Reset this
	
    m_num_generated_path_nodes = 0;
    
    // See if we got a node:

	if ( found_path == true )
	{
		// Start at the target:
		
		Node * current = end;
		
		// Continue until we are done:
		
		while ( current )
		{
			// Add this node to the list of generated nodes:
			
			m_generated_path[m_num_generated_path_nodes] = current; m_num_generated_path_nodes++;
			
			// Make sure ok:
			
			ENSURE( unsigned(current->m_previous_path_node) != 0xffffffff , "Invalid previous node pointer!" )
			
			// Move on:
			
			current = current->m_previous_path_node;
		}
	}
	
	//-----------------------------------------------------------------------------------------------------------------
	// Clear the data on all examined nodes afterwards so they can be used in the next A* call
	//-----------------------------------------------------------------------------------------------------------------
	
	for ( unsigned i = 0 ; i < m_num_examined_nodes ; ++i ) 
	{
		m_examined_nodes[i]->m_examined = false;
	}
	
	// Haven't examined any nodes for the next call to this:
		
	m_num_examined_nodes = 0;
	
	//-----------------------------------------------------------------------------------------------------------------
	// Clear the open list if not empty
	//-----------------------------------------------------------------------------------------------------------------
	
	m_open_list->Clear();
}

//=====================================================================================================================
/// @brief		Attempts to generate a path between the two given points in the level
/// @param		from - where to start from
/// @param		to - where we are going to
//=====================================================================================================================

void PathNetwork::GeneratePath( const Vec2f & from , const Vec2f & to )
{
	// Get the subsector of the level both points are in:
	
	const DoomGLSubsectorV2 * s1 = m_level->GetSubsector(from);
	const DoomGLSubsectorV2 * s2 = m_level->GetSubsector(to);
	
	// Make sure both are there:
	
	if ( s1 && s2 )
	{
		// Ok: get the subsector numbers
		
		unsigned num1 = m_level->GetSubsectorNumber(*s1);
		unsigned num2 = m_level->GetSubsectorNumber(*s2);
		
		// Now get the closest path nodes for these two subsectors:
		
		Node * n1 = m_subsector_nodes[num1];
		Node * n2 = m_subsector_nodes[num2];
		
		// Should never be the case:
		
		ENSURE( n1 && n2 , "Subsector marked as having no path node!")
		
		// Find the path between:
		
		Pathfind(n1,n2);
	}
	else
	{
		// No path between them:
		
		m_num_generated_path_nodes = 0;
	}
}


#ifdef USE_DEBUG_CONSOLE

//=====================================================================================================================
/// @brief		Debug only: draws all the path nodes
/// @param		c - camera to draw from
//=====================================================================================================================

void PathNetwork::DrawPathNodes( Camera3D * c )
{
	// Get the buffered line renderer from the game:

	BufferedLineRenderer * r = GameCore::GetSingleton()->GetBufferedLineRenderer();

	// Get the stack from the game

	Stack & s = GameCore::GetSingleton()->GetStack();
	
	// Store the number of lines to draw here:
	
	unsigned num_lines = 0;
	
	// Run through all the nodes:
	
	for ( unsigned i = 0 ; i < m_num_nodes ; ++i )
	{
		std::list<unsigned>::iterator sec_cur = m_node_subsectors[i].begin();
		std::list<unsigned>::iterator sec_end = m_node_subsectors[i].end();
	
		// Run through all the subsectors that this node is associated with:
		
		while ( sec_cur != sec_end )
		{
			// Get this subsector that the node is associated with:
			
			const DoomGLSubsectorV2 & subsector = m_level->GetSubsector( * sec_cur ); sec_cur++;
			
			// Run through all it's segs:
			
			for ( unsigned j = 0 ; j < subsector.m_count ; j++ )
			{
				// Get this seg:
				
				const DoomPPSeg & seg = m_level->GetSeg(subsector.m_first_seg+j);
				
				// See if there is an opposite seg:
				
				if ( seg.m_opposite_seg )
				{
					// Opposite seg: get the opposite subsector:
					
					const DoomGLSubsectorV2 & opposite_subsector = * ( seg.m_opposite_seg->m_subsector );
					
					// Get the opposite subsector number
					
					unsigned opposite_num = m_level->GetSubsectorNumber(opposite_subsector);
					
					// See if the opposite subsector is associated with a different path node: if so then draw a line border
					
					if ( m_subsector_nodes[opposite_num] != m_nodes + i )
					{
						// Opposite subsector has a different path node. Draw border to distinguish active area of this path node
						
						s.Push( Vec3f( seg.m_start.x , seg.m_floor_height , seg.m_start.y ) );
						s.Push(0xffffffff);
						s.Push( Vec3f( seg.m_end.x , seg.m_floor_height , seg.m_end.y ) );
						s.Push(0xffffffff);
						
						num_lines++;					
					}
				}
				else
				{
					// No opposite seg: draw a line
					
					s.Push( Vec3f( seg.m_start.x , seg.m_floor_height , seg.m_start.y ) );
					s.Push(0xffffffff);
					s.Push( Vec3f( seg.m_end.x , seg.m_floor_height , seg.m_end.y ) );
					s.Push(0xffffffff);
					
					num_lines++;
				}
			}
		}
	}
	
	// Draw all the nodes themselves:
	
	const float SIZE = 4.0f;
	
	for ( unsigned i = 0 ; i < m_num_nodes ; ++i )
	{
		// Get the node:
		
		Node & n = m_nodes[i];
		
		// Get sector it's in:
		
		const DoomSector & sector = m_level->GetSector( * m_level->GetSubsector(n.m_position) );
		
		// Position of the node in 3d:
		
		Vec3f p = Vec3f( n.m_position.x, sector.m_floor_y + SIZE * 8 , n.m_position.y );
		
		// Draw a box for the node:
		
		s.Push( p + Vec3f( - SIZE , - SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , - SIZE , -SIZE ) );
		s.Push(0xff00ffff);

		s.Push( p + Vec3f( - SIZE , - SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , - SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		
		s.Push( p + Vec3f( - SIZE , + SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , + SIZE , -SIZE ) );
		s.Push(0xff00ffff);

		s.Push( p + Vec3f( - SIZE , + SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , + SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		
		s.Push( p + Vec3f( - SIZE , - SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( - SIZE , - SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		
		s.Push( p + Vec3f( + SIZE , - SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , - SIZE , +SIZE ) );
		s.Push(0xff00ffff);	
		
		s.Push( p + Vec3f( - SIZE , + SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( - SIZE , + SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		
		s.Push( p + Vec3f( + SIZE , + SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , + SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		
		s.Push( p + Vec3f( - SIZE , - SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( - SIZE , + SIZE , -SIZE ) );
		s.Push(0xff00ffff);

		s.Push( p + Vec3f( + SIZE , - SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , + SIZE , -SIZE ) );
		s.Push(0xff00ffff);
		
		s.Push( p + Vec3f( - SIZE , - SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( - SIZE , + SIZE , +SIZE ) );
		s.Push(0xff00ffff);

		s.Push( p + Vec3f( + SIZE , - SIZE , +SIZE ) );
		s.Push(0xff00ffff);
		s.Push( p + Vec3f( + SIZE , + SIZE , +SIZE ) );
		s.Push(0xff00ffff);		
		
		num_lines += 12;
		
		// Draw connections to other nodes:
		
		for ( unsigned j = 0 ; j < n.m_num_connected_nodes ; j++ )
		{
			// Get this node:
			
			Node & n2 = * ( n.m_connected_nodes[j] );
			
			// Get sector it's in:
			
			const DoomSector & sector2 = m_level->GetSector( * m_level->GetSubsector(n2.m_position) );			
			
			// Get 3d pos
			
			Vec3f p2 = Vec3f( n2.m_position.x, sector2.m_floor_y + SIZE * 8 , n2.m_position.y );
			
			// Get vec between 
			
			Vec3f between = p - p2;
			
			// Get dir between
			
			Vec3f dir = between; dir.Normalize();
			
			// Get length between
			
			float len = between.Length();
			
			// Get perpendicular to dir
			
			Vec3f perp = dir.Cross(Vec3f::UnitY()); perp.Normalize();
			
			// Draw line between:
			
			s.Push( p );
			s.Push(0xff0000ff);
			s.Push( p2 );
			s.Push(0xff0000ff);
			
			// Draw direction arrow:
			
			s.Push( p2 - perp * SIZE * 4 + dir * SIZE * 4 + dir * len * 0.25f );
			s.Push(0xffff0000);
			s.Push( p2 + dir * len * 0.25f );
			s.Push(0xff0000ff);
			
			s.Push( p2 + perp * SIZE * 4 + dir * SIZE * 4 + dir * len * 0.25f );
			s.Push(0xffff0000);
			s.Push( p2 + dir * len * 0.25f );
			s.Push(0xff0000ff);
			
			num_lines += 3;
		}
	}
	
	// Draw all of our lines:
	
	if ( num_lines > 0 )
	{	
		r->Draw( c->GetViewProjectionMatrix() , num_lines , s );
	}
}

//=====================================================================================================================
/// @brief		Debug only: draws a path generated between the two given points in the level
/// @param		c - camera being viewed from
/// @param		p1 - from point
/// @param		p2 - to point
//=====================================================================================================================

void PathNetwork::DrawPath( Camera3D * c , const Vec2f & p1 , const Vec2f & p2 )
{
	// Generate a path between these:
	
	GeneratePath(p1,p2);
	
	// Make sure a path was generated
	
	if ( GetGeneratedPathSize() > 1 )
	{
		// Get the buffered line renderer from the game:

		BufferedLineRenderer * r = GameCore::GetSingleton()->GetBufferedLineRenderer();

		// Get the stack from the game

		Stack & s = GameCore::GetSingleton()->GetStack();
		
		// Store the number of lines to draw here:
		
		unsigned num_lines = 0;
		
		// Continue drawing all the path:
		
		for ( unsigned i = 0 ; i < ( GetGeneratedPathSize() - 1 ) ; ++i )
		{
			// Get current and next point:
			
			Vec2f p1 = GetGeneratedPathNode(i).m_position;			
			Vec2f p2 = GetGeneratedPathNode(i+1).m_position;
			
			// Get subsector of both nodes:
			
			const DoomGLSubsectorV2 * ssec1 = m_level->GetSubsector(p1);
			const DoomGLSubsectorV2 * ssec2 = m_level->GetSubsector(p2);
			
			// Get sectors:
			
			const DoomSector & sec1 = m_level->GetSector(*ssec1);
			const DoomSector & sec2 = m_level->GetSector(*ssec2);
			
			// Draw line between:
			
			s.Push( Vec3f( p1.x , float(sec1.m_floor_y) + 32 , p1.y ) );
			s.Push( 0xffffffff );
			s.Push( Vec3f( p2.x , float(sec2.m_floor_y) + 32 , p2.y ) );
			s.Push( 0xffffffff );
			
			num_lines++;			
		}
		
		// Draw:
		
		r->Draw( c->GetViewProjectionMatrix() , num_lines , s );
	}
}

#endif		// #ifdef USE_DEBUG_CONSOLE