#include "MW4Headers.hpp"
#include "server\comline.hpp"
#include "adept\application.hpp"
#include "rail_move.hpp"
#include "ai.hpp"

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

namespace MW4AI
{
	extern Stuff::Scalar MinZ, MaxZ, MinX, MaxX;
	extern __int64	tPathTime,tGridPathTime;
};
namespace MechWarrior4
{
	extern DWORD Path_Calcs_Count;
	extern DWORD GridPath_Calcs_Count;
	extern DWORD QuickPath_Calcs_Count;
	extern DWORD QuickRail_Calcs_Count;
	extern DWORD FailedPath_Calcs_Count;
}

using namespace MW4AI;
using namespace MechWarrior4;


unsigned int CGridPath::m_BlockGrid[BLOCK_GRID_SIZE][BLOCK_GRID_SIZE][2];
int CGridPath::m_BlockOriginX,CGridPath::m_BlockOriginY;
CGridPath *CGridPath::m_WorkingPath=NULL;

CGridPath::CGridPath (CRailGraph *graph,unsigned short mask,Stuff::Scalar sx,Stuff::Scalar sy,Stuff::Scalar dx,Stuff::Scalar dy,bool tenmeter)
{
	m_Working = false;
	m_WorkCount = 0;
	m_TenMeterAllowed = tenmeter;
	m_StartX = (int) (sx);
	m_StartY = (int) (sy);
	m_EndX = (int) (dx);
	m_EndY = (int) (dy);
	m_Calced = false;
	m_DirList.clear ();
	m_InDirPath = m_DirList.begin ();
	m_Graph = graph;
	m_QuickValid = false;
	m_Mask = mask;
}

CGridPath::CGridPath (CRailGraph *graph)
{
	m_Working = false;
	m_WorkCount = 0;
	m_TenMeterAllowed = false;
	m_StartX = 0;
	m_StartY = 0;
	m_EndX = 0;
	m_EndY = 0;
	m_Calced = false;
	m_DirList.clear ();
	m_InDirPath = m_DirList.begin ();
	m_Graph = graph;
	m_QuickValid = false;
	m_Mask = 0;
}

CGridPath::CGridPath (CRailGraph *graph,unsigned short mask,int sx,int sy,int dx,int dy,bool tenmeter)
{
	m_Working = false;
	m_WorkCount = 0;
	m_TenMeterAllowed = tenmeter;
	m_StartX = sx;
	m_StartY = sy;
	m_EndX = dx;
	m_EndY = dy;
	m_Calced = false;
	m_DirList.clear ();
	m_InDirPath = m_DirList.begin ();
	m_Graph = graph;
	m_QuickValid = false;
	m_Mask = mask;
}

CGridPath::~CGridPath (void)
{
	if (m_WorkingPath == this)
		m_WorkingPath = NULL;
	calc.clear ();
	m_DirList.clear ();
}

void CGridPath::UpdateSrcDest (int sx,int sy,int dx,int dy)
{
	if (m_Working)
		return;
	m_StartX = sx;
	m_StartY = sy;
	m_EndX = dx;
	m_EndY = dy;
}

void CGridPath::UpdateSrcDest (Scalar sx,Scalar sy,Scalar dx,Scalar dy)
{
	if (m_Working)
		return;
	m_StartX = (int) (sx);
	m_StartY = (int) (sy);
	m_EndX = (int) (dx);
	m_EndY = (int) (dy);
}

void CGridPath::Save (MemoryStream *stream)
{
	int count;

	stlport::vector<DirElement>::iterator iter;
	*stream << m_TenMeterAllowed;
	*stream << m_StartX;
	*stream << m_StartY;
	*stream << m_EndX;
	*stream << m_EndY;
	*stream << m_Calced;
	*stream << m_QuickValid;
	*stream << m_Mask;
	*stream << m_DirList.size ();
	for (count = 0,iter = m_DirList.begin ();(iter != m_DirList.end ()) && (m_InDirPath != iter);iter++,count++);
	*stream << count;
	for (iter = m_DirList.begin ();iter != m_DirList.end ();iter++)
	{
		stream->WriteBytes (&(*iter),sizeof (*iter));
	}
}

