#include "MW4Headers.hpp"
#include "adept\application.hpp"
#include "rail_move.hpp"
//#include "quadedge.h"
#include <MW4\MWMission.hpp>
#include <Adept\Map.hpp>
#include <Adept\CollisionGrid.hpp>
#include <ElementRenderer\StateChange.hpp>
#include "moverai.hpp"
#include "obstacle.hpp"
#include "move_rect.hpp"

#pragma warning (push)
#include <vector>
#pragma warning (pop)



using namespace MW4AI;
using namespace MechWarrior4;

namespace MechWarrior4
{
	extern DWORD Path_Calcs_Count;
	extern DWORD GridPath_Calcs_Count;
	extern DWORD QuickPath_Calcs_Count;
	extern DWORD QuickRail_Calcs_Count;
}

namespace MW4AI
{
	extern Stuff::Scalar MinZ, MaxZ, MinX, MaxX;
	CPathManager *g_PathManager = NULL;
	CRailGraph::ClassData* CRailGraph::DefaultData = NULL;
	CRailGraph *g_MissionGraph = NULL;
	bool twofalse[2] = {false,false};
	bool twotrue[2] = {true,true};
	int	twomax[2] = {0xffffffff,0xffffffff};
	__int64	tPathTime,tGridPathTime;
	__int64 tCalcPathTime;
	__int64 tQuickCheckTime;
	__int64 tCreatePathRequestsTime;
	__int64 tPathCalc1Time;
	__int64 tPathCalc2Time;
	__int64 tNodeToPathTime;
}


void CRailNode::ConstructNodeObjectStream(int id,Stuff::MemoryStream *stream,Stuff::NotationFile *model_file)
{
	char entryname[25];
	Stuff::Point3D loc;
	Stuff::Scalar rad;
	Stuff::Scalar maxrad;

	Verify (id>=0);
	Verify (id<MAXNODES);
	sprintf (entryname,"Node%d",id);
	loc.x = -1;
	loc.y = -1;
	loc.z = -1;
	Page *link_page = model_file->FindPage(entryname);
	if (link_page)
	{
		Check_Object(link_page);
		link_page->GetEntry("Location",&loc,false);
	}
	if ((loc.x == -1) || (loc.y==-1) || (loc.z == -1))
	{
		*stream << loc;
		return;
	}

	rad = 80.0;
	Check_Object(link_page);
	link_page->GetEntry("Radius",&rad,false);
	maxrad = 80.0;
	link_page->GetEntry("MaxRadius",&maxrad,false);
	*stream << loc;
	*stream << rad;
	*stream << maxrad;
}

void CRailNode::DumpNotationFile (Stuff::NotationFile *file)
{
	char entryname[25];

	sprintf (entryname,"Node%d",m_ID);
	Page *link_page = file->SetPage(entryname);
	link_page->SetEntry ("Location",m_Location);
	link_page->SetEntry ("Radius",m_Radius);
	link_page->SetEntry ("MaxRadius",m_MaxRadius);
}

CRailNode::CRailNode (Stuff::MemoryStream *stream,CRailGraph *parent)
{
	Check_Pointer(this);

	m_DistanceValid = false;

	Check_Object(stream);
	*stream >> m_Location;
	if ((m_Location.x == -1) || (m_Location.y == -1) || (m_Location.z == -1))
		return;
	*stream >> m_Radius;
	*stream >> m_MaxRadius;
	m_RadiusSquared = m_Radius*m_Radius;
	m_SubNodeRadiusMult = parent->m_SubNodeRadiusMult;
	m_Dirty = false;
}

CRailNode::CRailNode (const Stuff::Point3D& loc,Stuff::Scalar radius)
	: m_Location(loc)
{
	m_DistanceValid = false;
	m_Radius = radius;
	m_RadiusSquared = m_Radius*m_Radius;
	m_MaxRadius = radius;
	m_Dirty = true;

}

CRailNode::~CRailNode (void)
{
}

void CRailNode::DeleteLink (CRailLink *link)
{
	stlport::vector<CRailLink *>::iterator linkiter;
	stlport::vector<RailSubNode>::iterator subiter;

	for (subiter = m_SubNodes.begin (),linkiter = m_Links.begin ();linkiter!= m_Links.end ();linkiter++,subiter++)
	{
		if ((*linkiter) == link)
		{
			break;
		}
	}
	if (linkiter == m_Links.end ())
		return;
	m_Links.erase (linkiter);
	m_SubNodes.erase (subiter);
}

void CRailNode::AddLink (CRailLink *link)
{
	int i,size;
	Stuff::Point3D subloc;
	Stuff::Vector3D s,d,c;
	CRailNode *dest;

	AutoHeap local_heap (g_RailHeap);

	size = m_Links.size ();
	for (i=0;i<size;i++)
		if (m_Links[i] == link)
			return;
	m_Links.push_back (link);

	dest = link->OtherLocation (this);
	Verify (dest);
#if 0
	if (dest == 0)
	{
		return;
	}
#endif
	s = Location ();
	d = dest->Location ();
	s.y = 0;
	d.y = 0;
	c.Subtract (d,s);
	d.Normalize (c);
	d *= m_Radius*m_SubNodeRadiusMult;
	c.Add (s,d);
	subloc = c;
	m_SubNodes.push_back (RailSubNode (subloc));
	m_Dirty = true;
}

bool CRailNode::PtInNode (const Stuff::Point3D& pt)
{
	Stuff::Point3D extra;
	Stuff::Scalar dist;

	extra.Subtract (pt,m_Location);
	dist = extra.GetLengthSquared ();
	return dist < m_RadiusSquared;
}

float CRailNode::LinkLength (CRailLink *link)
{
	CRailNode *othernode;
	RailSubNode n1,n2;
	Point3D extra;

	othernode = link->OtherLocation (this);
	n1 = SubNode (link);
	n2 = othernode->SubNode (link);
	if (PtInNode (n2.location))
	{
		extra.Subtract (m_Location,othernode->Location ());
		return -extra.GetLength ();
	}
	extra.Subtract (n1.location,n2.location);
	return extra.GetLength ();
}