void CGridPath::Load (MemoryStream *stream)
{
	int count,size,i;

	*stream >> m_TenMeterAllowed;
	*stream >> m_StartX;
	*stream >> m_StartY;
	*stream >> m_EndX;
	*stream >> m_EndY;
	*stream >> m_Calced;
	*stream >> m_QuickValid;
	*stream >> m_Mask;
	*stream >> size;
	*stream >> count;
	m_DirList.assign (size,DirElement ());
	for (i=0;i<size;i++)
	{
		stream->ReadBytes (&(m_DirList[i]),sizeof (DirElement));
	}
	m_InDirPath = m_DirList.begin ();
	m_InDirPath += count;
}

void CGridPath::PointList (stlport::vector<Point3D>& dest)
{
	stlport::vector<DirElement>::iterator iter;

	AutoHeap local_heap (g_RailHeap);
	for (iter = m_InDirPath;iter != m_DirList.end ();iter++)
	{
		Scalar x,y;
		if (iter->locX < 0)
		{
			x = (iter->locX*BLOCK_GRID_RES) - (BLOCK_GRID_RES/2.0f);
		}
		else
		{
			x = (iter->locX*BLOCK_GRID_RES) + (BLOCK_GRID_RES/2.0f);
		}
		if (iter->locY < 0)
		{
			y = (iter->locY*BLOCK_GRID_RES) - (BLOCK_GRID_RES/2.0f);
		}
		else
		{
			y = (iter->locY*BLOCK_GRID_RES) + (BLOCK_GRID_RES/2.0f);
		}
		dest.push_back (Point3D (x,0,y));
	}
}

void CGridPath::ConstructQuickPath (void)
{
	m_DirList.clear ();

	Stuff::Point3D extra;
	float sx,sy,dx,dy;
	int ix,iy,ex,ey;
	int count,distcount;
	int startx,starty,endx,endy;

	AutoHeap local_heap (g_RailHeap);
	startx = m_StartX/BLOCK_GRID_RES;
	starty = m_StartY/BLOCK_GRID_RES;
	endx = m_EndX/BLOCK_GRID_RES;
	endy = m_EndY/BLOCK_GRID_RES;
	Verify ((startx != endx) || (starty != endy));

	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);
	distcount = 0;
	m_DirList.push_back (DirElement (ix,iy));
	bool flag = false;
	while (count)
	{
		count--;
		sx += dx;
		sy += dy;
		ix = (int) (sx);
		iy = (int) (sy);
		distcount += BLOCK_GRID_RES;

		flag = false;
		m_DirList.push_back (DirElement (ix,iy));
		flag = true;
	}
	if (!flag)
	{
		m_DirList.push_back (DirElement (ix,iy));
	}

	m_InDirPath = m_DirList.begin ();
	m_Calced = true;
}

void CGridPath::DumpPath (void)
{
	stlport::vector<DirElement>::iterator iter;
	char text[100];
	
	iter = m_DirList.begin ();
	while (iter != m_DirList.end ())
	{
		sprintf (text,"Move Point %d,%d",iter->locX,iter->locY);
		COMSPEW (text);
		iter++;
	}
	COMSPEW ("");
}


void CGridPath::Calc30Path (void)
{
	Verify (m_Graph);
	int i;
	GridPathElement temp;
	bool done,hackflag=false;
	bool allow10early=false;
	bool did10early=false;
	int startx,starty,endx,endy;
	const int SIZEDELTA = 8;
	static int delta[SIZEDELTA][5] = {			// first, second, cost
		{0,-1,7,1,10},	
		{1,-1,0,2,15},	
		{1,0,1,3,10},	
		{1,1,2,4,15},	
		{0,1,5,3,10},	
		{-1,1,6,4,15},	
		{-1,0,7,5,10},	
		{-1,-1,6,0,15}
	};
	AutoHeap local_heap (g_RailHeap);

	PATH_LOGIC("Calc30Path");
#if !defined(NO_TIMERS)
	my_AutoTimer fred ((void *) &tGridPathTime);
#endif
	Set_Statistic(GridPath_Calcs_Count, GridPath_Calcs_Count+1);

	m_QuickValid = false;

	startx = m_StartX/BLOCK_GRID_RES;
	starty = m_StartY/BLOCK_GRID_RES;
	endx = m_EndX/BLOCK_GRID_RES;
	endy = m_EndY/BLOCK_GRID_RES;
#if 0
	Point3D srcpt,destpt,extra;

	destpt.x = (float) m_EndX;
	destpt.z = (float) m_EndY;
	srcpt.x = (float) m_StartX;
	srcpt.z = (float) m_StartY;
	destpt.y = 0;
	srcpt.y = 0;
	extra = m_Graph->PullPointElementMask (srcpt,destpt,m_Mask);
	Verify (extra != Point3D (-1,-1,-1));
	endx = (int) (extra.x / BLOCK_GRID_RES);
	endy = (int) (extra.z / BLOCK_GRID_RES);

	memset (m_BlockGrid,0,sizeof (unsigned int) * BLOCK_GRID_SIZE * BLOCK_GRID_SIZE * 2);

	int dx,dy;
	dx = startx>endx ? startx - endx : endx - startx;
	dy = starty>endy ? starty - endy : endy - starty;

	Verify (dx < BLOCK_GRID_SIZE);
	Verify (dy < BLOCK_GRID_SIZE);

	m_BlockOriginX = startx - ((BLOCK_GRID_SIZE - dx) >> 1);
	m_BlockOriginY = starty - ((BLOCK_GRID_SIZE - dy) >> 1);
	if (m_BlockOriginX < MinX)
		m_BlockOriginX = (int) MinX;
	if (m_BlockOriginY < MinZ)
		m_BlockOriginY = (int) MinZ;
	
	if (m_BlockOriginX >= (MaxX - BLOCK_GRID_SIZE))
		m_BlockOriginX = (int) ( MaxX - BLOCK_GRID_SIZE - 1);
	if (m_BlockOriginY >= (MaxZ - BLOCK_GRID_SIZE))
		m_BlockOriginY = (int) ( MaxZ - BLOCK_GRID_SIZE-1 - 1);


	Cost (startx,starty,1,(unsigned short) CostGuess (startx,starty));
	calc.Insert (GridPathElement (startx,starty));
#endif
	GridPathElement end (endx,endy);
	done = false;
	m_Calced = false;

	{
		hackflag = false;
	top:
		int count = 0;

		while (!done)
		{
			count++;
			if (count > GRIDMOVE_FAIL_THRESHOLD)
				break;
			if (!calc.size ())
				break;
			temp = calc.ExtractMin ();
			if (temp == end)
			{
				done = true;
				break;
			}
			int cost;

			cost = Cost (temp);
			if (cost > 1)
				allow10early = false;
			for (i=0;i<SIZEDELTA;i++)
			{
				int newcost;
				int newx,newy;

				newx = temp.locX+delta[i][0];
				newy = temp.locY+delta[i][1];
				if (!m_Graph->MoveTo (newx,newy,i,m_Mask))
					continue;
				if (!allow10early)
				{
					int newx,newy;
					{
						newx = temp.locX+delta[delta[i][2]][0];
						newy = temp.locY+delta[delta[i][2]][1];
						if (!m_Graph->MoveTo (newx,newy,delta[i][2],m_Mask))
							continue;
					}
					{
						newx = temp.locX+delta[delta[i][3]][0];
						newy = temp.locY+delta[delta[i][3]][1];
						if (!m_Graph->MoveTo (newx,newy,delta[i][3],m_Mask))
							continue;
					}
				}
				int updown = m_Graph->UpDownTo (newx,newy,delta[i][3],m_Mask);
				if (updown == 0)
				{
					newcost = delta[i][4];
				}
				else if (updown == 1)
				{
					if (delta[i][4] == 10)
						newcost = 14;
					else
						newcost = 21;
				}
				else
				{
					if (delta[i][4] == 10)
						newcost = 13;
					else
						newcost = 19;
				}
				newcost += cost;
				newcost += AdjustCost (newx,newy);
				if (Cost (newx,newy,(unsigned int) newcost,(unsigned int) CostGuess (newx,newy)))
				{
					GridPathElement newelement(newx,newy);
					if (calc.Member (newelement))
					{
						calc.DecreaseKey (newelement);
					}
					else
					{
						calc.Insert	(newelement);
					}
				}
			}
		}

		if ((!hackflag) && (count <= 2) && (!done))		// all cells around itself are blocked
		{
			Cost (startx,starty,1,(unsigned short) CostGuess (startx,starty));
			calc.clear ();
			calc.Insert (GridPathElement (startx,starty));
			allow10early = true;
			did10early = true;
			hackflag = true;
			goto top;
		}
		
		if (!done && (count > GRIDMOVE_FAIL_THRESHOLD))					// check for can continue work later
		{
			m_WorkCount += count;
			if (m_WorkCount > GRIDMOVE_MAX_CHECK_THRESHOLD)
			{
				calc.clear ();
				m_Calced = false;
				g_PathManager->m_FailedCalcFrame = true;
				Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
				m_Working = false;
			}
			return;
		}
		else if (!done)																					// visited all possible cells
		{
			calc.clear ();
			m_Calced = false;
			g_PathManager->m_FailedCalcFrame = true;
			Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
			m_Working = false;
			return;
		}

		done = false;
		GridPathElement cur;
		int min;
		unsigned int mincost;
		int newx,newy;
		int lastdir = -1;
		GridPathElement start(startx,starty);
		int t;
		Stuff::Scalar dirdist = 0;
		

		cur.locX = endx;
		cur.locY = endy;
		m_DirList.push_back (DirElement (cur.locX,cur.locY));
		calc.clear ();

		min = NULL;
		count=0;
		while (!done)
		{
#if 1
			count++;
			if (count > (GRIDMOVE_MAX_CHECK_THRESHOLD/2))
			{
				Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
				m_Calced = false;
				g_PathManager->m_FailedCalcFrame = true;
				break;
			}
#endif
			min = 0;
			newx = cur.locX + delta[min][0];
			newy = cur.locY + delta[min][1];
			mincost = Cost (newx,newy);
			for (i=1;i<SIZEDELTA;i++)
			{
				if ((newx == startx) && (newy == starty))
				{
					min = i-1;
					mincost = 1;
					break;
				}
				newx = cur.locX + delta[i][0];
				newy = cur.locY + delta[i][1];
				t= Cost (newx,newy);

				if (t  && !hackflag && (t <= 16))
				{
					int newx,newy;

					{
						{
							newx = cur.locX+delta[delta[i][2]][0];
							newy = cur.locY+delta[delta[i][2]][1];
							if (!m_Graph->MoveTo (newx,newy,delta[i][2]+4,m_Mask))
								t = 0;
						}
						{
							newx = cur.locX+delta[delta[i][3]][0];
							newy = cur.locY+delta[delta[i][3]][1];
							if (!m_Graph->MoveTo (newx,newy,delta[i][3]+4,m_Mask))
								t = 0;
						}
					}

				}

				if (((t < mincost) && (t != 0)) || (mincost == 0))
				{
					mincost = t;
					min = i;
				}
			}
			if (mincost == 0)
			{
				min = 0;
				newx = cur.locX + delta[min][0];
				newy = cur.locY + delta[min][1];
				mincost = Cost (newx,newy);
				for (i=1;i<SIZEDELTA;i++)
				{
					newx = cur.locX + delta[i][0];
					newy = cur.locY + delta[i][1];
					t= Cost (newx,newy);

					if (((t < mincost) && (t != 0)) || (mincost == 0))
					{
						mincost = t;
						min = i;
					}
				}
			}
			Verify (mincost != 0);
			cur.locX+=delta[min][0];
			cur.locY+=delta[min][1];
			dirdist += BLOCK_GRID_RES;
			bool dirflag = false;
			if (lastdir != min)
			{
				dirdist = 0;
				m_DirList.push_back (DirElement (cur.locX,cur.locY));
				dirflag = true;
				lastdir = min;
			}
			if (cur == start)
			{
				if (!dirflag)
					m_DirList.push_back (DirElement (cur.locX,cur.locY));
				done = true;
				m_Calced = true;
				continue;
			}
		}
	}
	if (!m_Calced)
	{
//		STOP (("should not get here.  We have already weighted cells from start to finish"));
		Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
		g_PathManager->m_FailedCalcFrame = true;
		m_Working = false;
		return;
	}
	ReverseList ();
	Verify (m_DirList.size () >= 2);
//	SmoothList ();
	m_InDirPath = m_DirList.begin ();
	if (m_InDirPath != m_DirList.end ())
	{
		m_InDirPath++;
	}
	m_Calced = true;
	m_Working = false;
}