void CRailNode::Radius (Stuff::Scalar newrad)
{
	m_Radius = newrad;
	m_RadiusSquared = m_Radius*m_Radius;
	m_Dirty = true;

	CheckRadius();

}
void CRailNode::SetRadiusToMax (void)
{
	int i,size,min;

	min = 0;
	Verify (m_DistanceValid);
	size = m_EditorDistance.size ();
	for (i=1;i<size;i++)
		if (m_EditorDistance[i] < m_EditorDistance[min])
			min = i;

	m_Radius = m_EditorDistance[min] - 1.0f;
	CheckRadius();
	m_RadiusSquared = m_Radius*m_Radius;
}

CRailGraph::CRailGraph (ClassData *class_data) : Receiver (class_data)//, mesh (Point2d(0,INT_MIN),Point2d(INT_MIN,INT_MAX),Point2d(INT_MAX,INT_MAX),NULL,NULL,NULL)
{
	Check_Pointer (this);
	m_Nodes.assign (100,NULL);
	m_Links.assign (200,NULL);
	m_Dirty= false;
	m_WorkGrid = NULL;
	m_MoveGrid = NULL;
	m_AirMoveGrid = NULL;
	m_WorkGridRefCount = 0;
	m_RectList = NULL;

	Map::GetInstance()->GetMapExtents(&MinZ,&MaxZ,&MinX,&MaxX);\
	m_SaveLineElementMemory = false;
}

MString urbanMissionNames[] = {
	"urban01_recon",
	"urban02_rescuepilots",
	"urban03_destroybase",
	"urban05_rescuesister",
	"urban06_capturesupplies"
};

CRailGraph::CRailGraph (ClassData *class_data,Stuff::MemoryStream *stream) : Receiver (class_data)//, mesh (Point2d(0,INT_MIN),Point2d(INT_MIN,INT_MAX),Point2d(INT_MAX,INT_MAX),NULL,NULL,NULL)
{
	int numnode,numlink,numblock;
	int i;

	gos_PushCurrentHeap (g_RailHeap);
	m_RectList = NULL;
	m_WorkGrid = NULL;
	m_MoveGrid = NULL;
	m_AirMoveGrid = NULL;
	m_WorkGridRefCount = 0;
	m_SubNodeRadiusMult = 1.0f;

	Check_Pointer(this);
	Check_Object(stream);
	*stream >> numblock;
	if (!numblock)
		m_BlockedLinks.assign (10,Stuff::Point3D (-1,-1,-1));
	else
		m_BlockedLinks.assign (numblock,Stuff::Point3D (-1,-1,-1));
	for (i=0;i<numblock;i++)
	{
		Stuff::Point3D data;
		*stream >> data;
		m_BlockedLinks[i] = data;
	}

	*stream >> numnode;
	if (!numnode)
		m_Nodes.assign (10,NULL);
	else
		m_Nodes.assign (numnode,NULL);
	for (i=0;i<numnode;i++)
	{
		Stuff::Point3D loc;
		m_Nodes[i] = new CRailNode (stream,this);
		Check_Pointer (m_Nodes[i]);
		loc = m_Nodes[i]->Location ();
		if ((loc.x == -1) || (loc.y == -1) || (loc.z == -1))
		{
			delete m_Nodes[i];
			m_Nodes[i] = NULL;
		}
		else
			m_Nodes[i]->ID (i);
	}
	*stream >> numlink;
	if (!numlink)
		m_Links.assign (10,NULL);
	else
		m_Links.assign (numlink,NULL);
	for (i=0;i<numlink;i++)
	{
		m_Links[i] = new CRailLink (stream,this);
		Check_Pointer (m_Links[i]);
		if ((m_Links[i]->LocID(0) == -1) || (m_Links[i]->LocID(1) == -1))
		{
			delete m_Links[i];
			m_Links[i] = NULL;
		}
		else
			m_Links[i]->ID (i);
	}

	Map::GetInstance()->GetMapExtents(&MinZ,&MaxZ,&MinX,&MaxX);

	m_MoveGrid = new CMoveGrid (stream);
	m_AirMoveGrid = new CAirMoveGrid (stream);
	m_RectList = new CMoveRectList (stream);

	if (stream->GetBytesRemaining()>=sizeof(Stuff::Scalar)) {
		*stream >> m_SubNodeRadiusMult;
	}
	else {
		MWMission *mission = Cast_Object(MWMission*, MWMission::GetInstance());
		const char *name = mission->GetModelName ();
		Verify (name);

		MString missionName = name;
		missionName.ToLower();
		
		// If it's stock urban mission
		if (strncmp(missionName, "missions\\", strlen("missions\\")) == 0) {
			missionName.StripDirectory();
			missionName.StripExtension();
			if (missionName == urbanMissionNames[0] ||
				missionName == urbanMissionNames[1] ||
				missionName == urbanMissionNames[2] ||
				missionName == urbanMissionNames[3] ||
				missionName == urbanMissionNames[4] )
				m_SubNodeRadiusMult = 1.3f;
		}
	}

	for (i=0;i<numnode;i++)
	{
		if (m_Nodes[i])
			m_Nodes[i]->SubNodeRadiusMult(m_SubNodeRadiusMult);
	}

	for (i=0;i<numlink;i++)
	{
		if (m_Links[i])
		{
			if (m_Links[i]->Location(0))
			{
				m_Links[i]->Location (0)->AddLink (m_Links[i]);
			}
			
			if (m_Links[i]->Location(1))
			{
				m_Links[i]->Location (1)->AddLink (m_Links[i]);
			}
		}
	}

//	CalcDistances ();
	m_Dirty = false;
	m_SaveLineElementMemory = false;
	gos_PopCurrentHeap ();
}
void CRailGraph::CreateGrid (WorkCallBack callback,float maxpercent)
{
	gos_PushCurrentHeap (g_RailHeap);
	if (!m_WorkGrid)
	{
		if (!m_MoveGrid)
			m_MoveGrid = new CMoveGrid ();
		else
			m_MoveGrid->Construct (5120,5120);
		if (!m_AirMoveGrid)
			m_AirMoveGrid = new CAirMoveGrid ();
		else
			m_AirMoveGrid->Construct ((int) (MaxX - MinX),(int) (MaxZ-MinZ));

		m_WorkGrid = new CRailWorkGrid ();
		m_WorkGrid->Construct (callback,maxpercent,m_MoveGrid,m_AirMoveGrid);
		m_WorkGridRefCount = 1;
	}
	else
	{
		m_WorkGridRefCount++;
	}
	gos_PopCurrentHeap ();
}