void CGridPath::Calc10Path (void)
{
	Verify (m_Graph);
	int i;
	GridPathElement temp;
	bool done,hackflag=false;
	int startx,starty,endx,endy;
	const int SIZEDELTA = 8;
	static int delta[SIZEDELTA][5] = {			// first, second, cost
		{0,-1,7,1,10},	
		{1,-1,0,2,15},	
		{1,0,1,3,10},	
		{1,1,2,4,15},	
		{0,1,5,3,10},	
		{-1,1,6,4,15},	
		{-1,0,7,5,10},	
		{-1,-1,6,0,15}
	};
	AutoHeap local_heap (g_RailHeap);

	PATH_LOGIC("Calc10Path");
#if !defined(NO_TIMERS)
	my_AutoTimer fred ((void *) &tGridPathTime);
#endif
	Set_Statistic(GridPath_Calcs_Count, GridPath_Calcs_Count+1);

	m_QuickValid = false;

	startx = m_StartX/BLOCK_GRID_RES;
	starty = m_StartY/BLOCK_GRID_RES;
	endx = m_EndX/BLOCK_GRID_RES;
	endy = m_EndY/BLOCK_GRID_RES;
	Point3D srcpt,destpt,extra;
#if 0
	memset (m_BlockGrid,0,sizeof (unsigned int) * BLOCK_GRID_SIZE * BLOCK_GRID_SIZE * 2);
	int dx,dy;
	dx = startx>endx ? startx - endx : endx - startx;
	dy = starty>endy ? starty - endy : endy - starty;

	Verify (dx < BLOCK_GRID_SIZE);
	Verify (dy < BLOCK_GRID_SIZE);

	m_BlockOriginX = startx - ((BLOCK_GRID_SIZE - dx) >> 1);
	m_BlockOriginY = starty - ((BLOCK_GRID_SIZE - dy) >> 1);
	if (m_BlockOriginX < MinX)
		m_BlockOriginX = (int) MinX;
	if (m_BlockOriginY < MinZ)
		m_BlockOriginY = (int) MinZ;
	
	if (m_BlockOriginX >= (MaxX - BLOCK_GRID_SIZE))
		m_BlockOriginX = (int) ( MaxX - BLOCK_GRID_SIZE - 1);
	if (m_BlockOriginY >= (MaxZ - BLOCK_GRID_SIZE))
		m_BlockOriginY = (int) ( MaxZ - BLOCK_GRID_SIZE-1 - 1);

	Cost (startx,starty,1,(unsigned short) CostGuess (startx,starty));
	calc.Insert (GridPathElement (startx,starty));
#endif

	GridPathElement end (endx,endy);
	done = false;
	m_Calced = false;

	{
		hackflag = false;
	top:
		int count = 0;

		while (!done)
		{
			count++;
			if (count > GRIDMOVE_FAIL_THRESHOLD)
				break;
			if (!calc.size ())
				break;
			temp = calc.ExtractMin ();
			if (temp == end)
			{
				done = true;
				break;
			}
			int cost;

			cost = Cost (temp);
			for (i=0;i<SIZEDELTA;i++)
			{
				int newcost;
				int newx,newy;

				newx = temp.locX+delta[i][0];
				newy = temp.locY+delta[i][1];
				if (!m_Graph->MoveTo (newx,newy,i,m_Mask))
					continue;
				int updown = m_Graph->UpDownTo (newx,newy,delta[i][3],m_Mask);
				if (updown == 0)
				{
					newcost = delta[i][4];
				}
				else if (updown == 1)
				{
					if (delta[i][4] == 10)
						newcost = 14;
					else
						newcost = 21;
				}
				else
				{
					if (delta[i][4] == 10)
						newcost = 13;
					else
						newcost = 19;
				}
				newcost += cost;
				newcost += AdjustCost (newx,newy);
				if (Cost (newx,newy,(unsigned int) newcost,(unsigned int) CostGuess (newx,newy)))
				{
					GridPathElement newelement(newx,newy);
					if (calc.Member (newelement))
						calc.DecreaseKey (newelement);
					else
						calc.Insert	(newelement);
				}
			}
		}

		if (!done && (count > GRIDMOVE_FAIL_THRESHOLD))					// check for can continue work later
		{
			m_WorkCount += count;
			if (m_WorkCount > GRIDMOVE_MAX_CHECK_THRESHOLD)
			{
				calc.clear ();
				m_Calced = false;
				g_PathManager->m_FailedCalcFrame = true;
				Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
				m_Working = false;
			}
			return;
		}
		else if (!done)																					// visited all possible cells
		{
			calc.clear ();
			m_Calced = false;
			g_PathManager->m_FailedCalcFrame = true;
			Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
			m_Working = false;
			return;
		}

		calc.clear ();
		done = false;
		GridPathElement cur;
		int min;
		unsigned int mincost;
		int newx,newy;
		int lastdir = -1;
		GridPathElement start(startx,starty);
		int t;
		Stuff::Scalar dirdist = 0;

		cur.locX = endx;
		cur.locY = endy;
		m_DirList.push_back (DirElement (cur.locX,cur.locY));

		min = NULL;
		count=0;
		while (!done)
		{
#if 1
			count++;
			if (count > (GRIDMOVE_MAX_CHECK_THRESHOLD/2))
			{
				Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
				m_Calced = false;
				g_PathManager->m_FailedCalcFrame = true;
				break;
			}
#endif
			min = 0;
			newx = cur.locX + delta[min][0];
			newy = cur.locY + delta[min][1];
			mincost = Cost (newx,newy);
			for (i=1;i<SIZEDELTA;i++)
			{
				if ((newx == startx) && (newy == starty))
				{
					min = i-1;
					mincost = 1;
					break;
				}
				newx = cur.locX + delta[i][0];
				newy = cur.locY + delta[i][1];
				t= Cost (newx,newy);

				if (((t < mincost) && (t != 0)) || (mincost == 0))
				{
					mincost = t;
					min = i;
				}
			}
			if (mincost == 0)
			{
				min = 0;
				newx = cur.locX + delta[min][0];
				newy = cur.locY + delta[min][1];
				mincost = Cost (newx,newy);
				for (i=1;i<SIZEDELTA;i++)
				{
					newx = cur.locX + delta[i][0];
					newy = cur.locY + delta[i][1];
					t= Cost (newx,newy);

					if (((t < mincost) && (t != 0)) || (mincost == 0))
					{
						mincost = t;
						min = i;
					}
				}
			}
			Verify (mincost != 0);
			cur.locX+=delta[min][0];
			cur.locY+=delta[min][1];
			dirdist += BLOCK_GRID_RES;
			bool dirflag = false;
			if (lastdir != min)
			{
				dirdist = 0;
				m_DirList.push_back (DirElement (cur.locX,cur.locY));
				dirflag = true;
				lastdir = min;
			}
			if (cur == start)
			{
				if (!dirflag)
					m_DirList.push_back (DirElement (cur.locX,cur.locY));
				done = true;
				m_Calced = true;
				continue;
			}
		}
	}
	if (!m_Calced)
	{
		STOP (("should not get here. have already created costs from start to end"));
		Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
		g_PathManager->m_FailedCalcFrame = true;
		m_Working = false;
		return;
	}
	ReverseList ();
//	SmoothList ();
	Verify (m_DirList.size () >= 2);
	m_InDirPath = m_DirList.begin ();
	if (m_InDirPath != m_DirList.end ())
	{
		m_InDirPath++;
	}
	m_Calced = true;
	m_Working = false;
}

void CGridPath::ContinueCalcPath (void)
{
	Verify (m_Working);
	Verify (m_WorkingPath == this);
	if (m_TenMeterAllowed)
	{
		Calc10Path ();
	}
	else
	{
		Calc30Path ();
	}
}

void CGridPath::DoCalcPath (void)
{
	if (m_Working)
	{
		Verify (m_WorkingPath == this);
		ContinueCalcPath ();
	}
	else
	{
		StartCalcPath ();
	}
	if (!m_Working)
		m_WorkingPath = NULL;
}

void CGridPath::StartCalcPath (void)
{
	Verify (m_Graph);
	GridPathElement temp;
	int startx,starty,endx,endy;
	AutoHeap local_heap (g_RailHeap);

	Verify (!m_Working);
	Verify (m_WorkingPath == NULL);
	PATH_LOGIC("CalcPath");
#if !defined(NO_TIMERS)
	my_AutoTimer fred ((void *) &tGridPathTime);
#endif
	Set_Statistic(GridPath_Calcs_Count, GridPath_Calcs_Count+1);

	m_QuickValid = false;

	startx = m_StartX/BLOCK_GRID_RES;
	starty = m_StartY/BLOCK_GRID_RES;
	endx = m_EndX/BLOCK_GRID_RES;
	endy = m_EndY/BLOCK_GRID_RES;
	if ((startx == endx) && (starty == endy))
	{
		m_Calced = true;
		m_QuickValid = true;
		return;
	}

	Point3D srcpt,destpt,extra;

	destpt.x = (float) m_EndX;
	destpt.z = (float) m_EndY;
	srcpt.x = (float) m_StartX;
	srcpt.z = (float) m_StartY;
	destpt.y = 0;
	srcpt.y = 0;
	extra = m_Graph->PullPointElementMask (srcpt,destpt,m_Mask);
	if (extra == Point3D (-1,-1,-1))
	{
		Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
		m_Calced = false;
		g_PathManager->m_FailedCalcFrame = true;
		return;
	}
	m_EndX = (int) extra.x;
	m_EndY = (int) extra.z;
	endx = (int) (m_EndX / BLOCK_GRID_RES);
	endy = (int) (m_EndY / BLOCK_GRID_RES);
	if ((startx == endx) && (starty == endy))
	{
		m_Calced = true;
		m_QuickValid = true;
		return;
	}
	if (m_Graph->QuickCheck (m_StartX,m_StartY,m_EndX,m_EndY,m_Mask,!m_TenMeterAllowed))
	{
		Set_Statistic(QuickPath_Calcs_Count, QuickPath_Calcs_Count+1);
		m_QuickValid = true;
		m_Calced = true;
		return;
	}

	memset (m_BlockGrid,0,sizeof (unsigned int) * BLOCK_GRID_SIZE * BLOCK_GRID_SIZE * 2);

	int dx,dy;
	dx = startx>endx ? startx - endx : endx - startx;
	dy = starty>endy ? starty - endy : endy - starty;

	if (dx >= BLOCK_GRID_SIZE)
	{
		Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
		m_Calced = false;
		g_PathManager->m_FailedCalcFrame = true;
		return;
	}
	if (dy >= BLOCK_GRID_SIZE)
	{
		Set_Statistic(FailedPath_Calcs_Count, FailedPath_Calcs_Count+1);
		m_Calced = false;
		g_PathManager->m_FailedCalcFrame = true;
		return;
	}

	m_BlockOriginX = startx - ((BLOCK_GRID_SIZE - dx) >> 1);
	m_BlockOriginY = starty - ((BLOCK_GRID_SIZE - dy) >> 1);
	if (m_BlockOriginX < MinX)
		m_BlockOriginX = (int) MinX;
	if (m_BlockOriginY < MinZ)
		m_BlockOriginY = (int) MinZ;
	
	if (m_BlockOriginX >= (MaxX - BLOCK_GRID_SIZE))
		m_BlockOriginX = (int) ( MaxX - BLOCK_GRID_SIZE - 1);
	if (m_BlockOriginY >= (MaxZ - BLOCK_GRID_SIZE))
		m_BlockOriginY = (int) ( MaxZ - BLOCK_GRID_SIZE-1 - 1);

	Cost (startx,starty,1,(unsigned short) CostGuess (startx,starty));
	calc.Insert (GridPathElement (startx,starty));

	m_WorkingPath = this;
	m_WorkCount = 0;
	m_Working = true;
	ContinueCalcPath ();
}