void CRailGraph::KillGrid (void)
{
	Verify (m_WorkGridRefCount);
	m_WorkGridRefCount--;
	if (!m_WorkGridRefCount)
	{
		delete m_WorkGrid;
		m_WorkGrid = NULL;
	}
}

CRailGraph::~CRailGraph (void)
{
	int i,size;

	size = m_Links.size ();
	for (i=0;i<size;i++)
	{
		delete m_Links[i];
		m_Links[i] = NULL;
	}
	size = m_Nodes.size ();
	for (i=0;i<size;i++)
	{
		delete m_Nodes[i];
		m_Nodes[i] = NULL;
	}
	delete m_MoveGrid;
	m_MoveGrid = NULL;
	delete m_AirMoveGrid;
	m_AirMoveGrid = NULL;
	delete m_RectList;
	m_RectList = NULL;
}

void CRailGraph::ConstructGraphObjectStream(Stuff::MemoryStream *stream,Stuff::NotationFile *model_file)
{
	int nodecount,linkcount,blockcount;
	int i;
	char notename[25];

	RegisteredClass::ClassID class_id = DefaultData->GetClassID();

	*stream << class_id;

	blockcount = 0;
	Page *graph_page = model_file->FindPage("Graph");
	if (graph_page)
		graph_page->GetEntry("NumBlocks",&blockcount,false);

	*stream << blockcount;
	for (i=0;i<blockcount;i++)
	{
		Stuff::Point3D data;
		sprintf (notename,"Block%d",i);
		if (graph_page)
			graph_page->GetEntry(notename,&data,true);
		*stream << data;
	}

	if (graph_page)
		graph_page->GetEntry("NumNodes",&nodecount,true);

	*stream << nodecount;
	for (i=0;i<nodecount;i++)
	{
		CRailNode::ConstructNodeObjectStream (i,stream,model_file);
	}

	if (graph_page)
		graph_page->GetEntry("NumLinks",&linkcount,true);
	*stream << linkcount;
	for (i=0;i<linkcount;i++)
	{
		CRailLink::ConstructLinkObjectStream (i,stream,model_file);
	}

	const char *passfile = NULL;
	if (graph_page)
		graph_page->GetEntry("PassFile",&passfile,false);
	if (!CMoveGrid::ConstructStream (stream,passfile))
	{
#if defined(LAB_ONLY)
		SPEWALWAYS ((0,"bad passability map for %s, defaulting to impassable for AI",model_file->GetFileName ()));
#endif
	}

	const char *airpassfile = NULL;
	if (graph_page)
		graph_page->GetEntry("AirPassFile",&airpassfile,false);
	CAirMoveGrid::ConstructStream (stream,airpassfile);

	const char *rectfile = NULL;
	if (graph_page)
		graph_page->GetEntry("RectFile",&rectfile,false);
	CMoveRectList::ConstructStream (stream,rectfile);

	if (graph_page) {
		Stuff::Scalar subnodeMult = 1.0f;
		if (graph_page->GetEntry("SubNodeRadiusMult",&subnodeMult,false))
			*stream << subnodeMult;
	}
}

void CRailGraph::DumpNotationFile (Stuff::NotationFile *file,const char *passfile,const char *rectfile,const char *airpassfile)
{
	int nodecount,linkcount,blockcount;
	int i;
	char notename[25];

	file->DeleteAllPages ();
	nodecount = m_Nodes.size ();
	linkcount = m_Links.size ();
	blockcount = m_BlockedLinks.size ();
	Page *graph_page = file->SetPage("Graph");
	Check_Object(graph_page);
	graph_page->SetEntry ("NumNodes",nodecount);
	graph_page->SetEntry ("NumLinks",linkcount);
	graph_page->SetEntry ("NumBlocks",blockcount);
	graph_page->SetEntry ("PassFile",passfile);
	graph_page->SetEntry ("AirPassFile",airpassfile);
	graph_page->SetEntry ("RectFile",rectfile);
	for (i=0;i<blockcount;i++)
	{
		sprintf (notename,"Block%d",i);
		graph_page->SetEntry (notename,m_BlockedLinks[i]);
	}
	for (i=0;i<nodecount;i++)
	{
		if (m_Nodes[i])
			m_Nodes[i]->DumpNotationFile (file);
	}
	for (i=0;i<linkcount;i++)
	{
		if (m_Links[i])
			m_Links[i]->DumpNotationFile (file);
	}

	m_MoveGrid->SaveData (passfile);
	m_AirMoveGrid->SaveData (airpassfile);
	if (m_RectList)
		m_RectList->SaveFile (rectfile);
}


void CRailGraph::InitializeClass()
{
	Verify(!DefaultData);
	DefaultData = new ClassData(CRailGraphClassID,"MW4AI::CRailGraph",Receiver::DefaultData,0,NULL);
	Register_Object(DefaultData);
}

void CRailGraph::TerminateClass()
{
	Unregister_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

void CRailGraph::TestInstance (void) const
{
}

void CRailGraph::ClearLinks (void)
{
	int i,size;

	size = m_Links.size ();
	for (i=0;i<size;i++)
	{
		if (m_Links[i])
			delete m_Links[i];
		m_Links[i] = NULL;
	}
	size = m_Nodes.size ();
	for (i=0;i<size;i++)
	{
		if (m_Nodes[i])
			m_Nodes[i]->ClearLinks ();
	}
	m_Links.clear ();
}

void CRailGraph::ClearLinks (int *start,int numnodes)
{
	int i,j,id;

	for (i=0;i<(numnodes-1);i++)
	{
		for (j=i;j<numnodes;j++)
		{
			id = Connect (m_Nodes[start[i]],m_Nodes[start[j]]);
			if (id != -1)
			{
				DeleteLink (id);
			}
		}
	}
}

void CRailGraph::DeleteLink (unsigned int node1,unsigned int node2)
{
	stlport::vector<CRailLink *>::iterator iter;

	for (iter = m_Links.begin ();iter != m_Links.end ();iter++)
	{
		int id1,id2;
		id1 = (*iter)->LocID (0);
		id2 = (*iter)->LocID (1);
		if ((id1 == node1) && (id2 == node2))
		{
			DeleteLink ((*iter)->ID ());
			return;
		}
		if ((id1 == node2) && (id2 == node1))
		{
			DeleteLink ((*iter)->ID ());
			return;
		}
	}

}

void CRailGraph::DeleteLink (unsigned int id)
{
	Verify (id>=0);
	Verify (id<m_Links.size ());
	if (m_Links[id])
	{
		if (m_Links[id]->Location(0) != 0)
		{
			m_Links[id]->Location (0)->DeleteLink (m_Links[id]);
		}

		if (m_Links[id]->Location(1) != 0)
		{
			m_Links[id]->Location (1)->DeleteLink (m_Links[id]);
		}
	}
	delete m_Links[id];
	m_Links[id] = NULL;															
}

int CRailGraph::AddNode (CRailNode *node)
{
	int i,size;

	AutoHeap local_heap (g_RailHeap);

	size = m_Nodes.size ();
	for (i=0;i<size;i++)
	{
		if (m_Nodes[i] == NULL)
			break;
	}
	if (i == size)
		m_Nodes.push_back (node);
	else
		m_Nodes[i] = node;
	node->ID (i);		
	m_Dirty = true;
	return i;
}

int CRailGraph::AddLink (CRailLink *link)
{
	int i,size;

	AutoHeap local_heap (g_RailHeap);
	size = m_Links.size ();
	for (i=0;i<size;i++)
	{
		if (m_Links[i] == NULL)
			break;
	}
	if (i == size)
		m_Links.push_back (link);
	else
		m_Links[i] = link;

	if (link->Location(0) != 0)
	{
		link->Location (0)->AddLink (link);			// only add the top left link
	}

	if (link->Location(1) != 0)
	{
		link->Location (1)->AddLink (link);			// only add the top left link
	}

	link->ID (i);
	m_Dirty = true;
	return i;
}

int CRailGraph::Connect (CRailNode *src,CRailNode *dest) const
{
	int i,size;

	size = m_Links.size ();
	for (i=0;i<size;i++)
	{
		if (!m_Links[i])
			continue;
		if ((m_Links[i]->Location(0) == dest) && (m_Links[i]->Location(1) == src))
			return m_Links[i]->ID ();
		else if ((m_Links[i]->Location(0) == src) && (m_Links[i]->Location(1) == dest))
			return m_Links[i]->ID ();
	}

	return -1;
}

float CRailGraph::Connect (CRailNode *src,CRailNode *dest,int usagetype) const
{
	int i,size;

	size = m_Links.size ();
	for (i=0;i<size;i++)
	{
		if (!m_Links[i])
			continue;
		if ((m_Links[i]->Location(0) == dest) && (m_Links[i]->Location(1) == src))
			return m_Links[i]->Weight (usagetype,src);
		else if ((m_Links[i]->Location(0) == src) && (m_Links[i]->Location(1) == dest))
			return m_Links[i]->Weight (usagetype,src);
	}

	return -1.0f;
}
void CRailGraph::ClearPathFlags (void)
{
	int i,size;

	size = m_Nodes.size ();
	for (i=0;i<size;i++)
	{
		if (m_Nodes[i])
			m_Nodes[i]->ClearPathWeight (0,0,0);
	}
}

void CRailGraph::FindBestNode (const Stuff::Point3D& start,const Stuff::Point3D& end,bool strict,CRailNodePtr& startnode,CRailNodePtr& endnode)
{
	stlport::vector<CRailNode *> valid;
	stlport::vector<CRailNode *>::iterator nodeiter,validiter;
	Stuff::Point3D fakeloc,temploc,temp;
	float fred;

	valid.reserve (m_Nodes.size ());
	startnode = NULL;
	endnode = NULL;
	if (m_Nodes.size () == 0)
		return;
	else if (m_Nodes.size () == 1)
	{
		startnode = m_Nodes[0];
		endnode = m_Nodes[0];
		return;
	}
	AutoHeap local_heap (g_RailHeap);
	fakeloc = start;
	fakeloc.y = 0;
	nodeiter = m_Nodes.begin ();
	while (nodeiter != m_Nodes.end ())
	{
		if (*nodeiter)
		{
			temploc = (*nodeiter)->Location ();
			temploc.y = 0;
			temp.Subtract (fakeloc,temploc);
			temp.y = 0;
			fred = temp.GetLengthSquared ();
			if (fred <= (*nodeiter)->RadiusSquared ())
				valid.push_back (*nodeiter);
		}
		nodeiter++;
	}
	
	fakeloc = end;
	fakeloc.y = 0;
	validiter = valid.begin ();
	while (validiter != valid.end ())
	{
		temploc = (*validiter)->Location ();
		temploc.y = 0;
		temp.Subtract (fakeloc,temploc);
		temp.y = 0;
		fred = temp.GetLengthSquared ();
		if (fred <= (*validiter)->RadiusSquared ())
		{
			startnode = *validiter;
			endnode = *validiter;
			return;
		}
		validiter++;
	}

	startnode = FindClosestNode (start,false);
	endnode = FindClosestNode (end,strict);

}

CRailNode *CRailGraph::FindClosestNode (const Stuff::Point3D& loc,bool strict,CRailNode *firstcheck)
{
	int i;
	int size;
	Stuff::Point3D fakeloc,temploc;


// This caused problems in _ARMOR builds
//	Verify (CountNodes ());
//	Verify (CountLinks ());

	fakeloc = loc;
	fakeloc.y = 0;

	CRailNode *min;
	Stuff::Scalar mincost;
	Stuff::Vector3D temp;
	Stuff::Scalar fred;

	size = m_Nodes.size ();
	if (size == 0)
		return NULL;
	if (size == 1)
		return m_Nodes[0];
	min = NULL;
	mincost = FLT_MAX;

	if (firstcheck)
	{
		temploc = firstcheck->Location ();
		temploc.y = 0;
		temp.Subtract (fakeloc,temploc);
		temp.y = 0;
		fred = temp.GetLengthSquared ();
		if (fred < firstcheck->RadiusSquared())
			return firstcheck;
	}

	for (i=0;i<size;i++)
	{
		if (!m_Nodes[i])
			continue;
		temploc = m_Nodes[i]->Location ();
		temploc.y = 0;
		temp.Subtract (fakeloc,temploc);
		temp.y = 0;
		fred = temp.GetLengthSquared ();
		if (fred > m_Nodes[i]->RadiusSquared ())
			continue;
		if ((fred < mincost) || (mincost == FLT_MAX))
		{
			mincost = fred;
			min = m_Nodes[i];
		}
	}
	if ((!min) && (!strict))
	{
		for (i=0;i<size;i++)
		{
			if (!m_Nodes[i])
				continue;
			temploc = m_Nodes[i]->Location ();
			temploc.y = 0;
			temp.Subtract (fakeloc,temploc);
			temp.y = 0;
			fred = temp.GetLengthSquared ();
			if ((fred < mincost) || (mincost == FLT_MAX))
			{
				mincost = fred;
				min = m_Nodes[i];
			}
		}
	}

	return min;
}

bool CRailGraph::BridgeNear (Stuff::Point3D& start,Stuff::Point3D& end)
{
	if (m_RectList->RectWithin (start,500.0f))
		return true;
	if (m_RectList->RectWithin (end,500.0f))
		return true;
	return false;
}

int CRailGraph::UpDownTo (int x,int y,int dir,int mask)
{
	int curmask;
	int costtable[8][8] = {
		{1,1,0,2,2,2,0,1},
		{1,1,1,0,2,2,2,0},
		{0,1,1,1,0,2,2,2},
		{2,0,1,1,1,0,2,2},
		{2,2,0,1,1,1,0,2},
		{2,2,2,0,1,1,1,0},
		{0,2,2,2,0,1,1,1},
		{1,0,2,2,2,0,1,1},
	};

	curmask = PassableLocal (x,y);
	
	if ((mask & HOVERPASS_FLAG) && (curmask & WATER_MASK))		// no slope for hovercraft on water
		return 0;

	dir &= 0x00000007;
	int grdir;
	grdir = curmask & SLOPEDIR_MASK;
	grdir >>= SLOPEDIR_SHIFT;

	if (((curmask & STEEP_MASK) >> STEEP_SHIFT) == HOVERDOWN_SLOPE)
	{
		return 0;
	}
	
	return costtable[grdir][dir];
}

bool CRailGraph::MoveTo (int x,int y,int dir,int mask)
{
	int curmask;

	curmask = PassableLocal (x,y);

	dir &= 0x00000007;
	if ((mask & BRIDGE_FLAG) && (curmask & BRIDGE_FLAG))
		return true;
	if (curmask & MOVEPASSNOW_FLAG)
		return false;
	if (mask & HOVERPASS_FLAG)
	{
		if (((curmask & WATER_MASK) >> WATER_SHIFT) > 0)
			return true;
	}
	if ((curmask & BLOCKED_FLAGS) & (mask & BLOCKED_FLAGS))
		return false;
	if ((mask & WATERPASS_FLAG) && (((curmask & WATER_MASK) >> WATER_SHIFT) > 0))
		return true;
	int grdir;
	grdir = curmask & SLOPEDIR_MASK;
	grdir >>= SLOPEDIR_SHIFT;
	if (mask & LEGPASS_FLAG)
	{
		if (((curmask & STEEP_MASK) >> STEEP_SHIFT) == LEGDOWN_SLOPE)
		{
//			if ((grdir != dir) && (grdir != ((dir-1)&0x07)) && (grdir != ((dir+1)&0x07)))
				return false;
		}
	}
#if 0
	else if (mask & HOVERPASS_FLAG)
	{
		if (((curmask & STEEP_MASK) >> STEEP_SHIFT) == HOVERDOWN_SLOPE)
		{
			if ((grdir != dir) && (grdir != ((dir-1)&0x07)) && (grdir != ((dir+1)&0x07)))
				return false;
		}
	}
#endif
	else if (mask & TRACKPASS_FLAG)
	{
		if (((curmask & STEEP_MASK) >> STEEP_SHIFT) == TRACKDOWN_SLOPE)
		{
//			if ((grdir != dir) && (grdir != ((dir-1)&0x07)) && (grdir != ((dir+1)&0x07)))
				return false;
		}
	}
	else if (mask & WHEELPASS_FLAG)
	{
		if (((curmask & STEEP_MASK) >> STEEP_SHIFT) == WHEELDOWN_SLOPE)
		{
//			if ((grdir != dir) && (grdir != ((dir-1)&0x07)) && (grdir != ((dir+1)&0x07)))
				return false;
		}
	}
	return true;
}

bool CRailGraph::QuickCheck (int x1,int y1,int x2,int y2,unsigned short mask,bool widthvalid)
{
	PATH_LOGIC("Quick Check");
	TIME_FUNCTION(tQuickCheckTime);

	Stuff::Point3D extra;
	float sx,sy,dx,dy;
	int ix,iy,ex,ey,dir;
	int count;
	int startx,starty,endx,endy;
	const int SIZEDELTA = 8;
	static int delta[SIZEDELTA][2] = {
		{0,-1},	
		{1,-1},	
		{1,0},	
		{1,1},	
		{0,1},	
		{-1,1},	
		{-1,0},	
		{-1,-1}
	};


	startx = x1/BLOCK_GRID_RES;
	starty = y1/BLOCK_GRID_RES;
	endx = x2/BLOCK_GRID_RES;
	endy = y2/BLOCK_GRID_RES;
	if ((startx == endx) && (starty == endy))
		return true;

	dx = (float) endx - startx;
	dy = (float) endy - starty;
	sx = dx < 0 ? dx*-1.0f : dx;
	sy = dy < 0 ? dy*-1.0f : dy;

	if (sx > sy)
	{
		dy = dy / sx;
		count = (int) (dx<0.0 ? dx*-1.0f : dx);
		dx = dx<0 ? -1.0f : 1.0f;
		if (dx == 1.0f)
		{
			if (dy < -0.25f)
				dir = 1;
			else if (dy > 0.25f)
				dir = 3;
			else
				dir = 2;
		}
		else
		{
			if (dy < -0.25f)
				dir = 7;
			else if (dy > 0.25f)
				dir = 5;
			else
				dir = 6;
		}
	}
	else
	{
		dx = dx / sy;
		count = (int) (dy<0.0 ? dy*-1.0f : dy);
		dy = dy<0 ? -1.0f : 1.0f;
		if (dy == 1.0f)
		{
			if (dx < -0.25f)
				dir = 5;
			else if (dx > 0.25f)
				dir = 3;
			else
				dir = 4;
		}
		else
		{
			if (dx < -0.25f)
				dir = 7;
			else if (dx > 0.25f)
				dir = 1;
			else
				dir = 0;
		}
	}

	sx = (float) startx;
	sy = (float) starty;
	ex = endx;
	ey = endy;
	ix = (int) (sx);
	iy = (int) (sy);
	unsigned short passdata;
	passdata = PassableLocal (ix,iy);
	if (passdata & BRIDGE_FLAG)
		return false;
	while (count)
	{
		count--;
		sx += dx;
		sy += dy;
		ix = (int) (sx);
		iy = (int) (sy);


		if (!MoveTo (ix,iy,dir,mask))
			return false;
		if (widthvalid)
		{
			for (int i=0;i<SIZEDELTA;i++)
			{
				if (!MoveTo (ix+delta[i][0],iy+delta[i][1],dir,mask))
					return false;
			}
		}
	}

	return true;
}

bool CRailGraph::AirQuickCheck (int x1,int y1,int x2,int y2)
{
	Stuff::Point3D extra;
	float sx,sy,dx,dy;
	int ix,iy,ex,ey;
	int count;
	int startx,starty,endx,endy;

	startx = x1/AIR_BLOCK_GRID_RES;
	starty = y1/AIR_BLOCK_GRID_RES;
	endx = x2/AIR_BLOCK_GRID_RES;
	endy = y2/AIR_BLOCK_GRID_RES;
	if ((startx == endx) && (starty == endy))
		return !AirPassableLocal (startx,starty);

	dx = (float) endx - startx;
	dy = (float) endy - starty;
	sx = dx < 0 ? dx*-1.0f : dx;
	sy = dy < 0 ? dy*-1.0f : dy;

	if (sx > sy)
	{
		dy = dy / sx;
		count = (int) (dx<0.0 ? dx*-1.0f : dx);
		dx = dx<0 ? -1.0f : 1.0f;
	}
	else
	{
		dx = dx / sy;
		count = (int) (dy<0.0 ? dy*-1.0f : dy);
		dy = dy<0 ? -1.0f : 1.0f;
	}

	sx = (float) startx;
	sy = (float) starty;
	ex = endx;
	ey = endy;
	ix = (int) (sx);
	iy = (int) (sy);
	if (AirPassableLocal (ix,iy))
		return false;
	while (count)
	{
		count--;
		sx += dx;
		sy += dy;
		ix = (int) (sx);
		iy = (int) (sy);

		if (AirPassableLocal (ix,iy))
			return false;
	}

	return true;
}

CPathRequest::CPathRequest (Stuff::Time needby,CRailGraph *graph,CRailNode *start,CRailNode *end,int usage,MoverAI *who,bool tenmeter)
{
	m_Working = false;
	m_NeedBy = needby;
	m_Done = false;
	m_UsageAllowed = usage;
	m_Strict = true;
	gos_PushCurrentHeap (g_RailHeap);
	m_NodePath = new CRailPath (graph,start,end,who,tenmeter);
	gos_PopCurrentHeap ();
	m_GridPath = NULL;
	m_Who = who;
	Check_Pointer (m_NodePath);
}

CPathRequest::CPathRequest (Stuff::Time needby,CRailGraph *graph,Stuff::Point3D start,Stuff::Point3D end,int usage,MoverAI *who,bool tenmeter)
{
	m_Working = false;
	m_NeedBy = needby;
	m_Done = false;
	m_Strict = false;
	m_UsageAllowed = usage;
	gos_PushCurrentHeap (g_RailHeap);
	m_NodePath = new CRailPath (graph,start,end,m_Strict,who,tenmeter);
	gos_PopCurrentHeap ();
	m_GridPath = NULL;
	m_Who = who;
	Check_Pointer (m_NodePath);
}
CPathRequest::CPathRequest (CRailGraph *graph)
{
	m_Working = false;
	m_NeedBy = 0;
	m_Done = false;
	m_Strict = false;
	m_UsageAllowed = 0;
	m_NodePath = NULL;
	m_GridPath = NULL;
	m_Who = NULL;
	Check_Pointer (m_NodePath);
}
CPathRequest::CPathRequest (Stuff::Time needby,CRailGraph *graph,unsigned short mask,int sx,int sy,int dx,int dy,MoverAI *who,bool tenmeter)
{
	m_Working = false;
	m_NeedBy = needby;
	m_Done = false;
	m_Strict = false;
	m_UsageAllowed = 0;
	gos_PushCurrentHeap (g_RailHeap);
	m_GridPath = new CGridPath (graph,mask,sx,sy,dx,dy,tenmeter);
	gos_PopCurrentHeap ();
	m_NodePath = NULL;
	m_Who = who;
	Check_Pointer (m_GridPath);
}

CPathRequest::CPathRequest (Stuff::Time needby,CRailGraph *graph,unsigned short mask,Scalar sx,Scalar sy,Scalar dx,Scalar dy,MoverAI *who,bool tenmeter)
{
	m_Working = false;
	m_NeedBy = needby;
	m_Done = false;
	m_Strict = false;
	m_UsageAllowed = 0;
	gos_PushCurrentHeap (g_RailHeap);
	m_GridPath = new CGridPath (graph,mask,sx,sy,dx,dy,tenmeter);
	gos_PopCurrentHeap ();
	m_NodePath = NULL;
	m_Who = who;
	Check_Pointer (m_GridPath);
}

CPathRequest::~CPathRequest (void)
{
	Verify (g_PathManager);
	delete m_NodePath;
	delete m_GridPath;
	m_NodePath = NULL;
	m_GridPath = NULL;
	g_PathManager->RemoveRequest (this);
}

void CPathRequest::Save (MemoryStream *stream)
{
	*stream << m_NeedBy;
	*stream << m_Done;
	*stream << m_UsageAllowed;
	*stream << m_Strict;
	if (m_Who)
	{
		*stream << true;
		*stream << m_Who->objectID;
	}
	else
		*stream << false;
	if (m_NodePath)
	{
		*stream << true;
		m_NodePath->Save (stream);
	}
	else
		*stream << false;
	if (m_GridPath)
	{
		*stream << true;
		m_GridPath->Save (stream);
	}
	else
		*stream << false;
}

void CPathRequest::Load (MemoryStream *stream)
{
	bool flag;
	*stream >> m_NeedBy;
	*stream >> m_Done;
	*stream >> m_UsageAllowed;
	*stream >> m_Strict;
	delete m_NodePath;
	m_Who = NULL;
	*stream >> flag;
	if (flag)
	{
		ObjectID object_id;
		*stream >> object_id;
		m_Who = Cast_Object (MoverAI *,NameTable::GetInstance()->FindData(object_id));
	}

	*stream >> flag;
	gos_PushCurrentHeap (g_RailHeap);
	m_NodePath = NULL;
	if (flag)
	{
		m_NodePath = new CRailPath (g_MissionGraph,m_Who);
		Check_Pointer (m_NodePath);
		m_NodePath->Load (stream);
	}
	delete m_GridPath;
	m_GridPath = NULL;
	*stream >> flag;
	if (flag)
	{
		m_GridPath = new CGridPath (g_MissionGraph);
		Check_Pointer (m_GridPath);
		m_GridPath->Load (stream);
	}
	gos_PopCurrentHeap ();
}

void CPathRequest::Unlock (void)
{
	Verify (m_Who);
	m_Who->ClearPathLock (false,false);
	m_Who->CalcPathLock (this);
}

void CPathRequest::Lock (void)
{
	Verify (m_Who);
	m_Who->CalcPathUnlock (this);
	m_Who->PathLock (false,false);
}

Stuff::Point3D CPathRequest::GetEndPoint (void)
{
	if (m_NodePath)
	{
		return m_NodePath->End ();
	}
	else if (m_GridPath)
	{
		return m_GridPath->End ();
	}
	return Point3D (-1,-1,-1);
}

bool CPathRequest::UpdateSrcDest (CRailNode *src,CRailNode *end)
{
	if (!m_NodePath)
		return false;
	if (m_Done)
		return false;
	m_NodePath->UpdateSrcDest (src,end);
	return true;
}

bool CPathRequest::UpdateSrcDest (const Stuff::Point3D& src,const Stuff::Point3D& end)
{
	if (!m_NodePath)
		return false;
	if (m_Done)
		return false;
	m_NodePath->UpdateSrcDest (src,end);
	return true;
}

bool CPathRequest::UpdateSrcDest (int sx,int sy,int dx,int dy)
{
	if (!m_GridPath)
		return false;
	if (m_Done)
		return false;
	m_GridPath->UpdateSrcDest (sx,sy,dx,dy);
	return true;
}

CPathManager::CPathManager (void)
{
	m_Root = NULL;
}

CPathManager::~CPathManager (void)
{
	Verify (m_Root == NULL);
}

void CPathManager::Save (MemoryStream *stream)
{
	int count;
	CPathRequest *cur;
	cur = m_Root;
	count = 0;
	while (cur)
	{
		count++;
		cur = cur->m_Next;
	}
	*stream << count;
	cur = m_Root;
	while (cur)
	{
		cur->Save (stream);
		cur = cur->m_Next;
	}
}

void CPathManager::Load (MemoryStream *stream)
{
	int count,i;
	*stream >> count;
	gos_PushCurrentHeap (g_RailHeap);
	for (i=0;i<count;i++)
	{
		CPathRequest *cur = new CPathRequest (g_MissionGraph);
		Check_Pointer (cur);
		cur->Load (stream);
		InsertRequest (cur);
	}
	gos_PopCurrentHeap ();
}

CPathRequest *CPathManager::AddRequest (Stuff::Time needby,CRailGraph *graph,CRailNode *start,CRailNode *end,int usage,MoverAI *who,bool tenmeter)
{
	CPathRequest *toret;

	gos_PushCurrentHeap (g_RailHeap);
	toret = new CPathRequest (needby,graph,start,end,usage,who,tenmeter);
	gos_PopCurrentHeap ();
	Check_Pointer (toret);
	InsertRequest (toret);
	return toret;
}

CPathRequest *CPathManager::AddRequest (Stuff::Time needby,CRailGraph *graph,Stuff::Point3D start,Stuff::Point3D end,int usage,MoverAI *who,bool tenmeter)
{
	CPathRequest *toret;
	ConvertPointToGrid (start);
	ConvertPointToGrid (end);
	gos_PushCurrentHeap (g_RailHeap);
	toret = new CPathRequest (needby,graph,start,end,usage,who,tenmeter);
	gos_PopCurrentHeap ();
	Check_Pointer (toret);
	InsertRequest (toret);
	return toret;
}

CPathRequest *CPathManager::AddRequest (Stuff::Time needby,CRailGraph *graph,unsigned short mask,int sx,int sy,int dx,int dy,MoverAI *who,bool tenmeter)
{
	CPathRequest *toret;
	gos_PushCurrentHeap (g_RailHeap);
	toret = new CPathRequest (needby,graph,mask,sx,sy,dx,dy,who,tenmeter);
	gos_PopCurrentHeap ();
	Check_Pointer (toret);
	InsertRequest (toret);
	return toret;
}

CPathRequest *CPathManager::AddRequest (Stuff::Time needby,CRailGraph *graph,unsigned short mask,Scalar sx,Scalar sy,Scalar dx,Scalar dy,MechWarrior4::MoverAI *who,bool tenmeter)
{
	CPathRequest *toret;
	gos_PushCurrentHeap (g_RailHeap);
	toret = new CPathRequest (needby,graph,mask,sx,sy,dx,dy,who,tenmeter);
	gos_PopCurrentHeap ();
	Check_Pointer (toret);
	InsertRequest (toret);
	return toret;
}

void CPathManager::RemoveRequest (CPathRequest *req)
{
	Verify (req);
	if (req->m_Next)
		req->m_Next->m_Prev = req->m_Prev;
	if (req->m_Prev)
		req->m_Prev->m_Next = req->m_Next;
	if (req == m_Root)
		m_Root = req->m_Next;
	req->m_Next = req->m_Prev = NULL;
}

void CPathManager::InsertRequest (CPathRequest *req)
{
	CPathRequest *cur;

	Verify (req);
	req->m_Next = req->m_Prev = NULL;

	if (m_Root == NULL)
	{
		m_Root = req;
	}
	else if (m_Root->m_NeedBy > req->m_NeedBy)
	{
		if (m_Root->Working ())
		{
			req->m_Next = m_Root->m_Next;
			if (m_Root->m_Next)
				m_Root->m_Next->m_Prev = req;
			req->m_Prev = m_Root;
			m_Root->m_Next = req;
		}
		else
		{
			m_Root->m_Prev = req;
			req->m_Next = m_Root;
			m_Root = req;
		}
	}
	else
	{
		cur = m_Root;

		while (cur->m_Next != NULL)
		{
			if (cur->m_NeedBy > req->m_NeedBy)
			{
				req->m_Prev = cur->m_Prev;
				if (cur->m_Prev->m_Next)
					cur->m_Prev->m_Next = req;
				req->m_Next = cur;
				cur->m_Prev = req;
				return;
			}
			cur = cur->m_Next;
		}
		cur->m_Next = req;
		req->m_Prev = cur;
	}
}

bool CPathManager::Execute()
{
	int i;

	MW4AI::g_Rect4DHash->UpdateTime (gos_GetElapsedTime ());
	MW4AI::g_AirRect4DHash->UpdateTime (gos_GetElapsedTime ());

	PATH_LOGIC("PathManager");
#if !defined(NO_TIMERS)
	my_AutoTimer fred ((void *) &tPathTime);
#endif
	for (i = 0;(i<2)&& (!m_FailedCalcFrame);i++)
//	for (i = 0;i<3;i++)
	{
		if (m_Root)
		{
			CPathRequest *cur;

			cur = m_Root;
			m_Root = cur->m_Next;
			cur->m_Next = cur->m_Prev = NULL;
			if (m_Root)
				m_Root->m_Prev = NULL;
			cur->Unlock ();
			cur->m_Working = false;
			if (cur->PathTypeIsNode ())
			{
				Verify (cur->NodePath ());
//				if (cur->NodePath ())
				cur->NodePath ()->CalcPath (cur->UsageAllowed ());
				cur->Done (true);
			}
			else
			{
				if (cur->GridPath ())
				{
					cur->GridPath ()->DoCalcPath ();
					if (cur->GridPath ()->Working ())
					{
						cur->m_Working = true;
						cur->m_Prev = NULL;
						cur->m_Next = m_Root;
						if (m_Root)
							m_Root->m_Prev = cur;
						m_Root = cur;
					}
					else
						cur->Done (true);
				}
			}
			cur->Lock ();
		}
	}
	m_FailedCalcFrame = false;

	return false;
}

void CPathManager::InitializeClass()
{
#if !defined(NO_TIMERS)
	AddStatistic( "Path calculation", "%", gos_timedata, (void*)&tPathTime, 0 );
	AddStatistic( "+- CRailPath::CalcPath()", "%", gos_timedata, (void*)&tCalcPathTime, 0 );
	AddStatistic( "  +- CRailGraph::QuickCheck()", "%", gos_timedata, (void*)&tQuickCheckTime, 0 );
	AddStatistic( "  +- CRailGraph::CreatePathRequests()", "%", gos_timedata, (void*)&tCreatePathRequestsTime, 0 );
	AddStatistic( "  +- path calc part 1", "%", gos_timedata, (void*)&tPathCalc1Time, 0 );
	AddStatistic( "  +- path calc part 2", "%", gos_timedata, (void*)&tPathCalc2Time, 0 );
	AddStatistic( "  +- CRailPath::ConvertNodeToPath()", "%", gos_timedata, (void*)&tNodeToPathTime, 0 );
	AddStatistic( "Static Obstacle", "%", gos_timedata, (void*)&tGridPathTime, 0 );
#endif
}

void CPathManager::TerminateClass()
{
}

void CPathManager::CleanRequests (void)
{
	CPathRequest *request,*temp;
	
	request = m_Root;

	while (request != 0)
	{
		temp = request;
		request = request->m_Next;
		temp->m_Next = NULL;
		temp->m_Prev = NULL;

	}
	m_Root = NULL;

}

unsigned int CPathManager::GetNumPathRequests() const
{
	unsigned int rv = 0;
	CPathRequest* request = m_Root;

	while (request != 0)
	{
		++rv;
		request = request->m_Next;

		if (rv > 1000)
		{
			break;	// just in case ...
		}
	}

	return (rv);
}