void CGridPath::SmoothListHelper (stlport::vector<DirElement>& vec,int s,int e)
{
	int x1,x2,y1,y2,middle;

	AutoHeap local_heap (g_RailHeap);
	x1 = m_DirList[s].locX;
	y1 = m_DirList[s].locY;
	x2 = m_DirList[e].locX;
	y2 = m_DirList[e].locY;
	if (s >= e)
	{
		vec.push_back (DirElement (x1,y1));
		vec.push_back (DirElement (x2,y2));
		return;
	}
	Verify (e<m_DirList.size ());

	if (m_Graph->QuickCheck (x1,y1,x2,y2,m_Mask,!m_TenMeterAllowed))
	{
		vec.push_back (DirElement (x2,y2));
		return;
	}
	middle = s+e;
	middle >>= 1;
	SmoothListHelper (vec,s,middle);
	SmoothListHelper (vec,middle+1,e);

}

void CGridPath::SmoothList (void)
{
	stlport::vector<DirElement> newlist;
	int middle;
	AutoHeap local_heap (g_RailHeap);

	if (m_DirList.size () <=2 )
		return;

	newlist.clear ();
	middle = m_DirList.size ();
	middle >>= 1;

	newlist.push_back (m_DirList[0]);
	SmoothListHelper (newlist,0,middle-1);
	SmoothListHelper (newlist,middle,m_DirList.size ()-1);
	m_DirList = newlist;
}

void CGridPath::ReverseList (void)
{
	stlport::vector<DirElement>::iterator s,e;
	AutoHeap local_heap (g_RailHeap);

	s = m_DirList.begin ();
	e = m_DirList.end ();
	stlport::reverse (s,e);
}